nav2_simple_commander: use error_code and error_msg (#4341)

Signed-off-by: Mike Wake <[email protected]>
ros-navigation · Jun 23, 2024 · face00d · face00d
1 parent da5e15a
commit face00d
Show file tree

Hide file tree

Showing 9 changed files with 115 additions and 42 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,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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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!')