mirror of
https://github.com/dptech-corp/Uni-Lab-OS.git
synced 2025-12-14 13:14:39 +00:00
7c440d10ab
* Fix ResourceTreeSet load error * Raise error when using unsupported type to create ResourceTreeSet * Fix children key error * Fix children key error * Fix workstation resource not tracking * Fix workstation deck & children resource dupe * Fix workstation deck & children resource dupe * Fix multiple resource error * Fix resource tree update * Fix resource tree update * Force confirm uuid * Tip more error log * Refactor Bioyond workstation and experiment workflow (#105) Refactored the Bioyond workstation classes to improve parameter handling and workflow management. Updated experiment.py to use BioyondReactionStation with deck and material mappings, and enhanced workflow step parameter mapping and execution logic. Adjusted JSON experiment configs, improved workflow sequence handling, and added UUID assignment to PLR materials. Removed unused station_config and material cache logic, and added detailed docstrings and debug output for workflow methods. * Fix resource get. Fix resource parent not found. Mapping uuid for all resources. * mount parent uuid * Add logging configuration based on BasicConfig in main function * fix workstation node error * fix workstation node error * Update boot example * temp fix for resource get * temp fix for resource get * provide error info when cant find plr type * pack repo info * fix to plr type error * fix to plr type error * Update regular container method * support no size init * fix comprehensive_station.json * fix comprehensive_station.json * fix type conversion * fix state loading for regular container * Update deploy-docs.yml * Update deploy-docs.yml --------- Co-authored-by: ZiWei <131428629+ZiWei09@users.noreply.github.com>
431 lines
20 KiB
Python
431 lines
20 KiB
Python
import json
|
||
import time
|
||
import traceback
|
||
from pprint import pformat
|
||
from typing import List, Dict, Any, Optional, TYPE_CHECKING
|
||
|
||
import rclpy
|
||
from rosidl_runtime_py import message_to_ordereddict
|
||
from unilabos_msgs.msg import Resource
|
||
from unilabos_msgs.srv import ResourceUpdate
|
||
|
||
from unilabos.messages import * # type: ignore # protocol names
|
||
from rclpy.action import ActionServer, ActionClient
|
||
from rclpy.action.server import ServerGoalHandle
|
||
from rclpy.callback_groups import ReentrantCallbackGroup
|
||
from unilabos_msgs.srv._serial_command import SerialCommand_Request, SerialCommand_Response
|
||
|
||
from unilabos.compile import action_protocol_generators
|
||
from unilabos.resources.graphio import list_to_nested_dict, nested_dict_to_list
|
||
from unilabos.ros.initialize_device import initialize_device_from_dict
|
||
from unilabos.ros.msgs.message_converter import (
|
||
get_action_type,
|
||
convert_to_ros_msg,
|
||
convert_from_ros_msg_with_mapping,
|
||
)
|
||
from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode, DeviceNodeResourceTracker, ROS2DeviceNode
|
||
from unilabos.ros.nodes.resource_tracker import ResourceTreeSet
|
||
from unilabos.utils.type_check import get_result_info_str
|
||
|
||
if TYPE_CHECKING:
|
||
from unilabos.devices.workstation.workstation_base import WorkstationBase
|
||
|
||
|
||
class ROS2WorkstationNodeTempError(Exception):
|
||
pass
|
||
|
||
|
||
class ROS2WorkstationNode(BaseROS2DeviceNode):
|
||
"""
|
||
ROS2WorkstationNode代表管理ROS2环境中设备通信和动作的协议节点。
|
||
它初始化设备节点，处理动作客户端，并基于指定的协议执行工作流。
|
||
它还物理上代表一组协同工作的设备，如带夹持器的机械臂，带传送带的CNC机器等。
|
||
"""
|
||
|
||
driver_instance: "WorkstationBase"
|
||
|
||
def __init__(
|
||
self,
|
||
protocol_type: List[str],
|
||
children: Dict[str, Any],
|
||
*,
|
||
driver_instance: "WorkstationBase",
|
||
device_id: str,
|
||
device_uuid: str,
|
||
status_types: Dict[str, Any],
|
||
action_value_mappings: Dict[str, Any],
|
||
hardware_interface: Dict[str, Any],
|
||
print_publish=True,
|
||
resource_tracker: Optional["DeviceNodeResourceTracker"] = None,
|
||
):
|
||
self._setup_protocol_names(protocol_type)
|
||
|
||
# 初始化非BaseROSNode的属性
|
||
self.children = children
|
||
# 初始化基类，让基类处理常规动作
|
||
super().__init__(
|
||
driver_instance=driver_instance,
|
||
device_id=device_id,
|
||
device_uuid=device_uuid,
|
||
status_types=status_types,
|
||
action_value_mappings={**action_value_mappings, **self.protocol_action_mappings},
|
||
hardware_interface=hardware_interface,
|
||
print_publish=print_publish,
|
||
resource_tracker=resource_tracker,
|
||
)
|
||
|
||
self._busy = False
|
||
self.sub_devices = {}
|
||
self._action_clients = {}
|
||
|
||
# 初始化子设备
|
||
self.communication_node_id_to_instance = {}
|
||
|
||
for device_id, device_config in self.children.items():
|
||
if device_config.get("type", "device") != "device":
|
||
self.lab_logger().debug(
|
||
f"[Protocol Node] Skipping type {device_config['type']} {device_id} already existed, skipping."
|
||
)
|
||
continue
|
||
try:
|
||
d = self.initialize_device(device_id, device_config)
|
||
except Exception as ex:
|
||
self.lab_logger().error(
|
||
f"[Protocol Node] Failed to initialize device {device_id}: {ex}\n{traceback.format_exc()}"
|
||
)
|
||
d = None
|
||
if d is None:
|
||
continue
|
||
|
||
if "serial_" in device_id or "io_" in device_id:
|
||
self.communication_node_id_to_instance[device_id] = d
|
||
continue
|
||
|
||
for device_id, device_config in self.children.items():
|
||
if device_config.get("type", "device") != "device":
|
||
continue
|
||
# 设置硬件接口代理
|
||
if device_id not in self.sub_devices:
|
||
self.lab_logger().error(f"[Protocol Node] {device_id} 还没有正确初始化，跳过...")
|
||
continue
|
||
d = self.sub_devices[device_id]
|
||
if d:
|
||
hardware_interface = d.ros_node_instance._hardware_interface
|
||
if (
|
||
hasattr(d.driver_instance, hardware_interface["name"])
|
||
and hasattr(d.driver_instance, hardware_interface["write"])
|
||
and (hardware_interface["read"] is None or hasattr(d.driver_instance, hardware_interface["read"]))
|
||
):
|
||
|
||
name = getattr(d.driver_instance, hardware_interface["name"])
|
||
read = hardware_interface.get("read", None)
|
||
write = hardware_interface.get("write", None)
|
||
|
||
# 如果硬件接口是字符串，通过通信设备提供
|
||
if isinstance(name, str) and name in self.sub_devices:
|
||
communicate_device = self.sub_devices[name]
|
||
communicate_hardware_info = communicate_device.ros_node_instance._hardware_interface
|
||
self._setup_hardware_proxy(d, self.sub_devices[name], read, write)
|
||
self.lab_logger().info(
|
||
f"\n通信代理：为子设备{device_id}\n "
|
||
f"添加了{read}方法(来源：{name} {communicate_hardware_info['write']}) \n "
|
||
f"添加了{write}方法(来源：{name} {communicate_hardware_info['read']})"
|
||
)
|
||
|
||
self.lab_logger().info(f"ROS2WorkstationNode {device_id} initialized with protocols: {self.protocol_names}")
|
||
|
||
def _setup_protocol_names(self, protocol_type):
|
||
# 处理协议类型
|
||
if isinstance(protocol_type, str):
|
||
if "," not in protocol_type:
|
||
self.protocol_names = [protocol_type]
|
||
else:
|
||
self.protocol_names = [protocol.strip() for protocol in protocol_type.split(",")]
|
||
else:
|
||
self.protocol_names = protocol_type
|
||
# 准备协议相关的动作值映射
|
||
self.protocol_action_mappings = {}
|
||
for protocol_name in self.protocol_names:
|
||
protocol_type = globals()[protocol_name]
|
||
self.protocol_action_mappings[protocol_name] = get_action_type(protocol_type)
|
||
|
||
def initialize_device(self, device_id, device_config):
|
||
"""初始化设备并创建相应的动作客户端"""
|
||
# device_id_abs = f"{self.device_id}/{device_id}"
|
||
device_id_abs = f"{device_id}"
|
||
self.lab_logger().info(f"初始化子设备: {device_id_abs}")
|
||
d = self.sub_devices[device_id] = initialize_device_from_dict(device_id_abs, device_config)
|
||
|
||
# 为子设备的每个动作创建动作客户端
|
||
if d is not None and hasattr(d, "ros_node_instance"):
|
||
node = d.ros_node_instance
|
||
node.resource_tracker = self.resource_tracker # 站内应当共享资源跟踪器
|
||
for action_name, action_mapping in node._action_value_mappings.items():
|
||
if action_name.startswith("auto-") or str(action_mapping.get("type", "")).startswith(
|
||
"UniLabJsonCommand"
|
||
):
|
||
continue
|
||
action_id = f"/devices/{device_id_abs}/{action_name}"
|
||
if action_id not in self._action_clients:
|
||
try:
|
||
self._action_clients[action_id] = ActionClient(
|
||
self, action_mapping["type"], action_id, callback_group=self.callback_group
|
||
)
|
||
except Exception as ex:
|
||
self.lab_logger().error(f"创建动作客户端失败: {action_id}, 错误: {ex}")
|
||
continue
|
||
self.lab_logger().trace(f"为子设备 {device_id} 创建动作客户端: {action_name}")
|
||
return d
|
||
|
||
def create_ros_action_server(self, action_name, action_value_mapping):
|
||
"""创建ROS动作服务器"""
|
||
if action_name not in self.protocol_names:
|
||
# 非protocol方法调用父类注册
|
||
return super().create_ros_action_server(action_name, action_value_mapping)
|
||
# 和Base创建的路径是一致的
|
||
protocol_name = action_name
|
||
action_type = action_value_mapping["type"]
|
||
str_action_type = str(action_type)[8:-2]
|
||
protocol_type = globals()[protocol_name]
|
||
protocol_steps_generator = action_protocol_generators[protocol_type]
|
||
|
||
self._action_servers[action_name] = ActionServer(
|
||
self,
|
||
action_type,
|
||
action_name,
|
||
execute_callback=self._create_protocol_execute_callback(action_name, protocol_steps_generator),
|
||
callback_group=ReentrantCallbackGroup(),
|
||
)
|
||
self.lab_logger().trace(f"发布动作: {action_name}, 类型: {str_action_type}")
|
||
return
|
||
|
||
def _create_protocol_execute_callback(self, protocol_name, protocol_steps_generator):
|
||
async def execute_protocol(goal_handle: ServerGoalHandle):
|
||
"""执行完整的工作流"""
|
||
# 初始化结果信息变量
|
||
execution_error = ""
|
||
execution_success = False
|
||
protocol_return_value = None
|
||
self.lab_logger().info(f"Executing {protocol_name} action...")
|
||
action_value_mapping = self._action_value_mappings[protocol_name]
|
||
step_results = []
|
||
try:
|
||
self.lab_logger().warning("+" * 30)
|
||
self.lab_logger().info(protocol_steps_generator)
|
||
# 从目标消息中提取参数, 并调用Protocol生成器(根据设备连接图)生成action步骤
|
||
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')}")
|
||
|
||
# # 🔧 完全禁用Host查询，直接使用转换后的数据
|
||
# print(f"🔧 跳过Host查询，直接使用转换后的数据")
|
||
# 向Host查询物料当前状态
|
||
for k, v in goal.get_fields_and_field_types().items():
|
||
if v in ["unilabos_msgs/Resource", "sequence<unilabos_msgs/Resource>"]:
|
||
self.lab_logger().info(f"{protocol_name} 查询资源状态: Key: {k} Type: {v}")
|
||
|
||
try:
|
||
# 统一处理单个或多个资源
|
||
resource_id = (
|
||
protocol_kwargs[k]["id"] if v == "unilabos_msgs/Resource" else protocol_kwargs[k][0]["id"]
|
||
)
|
||
r = SerialCommand_Request()
|
||
r.command = json.dumps({"id": resource_id, "with_children": True})
|
||
# 发送请求并等待响应
|
||
response: SerialCommand_Response = await self._resource_clients[
|
||
"resource_get"
|
||
].call_async(
|
||
r
|
||
) # type: ignore
|
||
raw_data = json.loads(response.response)
|
||
tree_set = ResourceTreeSet.from_raw_list(raw_data)
|
||
target = tree_set.dump()
|
||
protocol_kwargs[k] = target[0][0] if v == "unilabos_msgs/Resource" else target
|
||
except Exception as ex:
|
||
self.lab_logger().error(f"查询资源失败: {k}, 错误: {ex}\n{traceback.format_exc()}")
|
||
raise
|
||
|
||
self.lab_logger().info(f"🔍 最终的 vessel: {protocol_kwargs.get('vessel', 'NOT_FOUND')}")
|
||
|
||
from unilabos.resources.graphio import physical_setup_graph
|
||
|
||
self.lab_logger().info(f"Working on physical setup: {physical_setup_graph}")
|
||
protocol_steps = protocol_steps_generator(G=physical_setup_graph, **protocol_kwargs)
|
||
logs = []
|
||
for step in protocol_steps:
|
||
if isinstance(step, dict) and "log_message" in step.get("action_kwargs", {}):
|
||
logs.append(step)
|
||
elif isinstance(step, list):
|
||
logs.append(step)
|
||
self.lab_logger().info(
|
||
f"Goal received: {protocol_kwargs}, running steps: "
|
||
f"{json.dumps(logs, indent=4, ensure_ascii=False)}"
|
||
)
|
||
|
||
time_start = time.time()
|
||
time_overall = 100
|
||
self._busy = True
|
||
|
||
# 逐步执行工作流
|
||
for i, action in enumerate(protocol_steps):
|
||
# self.get_logger().info(f"Running step {i + 1}: {action}")
|
||
if isinstance(action, dict):
|
||
# 如果是单个动作，直接执行
|
||
if action["action_name"] == "wait":
|
||
time.sleep(action["action_kwargs"]["time"])
|
||
step_results.append({"step": i + 1, "action": "wait", "result": "completed"})
|
||
else:
|
||
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 ROS2WorkstationNodeTempError as ex:
|
||
step_results.append(
|
||
{"step": i + 1, "action": action["action_name"], "result": ex.args[0]}
|
||
)
|
||
elif isinstance(action, list):
|
||
# 如果是并行动作，同时执行
|
||
actions = action
|
||
futures = [
|
||
rclpy.get_global_executor().create_task(self.execute_single_action(**a)) for a in actions
|
||
]
|
||
results = [await f for f in futures]
|
||
step_results.append(
|
||
{
|
||
"step": i + 1,
|
||
"parallel_actions": [a["action_name"] for a in actions],
|
||
"results": results,
|
||
}
|
||
)
|
||
|
||
# 向Host更新物料当前状态
|
||
for k, v in goal.get_fields_and_field_types().items():
|
||
if v in ["unilabos_msgs/Resource", "sequence<unilabos_msgs/Resource>"]:
|
||
r = ResourceUpdate.Request()
|
||
r.resources = [
|
||
convert_to_ros_msg(Resource, rs) for rs in nested_dict_to_list(protocol_kwargs[k])
|
||
]
|
||
response = await self._resource_clients["resource_update"].call_async(r)
|
||
|
||
# 设置成功状态和返回值
|
||
execution_success = True
|
||
protocol_return_value = {
|
||
"protocol_name": protocol_name,
|
||
"steps_executed": len(protocol_steps),
|
||
"step_results": step_results,
|
||
"total_time": time.time() - time_start,
|
||
}
|
||
|
||
goal_handle.succeed()
|
||
|
||
except Exception as e:
|
||
# 捕获并记录错误信息
|
||
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
|
||
self.lab_logger().error(f"协议 {protocol_name} 执行出错: {str(e)} \n{traceback.format_exc()}")
|
||
|
||
# 设置动作失败
|
||
goal_handle.abort()
|
||
|
||
finally:
|
||
self._busy = False
|
||
|
||
# 创建结果消息
|
||
result = action_value_mapping["type"].Result()
|
||
result.success = execution_success
|
||
|
||
# 获取结果消息类型信息，检查是否有return_info字段
|
||
result_msg_types = action_value_mapping["type"].Result.get_fields_and_field_types()
|
||
|
||
# 设置return_info字段（如果存在）
|
||
for attr_name in result_msg_types.keys():
|
||
if attr_name in ["success", "reached_goal"]:
|
||
setattr(result, attr_name, execution_success)
|
||
elif attr_name == "return_info":
|
||
setattr(
|
||
result,
|
||
attr_name,
|
||
get_result_info_str(execution_error, execution_success, protocol_return_value),
|
||
)
|
||
|
||
self.lab_logger().info(f"协议 {protocol_name} 完成并返回结果")
|
||
return result
|
||
|
||
return execute_protocol
|
||
|
||
async def execute_single_action(self, device_id, action_name, action_kwargs):
|
||
"""执行单个动作"""
|
||
# 构建动作ID
|
||
if action_name == "log_message":
|
||
self.lab_logger().info(f"[Protocol Log] {action_kwargs}")
|
||
raise ROS2WorkstationNodeTempError(f"[Protocol Log] {action_kwargs}")
|
||
if device_id in ["", None, "self"]:
|
||
action_id = f"/devices/{self.device_id}/{action_name}"
|
||
else:
|
||
action_id = f"/devices/{device_id}/{action_name}" # 执行时取消了主节点信息 /{self.device_id}
|
||
|
||
# 检查动作客户端是否存在
|
||
if action_id not in self._action_clients:
|
||
self.lab_logger().error(f"找不到动作客户端: {action_id}")
|
||
return None
|
||
|
||
# 发送动作请求
|
||
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}")
|
||
action_client.wait_for_server()
|
||
|
||
# 等待动作完成
|
||
request_future = action_client.send_goal_async(goal_msg)
|
||
handle = await request_future
|
||
|
||
if not handle.accepted:
|
||
self.lab_logger().error(f"动作请求被拒绝: {action_name}")
|
||
return None
|
||
|
||
result_future = await handle.get_result_async()
|
||
##### self.lab_logger().info(f"动作完成: {action_name}")
|
||
|
||
return result_future.result
|
||
|
||
"""还没有改过的部分"""
|
||
|
||
def _setup_hardware_proxy(
|
||
self, device: ROS2DeviceNode, communication_device: ROS2DeviceNode, read_method, write_method
|
||
):
|
||
"""为设备设置硬件接口代理"""
|
||
# extra_info = [getattr(device.driver_instance, info) for info in communication_device.ros_node_instance._hardware_interface.get("extra_info", [])]
|
||
write_func = getattr(
|
||
communication_device.driver_instance, communication_device.ros_node_instance._hardware_interface["write"]
|
||
)
|
||
read_func = getattr(
|
||
communication_device.driver_instance, communication_device.ros_node_instance._hardware_interface["read"]
|
||
)
|
||
|
||
def _read(*args, **kwargs):
|
||
return read_func(*args, **kwargs)
|
||
|
||
def _write(*args, **kwargs):
|
||
return write_func(*args, **kwargs)
|
||
|
||
if read_method:
|
||
# bound_read = MethodType(_read, device.driver_instance)
|
||
setattr(device.driver_instance, read_method, _read)
|
||
|
||
if write_method:
|
||
# bound_write = MethodType(_write, device.driver_instance)
|
||
setattr(device.driver_instance, write_method, _write)
|