diff --git a/nav2_simple_commander/README.md b/nav2_simple_commander/README.md index c44d4fea08..3a368511f2 100644 --- a/nav2_simple_commander/README.md +++ b/nav2_simple_commander/README.md @@ -19,10 +19,12 @@ The methods provided by the basic navigator are shown below, with inputs and exp | goToPose(pose) | 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`. | -| cancelNav() | Cancel an ongoing `goThroughPoses` `goToPose` or `followWaypoints` request.| -| isNavComplete() | Checks if navigation is complete yet, times out at `100ms`. Returns `True` if completed and `False` if still going. | -| getFeedback() | Gets feedback from navigation task, returns action server feedback object. | -| getResult() | Gets final result of navigation task, to be called after `isNavComplete` returns `True`. Returns action server result object. | +| 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. | +| 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`. | | changeMap(map_filepath) | Requests a change from the current map to `map_filepath`'s yaml. | @@ -50,17 +52,17 @@ nav.setInitialPose(init_pose) nav.waitUntilNav2Active() # if autostarted, else use `lifecycleStartup()` ... nav.goToPose(goal_pose) -while not nav.isNavComplete(): +while not nav.isTaskComplete(): feedback = nav.getFeedback() if feedback.navigation_duration > 600: - nav.cancelNav() + nav.cancelTask() ... result = nav.getResult() -if result == NavigationResult.SUCCEEDED: +if result == TaskResult.SUCCEEDED: print('Goal succeeded!') -elif result == NavigationResult.CANCELED: +elif result == TaskResult.CANCELED: print('Goal was canceled!') -elif result == NavigationResult.FAILED: +elif result == TaskResult.FAILED: print('Goal failed!') ``` diff --git a/nav2_simple_commander/launch/recoveries_example_launch.py b/nav2_simple_commander/launch/recoveries_example_launch.py new file mode 100644 index 0000000000..0d50500034 --- /dev/null +++ b/nav2_simple_commander/launch/recoveries_example_launch.py @@ -0,0 +1,77 @@ +# Copyright (c) 2021 Samsung Research America +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import ExecuteProcess, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node + + +def generate_launch_description(): + warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + python_commander_dir = get_package_share_directory('nav2_simple_commander') + + map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') + world = os.path.join(python_commander_dir, 'warehouse.world') + + # start the simulation + start_gazebo_server_cmd = ExecuteProcess( + cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world], + cwd=[warehouse_dir], output='screen') + + start_gazebo_client_cmd = ExecuteProcess( + cmd=['gzclient'], + cwd=[warehouse_dir], output='screen') + + urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') + start_robot_state_publisher_cmd = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + arguments=[urdf]) + + # start the visualization + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + launch_arguments={'namespace': '', + 'use_namespace': 'False'}.items()) + + # start navigation + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={'map': map_yaml_file}.items()) + + # start the demo autonomy task + demo_cmd = Node( + package='nav2_simple_commander', + executable='demo_recoveries', + emulate_tty=True, + output='screen') + + ld = LaunchDescription() + ld.add_action(start_gazebo_server_cmd) + ld.add_action(start_gazebo_client_cmd) + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + ld.add_action(demo_cmd) + return ld diff --git a/nav2_simple_commander/nav2_simple_commander/demo_inspection.py b/nav2_simple_commander/nav2_simple_commander/demo_inspection.py index 111e54dc9d..429e11b68f 100644 --- a/nav2_simple_commander/nav2_simple_commander/demo_inspection.py +++ b/nav2_simple_commander/nav2_simple_commander/demo_inspection.py @@ -16,7 +16,7 @@ from copy import deepcopy from geometry_msgs.msg import PoseStamped -from nav2_simple_commander.robot_navigator import BasicNavigator, NavigationResult +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult import rclpy """ @@ -72,7 +72,7 @@ def main(): # Do something during our route (e.x. AI to analyze stock information or upload to the cloud) # Simply the current waypoint ID for the demonstation i = 0 - while not navigator.isNavComplete(): + while not navigator.isTaskComplete(): i += 1 feedback = navigator.getFeedback() if feedback and i % 5 == 0: @@ -80,18 +80,17 @@ def main(): str(feedback.current_waypoint + 1) + '/' + str(len(inspection_points))) result = navigator.getResult() - if result == NavigationResult.SUCCEEDED: + if result == TaskResult.SUCCEEDED: print('Inspection of shelves complete! Returning to start...') - elif result == NavigationResult.CANCELED: + elif result == TaskResult.CANCELED: print('Inspection of shelving was canceled. Returning to start...') - exit(1) - elif result == NavigationResult.FAILED: + elif result == TaskResult.FAILED: print('Inspection of shelving failed! Returning to start...') # go back to start initial_pose.header.stamp = navigator.get_clock().now().to_msg() navigator.goToPose(initial_pose) - while not navigator.isNavComplete(): + while not navigator.isTaskComplete(): pass exit(0) diff --git a/nav2_simple_commander/nav2_simple_commander/demo_picking.py b/nav2_simple_commander/nav2_simple_commander/demo_picking.py index 8cfcb2aec3..6a7a177acb 100644 --- a/nav2_simple_commander/nav2_simple_commander/demo_picking.py +++ b/nav2_simple_commander/nav2_simple_commander/demo_picking.py @@ -14,7 +14,7 @@ # limitations under the License. from geometry_msgs.msg import PoseStamped -from nav2_simple_commander.robot_navigator import BasicNavigator, NavigationResult +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult import rclpy from rclpy.duration import Duration @@ -82,7 +82,7 @@ def main(): # (e.x. queue up future tasks or detect person for fine-tuned positioning) # Simply print information for workers on the robot's ETA for the demonstation i = 0 - while not navigator.isNavComplete(): + while not navigator.isTaskComplete(): i += 1 feedback = navigator.getFeedback() if feedback and i % 5 == 0: @@ -92,7 +92,7 @@ def main(): + ' seconds.') result = navigator.getResult() - if result == NavigationResult.SUCCEEDED: + if result == TaskResult.SUCCEEDED: print('Got product from ' + request_item_location + '! Bringing product to shipping destination (' + request_destination + ')...') shipping_destination = PoseStamped() @@ -104,16 +104,16 @@ def main(): shipping_destination.pose.orientation.w = 0.0 navigator.goToPose(shipping_destination) - elif result == NavigationResult.CANCELED: + elif result == TaskResult.CANCELED: print(f'Task at {request_item_location} was canceled. Returning to staging point...') initial_pose.header.stamp = navigator.get_clock().now().to_msg() navigator.goToPose(initial_pose) - elif result == NavigationResult.FAILED: + elif result == TaskResult.FAILED: print(f'Task at {request_item_location} failed!') exit(-1) - while not navigator.isNavComplete(): + while not navigator.isTaskComplete(): pass exit(0) diff --git a/nav2_simple_commander/nav2_simple_commander/demo_recoveries.py b/nav2_simple_commander/nav2_simple_commander/demo_recoveries.py new file mode 100644 index 0000000000..0313f956ff --- /dev/null +++ b/nav2_simple_commander/nav2_simple_commander/demo_recoveries.py @@ -0,0 +1,105 @@ +#! /usr/bin/env python3 +# Copyright 2021 Samsung Research America +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from geometry_msgs.msg import PoseStamped +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult +import rclpy +from rclpy.duration import Duration + + +""" +Basic recoveries demo. In this demonstration, the robot navigates +to a dead-end where recoveries such as backup and spin are used +to get out of it. +""" + + +def main(): + rclpy.init() + + navigator = BasicNavigator() + + # Set our demo's initial pose + initial_pose = PoseStamped() + initial_pose.header.frame_id = 'map' + initial_pose.header.stamp = navigator.get_clock().now().to_msg() + initial_pose.pose.position.x = 3.45 + initial_pose.pose.position.y = 2.15 + initial_pose.pose.orientation.z = 1.0 + initial_pose.pose.orientation.w = 0.0 + navigator.setInitialPose(initial_pose) + + # Wait for navigation to fully activate + navigator.waitUntilNav2Active() + + goal_pose = PoseStamped() + goal_pose.header.frame_id = 'map' + goal_pose.header.stamp = navigator.get_clock().now().to_msg() + goal_pose.pose.position.x = 6.13 + goal_pose.pose.position.y = 1.90 + goal_pose.pose.orientation.w = 1.0 + + navigator.goToPose(goal_pose) + + i = 0 + while not navigator.isTaskComplete(): + i += 1 + feedback = navigator.getFeedback() + if feedback and i % 5 == 0: + print( + f'Estimated time of arrival to destination is: \ + {Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9}' + ) + + # Robot hit a dead end, back it up + print('Robot hit a dead end, backing up...') + navigator.backup(backup_dist=0.5, backup_speed=0.1) + + i = 0 + while not navigator.isTaskComplete(): + i += 1 + feedback = navigator.getFeedback() + if feedback and i % 5 == 0: + print(f'Distance traveled: {feedback.distance_traveled}') + + # Turn it around + print('Spinning robot around...') + navigator.spin(spin_dist=3.14) + + i = 0 + while not navigator.isTaskComplete(): + i += 1 + feedback = navigator.getFeedback() + if feedback and i % 5 == 0: + print(f'Spin angle traveled: {feedback.angular_distance_traveled}') + + result = navigator.getResult() + if result == TaskResult.SUCCEEDED: + print('Dead end confirmed! Returning to start...') + elif result == TaskResult.CANCELED: + print('Recovery was canceled. Returning to start...') + elif result == TaskResult.FAILED: + print('Recovering from dead end failed! Returning to start...') + + initial_pose.header.stamp = navigator.get_clock().now().to_msg() + navigator.goToPose(initial_pose) + while not navigator.isTaskComplete(): + pass + + exit(0) + + +if __name__ == '__main__': + main() diff --git a/nav2_simple_commander/nav2_simple_commander/demo_security.py b/nav2_simple_commander/nav2_simple_commander/demo_security.py index 8180bc36b4..f727cc8d72 100644 --- a/nav2_simple_commander/nav2_simple_commander/demo_security.py +++ b/nav2_simple_commander/nav2_simple_commander/demo_security.py @@ -16,7 +16,7 @@ from copy import deepcopy from geometry_msgs.msg import PoseStamped -from nav2_simple_commander.robot_navigator import BasicNavigator, NavigationResult +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult import rclpy from rclpy.duration import Duration @@ -75,7 +75,7 @@ def main(): # Do something during our route (e.x. AI detection on camera images for anomalies) # Simply print ETA for the demonstation i = 0 - while not navigator.isNavComplete(): + while not navigator.isTaskComplete(): i += 1 feedback = navigator.getFeedback() if feedback and i % 5 == 0: @@ -86,18 +86,18 @@ def main(): # Some failure mode, must stop since the robot is clearly stuck if Duration.from_msg(feedback.navigation_time) > Duration(seconds=180.0): print('Navigation has exceeded timeout of 180s, canceling request.') - navigator.cancelNav() + navigator.cancelTask() # If at end of route, reverse the route to restart security_route.reverse() result = navigator.getResult() - if result == NavigationResult.SUCCEEDED: + if result == TaskResult.SUCCEEDED: print('Route complete! Restarting...') - elif result == NavigationResult.CANCELED: + elif result == TaskResult.CANCELED: print('Security route was canceled, exiting.') exit(1) - elif result == NavigationResult.FAILED: + elif result == TaskResult.FAILED: print('Security route failed! Restarting from other side...') exit(0) diff --git a/nav2_simple_commander/nav2_simple_commander/example_follow_path.py b/nav2_simple_commander/nav2_simple_commander/example_follow_path.py index fe94f98798..084d06f08b 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_follow_path.py +++ b/nav2_simple_commander/nav2_simple_commander/example_follow_path.py @@ -14,7 +14,7 @@ # limitations under the License. from geometry_msgs.msg import PoseStamped -from nav2_simple_commander.robot_navigator import BasicNavigator, NavigationResult +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult import rclpy @@ -56,7 +56,7 @@ def main(): navigator.followPath(path) i = 0 - while not navigator.isNavComplete(): + while not navigator.isTaskComplete(): ################################################ # # Implement some code here for your application! @@ -74,11 +74,11 @@ def main(): # Do something depending on the return code result = navigator.getResult() - if result == NavigationResult.SUCCEEDED: + if result == TaskResult.SUCCEEDED: print('Goal succeeded!') - elif result == NavigationResult.CANCELED: + elif result == TaskResult.CANCELED: print('Goal was canceled!') - elif result == NavigationResult.FAILED: + elif result == TaskResult.FAILED: print('Goal failed!') else: print('Goal has an invalid return status!') diff --git a/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py b/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py index f4523b8974..58c47c75e1 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py +++ b/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py @@ -14,7 +14,7 @@ # limitations under the License. from geometry_msgs.msg import PoseStamped -from nav2_simple_commander.robot_navigator import BasicNavigator, NavigationResult +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult import rclpy from rclpy.duration import Duration @@ -89,7 +89,7 @@ def main(): navigator.goThroughPoses(goal_poses) i = 0 - while not navigator.isNavComplete(): + while not navigator.isTaskComplete(): ################################################ # # Implement some code here for your application! @@ -106,7 +106,7 @@ def main(): # Some navigation timeout to demo cancellation if Duration.from_msg(feedback.navigation_time) > Duration(seconds=600.0): - navigator.cancelNav() + navigator.cancelTask() # Some navigation request change to demo preemption if Duration.from_msg(feedback.navigation_time) > Duration(seconds=35.0): @@ -121,11 +121,11 @@ def main(): # Do something depending on the return code result = navigator.getResult() - if result == NavigationResult.SUCCEEDED: + if result == TaskResult.SUCCEEDED: print('Goal succeeded!') - elif result == NavigationResult.CANCELED: + elif result == TaskResult.CANCELED: print('Goal was canceled!') - elif result == NavigationResult.FAILED: + elif result == TaskResult.FAILED: print('Goal failed!') else: print('Goal has an invalid return status!') diff --git a/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py b/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py index cd302e418c..58dee813c9 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py +++ b/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py @@ -14,7 +14,7 @@ # limitations under the License. from geometry_msgs.msg import PoseStamped -from nav2_simple_commander.robot_navigator import BasicNavigator, NavigationResult +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult import rclpy from rclpy.duration import Duration @@ -68,7 +68,7 @@ def main(): navigator.goToPose(goal_pose) i = 0 - while not navigator.isNavComplete(): + while not navigator.isTaskComplete(): ################################################ # # Implement some code here for your application! @@ -85,7 +85,7 @@ def main(): # Some navigation timeout to demo cancellation if Duration.from_msg(feedback.navigation_time) > Duration(seconds=600.0): - navigator.cancelNav() + navigator.cancelTask() # Some navigation request change to demo preemption if Duration.from_msg(feedback.navigation_time) > Duration(seconds=18.0): @@ -94,11 +94,11 @@ def main(): # Do something depending on the return code result = navigator.getResult() - if result == NavigationResult.SUCCEEDED: + if result == TaskResult.SUCCEEDED: print('Goal succeeded!') - elif result == NavigationResult.CANCELED: + elif result == TaskResult.CANCELED: print('Goal was canceled!') - elif result == NavigationResult.FAILED: + elif result == TaskResult.FAILED: print('Goal failed!') else: print('Goal has an invalid return status!') diff --git a/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py b/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py index 8ed0fc3d16..9d02341a66 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py +++ b/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py @@ -14,7 +14,7 @@ # limitations under the License. from geometry_msgs.msg import PoseStamped -from nav2_simple_commander.robot_navigator import BasicNavigator, NavigationResult +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult import rclpy from rclpy.duration import Duration @@ -90,7 +90,7 @@ def main(): navigator.followWaypoints(goal_poses) i = 0 - while not navigator.isNavComplete(): + while not navigator.isTaskComplete(): ################################################ # # Implement some code here for your application! @@ -107,7 +107,7 @@ def main(): # Some navigation timeout to demo cancellation if now - nav_start > Duration(seconds=600.0): - navigator.cancelNav() + navigator.cancelTask() # Some follow waypoints request change to demo preemption if now - nav_start > Duration(seconds=35.0): @@ -124,11 +124,11 @@ def main(): # Do something depending on the return code result = navigator.getResult() - if result == NavigationResult.SUCCEEDED: + if result == TaskResult.SUCCEEDED: print('Goal succeeded!') - elif result == NavigationResult.CANCELED: + elif result == TaskResult.CANCELED: print('Goal was canceled!') - elif result == NavigationResult.FAILED: + elif result == TaskResult.FAILED: print('Goal failed!') else: print('Goal has an invalid return status!') diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index ce0ee8d01b..2a7bfff430 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -18,12 +18,14 @@ import time from action_msgs.msg import GoalStatus +from builtin_interfaces.msg import Duration +from geometry_msgs.msg import Point from geometry_msgs.msg import PoseStamped from geometry_msgs.msg import PoseWithCovarianceStamped from lifecycle_msgs.srv import GetState +from nav2_msgs.action import BackUp, Spin from nav2_msgs.action import ComputePathThroughPoses, ComputePathToPose -from nav2_msgs.action import FollowWaypoints, NavigateThroughPoses, NavigateToPose -from nav2_msgs.action import FollowPath +from nav2_msgs.action import FollowPath, FollowWaypoints, NavigateThroughPoses, NavigateToPose from nav2_msgs.srv import ClearEntireCostmap, GetCostmap, LoadMap, ManageLifecycleNodes import rclpy @@ -33,8 +35,8 @@ from rclpy.qos import QoSProfile, QoSReliabilityPolicy -class NavigationResult(Enum): - UKNOWN = 0 +class TaskResult(Enum): + UNKNOWN = 0 SUCCEEDED = 1 CANCELED = 2 FAILED = 3 @@ -68,6 +70,8 @@ def __init__(self): 'compute_path_to_pose') self.compute_path_through_poses_client = ActionClient(self, ComputePathThroughPoses, 'compute_path_through_poses') + self.spin_client = ActionClient(self, Spin, 'spin') + self.backup_client = ActionClient(self, BackUp, 'backup') self.localization_pose_sub = self.create_subscription(PoseWithCovarianceStamped, 'amcl_pose', self._amclPoseCallback, @@ -157,6 +161,47 @@ def followWaypoints(self, poses): self.result_future = self.goal_handle.get_result_async() return True + def spin(self, spin_dist=1.57, time_allowance=10): + self.debug("Waiting for 'Spin' action server") + while not self.spin_client.wait_for_server(timeout_sec=1.0): + self.info("'Spin' action server not available, waiting...") + goal_msg = Spin.Goal() + goal_msg.target_yaw = spin_dist + goal_msg.time_allowance = Duration(sec=time_allowance) + + self.info(f'Spinning to angle {goal_msg.target_yaw}....') + send_goal_future = self.spin_client.send_goal_async(goal_msg, self._feedbackCallback) + rclpy.spin_until_future_complete(self, send_goal_future) + self.goal_handle = send_goal_future.result() + + if not self.goal_handle.accepted: + self.error('Spin request was rejected!') + return False + + self.result_future = self.goal_handle.get_result_async() + return True + + def backup(self, backup_dist=0.15, backup_speed=0.025, time_allowance=10): + self.debug("Waiting for 'Backup' action server") + while not self.backup_client.wait_for_server(timeout_sec=1.0): + self.info("'Backup' action server not available, waiting...") + goal_msg = BackUp.Goal() + goal_msg.target = Point(x=float(backup_dist)) + goal_msg.speed = backup_speed + goal_msg.time_allowance = Duration(sec=time_allowance) + + self.info(f'Backing up {goal_msg.target.x} m at {goal_msg.speed} m/s....') + send_goal_future = self.backup_client.send_goal_async(goal_msg, self._feedbackCallback) + rclpy.spin_until_future_complete(self, send_goal_future) + self.goal_handle = send_goal_future.result() + + if not self.goal_handle.accepted: + self.error('Backup request was rejected!') + return False + + self.result_future = self.goal_handle.get_result_async() + return True + def followPath(self, path): """Send a `FollowPath` action request.""" self.debug("Waiting for 'FollowPath' action server") @@ -179,16 +224,16 @@ def followPath(self, path): self.result_future = self.goal_handle.get_result_async() return True - def cancelNav(self): - """Cancel pending navigation request of any type.""" - self.info('Canceling current goal.') + def cancelTask(self): + """Cancel pending task request of any type.""" + self.info('Canceling current task.') if self.result_future: future = self.goal_handle.cancel_goal_async() rclpy.spin_until_future_complete(self, future) return - def isNavComplete(self): - """Check if the navigation request of any type is complete yet.""" + def isTaskComplete(self): + """Check if the task request of any type is complete yet.""" if not self.result_future: # task was cancelled or completed return True @@ -196,13 +241,13 @@ def isNavComplete(self): if self.result_future.result(): self.status = self.result_future.result().status if self.status != GoalStatus.STATUS_SUCCEEDED: - self.debug(f'Goal with failed with status code: {self.status}') + self.debug(f'Task with failed with status code: {self.status}') return True else: # Timed out, still processing, not complete yet return False - self.debug('Goal succeeded!') + self.debug('Task succeeded!') return True def getFeedback(self): @@ -212,13 +257,13 @@ def getFeedback(self): def getResult(self): """Get the pending action result message.""" if self.status == GoalStatus.STATUS_SUCCEEDED: - return NavigationResult.SUCCEEDED + return TaskResult.SUCCEEDED elif self.status == GoalStatus.STATUS_ABORTED: - return NavigationResult.FAILED + return TaskResult.FAILED elif self.status == GoalStatus.STATUS_CANCELED: - return NavigationResult.CANCELED + return TaskResult.CANCELED else: - return NavigationResult.UNKNOWN + return TaskResult.UNKNOWN def waitUntilNav2Active(self): """Block until the full navigation system is up and running.""" diff --git a/nav2_simple_commander/setup.py b/nav2_simple_commander/setup.py index 8fbdd00f11..ab19a26e47 100644 --- a/nav2_simple_commander/setup.py +++ b/nav2_simple_commander/setup.py @@ -32,6 +32,7 @@ 'demo_picking = nav2_simple_commander.demo_picking:main', 'demo_inspection = nav2_simple_commander.demo_inspection:main', 'demo_security = nav2_simple_commander.demo_security:main', + 'demo_recoveries = nav2_simple_commander.demo_recoveries:main', ], }, )