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

stir和adjustph的中的bug修不好

Browse Source
This commit is contained in:
KCFeng425
2025-07-18 00:28:12 +08:00
parent 6b7564b9f9
commit 79df194ca0
7 changed files with 328 additions and 121 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -629,7 +629,7 @@ class BaseROS2DeviceNode(Node, Generic[T]):
execution_success = False
action_return_value = None
self.lab_logger().info(f"执行动作: {action_name}")
##### self.lab_logger().info(f"执行动作: {action_name}")
goal = goal_handle.request
# 从目标消息中提取参数, 并调用对应的方法
@@ -685,14 +685,14 @@ class BaseROS2DeviceNode(Node, Generic[T]):
final_resource = [convert_resources_to_type([i], final_type)[0] for i in resources_list]
action_kwargs[k] = self.resource_tracker.figure_resource(final_resource)
self.lab_logger().info(f"准备执行: {action_kwargs}, 函数: {ACTION.__name__}")
##### self.lab_logger().info(f"准备执行: {action_kwargs}, 函数: {ACTION.__name__}")
time_start = time.time()
time_overall = 100
# 将阻塞操作放入线程池执行
if asyncio.iscoroutinefunction(ACTION):
try:
self.lab_logger().info(f"异步执行动作 {ACTION}")
##### self.lab_logger().info(f"异步执行动作 {ACTION}")
future = ROS2DeviceNode.run_async_func(ACTION, trace_error=False, **action_kwargs)
def _handle_future_exception(fut):
@@ -702,7 +702,7 @@ class BaseROS2DeviceNode(Node, Generic[T]):
execution_success = True
except Exception as e:
execution_error = traceback.format_exc()
error(f"异步任务 {ACTION.__name__} 报错了")
##### error(f"异步任务 {ACTION.__name__} 报错了")
error(traceback.format_exc())
future.add_done_callback(_handle_future_exception)
@@ -710,7 +710,7 @@ class BaseROS2DeviceNode(Node, Generic[T]):
self.lab_logger().error(f"创建异步任务失败: {traceback.format_exc()}")
raise e
else:
self.lab_logger().info(f"同步执行动作 {ACTION}")
##### self.lab_logger().info(f"同步执行动作 {ACTION}")
future = self._executor.submit(ACTION, **action_kwargs)
def _handle_future_exception(fut):
@@ -765,7 +765,7 @@ class BaseROS2DeviceNode(Node, Generic[T]):
self.lab_logger().info(f"动作 {action_name} 已取消")
return action_type.Result()
self.lab_logger().info(f"动作执行完成: {action_name}")
##### self.lab_logger().info(f"动作执行完成: {action_name}")
del future
# 向Host更新物料当前状态
@@ -801,7 +801,7 @@ class BaseROS2DeviceNode(Node, Generic[T]):
# 发布结果
goal_handle.succeed()
self.lab_logger().info(f"设置动作成功: {action_name}")
##### self.lab_logger().info(f"设置动作成功: {action_name}")
result_values = {}
for msg_name, attr_name in action_value_mapping["result"].items():
@@ -820,7 +820,7 @@ class BaseROS2DeviceNode(Node, Generic[T]):
elif attr_name == "return_info":
setattr(result_msg, attr_name, serialize_result_info(execution_error, execution_success, action_return_value))
self.lab_logger().info(f"动作 {action_name} 完成并返回结果")
##### self.lab_logger().info(f"动作 {action_name} 完成并返回结果")
return result_msg
return execute_callback

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

@@ -562,9 +562,9 @@ class HostNode(BaseROS2DeviceNode):
for bridge in self.bridges:
if hasattr(bridge, "publish_device_status"):
bridge.publish_device_status(self.device_status, device_id, property_name)
#self.lab_logger().debug(
# f"[Host Node] Status updated: {device_id}.{property_name} = {msg.data}"
#)
self.lab_logger().debug(
f"[Host Node] Status updated: {device_id}.{property_name} = {msg.data}"
)
def send_goal(
self,

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

@@ -183,25 +183,25 @@ class ROS2ProtocolNode(BaseROS2DeviceNode):
goal = goal_handle.request
protocol_kwargs = convert_from_ros_msg_with_mapping(goal, action_value_mapping["goal"])
# 🔧 添加调试信息
print(f"🔍 转换后的 protocol_kwargs: {protocol_kwargs}")
print(f"🔍 vessel 在转换后: {protocol_kwargs.get('vessel', 'NOT_FOUND')}")
# # 🔧 添加调试信息
# print(f"🔍 转换后的 protocol_kwargs: {protocol_kwargs}")
# print(f"🔍 vessel 在转换后: {protocol_kwargs.get('vessel', 'NOT_FOUND')}")
# 🔧 完全禁用Host查询，直接使用转换后的数据
print(f"🔧 跳过Host查询，直接使用转换后的数据")
# # 🔧 完全禁用Host查询，直接使用转换后的数据
# print(f"🔧 跳过Host查询，直接使用转换后的数据")
# 🔧 额外验证：确保vessel数据完整
if 'vessel' in protocol_kwargs:
vessel_data = protocol_kwargs['vessel']
print(f"🔍 验证vessel数据: {vessel_data}")
#print(f"🔍 验证vessel数据: {vessel_data}")
# 如果vessel是空字典，尝试重新构建
if not vessel_data or (isinstance(vessel_data, dict) and not vessel_data):
print(f"⚠️ vessel数据为空，尝试从原始goal重新提取...")
# print(f"⚠️ vessel数据为空，尝试从原始goal重新提取...")
# 直接从原始goal提取vessel
if hasattr(goal, 'vessel') and goal.vessel:
print(f"🔍 原始goal.vessel: {goal.vessel}")
# print(f"🔍 原始goal.vessel: {goal.vessel}")
# 手动转换vessel
vessel_data = {
'id': goal.vessel.id,
@@ -212,16 +212,16 @@ class ROS2ProtocolNode(BaseROS2DeviceNode):
'data': goal.vessel.data
}
protocol_kwargs['vessel'] = vessel_data
print(f"✅ 手动重建vessel数据: {vessel_data}")
# print(f"✅ 手动重建vessel数据: {vessel_data}")
else:
print(f"❌ 无法从原始goal提取vessel数据")
# print(f"❌ 无法从原始goal提取vessel数据")
# 创建一个基本的vessel
vessel_data = {'id': 'default_vessel'}
protocol_kwargs['vessel'] = vessel_data
print(f"🔧 创建默认vessel: {vessel_data}")
# print(f"🔧 创建默认vessel: {vessel_data}")
print(f"🔍 最终传递给协议的 protocol_kwargs: {protocol_kwargs}")
print(f"🔍 最终的 vessel: {protocol_kwargs.get('vessel', 'NOT_FOUND')}")
#print(f"🔍 最终传递给协议的 protocol_kwargs: {protocol_kwargs}")
#print(f"🔍 最终的 vessel: {protocol_kwargs.get('vessel', 'NOT_FOUND')}")
from unilabos.resources.graphio import physical_setup_graph
@@ -315,7 +315,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
@@ -337,7 +337,7 @@ class ROS2ProtocolNode(BaseROS2DeviceNode):
action_client = self._action_clients[action_id]
goal_msg = convert_to_ros_msg(action_client._action_type.Goal(), action_kwargs)
self.lab_logger().info(f"发送动作请求到: {action_id}")
##### self.lab_logger().info(f"发送动作请求到: {action_id}")
action_client.wait_for_server()
# 等待动作完成
@@ -349,7 +349,7 @@ class ROS2ProtocolNode(BaseROS2DeviceNode):
return None
result_future = await handle.get_result_async()
self.lab_logger().info(f"动作完成: {action_name}")
##### self.lab_logger().info(f"动作完成: {action_name}")
return result_future.result