nav2_system_tests: log error_code and error_msg. (#4341)

Signed-off-by: Mike Wake <[email protected]>
ros-navigation · Oct 4, 2024 · f76ec0a · f76ec0a
1 parent 52eecbf
commit f76ec0a
Show file tree

Hide file tree

Showing 2 changed files with 16 additions and 4 deletions.
diff --git a/nav2_system_tests/src/system/nav_through_poses_tester_node.py b/nav2_system_tests/src/system/nav_through_poses_tester_node.py
@@ -116,7 +116,10 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None):
         rclpy.spin_until_future_complete(self, get_result_future)
         status = get_result_future.result().status
         if status != GoalStatus.STATUS_SUCCEEDED:
-            self.info_msg(f'Goal failed with status code: {status}')
+            result = get_result_future.result().result
+            self.info_msg(f'Goal failed with status code: {status}'
+                          f' error code:{result.error_code}'
+                          f' error msg:{result.error_msg}')
             return False
 
         self.info_msg('Goal succeeded!')
@@ -149,7 +152,10 @@ def runFakeNavigateAction(self):
         rclpy.spin_until_future_complete(self, get_result_future)
         status = get_result_future.result().status
         if status != GoalStatus.STATUS_SUCCEEDED:
-            self.info_msg(f'Goal failed with status code: {status}')
+            result = get_result_future.result().result
+            self.info_msg(f'Goal failed with status code: {status}'
+                          f' error code:{result.error_code}'
+                          f' error msg:{result.error_msg}')
             return False
 
         self.info_msg('Goal succeeded!')
@@ -186,7 +192,10 @@ def runNavigatePreemptionAction(self, block):
         rclpy.spin_until_future_complete(self, get_result_future)
         status = get_result_future.result().status
         if status != GoalStatus.STATUS_SUCCEEDED:
-            self.info_msg(f'Goal failed with status code: {status}')
+            result = get_result_future.result().result
+            self.info_msg(f'Goal failed with status code: {status}'
+                          f' error code:{result.error_code}'
+                          f' error msg:{result.error_msg}')
             return False
 
         self.info_msg('Goal succeeded!')

diff --git a/nav2_system_tests/src/system/nav_to_pose_tester_node.py b/nav2_system_tests/src/system/nav_to_pose_tester_node.py
@@ -118,7 +118,10 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None):
         rclpy.spin_until_future_complete(self, get_result_future)
         status = get_result_future.result().status
         if status != GoalStatus.STATUS_SUCCEEDED:
-            self.info_msg(f'Goal failed with status code: {status}')
+            result = get_result_future.result().result
+            self.info_msg(f'Goal failed with status code: {status}'
+                          f' error code:{result.error_code}'
+                          f' error msg:{result.error_msg}')
             return False
 
         if not future_return: