From 0ab507b9657e82acae5b292553b3e90c7d95abc6 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Sun, 27 Feb 2022 18:47:08 +0000 Subject: [PATCH 1/2] added options --- .../nav2_simple_commander/robot_navigator.py | 24 ++++++++++++------- 1 file changed, 15 insertions(+), 9 deletions(-) diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index e3071d1b5b4..4bcf8093d63 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -93,7 +93,7 @@ def setInitialPose(self, initial_pose): self.initial_pose = initial_pose self._setInitialPose() - def goThroughPoses(self, poses): + def goThroughPoses(self, poses, behavior_tree=''): """Send a `NavThroughPoses` action request.""" self.debug("Waiting for 'NavigateThroughPoses' action server") while not self.nav_through_poses_client.wait_for_server(timeout_sec=1.0): @@ -101,6 +101,7 @@ def goThroughPoses(self, poses): goal_msg = NavigateThroughPoses.Goal() goal_msg.poses = poses + goal_msg.behavior_tree = behavior_tree self.info(f'Navigating with {len(goal_msg.poses)} goals....') send_goal_future = self.nav_through_poses_client.send_goal_async(goal_msg, @@ -115,7 +116,7 @@ def goThroughPoses(self, poses): self.result_future = self.goal_handle.get_result_async() return True - def goToPose(self, pose): + def goToPose(self, pose, behavior_tree=''): """Send a `NavToPose` action request.""" self.debug("Waiting for 'NavigateToPose' action server") while not self.nav_to_pose_client.wait_for_server(timeout_sec=1.0): @@ -123,6 +124,7 @@ def goToPose(self, pose): goal_msg = NavigateToPose.Goal() goal_msg.pose = pose + goal_msg.behavior_tree = behavior_tree self.info('Navigating to goal: ' + str(pose.pose.position.x) + ' ' + str(pose.pose.position.y) + '...') @@ -202,7 +204,7 @@ def backup(self, backup_dist=0.15, backup_speed=0.025, time_allowance=10): self.result_future = self.goal_handle.get_result_async() return True - def followPath(self, path): + def followPath(self, path, controller_id='', goal_checker_id=''): """Send a `FollowPath` action request.""" self.debug("Waiting for 'FollowPath' action server") while not self.follow_path_client.wait_for_server(timeout_sec=1.0): @@ -210,6 +212,8 @@ def followPath(self, path): goal_msg = FollowPath.Goal() goal_msg.path = path + goal_msg.controller_id = controller_id + goal_msg.goal_checker_id = goal_checker_id self.info('Executing path...') send_goal_future = self.follow_path_client.send_goal_async(goal_msg, @@ -274,17 +278,17 @@ def waitUntilNav2Active(self, navigator='bt_navigator', localizer='amcl'): self.info('Nav2 is ready for use!') return - def getPath(self, start, goal, planner_id=None): + def getPath(self, start, goal, planner_id='', use_start=False): """Send a `ComputePathToPose` action request.""" self.debug("Waiting for 'ComputePathToPose' action server") while not self.compute_path_to_pose_client.wait_for_server(timeout_sec=1.0): self.info("'ComputePathToPose' action server not available, waiting...") goal_msg = ComputePathToPose.Goal() - goal_msg.goal = goal goal_msg.start = start - if planner_id is not None: - goal_msg.planner_id = planner_id + goal_msg.goal = goal + goal_msg.planner_id = planner_id + goal_msg.use_start = use_start self.info('Getting path...') send_goal_future = self.compute_path_to_pose_client.send_goal_async(goal_msg) @@ -304,15 +308,17 @@ def getPath(self, start, goal, planner_id=None): return self.result_future.result().result.path - def getPathThroughPoses(self, start, goals): + def getPathThroughPoses(self, start, goals, planner_id='', use_start=False): """Send a `ComputePathThroughPoses` action request.""" self.debug("Waiting for 'ComputePathThroughPoses' action server") while not self.compute_path_through_poses_client.wait_for_server(timeout_sec=1.0): self.info("'ComputePathThroughPoses' action server not available, waiting...") goal_msg = ComputePathThroughPoses.Goal() - goal_msg.goals = goals goal_msg.start = start + goal_msg.goals = goals + goal_msg.planner_id = planner_id + goal_msg.use_start = use_start self.info('Getting path...') send_goal_future = self.compute_path_through_poses_client.send_goal_async(goal_msg) From c88c1fbd17e487c093e4b5fe1dd0088644d6d413 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Sun, 27 Feb 2022 18:52:37 +0000 Subject: [PATCH 2/2] update docs --- nav2_simple_commander/README.md | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/nav2_simple_commander/README.md b/nav2_simple_commander/README.md index a083c13116a..0de11bdd485 100644 --- a/nav2_simple_commander/README.md +++ b/nav2_simple_commander/README.md @@ -17,18 +17,18 @@ The methods provided by the basic navigator are shown below, with inputs and exp | Robot Navigator Method | Description | | --------------------------------- | -------------------------------------------------------------------------- | | setInitialPose(initial_pose) | Sets the initial pose (`PoseStamped`) of the robot to localization. | -| goThroughPoses(poses) | Requests the robot to drive through a set of poses (list of `PoseStamped`).| -| goToPose(pose) | Requests the robot to drive to a pose (`PoseStamped`). | +| goThroughPoses(poses, behavior_tree='') | Requests the robot to drive through a set of poses (list of `PoseStamped`).| +| goToPose(pose, behavior_tree='') | Requests the robot to drive to a pose (`PoseStamped`). | | followWaypoints(poses) | Requests the robot to follow a set of waypoints (list of `PoseStamped`). This will execute the specific `TaskExecutor` at each pose. | -| followPath(path) | Requests the robot to follow a path from a starting to a goal `PoseStamped`, `nav_msgs/Path`. | -| spin(spin_dist, time_allowance) | Requests the robot to performs an in-place rotation by a given angle. | -| backup(backup_dist, backup_speed, time_allowance) | Requests the robot to back up by a given distance. | +| followPath(path, controller_id='', goal_checker_id='') | Requests the robot to follow a path from a starting to a goal `PoseStamped`, `nav_msgs/Path`. | +| spin(spin_dist=1.57, time_allowance=10) | Requests the robot to performs an in-place rotation by a given angle. | +| backup(backup_dist=0.15, backup_speed=0.025, time_allowance=10) | Requests the robot to back up by a given distance. | | cancelTask() | Cancel an ongoing task request.| | isTaskComplete() | Checks if task is complete yet, times out at `100ms`. Returns `True` if completed and `False` if still going. | | getFeedback() | Gets feedback from task, returns action server feedback object. | | getResult() | Gets final result of task, to be called after `isTaskComplete` returns `True`. Returns action server result object. | -| getPath(start, goal) | Gets a path from a starting to a goal `PoseStamped`, `nav_msgs/Path`. | -| getPathThroughPoses(start, goals) | Gets a path through a starting to a set of goals, a list of `PoseStamped`, `nav_msgs/Path`. | +| getPath(start, goal, planner_id='', use_start=False) | Gets a path from a starting to a goal `PoseStamped`, `nav_msgs/Path`. | +| getPathThroughPoses(start, goals, planner_id='', use_start=False) | Gets a path through a starting to a set of goals, a list of `PoseStamped`, `nav_msgs/Path`. | | changeMap(map_filepath) | Requests a change from the current map to `map_filepath`'s yaml. | | clearAllCostmaps() | Clears both the global and local costmaps. | | clearLocalCostmap() | Clears the local costmap. |