{"action":"created","comment":{"url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/pulls/comments/178772264","pull_request_review_id":108875277,"id":178772264,"diff_hunk":"@@ -0,0 +1,190 @@\n+import math\n+import rospy\n+import tf2_ros\n+import tf2_geometry_msgs\n+import threading\n+\n+from geometry_msgs.msg import TwistStamped\n+from geometry_msgs.msg import Point\n+from geometry_msgs.msg import PointStamped\n+\n+from task_utilities.acceleration_limiter import AccelerationLimiter\n+\n+from .abstract_task import AbstractTask\n+from iarc_tasks.task_states import (TaskRunning,\n+                                    TaskDone,\n+                                    TaskCanceled,\n+                                    TaskAborted,\n+                                    TaskFailed)\n+from iarc_tasks.task_commands import (VelocityCommand, NopCommand)\n+\n+from translate_stop_planner import TranslateStopPlanner\n+\n+class GoToRoombaState(object):\n+    init = 0\n+    translate = 1\n+\n+class GoToRoombaTask(AbstractTask):\n+\n+    def __init__(self, task_request):\n+        super(GoToRoombaTask, self).__init__()\n+\n+        self._roomba_id = task_request.frame_id + '/base_link'\n+\n+        if self._roomba_id is None:\n+            raise ValueError('A null roomba id was provided')\n+\n+        self._roomba_odometry = None\n+        self._roomba_point = None\n+        self._limiter = AccelerationLimiter()\n+\n+        self._canceled = False;\n+        self._last_update_time = None\n+        self._current_velocity = None\n+        self._transition = None\n+\n+        self._lock = threading.RLock()\n+\n+        self._x_position = task_request.x_position\n+        self._y_position = task_request.y_position\n+\n+\n+        try:\n+            self._TRANSLATION_XYZ_TOLERANCE = rospy.get_param('~translation_xyz_tolerance')\n+            self._TRANSFORM_TIMEOUT = rospy.get_param('~transform_timeout')\n+            self._MIN_MANEUVER_HEIGHT = rospy.get_param('~min_maneuver_height')\n+            self._MAX_HORIZ_SPEED = rospy.get_param('~max_translation_speed')\n+            self._MAX_TASK_DIST = rospy.get_param('~max_roomba_dist')\n+            self._K_X = rospy.get_param('~k_term_tracking_x')\n+            self._K_Y = rospy.get_param('~k_term_tracking_y')\n+            self._z_position = rospy.get_param('~track_roomba_height')\n+        except KeyError as e:\n+            rospy.logerr('Could not lookup a parameter for xyztranslation task')\n+            raise\n+\n+        # Check that we aren't being requested to go below the minimum maneuver height\n+        # Error straight out if that's the case. If we are currently below the minimum height\n+        # It will be caught and handled on the next update\n+        if self._z_position < self._MIN_MANEUVER_HEIGHT :\n+            raise ValueError('Requested z height was below the minimum maneuver height')\n+\n+        # fc status?\n+        self._path_holder = TranslateStopPlanner(self._x_position,\n+                                                    self._y_position,\n+                                                    self._z_position)\n+\n+        self._state = GoToRoombaState.init\n+\n+\n+    def get_desired_command(self):\n+        with self._lock:\n+            if self._canceled:\n+                return (TaskCanceled(),)\n+            if self._state == GoToRoombaState.init:\n+                self._state = GoToRoombaState.translate\n+\n+            if self._state == GoToRoombaState.translate:\n+                try:\n+                    transStamped = self.topic_buffer.get_tf_buffer().lookup_transform(\n+                            'map',\n+                            'quad',\n+                            rospy.Time(0),\n+                            rospy.Duration(self._TRANSFORM_TIMEOUT))\n+                except (tf2_ros.LookupException,\n+                        tf2_ros.ConnectivityException,\n+                        tf2_ros.ExtrapolationException) as ex:\n+                    rospy.logerr('GoToRoombaTask: Exception when looking up transform')\n+                    rospy.logerr(ex.message)\n+                    return (TaskAborted(msg = 'Exception when looking up transform during go_to_roomba'),)\n+\n+                if(transStamped.transform.translation.z > self._MIN_MANEUVER_HEIGHT):\n+                    \"\"\" quarantine\"\"\"\n+                    # check if the roomba is in range\n+                    if not self._check_roomba_in_sight():\n+                        return (TaskAborted(msg='The provided roomba is not in sight of quad'),)\n+                    elif self._on_ground():\n+                        return (TaskDone(),)\n+\n+                    try:\n+                        roomba_transform = self.topic_buffer.get_tf_buffer().lookup_transform(","path":"src/iarc7_motion/iarc_tasks/go_to_roomba_task.py","position":109,"original_position":109,"commit_id":"dc7d00540c5804b78123936534c235d27fbe92d2","original_commit_id":"dc7d00540c5804b78123936534c235d27fbe92d2","user":{"login":"asaba96","id":18127273,"avatar_url":"https://avatars2.githubusercontent.com/u/18127273?v=4","gravatar_id":"","url":"https://api.github.com/users/asaba96","html_url":"https://github.com/asaba96","followers_url":"https://api.github.com/users/asaba96/followers","following_url":"https://api.github.com/users/asaba96/following{/other_user}","gists_url":"https://api.github.com/users/asaba96/gists{/gist_id}","starred_url":"https://api.github.com/users/asaba96/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/asaba96/subscriptions","organizations_url":"https://api.github.com/users/asaba96/orgs","repos_url":"https://api.github.com/users/asaba96/repos","events_url":"https://api.github.com/users/asaba96/events{/privacy}","received_events_url":"https://api.github.com/users/asaba96/received_events","type":"User","site_admin":false},"body":"Same comment as below. Why do we need to lookup any transforms? Why can’t we just use the current roomba position in ‘self._roomba_odometry‘? Is it to guarantee the appropriate frame? ","created_at":"2018-04-03T10:01:42Z","updated_at":"2018-04-03T10:21:32Z","html_url":"https://github.com/Pitt-RAS/iarc7_motion/pull/123#discussion_r178772264","pull_request_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/pulls/123","author_association":"CONTRIBUTOR","_links":{"self":{"href":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/pulls/comments/178772264"},"html":{"href":"https://github.com/Pitt-RAS/iarc7_motion/pull/123#discussion_r178772264"},"pull_request":{"href":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/pulls/123"}}},"pull_request":{"url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/pulls/123","id":178427938,"html_url":"https://github.com/Pitt-RAS/iarc7_motion/pull/123","diff_url":"https://github.com/Pitt-RAS/iarc7_motion/pull/123.diff","patch_url":"https://github.com/Pitt-RAS/iarc7_motion/pull/123.patch","issue_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/issues/123","number":123,"state":"open","locked":false,"title":"Go to roomba","user":{"login":"Quinnan-Gill","id":5713287,"avatar_url":"https://avatars0.githubusercontent.com/u/5713287?v=4","gravatar_id":"","url":"https://api.github.com/users/Quinnan-Gill","html_url":"https://github.com/Quinnan-Gill","followers_url":"https://api.github.com/users/Quinnan-Gill/followers","following_url":"https://api.github.com/users/Quinnan-Gill/following{/other_user}","gists_url":"https://api.github.com/users/Quinnan-Gill/gists{/gist_id}","starred_url":"https://api.github.com/users/Quinnan-Gill/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/Quinnan-Gill/subscriptions","organizations_url":"https://api.github.com/users/Quinnan-Gill/orgs","repos_url":"https://api.github.com/users/Quinnan-Gill/repos","events_url":"https://api.github.com/users/Quinnan-Gill/events{/privacy}","received_events_url":"https://api.github.com/users/Quinnan-Gill/received_events","type":"User","site_admin":false},"body":"This is a new file in order that uses the translate stop planner to move to a roomba making it them easier to use track roomba","created_at":"2018-03-29T20:05:27Z","updated_at":"2018-04-03T10:21:32Z","closed_at":null,"merged_at":null,"merge_commit_sha":"0be5c940397f3536d03bce8c47304d315ac91d7f","assignee":null,"assignees":[],"requested_reviewers":[],"requested_teams":[],"labels":[],"milestone":null,"commits_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/pulls/123/commits","review_comments_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/pulls/123/comments","review_comment_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/pulls/comments{/number}","comments_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/issues/123/comments","statuses_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/statuses/dc7d00540c5804b78123936534c235d27fbe92d2","head":{"label":"Pitt-RAS:go_to_roomba","ref":"go_to_roomba","sha":"dc7d00540c5804b78123936534c235d27fbe92d2","user":{"login":"Pitt-RAS","id":15024509,"avatar_url":"https://avatars3.githubusercontent.com/u/15024509?v=4","gravatar_id":"","url":"https://api.github.com/users/Pitt-RAS","html_url":"https://github.com/Pitt-RAS","followers_url":"https://api.github.com/users/Pitt-RAS/followers","following_url":"https://api.github.com/users/Pitt-RAS/following{/other_user}","gists_url":"https://api.github.com/users/Pitt-RAS/gists{/gist_id}","starred_url":"https://api.github.com/users/Pitt-RAS/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/Pitt-RAS/subscriptions","organizations_url":"https://api.github.com/users/Pitt-RAS/orgs","repos_url":"https://api.github.com/users/Pitt-RAS/repos","events_url":"https://api.github.com/users/Pitt-RAS/events{/privacy}","received_events_url":"https://api.github.com/users/Pitt-RAS/received_events","type":"Organization","site_admin":false},"repo":{"id":67438943,"name":"iarc7_motion","full_name":"Pitt-RAS/iarc7_motion","owner":{"login":"Pitt-RAS","id":15024509,"avatar_url":"https://avatars3.githubusercontent.com/u/15024509?v=4","gravatar_id":"","url":"https://api.github.com/users/Pitt-RAS","html_url":"https://github.com/Pitt-RAS","followers_url":"https://api.github.com/users/Pitt-RAS/followers","following_url":"https://api.github.com/users/Pitt-RAS/following{/other_user}","gists_url":"https://api.github.com/users/Pitt-RAS/gists{/gist_id}","starred_url":"https://api.github.com/users/Pitt-RAS/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/Pitt-RAS/subscriptions","organizations_url":"https://api.github.com/users/Pitt-RAS/orgs","repos_url":"https://api.github.com/users/Pitt-RAS/repos","events_url":"https://api.github.com/users/Pitt-RAS/events{/privacy}","received_events_url":"https://api.github.com/users/Pitt-RAS/received_events","type":"Organization","site_admin":false},"private":false,"html_url":"https://github.com/Pitt-RAS/iarc7_motion","description":"Motion Planning and Control code for generic multirotor, developed for IARC Mission 7","fork":false,"url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion","forks_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/forks","keys_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/keys{/key_id}","collaborators_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/collaborators{/collaborator}","teams_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/teams","hooks_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/hooks","issue_events_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/issues/events{/number}","events_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/events","assignees_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/assignees{/user}","branches_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/branches{/branch}","tags_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/tags","blobs_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/git/blobs{/sha}","git_tags_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/git/tags{/sha}","git_refs_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/git/refs{/sha}","trees_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/git/trees{/sha}","statuses_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/statuses/{sha}","languages_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/languages","stargazers_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/stargazers","contributors_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/contributors","subscribers_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/subscribers","subscription_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/subscription","commits_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/commits{/sha}","git_commits_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/git/commits{/sha}","comments_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/comments{/number}","issue_comment_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/issues/comments{/number}","contents_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/contents/{+path}","compare_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/compare/{base}...{head}","merges_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/merges","archive_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/{archive_format}{/ref}","downloads_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/downloads","issues_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/issues{/number}","pulls_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/pulls{/number}","milestones_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/milestones{/number}","notifications_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/notifications{?since,all,participating}","labels_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/labels{/name}","releases_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/releases{/id}","deployments_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/deployments","created_at":"2016-09-05T17:23:18Z","updated_at":"2018-04-02T21:20:50Z","pushed_at":"2018-04-02T21:42:22Z","git_url":"git://github.com/Pitt-RAS/iarc7_motion.git","ssh_url":"git@github.com:Pitt-RAS/iarc7_motion.git","clone_url":"https://github.com/Pitt-RAS/iarc7_motion.git","svn_url":"https://github.com/Pitt-RAS/iarc7_motion","homepage":"","size":12436,"stargazers_count":1,"watchers_count":1,"language":"Python","has_issues":true,"has_projects":true,"has_downloads":true,"has_wiki":true,"has_pages":false,"forks_count":1,"mirror_url":null,"archived":false,"open_issues_count":2,"license":{"key":"other","name":"Other","spdx_id":null,"url":null},"forks":1,"open_issues":2,"watchers":1,"default_branch":"master"}},"base":{"label":"Pitt-RAS:master","ref":"master","sha":"05cbb4b67746273b2c457a07570d6176a093e716","user":{"login":"Pitt-RAS","id":15024509,"avatar_url":"https://avatars3.githubusercontent.com/u/15024509?v=4","gravatar_id":"","url":"https://api.github.com/users/Pitt-RAS","html_url":"https://github.com/Pitt-RAS","followers_url":"https://api.github.com/users/Pitt-RAS/followers","following_url":"https://api.github.com/users/Pitt-RAS/following{/other_user}","gists_url":"https://api.github.com/users/Pitt-RAS/gists{/gist_id}","starred_url":"https://api.github.com/users/Pitt-RAS/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/Pitt-RAS/subscriptions","organizations_url":"https://api.github.com/users/Pitt-RAS/orgs","repos_url":"https://api.github.com/users/Pitt-RAS/repos","events_url":"https://api.github.com/users/Pitt-RAS/events{/privacy}","received_events_url":"https://api.github.com/users/Pitt-RAS/received_events","type":"Organization","site_admin":false},"repo":{"id":67438943,"name":"iarc7_motion","full_name":"Pitt-RAS/iarc7_motion","owner":{"login":"Pitt-RAS","id":15024509,"avatar_url":"https://avatars3.githubusercontent.com/u/15024509?v=4","gravatar_id":"","url":"https://api.github.com/users/Pitt-RAS","html_url":"https://github.com/Pitt-RAS","followers_url":"https://api.github.com/users/Pitt-RAS/followers","following_url":"https://api.github.com/users/Pitt-RAS/following{/other_user}","gists_url":"https://api.github.com/users/Pitt-RAS/gists{/gist_id}","starred_url":"https://api.github.com/users/Pitt-RAS/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/Pitt-RAS/subscriptions","organizations_url":"https://api.github.com/users/Pitt-RAS/orgs","repos_url":"https://api.github.com/users/Pitt-RAS/repos","events_url":"https://api.github.com/users/Pitt-RAS/events{/privacy}","received_events_url":"https://api.github.com/users/Pitt-RAS/received_events","type":"Organization","site_admin":false},"private":false,"html_url":"https://github.com/Pitt-RAS/iarc7_motion","description":"Motion Planning and Control code for generic multirotor, developed for IARC Mission 7","fork":false,"url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion","forks_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/forks","keys_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/keys{/key_id}","collaborators_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/collaborators{/collaborator}","teams_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/teams","hooks_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/hooks","issue_events_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/issues/events{/number}","events_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/events","assignees_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/assignees{/user}","branches_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/branches{/branch}","tags_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/tags","blobs_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/git/blobs{/sha}","git_tags_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/git/tags{/sha}","git_refs_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/git/refs{/sha}","trees_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/git/trees{/sha}","statuses_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/statuses/{sha}","languages_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/languages","stargazers_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/stargazers","contributors_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/contributors","subscribers_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/subscribers","subscription_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/subscription","commits_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/commits{/sha}","git_commits_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/git/commits{/sha}","comments_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/comments{/number}","issue_comment_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/issues/comments{/number}","contents_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/contents/{+path}","compare_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/compare/{base}...{head}","merges_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/merges","archive_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/{archive_format}{/ref}","downloads_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/downloads","issues_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/issues{/number}","pulls_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/pulls{/number}","milestones_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/milestones{/number}","notifications_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/notifications{?since,all,participating}","labels_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/labels{/name}","releases_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/releases{/id}","deployments_url":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/deployments","created_at":"2016-09-05T17:23:18Z","updated_at":"2018-04-02T21:20:50Z","pushed_at":"2018-04-02T21:42:22Z","git_url":"git://github.com/Pitt-RAS/iarc7_motion.git","ssh_url":"git@github.com:Pitt-RAS/iarc7_motion.git","clone_url":"https://github.com/Pitt-RAS/iarc7_motion.git","svn_url":"https://github.com/Pitt-RAS/iarc7_motion","homepage":"","size":12436,"stargazers_count":1,"watchers_count":1,"language":"Python","has_issues":true,"has_projects":true,"has_downloads":true,"has_wiki":true,"has_pages":false,"forks_count":1,"mirror_url":null,"archived":false,"open_issues_count":2,"license":{"key":"other","name":"Other","spdx_id":null,"url":null},"forks":1,"open_issues":2,"watchers":1,"default_branch":"master"}},"_links":{"self":{"href":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/pulls/123"},"html":{"href":"https://github.com/Pitt-RAS/iarc7_motion/pull/123"},"issue":{"href":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/issues/123"},"comments":{"href":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/issues/123/comments"},"review_comments":{"href":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/pulls/123/comments"},"review_comment":{"href":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/pulls/comments{/number}"},"commits":{"href":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/pulls/123/commits"},"statuses":{"href":"https://api.github.com/repos/Pitt-RAS/iarc7_motion/statuses/dc7d00540c5804b78123936534c235d27fbe92d2"}},"author_association":"NONE"}}
{"actor":{"display_login":"asaba96"}}