nav2_simple_command track LastActionError (#4341)

Only actions provide error_code and error_msg in their result msg. Signed-off-by: Mike Wake <[email protected]>
ros-navigation · Jun 26, 2024 · a7146fd · a7146fd
1 parent 6fbbbc0
commit a7146fd
Show file tree

Hide file tree

Showing 9 changed files with 34 additions and 37 deletions.
diff --git a/nav2_simple_commander/nav2_simple_commander/demo_inspection.py b/nav2_simple_commander/nav2_simple_commander/demo_inspection.py
@@ -98,7 +98,7 @@ def main():
     elif result == TaskResult.CANCELED:
         print('Inspection of shelving was canceled. Returning to start...')
     elif result == TaskResult.FAILED:
-        (error_code, error_msg) = navigator.getLastError()
+        (error_code, error_msg) = navigator.getLastActionError()
         print(f'Inspection of shelving failed!:{error_code}:{error_msg}')
         print('Returning to start...')
 

diff --git a/nav2_simple_commander/nav2_simple_commander/demo_picking.py b/nav2_simple_commander/nav2_simple_commander/demo_picking.py
@@ -133,7 +133,7 @@ def main():
         navigator.goToPose(initial_pose)
 
     elif result == TaskResult.FAILED:
-        (error_code, error_msg) = navigator.getLastError()
+        (error_code, error_msg) = navigator.getLastActionError()
         print(f'Task at {request_item_location} failed!:'
               f'{error_code}:{error_msg}')
         exit(-1)

diff --git a/nav2_simple_commander/nav2_simple_commander/demo_recoveries.py b/nav2_simple_commander/nav2_simple_commander/demo_recoveries.py
@@ -91,7 +91,7 @@ def main():
     elif result == TaskResult.CANCELED:
         print('Recovery was canceled. Returning to start...')
     elif result == TaskResult.FAILED:
-        (error_code, error_msg) = navigator.getLastError()
+        (error_code, error_msg) = navigator.getLastActionError()
         print(f'Recovering from dead end failed!:{error_code}:{error_msg}')
         print('Returning to start...')
 

diff --git a/nav2_simple_commander/nav2_simple_commander/demo_security.py b/nav2_simple_commander/nav2_simple_commander/demo_security.py
@@ -104,7 +104,7 @@ def main():
             print('Security route was canceled, exiting.')
             exit(1)
         elif result == TaskResult.FAILED:
-            (error_code, error_msg) = navigator.getLastError()
+            (error_code, error_msg) = navigator.getLastActionError()
             print(f'Security route failed!:{error_code}:{error_msg}')
             print('Restarting from other side...')
 

diff --git 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,7 @@ def main():
     elif result == TaskResult.CANCELED:
         print('Goal was canceled!')
     elif result == TaskResult.FAILED:
-        (error_code, error_msg) = navigator.getLastError()
+        (error_code, error_msg) = navigator.getLastActionError()
         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
@@ -131,7 +131,7 @@ def main():
     elif result == TaskResult.CANCELED:
         print('Goal was canceled!')
     elif result == TaskResult.FAILED:
-        (error_code, error_msg) = navigator.getLastError()
+        (error_code, error_msg) = navigator.getLastActionError()
         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
@@ -106,7 +106,7 @@ def main():
     elif result == TaskResult.CANCELED:
         print('Goal was canceled!')
     elif result == TaskResult.FAILED:
-        (error_code, error_msg) = navigator.getLastError()
+        (error_code, error_msg) = navigator.getLastActionError()
         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
@@ -133,7 +133,7 @@ def main():
     elif result == TaskResult.CANCELED:
         print('Goal was canceled!')
     elif result == TaskResult.FAILED:
-        (error_code, error_msg) = navigator.getLastError()
+        (error_code, error_msg) = navigator.getLastActionError()
         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
@@ -62,8 +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 = ''
+        self.last_action_error_code = 0
+        self.last_action_error_msg = ''
 
         amcl_pose_qos = QoSProfile(
             durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
@@ -142,14 +142,13 @@ 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.clearLastActionError()
         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...")
@@ -174,7 +173,7 @@ def goThroughPoses(self, poses, behavior_tree=''):
 
     def goToPose(self, pose, behavior_tree=''):
         """Send a `NavToPose` action request."""
-        self.clearLastError()
+        self.clearLastActionError()
         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...")
@@ -211,7 +210,7 @@ def goToPose(self, pose, behavior_tree=''):
 
     def followWaypoints(self, poses):
         """Send a `FollowWaypoints` action request."""
-        self.clearLastError()
+        self.clearLastActionError()
         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...")
@@ -235,7 +234,7 @@ def followWaypoints(self, poses):
 
     def followGpsWaypoints(self, gps_poses):
         """Send a `FollowGPSWaypoints` action request."""
-        self.clearLastError()
+        self.clearLastActionError()
         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...")
@@ -260,7 +259,7 @@ def followGpsWaypoints(self, gps_poses):
         return True
 
     def spin(self, spin_dist=1.57, time_allowance=10):
-        self.clearLastError()
+        self.clearLastActionError()
         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...")
@@ -283,7 +282,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.clearLastActionError()
         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...")
@@ -307,7 +306,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.clearLastActionError()
         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...")
@@ -331,7 +330,7 @@ def driveOnHeading(self, dist=0.15, speed=0.025, time_allowance=10):
         return True
 
     def assistedTeleop(self, time_allowance=30):
-        self.clearLastError()
+        self.clearLastActionError()
         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...")
@@ -353,7 +352,7 @@ def assistedTeleop(self, time_allowance=30):
         return True
 
     def followPath(self, path, controller_id='', goal_checker_id=''):
-        self.clearLastError()
+        self.clearLastActionError()
         """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):
@@ -379,7 +378,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()
+        self.clearLastActionError()
         """Send a `DockRobot` action request."""
         self.info("Waiting for 'DockRobot' action server")
         while not self.docking_client.wait_for_server(timeout_sec=1.0):
@@ -407,7 +406,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.clearLastActionError()
         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...')
@@ -433,7 +432,7 @@ def dockRobotByID(self, dock_id, nav_to_dock=True):
 
     def undockRobot(self, dock_type=''):
         """Send a `UndockRobot` action request."""
-        self.clearLastError()
+        self.clearLastActionError()
         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...')
@@ -457,7 +456,7 @@ def undockRobot(self, dock_type=''):
 
     def cancelTask(self):
         """Cancel pending task request of any type."""
-        self.clearLastError()
+        self.clearLastActionError()
         self.info('Canceling current task.')
         if self.result_future:
             future = self.goal_handle.cancel_goal_async()
@@ -500,16 +499,16 @@ def getResult(self):
         else:
             return TaskResult.UNKNOWN
 
-    def clearLastError(self):
-        self.last_error_code = 0
-        self.last_error_msg = ''
+    def clearLastActionError(self):
+        self.last_action_error_code = 0
+        self.last_action_error_msg = ''
 
-    def setLastError(self, error_code, error_msg):
-        self.last_error_code = error_code
-        self.last_error_msg = error_msg
+    def setLastActionError(self, error_code, error_msg):
+        self.last_action_error_code = error_code
+        self.last_action_error_msg = error_msg
 
-    def getLastError(self) -> tuple[int, str]:
-        return (self.last_error_code, self.last_error_msg)
+    def getLastActionError(self) -> tuple[int, str]:
+        return (self.last_action_error_code, self.last_action_error_msg)
 
     def waitUntilNav2Active(self, navigator='bt_navigator', localizer='amcl'):
         """Block until the full navigation system is up and running."""
@@ -560,7 +559,7 @@ def _getPathImpl(
 
     def getPath(self, start, goal, planner_id='', use_start=False):
         """Send a `ComputePathToPose` action request."""
-        self.clearLastError()
+        self.clearLastActionError()
         rtn = self._getPathImpl(start, goal, planner_id, use_start)
 
         if self.status == GoalStatus.STATUS_SUCCEEDED:
@@ -615,7 +614,7 @@ def _getPathThroughPosesImpl(
 
     def getPathThroughPoses(self, start, goals, planner_id='', use_start=False):
         """Send a `ComputePathThroughPoses` action request."""
-        self.clearLastError()
+        self.clearLastActionError()
         rtn = self._getPathThroughPosesImpl(start, goals, planner_id, use_start)
 
         if self.status == GoalStatus.STATUS_SUCCEEDED:
@@ -666,7 +665,7 @@ def smoothPath(
         self, path, smoother_id='', max_duration=2.0, check_for_collision=False
     ):
         """Send a `SmoothPath` action request."""
-        self.clearLastError()
+        self.clearLastActionError()
         rtn = self._smoothPathImpl(path, smoother_id, max_duration, check_for_collision)
 
         if self.status == GoalStatus.STATUS_SUCCEEDED:
@@ -679,7 +678,6 @@ def smoothPath(
 
     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()
@@ -707,7 +705,6 @@ def changeMap(self, map_filepath) -> bool:
 
     def clearAllCostmaps(self):
         """Clear all costmaps."""
-        self.clearLastError()
         self.clearLocalCostmap()
         self.clearGlobalCostmap()
         return