Skip to content

Add all action options #2834

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
SteveMacenski merged 2 commits into from
Mar 1, 2022
Merged

Add all action options #2834

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
0ab507b
added options
Feb 27, 2022
c88c1fb
update docs
Feb 27, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 7 additions & 7 deletions nav2_simple_commander/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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. |
Expand Down
24 changes: 15 additions & 9 deletions nav2_simple_commander/nav2_simple_commander/robot_navigator.py
View file
Open in desktop
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):
Copy link
Copy Markdown
Contributor Author

@tonynajjar tonynajjar Feb 27, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We could also use the "None way" everywhere (set the variable if it is not 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