feat: 优化protocol node节点运行日志

unilabos/ros/nodes/presets/protocol_node.py

@@ -1,8 +1,12 @@
+import json
 import time
 import traceback
+from pprint import pprint, saferepr, pformat
 from typing import Union
 
 import rclpy
+from rosidl_runtime_py import message_to_ordereddict
+
 from unilabos.messages import *  # type: ignore  # protocol names
 from rclpy.action import ActionServer, ActionClient
 from rclpy.action.server import ServerGoalHandle
@@ -185,6 +189,7 @@ class ROS2ProtocolNode(BaseROS2DeviceNode):
             protocol_return_value = None
             self.get_logger().info(f"Executing {protocol_name} action...")
             action_value_mapping = self._action_value_mappings[protocol_name]
+            step_results = []
             try:
                 print("+" * 30)
                 print(protocol_steps_generator)
@@ -227,7 +232,6 @@ class ROS2ProtocolNode(BaseROS2DeviceNode):
                 self._busy = True
 
                 # 逐步执行工作流
-                step_results = []
                 for i, action in enumerate(protocol_steps):
                     # self.get_logger().info(f"Running step {i + 1}: {action}")
                     if isinstance(action, dict):
@@ -238,6 +242,9 @@ class ROS2ProtocolNode(BaseROS2DeviceNode):
                         else:
                             result = await self.execute_single_action(**action)
                             step_results.append({"step": i + 1, "action": action["action_name"], "result": result})
+                            ret_info = json.loads(getattr(result, "return_info", "{}"))
+                            if not ret_info.get("suc", False):
+                                raise RuntimeError(f"Step {i + 1} failed.")
                     elif isinstance(action, list):
                         # 如果是并行动作，同时执行
                         actions = action
@@ -275,11 +282,10 @@ class ROS2ProtocolNode(BaseROS2DeviceNode):
 
             except Exception as e:
                 # 捕获并记录错误信息
-                execution_error = traceback.format_exc()
+                str_step_results = [{k: dict(message_to_ordereddict(v)) if k == "result" and hasattr(v, "SLOT_TYPES") else v for k, v in i.items()} for i in step_results]
+                execution_error = f"{traceback.format_exc()}\n\nStep Result: {pformat(str_step_results)}"
                 execution_success = False
-                error(f"协议 {protocol_name} 执行失败")
-                error(traceback.format_exc())
-                self.lab_logger().error(f"协议执行出错: {str(e)}")
+                self.lab_logger().error(f"协议 {protocol_name} 执行出错: {str(e)} \n{traceback.format_exc()}")
 
                 # 设置动作失败
                 goal_handle.abort()
@@ -305,7 +311,7 @@ class ROS2ProtocolNode(BaseROS2DeviceNode):
                         serialize_result_info(execution_error, execution_success, protocol_return_value),
                     )
 
-            self.lab_logger().info(f"🤩🤩🤩🤩🤩🤩协议 {protocol_name} 完成并返回结果😎😎😎😎😎😎")
+            self.lab_logger().info(f"协议 {protocol_name} 完成并返回结果")
             return result
 
         return execute_protocol