From face00d32007c209f8c54dd6464fc12410700bfd Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Sun, 23 Jun 2024 14:49:22 +1000 Subject: [PATCH] nav2_simple_commander: use error_code and error_msg (#4341) Signed-off-by: Mike Wake --- .../nav2_simple_commander/demo_inspection.py | 4 +- .../nav2_simple_commander/demo_picking.py | 4 +- .../nav2_simple_commander/demo_recoveries.py | 4 +- .../nav2_simple_commander/demo_security.py | 4 +- .../example_follow_path.py | 3 +- .../example_nav_through_poses.py | 3 +- .../example_nav_to_pose.py | 3 +- .../example_waypoint_follower.py | 3 +- .../nav2_simple_commander/robot_navigator.py | 129 +++++++++++++----- 9 files changed, 115 insertions(+), 42 deletions(-) diff --git a/nav2_simple_commander/nav2_simple_commander/demo_inspection.py b/nav2_simple_commander/nav2_simple_commander/demo_inspection.py index 8e5ff073940..a483e4afe4f 100644 --- a/nav2_simple_commander/nav2_simple_commander/demo_inspection.py +++ b/nav2_simple_commander/nav2_simple_commander/demo_inspection.py @@ -98,7 +98,9 @@ def main(): elif result == TaskResult.CANCELED: print('Inspection of shelving was canceled. Returning to start...') elif result == TaskResult.FAILED: - print('Inspection of shelving failed! Returning to start...') + (error_code, error_msg) = navigator.getLastError() + print(f'Inspection of shelving failed!:{error_code}:{error_msg}') + print('Returning to start...') # go back to start initial_pose.header.stamp = navigator.get_clock().now().to_msg() diff --git a/nav2_simple_commander/nav2_simple_commander/demo_picking.py b/nav2_simple_commander/nav2_simple_commander/demo_picking.py index d0975d14466..09d3691cb44 100644 --- a/nav2_simple_commander/nav2_simple_commander/demo_picking.py +++ b/nav2_simple_commander/nav2_simple_commander/demo_picking.py @@ -133,7 +133,9 @@ def main(): navigator.goToPose(initial_pose) elif result == TaskResult.FAILED: - print(f'Task at {request_item_location} failed!') + (error_code, error_msg) = navigator.getLastError() + print(f'Task at {request_item_location} failed!:' + f'{error_code}:{error_msg}') exit(-1) while not navigator.isTaskComplete(): diff --git a/nav2_simple_commander/nav2_simple_commander/demo_recoveries.py b/nav2_simple_commander/nav2_simple_commander/demo_recoveries.py index 5d6aea783b4..ccaf9a1762a 100644 --- a/nav2_simple_commander/nav2_simple_commander/demo_recoveries.py +++ b/nav2_simple_commander/nav2_simple_commander/demo_recoveries.py @@ -91,7 +91,9 @@ def main(): elif result == TaskResult.CANCELED: print('Recovery was canceled. Returning to start...') elif result == TaskResult.FAILED: - print('Recovering from dead end failed! Returning to start...') + (error_code, error_msg) = navigator.getLastError() + print(f'Recovering from dead end failed!:{error_code}:{error_msg}') + print('Returning to start...') initial_pose.header.stamp = navigator.get_clock().now().to_msg() navigator.goToPose(initial_pose) diff --git a/nav2_simple_commander/nav2_simple_commander/demo_security.py b/nav2_simple_commander/nav2_simple_commander/demo_security.py index 38cc2653c88..9109f379565 100644 --- a/nav2_simple_commander/nav2_simple_commander/demo_security.py +++ b/nav2_simple_commander/nav2_simple_commander/demo_security.py @@ -104,7 +104,9 @@ def main(): print('Security route was canceled, exiting.') exit(1) elif result == TaskResult.FAILED: - print('Security route failed! Restarting from other side...') + (error_code, error_msg) = navigator.getLastError() + print(f'Security route failed!:{error_code}:{error_msg}') + print('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 713857eed5a..ef66a5b4a82 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_follow_path.py +++ b/nav2_simple_commander/nav2_simple_commander/example_follow_path.py @@ -82,7 +82,8 @@ def main(): elif result == TaskResult.CANCELED: print('Goal was canceled!') elif result == TaskResult.FAILED: - print('Goal failed!') + (error_code, error_msg) = navigator.getLastError() + print('Goal failed!{error_code}:{error_msg}') 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 0388522eb84..7c38a8c17c3 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 @@ -131,7 +131,8 @@ def main(): elif result == TaskResult.CANCELED: print('Goal was canceled!') elif result == TaskResult.FAILED: - print('Goal failed!') + (error_code, error_msg) = navigator.getLastError() + print('Goal failed!{error_code}:{error_msg}') 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 d923a834b8d..8a1db456b44 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 @@ -106,7 +106,8 @@ def main(): elif result == TaskResult.CANCELED: print('Goal was canceled!') elif result == TaskResult.FAILED: - print('Goal failed!') + (error_code, error_msg) = navigator.getLastError() + print('Goal failed!{error_code}:{error_msg}') 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 0d0ea4c77f3..13485a098a9 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py +++ b/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py @@ -133,7 +133,8 @@ def main(): elif result == TaskResult.CANCELED: print('Goal was canceled!') elif result == TaskResult.FAILED: - print('Goal failed!') + (error_code, error_msg) = navigator.getLastError() + print('Goal failed!{error_code}:{error_msg}') 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 54d76044331..136bcb406ba 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -62,6 +62,8 @@ def __init__(self, node_name='basic_navigator', namespace=''): self.result_future = None self.feedback = None self.status = None + self.last_error_code = 0 + self.last_error_msg = '' amcl_pose_qos = QoSProfile( durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, @@ -140,12 +142,14 @@ def destroy_node(self): def setInitialPose(self, initial_pose): """Set the initial pose to the localization system.""" + self.clearLastError() self.initial_pose_received = False self.initial_pose = initial_pose self._setInitialPose() def goThroughPoses(self, poses, behavior_tree=''): """Send a `NavThroughPoses` action request.""" + self.clearLastError() 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...") @@ -170,6 +174,7 @@ def goThroughPoses(self, poses, behavior_tree=''): def goToPose(self, pose, behavior_tree=''): """Send a `NavToPose` action request.""" + self.clearLastError() 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...") @@ -206,6 +211,7 @@ def goToPose(self, pose, behavior_tree=''): def followWaypoints(self, poses): """Send a `FollowWaypoints` action request.""" + self.clearLastError() self.debug("Waiting for 'FollowWaypoints' action server") while not self.follow_waypoints_client.wait_for_server(timeout_sec=1.0): self.info("'FollowWaypoints' action server not available, waiting...") @@ -229,6 +235,7 @@ def followWaypoints(self, poses): def followGpsWaypoints(self, gps_poses): """Send a `FollowGPSWaypoints` action request.""" + self.clearLastError() self.debug("Waiting for 'FollowWaypoints' action server") while not self.follow_gps_waypoints_client.wait_for_server(timeout_sec=1.0): self.info("'FollowWaypoints' action server not available, waiting...") @@ -253,6 +260,7 @@ def followGpsWaypoints(self, gps_poses): return True def spin(self, spin_dist=1.57, time_allowance=10): + self.clearLastError() 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...") @@ -275,6 +283,7 @@ def spin(self, spin_dist=1.57, time_allowance=10): return True def backup(self, backup_dist=0.15, backup_speed=0.025, time_allowance=10): + self.clearLastError() 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...") @@ -298,6 +307,7 @@ def backup(self, backup_dist=0.15, backup_speed=0.025, time_allowance=10): return True def driveOnHeading(self, dist=0.15, speed=0.025, time_allowance=10): + self.clearLastError() self.debug("Waiting for 'DriveOnHeading' action server") while not self.backup_client.wait_for_server(timeout_sec=1.0): self.info("'DriveOnHeading' action server not available, waiting...") @@ -321,6 +331,7 @@ def driveOnHeading(self, dist=0.15, speed=0.025, time_allowance=10): return True def assistedTeleop(self, time_allowance=30): + self.clearLastError() self.debug("Wainting for 'assisted_teleop' action server") while not self.assisted_teleop_client.wait_for_server(timeout_sec=1.0): self.info("'assisted_teleop' action server not available, waiting...") @@ -342,6 +353,7 @@ def assistedTeleop(self, time_allowance=30): return True def followPath(self, path, controller_id='', goal_checker_id=''): + self.clearLastError() """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): @@ -367,6 +379,7 @@ def followPath(self, path, controller_id='', goal_checker_id=''): return True def dockRobotByPose(self, dock_pose, dock_type, nav_to_dock=True): + self.clearLastError() """Send a `DockRobot` action request.""" self.info("Waiting for 'DockRobot' action server") while not self.docking_client.wait_for_server(timeout_sec=1.0): @@ -385,6 +398,7 @@ def dockRobotByPose(self, dock_pose, dock_type, nav_to_dock=True): self.goal_handle = send_goal_future.result() if not self.goal_handle.accepted: + self.SetLastError(DockRobot.UNKNOWN, 'Docking request was rejected') self.info('Docking request was rejected!') return False @@ -393,6 +407,7 @@ def dockRobotByPose(self, dock_pose, dock_type, nav_to_dock=True): def dockRobotByID(self, dock_id, nav_to_dock=True): """Send a `DockRobot` action request.""" + self.clearLastError() self.info("Waiting for 'DockRobot' action server") while not self.docking_client.wait_for_server(timeout_sec=1.0): self.info('"DockRobot" action server not available, waiting...') @@ -409,6 +424,7 @@ def dockRobotByID(self, dock_id, nav_to_dock=True): self.goal_handle = send_goal_future.result() if not self.goal_handle.accepted: + self.SetLastError(DockRobot.UNKNOWN, 'Docking request was rejected') self.info('Docking request was rejected!') return False @@ -417,6 +433,7 @@ def dockRobotByID(self, dock_id, nav_to_dock=True): def undockRobot(self, dock_type=''): """Send a `UndockRobot` action request.""" + self.clearLastError() self.info("Waiting for 'UndockRobot' action server") while not self.undocking_client.wait_for_server(timeout_sec=1.0): self.info('"UndockRobot" action server not available, waiting...') @@ -431,6 +448,7 @@ def undockRobot(self, dock_type=''): self.goal_handle = send_goal_future.result() if not self.goal_handle.accepted: + self.SetLastError(UndockRobot.UNKNOWN, 'Undocking request was rejected') self.info('Undocking request was rejected!') return False @@ -439,13 +457,14 @@ def undockRobot(self, dock_type=''): def cancelTask(self): """Cancel pending task request of any type.""" + self.clearLastError() 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 isTaskComplete(self): + def isTaskComplete(self) -> bool: """Check if the task request of any type is complete yet.""" if not self.result_future: # task was cancelled or completed @@ -454,7 +473,10 @@ def isTaskComplete(self): if self.result_future.result(): self.status = self.result_future.result().status if self.status != GoalStatus.STATUS_SUCCEEDED: - self.debug(f'Task with failed with status code: {self.status}') + result = self.result_future.result().result + self.setLastError(result.error_code, result.error_msg) + self.debug('Task with failed with status code:' + f'{self.status}:{result.error_code}:{result.error_msg}') return True else: # Timed out, still processing, not complete yet @@ -478,6 +500,17 @@ def getResult(self): else: return TaskResult.UNKNOWN + def clearLastError(self): + self.last_error_code = 0 + self.last_error_msg = '' + + def setLastError(self, error_code, error_msg): + self.last_error_code = error_code + self.last_error_msg = error_msg + + def getLastError(self) -> tuple[int, str]: + return (self.last_error_code, self.last_error_msg) + def waitUntilNav2Active(self, navigator='bt_navigator', localizer='amcl'): """Block until the full navigation system is up and running.""" if localizer != 'robot_localization': # non-lifecycle node @@ -488,7 +521,9 @@ def waitUntilNav2Active(self, navigator='bt_navigator', localizer='amcl'): self.info('Nav2 is ready for use!') return - def _getPathImpl(self, start, goal, planner_id='', use_start=False): + def _getPathImpl( + self, start, goal, planner_id='', use_start=False + ) -> ComputePathToPose.Result: """ Send a `ComputePathToPose` action request. @@ -511,7 +546,11 @@ def _getPathImpl(self, start, goal, planner_id='', use_start=False): if not self.goal_handle.accepted: self.error('Get path was rejected!') - return None + self.status = GoalStatus.UNKNOWN + result = ComputePathToPose.Result() + result.error_code = ComputePathToPose.NONE + result.error_msg = 'Get path was rejected' + return result self.result_future = self.goal_handle.get_result_async() rclpy.spin_until_future_complete(self, self.result_future) @@ -521,18 +560,20 @@ def _getPathImpl(self, start, goal, planner_id='', use_start=False): def getPath(self, start, goal, planner_id='', use_start=False): """Send a `ComputePathToPose` action request.""" + self.clearLastError() rtn = self._getPathImpl(start, goal, planner_id, use_start) - if self.status != GoalStatus.STATUS_SUCCEEDED: - self.warn(f'Getting path failed with status code: {self.status}') - return None - - if not rtn: - return None - else: + if self.status == GoalStatus.STATUS_SUCCEEDED: return rtn.path + else: + self.setLastError(rtn.error_code, rtn.error_msg) + self.warn('Getting path failed with status code:' + f'{self.status}:{rtn.error_code}:{rtn.error_msg}') + return None - def _getPathThroughPosesImpl(self, start, goals, planner_id='', use_start=False): + def _getPathThroughPosesImpl( + self, start, goals, planner_id='', use_start=False + ) -> ComputePathThroughPoses.Result: """ Send a `ComputePathThroughPoses` action request. @@ -561,7 +602,10 @@ def _getPathThroughPosesImpl(self, start, goals, planner_id='', use_start=False) if not self.goal_handle.accepted: self.error('Get path was rejected!') - return None + result = ComputePathThroughPoses.Result() + result.error_code = ComputePathThroughPoses.UNKNOWN + result.error_msg = 'Get path was rejected!' + return result self.result_future = self.goal_handle.get_result_async() rclpy.spin_until_future_complete(self, self.result_future) @@ -571,20 +615,20 @@ def _getPathThroughPosesImpl(self, start, goals, planner_id='', use_start=False) def getPathThroughPoses(self, start, goals, planner_id='', use_start=False): """Send a `ComputePathThroughPoses` action request.""" + self.clearLastError() rtn = self._getPathThroughPosesImpl(start, goals, planner_id, use_start) - if self.status != GoalStatus.STATUS_SUCCEEDED: - self.warn(f'Getting path failed with status code: {self.status}') - return None - - if not rtn: - return None - else: + if self.status == GoalStatus.STATUS_SUCCEEDED: return rtn.path + else: + self.setLastError(rtn.error_code, rtn.error_msg) + self.warn('Getting path failed with status code:' + f'{self.status}:{rtn.error_code}:{rtn.error_msg}') + return None def _smoothPathImpl( self, path, smoother_id='', max_duration=2.0, check_for_collision=False - ): + ) -> SmoothPath.Result: """ Send a `SmoothPath` action request. @@ -607,7 +651,10 @@ def _smoothPathImpl( if not self.goal_handle.accepted: self.error('Smooth path was rejected!') - return None + result = SmoothPath.Result() + result.error_code = SmoothPath.UNKNOWN + result.error_msg = 'Smooth path was rejected' + return result self.result_future = self.goal_handle.get_result_async() rclpy.spin_until_future_complete(self, self.result_future) @@ -619,34 +666,48 @@ def smoothPath( self, path, smoother_id='', max_duration=2.0, check_for_collision=False ): """Send a `SmoothPath` action request.""" + self.clearLastError() rtn = self._smoothPathImpl(path, smoother_id, max_duration, check_for_collision) - if self.status != GoalStatus.STATUS_SUCCEEDED: - self.warn(f'Getting path failed with status code: {self.status}') - return None - - if not rtn: - return None - else: + if self.status == GoalStatus.STATUS_SUCCEEDED: return rtn.path + else: + self.setLastError(rtn.error_code, rtn.error_msg) + self.warn('Getting path failed with status code:' + f'{self.status}:{rtn.error_code}:{rtn.error_msg}') + return None - def changeMap(self, map_filepath): + def changeMap(self, map_filepath) -> bool: """Change the current static map in the map server.""" + self.clearLastError() while not self.change_maps_srv.wait_for_service(timeout_sec=1.0): self.info('change map service not available, waiting...') req = LoadMap.Request() req.map_url = map_filepath future = self.change_maps_srv.call_async(req) rclpy.spin_until_future_complete(self, future) - status = future.result().result - if status != LoadMap.Response().RESULT_SUCCESS: - self.error('Change map request failed!') + result = future.result().result + if result != LoadMap.Response().RESULT_SUCCESS: + if result == LoadMap.RESULT_MAP_DOES_NOT_EXIST: + reason = 'Map does not exist' + elif result == LoadMap.INVALID_MAP_DATA: + reason = 'Invalid map data' + elif result == LoadMap.INVALID_MAP_METADATA: + reason = 'Invalid map metadata' + elif result == LoadMap.UNDEFINED_FAILURE: + reason = 'Undefined failure' + else: + reason = 'Unknown' + self.setLastError(result, reason) + self.error(f'Change map request failed:{reason}!') + return False else: self.info('Change map request was successful!') - return + return True def clearAllCostmaps(self): """Clear all costmaps.""" + self.clearLastError() self.clearLocalCostmap() self.clearGlobalCostmap() return