Skip to content

Commit

Permalink
Add all action options (#2834)
Browse files Browse the repository at this point in the history 
* added options

* update docs
  • Loading branch information
@SteveMacenski
Tony Najjar authored and SteveMacenski committed May 6, 2022
1 parent 36a0104 commit 09643c8
Show file tree
Hide file tree
Showing 2 changed files with 22 additions and 16 deletions.
14 changes: 7 additions & 7 deletions nav2_simple_commander/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,18 +15,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. |
Expand Down
24 changes: 15 additions & 9 deletions nav2_simple_commander/nav2_simple_commander/robot_navigator.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -93,14 +93,15 @@ 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):
self.info("'NavigateThroughPoses' action server not available, waiting...")

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,
Expand All @@ -115,14 +116,15 @@ 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):
self.info("'NavigateToPose' action server not available, waiting...")

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) + '...')
Expand Down Expand Up @@ -202,14 +204,16 @@ 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):
self.info("'FollowPath' action server not available, waiting...")

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,
Expand Down Expand Up @@ -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)
Expand All @@ -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)
Expand Down

0 comments on commit 09643c8

Please sign in to comment.