mirror of
https://github.com/dptech-corp/Uni-Lab-OS.git
synced 2026-02-07 15:35:10 +00:00
stir和adjustph的中的bug修不好
This commit is contained in:
KCFeng425
@@ -509,68 +509,68 @@ def convert_from_ros_msg_with_mapping(ros_msg: Any, value_mapping: Dict[str, str
|
||||
"""
|
||||
data: Dict[str, Any] = {}
|
||||
|
||||
# 🔧 添加调试信息
|
||||
print(f"🔍 convert_from_ros_msg_with_mapping 开始")
|
||||
print(f"🔍 ros_msg 类型: {type(ros_msg)}")
|
||||
print(f"🔍 ros_msg 内容: {ros_msg}")
|
||||
print(f"🔍 value_mapping: {value_mapping}")
|
||||
print("-" * 60)
|
||||
# # 🔧 添加调试信息
|
||||
# print(f"🔍 convert_from_ros_msg_with_mapping 开始")
|
||||
# print(f"🔍 ros_msg 类型: {type(ros_msg)}")
|
||||
# print(f"🔍 ros_msg 内容: {ros_msg}")
|
||||
# print(f"🔍 value_mapping: {value_mapping}")
|
||||
# print("-" * 60)
|
||||
|
||||
for msg_name, attr_name in value_mapping.items():
|
||||
print(f"🔍 处理映射: {msg_name} -> {attr_name}")
|
||||
# print(f"🔍 处理映射: {msg_name} -> {attr_name}")
|
||||
|
||||
msg_path = msg_name.split(".")
|
||||
current = ros_msg
|
||||
|
||||
print(f"🔍 msg_path: {msg_path}")
|
||||
print(f"🔍 current 初始值: {current} (类型: {type(current)})")
|
||||
# print(f"🔍 msg_path: {msg_path}")
|
||||
# print(f"🔍 current 初始值: {current} (类型: {type(current)})")
|
||||
|
||||
try:
|
||||
if not attr_name.endswith("[]"):
|
||||
# 处理单值映射
|
||||
print(f"🔍 处理单值映射")
|
||||
# print(f"🔍 处理单值映射")
|
||||
for i, name in enumerate(msg_path):
|
||||
print(f"🔍 步骤 {i}: 获取属性 '{name}' 从 {type(current)}")
|
||||
# print(f"🔍 步骤 {i}: 获取属性 '{name}' 从 {type(current)}")
|
||||
if hasattr(current, name):
|
||||
current = getattr(current, name)
|
||||
print(f"🔍 获取到: {current} (类型: {type(current)})")
|
||||
# print(f"🔍 获取到: {current} (类型: {type(current)})")
|
||||
else:
|
||||
print(f"❌ 属性 '{name}' 不存在于 {type(current)}")
|
||||
# print(f"❌ 属性 '{name}' 不存在于 {type(current)}")
|
||||
break
|
||||
|
||||
converted_value = convert_from_ros_msg(current)
|
||||
print(f"🔍 转换后的值: {converted_value} (类型: {type(converted_value)})")
|
||||
# print(f"🔍 转换后的值: {converted_value} (类型: {type(converted_value)})")
|
||||
data[attr_name] = converted_value
|
||||
print(f"✅ 设置 data['{attr_name}'] = {converted_value}")
|
||||
# print(f"✅ 设置 data['{attr_name}'] = {converted_value}")
|
||||
else:
|
||||
# 处理列表值映射
|
||||
print(f"🔍 处理列表值映射")
|
||||
# print(f"🔍 处理列表值映射")
|
||||
for i, name in enumerate(msg_path):
|
||||
print(f"🔍 列表步骤 {i}: 处理 '{name}' 从 {type(current)}")
|
||||
# print(f"🔍 列表步骤 {i}: 处理 '{name}' 从 {type(current)}")
|
||||
if name.endswith("[]"):
|
||||
base_name = name[:-2]
|
||||
print(f"🔍 数组字段 base_name: '{base_name}'")
|
||||
# print(f"🔍 数组字段 base_name: '{base_name}'")
|
||||
if hasattr(current, base_name):
|
||||
current = list(getattr(current, base_name))
|
||||
print(f"🔍 获取数组: {current} (长度: {len(current)})")
|
||||
# print(f"🔍 获取数组: {current} (长度: {len(current)})")
|
||||
else:
|
||||
print(f"❌ 数组字段 '{base_name}' 不存在")
|
||||
# print(f"❌ 数组字段 '{base_name}' 不存在")
|
||||
current = []
|
||||
break
|
||||
else:
|
||||
if isinstance(current, list):
|
||||
print(f"🔍 从列表中获取属性 '{name}'")
|
||||
# print(f"🔍 从列表中获取属性 '{name}'")
|
||||
next_level = []
|
||||
for item in current:
|
||||
if hasattr(item, name):
|
||||
next_level.append(getattr(item, name))
|
||||
current = next_level
|
||||
print(f"🔍 列表处理结果: {current} (长度: {len(current)})")
|
||||
# print(f"🔍 列表处理结果: {current} (长度: {len(current)})")
|
||||
elif hasattr(current, name):
|
||||
current = getattr(current, name)
|
||||
print(f"🔍 获取到属性: {current} (类型: {type(current)})")
|
||||
# print(f"🔍 获取到属性: {current} (类型: {type(current)})")
|
||||
else:
|
||||
print(f"❌ 属性 '{name}' 不存在")
|
||||
# print(f"❌ 属性 '{name}' 不存在")
|
||||
current = []
|
||||
break
|
||||
|
||||
@@ -578,20 +578,20 @@ def convert_from_ros_msg_with_mapping(ros_msg: Any, value_mapping: Dict[str, str
|
||||
if current:
|
||||
converted_list = [convert_from_ros_msg(item) for item in current]
|
||||
data[attr_key] = converted_list
|
||||
print(f"✅ 设置 data['{attr_key}'] = {converted_list}")
|
||||
# print(f"✅ 设置 data['{attr_key}'] = {converted_list}")
|
||||
else:
|
||||
print(f"⚠️ 列表为空，跳过 '{attr_key}'")
|
||||
except (AttributeError, TypeError) as e:
|
||||
print(f"❌ 映射转换错误 {msg_name} -> {attr_name}: {e}")
|
||||
# print(f"❌ 映射转换错误 {msg_name} -> {attr_name}: {e}")
|
||||
logger.debug(f"Mapping conversion error for {msg_name} -> {attr_name}")
|
||||
continue
|
||||
|
||||
print(f"🔍 当前 data 状态: {data}")
|
||||
print("-" * 40)
|
||||
# print(f"🔍 当前 data 状态: {data}")
|
||||
# print("-" * 40)
|
||||
|
||||
print(f"🔍 convert_from_ros_msg_with_mapping 结束")
|
||||
print(f"🔍 最终 data: {data}")
|
||||
print("=" * 60)
|
||||
#print(f"🔍 convert_from_ros_msg_with_mapping 结束")
|
||||
#print(f"🔍 最终 data: {data}")
|
||||
#print("=" * 60)
|
||||
return data
|
||||
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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,
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
Reference in New Issue
Block a user