diff --git a/unilabos/ros/nodes/presets/protocol_node.py b/unilabos/ros/nodes/presets/protocol_node.py index 24e89703..25b9c11a 100644 --- a/unilabos/ros/nodes/presets/protocol_node.py +++ b/unilabos/ros/nodes/presets/protocol_node.py @@ -28,6 +28,9 @@ from unilabos.utils.log import error from unilabos.utils.type_check import serialize_result_info, get_result_info_str +class ROS2ProtocolNodeTempError(Exception): + pass + class ROS2ProtocolNode(BaseROS2DeviceNode): """ ROS2ProtocolNode代表管理ROS2环境中设备通信和动作的协议节点。 @@ -246,11 +249,14 @@ class ROS2ProtocolNode(BaseROS2DeviceNode): time.sleep(action["action_kwargs"]["time"]) step_results.append({"step": i + 1, "action": "wait", "result": "completed"}) 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.") + try: + 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.") + except ROS2ProtocolNodeTempError as ex: + step_results.append({"step": i + 1, "action": action["action_name"], "result": ex.args[0]}) elif isinstance(action, list): # 如果是并行动作,同时执行 actions = action @@ -327,6 +333,7 @@ class ROS2ProtocolNode(BaseROS2DeviceNode): # 构建动作ID if action_name == "log_message": self.lab_logger().info(f"[Protocol Log] {action_kwargs}") + raise ROS2ProtocolNodeTempError(f"[Protocol Log] {action_kwargs}") if device_id in ["", None, "self"]: action_id = f"/devices/{self.device_id}/{action_name}" else: