change robot_navigator api to (get|set|clear)TaskError() (ros-navigat…

…ion#4341)

Signed-off-by: Mike Wake <[email protected]>

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.getLastActionError()
+        (error_code, error_msg) = navigator.getTaskError()
         print(f'Inspection of shelving failed!:{error_code}:{error_msg}')
         print('Returning to start...')
 

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.getLastActionError()
+        (error_code, error_msg) = navigator.getTaskError()
         print(f'Task at {request_item_location} failed!:'
               f'{error_code}:{error_msg}')
         exit(-1)

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.getLastActionError()
+        (error_code, error_msg) = navigator.getTaskError()
         print(f'Recovering from dead end failed!:{error_code}:{error_msg}')
         print('Returning to start...')
 

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.getLastActionError()
+            (error_code, error_msg) = navigator.getTaskError()
             print(f'Security route failed!:{error_code}:{error_msg}')
             print('Restarting from other side...')
 

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.getLastActionError()
+        (error_code, error_msg) = navigator.getTaskError()
         print('Goal failed!{error_code}:{error_msg}')
     else:
         print('Goal has an invalid return status!')

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.getLastActionError()
+        (error_code, error_msg) = navigator.getTaskError()
         print('Goal failed!{error_code}:{error_msg}')
     else:
         print('Goal has an invalid return status!')

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.getLastActionError()
+        (error_code, error_msg) = navigator.getTaskError()
         print('Goal failed!{error_code}:{error_msg}')
     else:
         print('Goal has an invalid return status!')

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.getLastActionError()
+        (error_code, error_msg) = navigator.getTaskError()
         print('Goal failed!{error_code}:{error_msg}')
     else:
         print('Goal has an invalid return status!')

nav2_simple_commander/nav2_simple_commander/robot_navigator.py

@@ -148,7 +148,7 @@ def setInitialPose(self, initial_pose):
 
     def goThroughPoses(self, poses, behavior_tree=''):
         """Send a `NavThroughPoses` action request."""
-        self.clearLastActionError()
+        self.clearTaskError()
         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...")
@@ -173,7 +173,7 @@ def goThroughPoses(self, poses, behavior_tree=''):
 
     def goToPose(self, pose, behavior_tree=''):
         """Send a `NavToPose` action request."""
-        self.clearLastActionError()
+        self.clearTaskError()
         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...")
@@ -210,7 +210,7 @@ def goToPose(self, pose, behavior_tree=''):
 
     def followWaypoints(self, poses):
         """Send a `FollowWaypoints` action request."""
-        self.clearLastActionError()
+        self.clearTaskError()
         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...")
@@ -234,7 +234,7 @@ def followWaypoints(self, poses):
 
     def followGpsWaypoints(self, gps_poses):
         """Send a `FollowGPSWaypoints` action request."""
-        self.clearLastActionError()
+        self.clearTaskError()
         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...")
@@ -259,7 +259,7 @@ def followGpsWaypoints(self, gps_poses):
         return True
 
     def spin(self, spin_dist=1.57, time_allowance=10):
-        self.clearLastActionError()
+        self.clearTaskError()
         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...")
@@ -282,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.clearLastActionError()
+        self.clearTaskError()
         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...")
@@ -306,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.clearLastActionError()
+        self.clearTaskError()
         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...")
@@ -330,7 +330,7 @@ def driveOnHeading(self, dist=0.15, speed=0.025, time_allowance=10):
         return True
 
     def assistedTeleop(self, time_allowance=30):
-        self.clearLastActionError()
+        self.clearTaskError()
         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...")
@@ -352,7 +352,7 @@ def assistedTeleop(self, time_allowance=30):
         return True
 
     def followPath(self, path, controller_id='', goal_checker_id=''):
-        self.clearLastActionError()
+        self.clearTaskError()
         """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):
@@ -378,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.clearLastActionError()
+        self.clearTaskError()
         """Send a `DockRobot` action request."""
         self.info("Waiting for 'DockRobot' action server")
         while not self.docking_client.wait_for_server(timeout_sec=1.0):
@@ -397,7 +397,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.setTaskError(DockRobot.UNKNOWN, 'Docking request was rejected')
             self.info('Docking request was rejected!')
             return False
 
@@ -406,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.clearLastActionError()
+        self.clearTaskError()
         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...')
@@ -423,7 +423,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.setTaskError(DockRobot.UNKNOWN, 'Docking request was rejected')
             self.info('Docking request was rejected!')
             return False
 
@@ -432,7 +432,7 @@ def dockRobotByID(self, dock_id, nav_to_dock=True):
 
     def undockRobot(self, dock_type=''):
         """Send a `UndockRobot` action request."""
-        self.clearLastActionError()
+        self.clearTaskError()
         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...')
@@ -447,7 +447,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.setTaskError(UndockRobot.UNKNOWN, 'Undocking request was rejected')
             self.info('Undocking request was rejected!')
             return False
 
@@ -456,11 +456,11 @@ def undockRobot(self, dock_type=''):
 
     def cancelTask(self):
         """Cancel pending task request of any type."""
-        self.clearLastActionError()
         self.info('Canceling current task.')
         if self.result_future:
             future = self.goal_handle.cancel_goal_async()
             rclpy.spin_until_future_complete(self, future)
+        self.clearTaskError()
         return
 
     def isTaskComplete(self) -> bool:
@@ -473,9 +473,11 @@ def isTaskComplete(self) -> bool:
             self.status = self.result_future.result().status
             if self.status != GoalStatus.STATUS_SUCCEEDED:
                 result = self.result_future.result().result
-                self.setLastActionError(result.error_code, result.error_msg)
-                self.debug('Task with failed with status code:'
-                           f'{self.status}:{result.error_code}:{result.error_msg}')
+                self.setTaskError(result.error_code, result.error_msg)
+                self.debug('Task with failed with'
+                           f' status code:{self.status}'
+                           f' error code:{result.error_code}'
+                           f' error msg:{result.error_msg}')
                 return True
         else:
             # Timed out, still processing, not complete yet
@@ -499,15 +501,15 @@ def getResult(self):
         else:
             return TaskResult.UNKNOWN
 
-    def clearLastActionError(self):
+    def clearTaskError(self):
         self.last_action_error_code = 0
         self.last_action_error_msg = ''
 
-    def setLastActionError(self, error_code, error_msg):
+    def setTaskError(self, error_code, error_msg):
         self.last_action_error_code = error_code
         self.last_action_error_msg = error_msg
 
-    def getLastActionError(self) -> tuple[int, str]:
+    def getTaskError(self) -> tuple[int, str]:
         return (self.last_action_error_code, self.last_action_error_msg)
 
     def waitUntilNav2Active(self, navigator='bt_navigator', localizer='amcl'):
@@ -559,15 +561,17 @@ def _getPathImpl(
 
     def getPath(self, start, goal, planner_id='', use_start=False):
         """Send a `ComputePathToPose` action request."""
-        self.clearLastActionError()
+        self.clearTaskError()
         rtn = self._getPathImpl(start, goal, planner_id, use_start)
 
         if self.status == GoalStatus.STATUS_SUCCEEDED:
             return rtn.path
         else:
-            self.setLastActionError(rtn.error_code, rtn.error_msg)
-            self.warn('Getting path failed with status code:'
-                      f'{self.status}:{rtn.error_code}:{rtn.error_msg}')
+            self.setTaskError(rtn.error_code, rtn.error_msg)
+            self.warn('Getting path failed with'
+                      f' status code:{self.status}'
+                      f' error code:{rtn.error_code}'
+                      f' error msg:{rtn.error_msg}')
             return None
 
     def _getPathThroughPosesImpl(
@@ -614,15 +618,17 @@ def _getPathThroughPosesImpl(
 
     def getPathThroughPoses(self, start, goals, planner_id='', use_start=False):
         """Send a `ComputePathThroughPoses` action request."""
-        self.clearLastActionError()
+        self.clearTaskError()
         rtn = self._getPathThroughPosesImpl(start, goals, planner_id, use_start)
 
         if self.status == GoalStatus.STATUS_SUCCEEDED:
             return rtn.path
         else:
-            self.setLastActionError(rtn.error_code, rtn.error_msg)
-            self.warn('Getting path failed with status code:'
-                      f'{self.status}:{rtn.error_code}:{rtn.error_msg}')
+            self.setTaskError(rtn.error_code, rtn.error_msg)
+            self.warn('Getting path failed with'
+                      f' status code:{self.status}'
+                      f' error code:{rtn.error_code}'
+                      f' error msg:{rtn.error_msg}')
             return None
 
     def _smoothPathImpl(
@@ -665,15 +671,17 @@ def smoothPath(
         self, path, smoother_id='', max_duration=2.0, check_for_collision=False
     ):
         """Send a `SmoothPath` action request."""
-        self.clearLastActionError()
+        self.clearTaskError()
         rtn = self._smoothPathImpl(path, smoother_id, max_duration, check_for_collision)
 
         if self.status == GoalStatus.STATUS_SUCCEEDED:
             return rtn.path
         else:
-            self.setLastActionError(rtn.error_code, rtn.error_msg)
-            self.warn('Getting path failed with status code:'
-                      f'{self.status}:{rtn.error_code}:{rtn.error_msg}')
+            self.setTaskError(rtn.error_code, rtn.error_msg)
+            self.warn('Getting path failed with'
+                      f' status code:{self.status}'
+                      f' error code:{rtn.error_code}'
+                      f' error msg:{rtn.error_msg}')
             return None
 
     def changeMap(self, map_filepath) -> bool:
@@ -696,7 +704,7 @@ def changeMap(self, map_filepath) -> bool:
                 reason = 'Undefined failure'
             else:
                 reason = 'Unknown'
-            self.setLastActionError(result, reason)
+            self.setTaskError(result, reason)
             self.error(f'Change map request failed:{reason}!')
             return False
         else: