open_pub/Uni-Lab-OS
1
0
Fork 0
mirror of https://github.com/dptech-corp/Uni-Lab-OS.git synced 2025-12-17 13:01:12 +00:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Xuwznln
2025-08-10 17:31:44 +08:00
parent 662c063f50
commit fe1a029a9b
3 changed files with 54 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
unilabos/ros/nodes/base_device_node.py
View File

@@ -654,7 +654,7 @@ class BaseROS2DeviceNode(Node, Generic[T]):
ACTION, action_paramtypes = self.get_real_function(self.driver_instance, action_name) ACTION, action_paramtypes = self.get_real_function(self.driver_instance, action_name)
action_kwargs = convert_from_ros_msg_with_mapping(goal, action_value_mapping["goal"]) action_kwargs = convert_from_ros_msg_with_mapping(goal, action_value_mapping["goal"])
self.lab_logger().debug(f"接收到原始目标: {action_kwargs}") self.lab_logger().debug(f"任务 {ACTION.__name__} 接收到原始目标: {action_kwargs}")
# 向Host查询物料当前状态，如果是host本身的增加物料的请求，则直接跳过 # 向Host查询物料当前状态，如果是host本身的增加物料的请求，则直接跳过
if action_name not in ["create_resource_detailed", "create_resource"]: if action_name not in ["create_resource_detailed", "create_resource"]:
for k, v in goal.get_fields_and_field_types().items(): for k, v in goal.get_fields_and_field_types().items():
@@ -699,7 +699,7 @@ class BaseROS2DeviceNode(Node, Generic[T]):
##### self.lab_logger().info(f"准备执行: {action_kwargs}, 函数: {ACTION.__name__}") ##### self.lab_logger().info(f"准备执行: {action_kwargs}, 函数: {ACTION.__name__}")
time_start = time.time() time_start = time.time()
time_overall = 100 time_overall = 100
future = None
# 将阻塞操作放入线程池执行 # 将阻塞操作放入线程池执行
if asyncio.iscoroutinefunction(ACTION): if asyncio.iscoroutinefunction(ACTION):
try: try:
@@ -713,13 +713,14 @@ class BaseROS2DeviceNode(Node, Generic[T]):
execution_success = True execution_success = True
except Exception as e: except Exception as e:
execution_error = traceback.format_exc() execution_error = traceback.format_exc()
##### error(f"异步任务 {ACTION.__name__} 报错了") error(f"异步任务 {ACTION.__name__} 报错了\n{traceback.format_exc()}\n原始输入：{action_kwargs}")
error(traceback.format_exc()) error(traceback.format_exc())
future.add_done_callback(_handle_future_exception) future.add_done_callback(_handle_future_exception)
except Exception as e: except Exception as e:
execution_error = traceback.format_exc()
execution_success = False
self.lab_logger().error(f"创建异步任务失败: {traceback.format_exc()}") self.lab_logger().error(f"创建异步任务失败: {traceback.format_exc()}")
raise e
else: else:
##### self.lab_logger().info(f"同步执行动作 {ACTION}") ##### self.lab_logger().info(f"同步执行动作 {ACTION}")
future = self._executor.submit(ACTION, **action_kwargs) future = self._executor.submit(ACTION, **action_kwargs)
@@ -730,9 +731,7 @@ class BaseROS2DeviceNode(Node, Generic[T]):
action_return_value = fut.result() action_return_value = fut.result()
execution_success = True execution_success = True
except Exception as e: except Exception as e:
execution_error = traceback.format_exc() error(f"同步任务 {ACTION.__name__} 报错了\n{traceback.format_exc()}\n原始输入：{action_kwargs}")
error(f"同步任务 {ACTION.__name__} 报错了")
error(traceback.format_exc())
future.add_done_callback(_handle_future_exception) future.add_done_callback(_handle_future_exception)
@@ -740,7 +739,7 @@ class BaseROS2DeviceNode(Node, Generic[T]):
feedback_msg_types = action_type.Feedback.get_fields_and_field_types() feedback_msg_types = action_type.Feedback.get_fields_and_field_types()
result_msg_types = action_type.Result.get_fields_and_field_types() result_msg_types = action_type.Result.get_fields_and_field_types()
while not future.done(): while future is not None and not future.done():
if goal_handle.is_cancel_requested: if goal_handle.is_cancel_requested:
self.lab_logger().info(f"取消动作: {action_name}") self.lab_logger().info(f"取消动作: {action_name}")
future.cancel() # 尝试取消线程池中的任务 future.cancel() # 尝试取消线程池中的任务
@@ -772,7 +771,7 @@ class BaseROS2DeviceNode(Node, Generic[T]):
goal_handle.publish_feedback(feedback_msg) goal_handle.publish_feedback(feedback_msg)
time.sleep(0.5) time.sleep(0.5)
if future.cancelled(): if future is not None and future.cancelled():
self.lab_logger().info(f"动作 {action_name} 已取消") self.lab_logger().info(f"动作 {action_name} 已取消")
return action_type.Result() return action_type.Result()

50
unilabos/ros/nodes/presets/host_node.py
View File

@@ -39,6 +39,7 @@ from unilabos.ros.msgs.message_converter import (
from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode, ROS2DeviceNode, DeviceNodeResourceTracker from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode, ROS2DeviceNode, DeviceNodeResourceTracker
from unilabos.ros.nodes.presets.controller_node import ControllerNode from unilabos.ros.nodes.presets.controller_node import ControllerNode
from unilabos.utils.exception import DeviceClassInvalid from unilabos.utils.exception import DeviceClassInvalid
from unilabos.utils.type_check import serialize_result_info
class HostNode(BaseROS2DeviceNode): class HostNode(BaseROS2DeviceNode):
@@ -408,10 +409,11 @@ class HostNode(BaseROS2DeviceNode):
liquid_volume: list[int], liquid_volume: list[int],
slot_on_deck: str, slot_on_deck: str,
): ):
# 暂不支持多对同名父子同时存在
res_creation_input = { res_creation_input = {
"name": res_id, "name": res_id.split("/")[-1],
"class": class_name, "class": class_name,
"parent": parent, "parent": parent.split("/")[-1],
"position": { "position": {
"x": bind_locations.x, "x": bind_locations.x,
"y": bind_locations.y, "y": bind_locations.y,
@@ -419,18 +421,20 @@ class HostNode(BaseROS2DeviceNode):
}, },
} }
if len(liquid_input_slot) and liquid_input_slot[0] == -1: # 目前container只逐个创建 if len(liquid_input_slot) and liquid_input_slot[0] == -1: # 目前container只逐个创建
res_creation_input.update({ res_creation_input.update(
"data": { {
"liquid_type": liquid_type[0] if liquid_type else None, "data": {
"liquid_volume": liquid_volume[0] if liquid_volume else None, "liquid_type": liquid_type[0] if liquid_type else None,
"liquid_volume": liquid_volume[0] if liquid_volume else None,
}
} }
}) )
init_new_res = initialize_resource(res_creation_input) # flatten的格式 init_new_res = initialize_resource(res_creation_input) # flatten的格式
if len(init_new_res) > 1: # 一个物料，多个子节点 if len(init_new_res) > 1: # 一个物料，多个子节点
init_new_res = [init_new_res] init_new_res = [init_new_res]
resources: List[Resource] | List[List[Resource]] = init_new_res # initialize_resource已经返回list[dict] resources: List[Resource] | List[List[Resource]] = init_new_res # initialize_resource已经返回list[dict]
device_ids = [device_id] device_ids = [device_id]
bind_parent_id = [parent] bind_parent_id = [res_creation_input["parent"]]
bind_location = [bind_locations] bind_location = [bind_locations]
other_calling_param = [ other_calling_param = [
json.dumps( json.dumps(
@@ -680,20 +684,34 @@ class HostNode(BaseROS2DeviceNode):
result_msg = future.result().result result_msg = future.result().result
result_data = convert_from_ros_msg(result_msg) result_data = convert_from_ros_msg(result_msg)
status = "success" status = "success"
try: return_info_str = result_data.get("return_info")
ret = json.loads(result_data.get("return_info", "{}")) # 确保返回信息是有效的JSON
suc = ret.get("suc", False) if return_info_str is not None:
if not suc: try:
ret = json.loads(return_info_str)
suc = ret.get("suc", False)
if not suc:
status = "failed"
except json.JSONDecodeError:
status = "failed" status = "failed"
except json.JSONDecodeError: else:
status = "failed" # 无 return_info 字段时，回退到 success 字段（若存在）
self.lab_logger().info(f"[Host Node] Result for {action_id} ({uuid_str}): success") suc_field = result_data.get("success")
if isinstance(suc_field, bool):
status = "success" if suc_field else "failed"
return_info_str = serialize_result_info("", suc_field, result_data)
else:
# 最保守的回退：标记失败并返回空JSON
status = "failed"
return_info_str = serialize_result_info("缺少return_info", False, result_data)
self.lab_logger().info(f"[Host Node] Result for {action_id} ({uuid_str}): {status}")
self.lab_logger().debug(f"[Host Node] Result data: {result_data}") self.lab_logger().debug(f"[Host Node] Result data: {result_data}")
if uuid_str: if uuid_str:
for bridge in self.bridges: for bridge in self.bridges:
if hasattr(bridge, "publish_job_status"): if hasattr(bridge, "publish_job_status"):
bridge.publish_job_status(result_data, uuid_str, status, result_data.get("return_info", "{}")) bridge.publish_job_status(result_data, uuid_str, status, return_info_str)
def cancel_goal(self, goal_uuid: str) -> None: def cancel_goal(self, goal_uuid: str) -> None:
"""取消目标""" """取消目标"""

18
unilabos/ros/nodes/presets/protocol_node.py
View File

@@ -1,8 +1,12 @@
import json
import time import time
import traceback import traceback
from pprint import pprint, saferepr, pformat
from typing import Union from typing import Union
import rclpy import rclpy
from rosidl_runtime_py import message_to_ordereddict
from unilabos.messages import * # type: ignore # protocol names from unilabos.messages import * # type: ignore # protocol names
from rclpy.action import ActionServer, ActionClient from rclpy.action import ActionServer, ActionClient
from rclpy.action.server import ServerGoalHandle from rclpy.action.server import ServerGoalHandle
@@ -185,6 +189,7 @@ class ROS2ProtocolNode(BaseROS2DeviceNode):
protocol_return_value = None protocol_return_value = None
self.get_logger().info(f"Executing {protocol_name} action...") self.get_logger().info(f"Executing {protocol_name} action...")
action_value_mapping = self._action_value_mappings[protocol_name] action_value_mapping = self._action_value_mappings[protocol_name]
step_results = []
try: try:
print("+" * 30) print("+" * 30)
print(protocol_steps_generator) print(protocol_steps_generator)
@@ -227,7 +232,6 @@ class ROS2ProtocolNode(BaseROS2DeviceNode):
self._busy = True self._busy = True
# 逐步执行工作流 # 逐步执行工作流
step_results = []
for i, action in enumerate(protocol_steps): for i, action in enumerate(protocol_steps):
# self.get_logger().info(f"Running step {i + 1}: {action}") # self.get_logger().info(f"Running step {i + 1}: {action}")
if isinstance(action, dict): if isinstance(action, dict):
@@ -238,6 +242,9 @@ class ROS2ProtocolNode(BaseROS2DeviceNode):
else: else:
result = await self.execute_single_action(**action) result = await self.execute_single_action(**action)
step_results.append({"step": i + 1, "action": action["action_name"], "result": result}) 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): elif isinstance(action, list):
# 如果是并行动作，同时执行 # 如果是并行动作，同时执行
actions = action actions = action
@@ -275,11 +282,10 @@ class ROS2ProtocolNode(BaseROS2DeviceNode):
except Exception as e: 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 execution_success = False
error(f"协议 {protocol_name} 执行失败") self.lab_logger().error(f"协议 {protocol_name} 执行出错: {str(e)} \n{traceback.format_exc()}")
error(traceback.format_exc())
self.lab_logger().error(f"协议执行出错: {str(e)}")
# 设置动作失败 # 设置动作失败
goal_handle.abort() goal_handle.abort()
@@ -305,7 +311,7 @@ class ROS2ProtocolNode(BaseROS2DeviceNode):
serialize_result_info(execution_error, execution_success, protocol_return_value), 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 result
return execute_protocol return execute_protocol