diff --git a/unilabos/registry/device_comms/serial.yaml b/unilabos/registry/device_comms/serial.yaml index 3a0d72a5..86cf7e50 100644 --- a/unilabos/registry/device_comms/serial.yaml +++ b/unilabos/registry/device_comms/serial.yaml @@ -1,7 +1,7 @@ serial: description: Serial communication interface, used when sharing same serial port for multiple devices class: - module: unilabos.ros.nodes.presets:ROS2SerialNode + module: unilabos.ros.nodes.presets.serial_node:ROS2SerialNode type: ros2 schema: properties: {} \ No newline at end of file diff --git a/unilabos/registry/devices/organic_miscellaneous.yaml b/unilabos/registry/devices/organic_miscellaneous.yaml index 3085efac..a3f6f0e3 100644 --- a/unilabos/registry/devices/organic_miscellaneous.yaml +++ b/unilabos/registry/devices/organic_miscellaneous.yaml @@ -1,7 +1,7 @@ separator.homemade: description: Separator device with homemade grbl controller class: - module: unilabos.devices.separator.homemade_grbl_conductivity:Separator_Controller + module: unilabos.devices.separator.homemade_grbl_conductivity:SeparatorController type: python status_types: sensordata: Float64 diff --git a/unilabos/registry/devices/pump_and_valve.yaml b/unilabos/registry/devices/pump_and_valve.yaml index ba35700d..5fdd8606 100644 --- a/unilabos/registry/devices/pump_and_valve.yaml +++ b/unilabos/registry/devices/pump_and_valve.yaml @@ -3,6 +3,10 @@ syringe_pump_with_valve.runze: class: module: unilabos.devices.pump_and_valve.runze_backbone:RunzeSyringePump type: python + hardware_interface: + name: hardware_interface + read: send_command + write: send_command schema: type: object properties: diff --git a/unilabos/ros/nodes/base_device_node.py b/unilabos/ros/nodes/base_device_node.py index 1424b0c6..de2c5e15 100644 --- a/unilabos/ros/nodes/base_device_node.py +++ b/unilabos/ros/nodes/base_device_node.py @@ -430,6 +430,15 @@ class BaseROS2DeviceNode(Node, Generic[T]): self.lab_logger().info(f"同步执行动作 {ACTION}") future = self._executor.submit(ACTION, **action_kwargs) + def _handle_future_exception(fut): + try: + fut.result() + except Exception as e: + error(f"同步任务 {ACTION.__name__} 报错了") + error(traceback.format_exc()) + + future.add_done_callback(_handle_future_exception) + action_type = action_value_mapping["type"] feedback_msg_types = action_type.Feedback.get_fields_and_field_types() result_msg_types = action_type.Result.get_fields_and_field_types() diff --git a/unilabos/ros/nodes/presets/protocol_node.py b/unilabos/ros/nodes/presets/protocol_node.py index 323f6880..c4dfd083 100644 --- a/unilabos/ros/nodes/presets/protocol_node.py +++ b/unilabos/ros/nodes/presets/protocol_node.py @@ -20,7 +20,7 @@ from unilabos.ros.msgs.message_converter import ( convert_from_ros_msg, convert_from_ros_msg_with_mapping, ) -from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode, DeviceNodeResourceTracker +from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode, DeviceNodeResourceTracker, ROS2DeviceNode class ROS2ProtocolNode(BaseROS2DeviceNode): @@ -55,34 +55,39 @@ class ROS2ProtocolNode(BaseROS2DeviceNode): ) # 初始化子设备 - communication_node_id = None + 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 - d = self.initialize_device(device_id, device_config) + 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}") + d = None if d is None: continue if "serial_" in device_id or "io_" in device_id: - communication_node_id = device_id + self.communication_node_id_to_instance[device_id] = d continue # 设置硬件接口代理 - if d and hasattr(d, "_hardware_interface"): + if d: if ( - hasattr(d, d._hardware_interface["name"]) - and hasattr(d, d._hardware_interface["write"]) - and (d._hardware_interface["read"] is None or hasattr(d, d._hardware_interface["read"])) + hasattr(d.driver_instance, d.ros_node_instance._hardware_interface["name"]) + and hasattr(d.driver_instance, d.ros_node_instance._hardware_interface["write"]) + and (d.ros_node_instance._hardware_interface["read"] is None or hasattr(d.driver_instance, d.ros_node_instance._hardware_interface["read"])) ): - name = getattr(d, d._hardware_interface["name"]) - read = d._hardware_interface.get("read", None) - write = d._hardware_interface.get("write", None) + name = getattr(d.driver_instance, d.ros_node_instance._hardware_interface["name"]) + read = d.ros_node_instance._hardware_interface.get("read", None) + write = d.ros_node_instance._hardware_interface.get("write", None) # 如果硬件接口是字符串，通过通信设备提供 - if isinstance(name, str) and communication_node_id in self.sub_devices: - self._setup_hardware_proxy(d, self.sub_devices[communication_node_id], read, write) + if isinstance(name, str) and name in self.sub_devices: + self._setup_hardware_proxy(d, self.sub_devices[name], read, write) def _setup_protocol_names(self, protocol_type): # 处理协议类型 @@ -234,11 +239,11 @@ class ROS2ProtocolNode(BaseROS2DeviceNode): """还没有改过的部分""" - def _setup_hardware_proxy(self, device, communication_device, read_method, write_method): + def _setup_hardware_proxy(self, device: ROS2DeviceNode, communication_device: ROS2DeviceNode, read_method, write_method): """为设备设置硬件接口代理""" - extra_info = [getattr(device, info) for info in communication_device._hardware_interface.get("extra_info", [])] - write_func = getattr(communication_device, communication_device._hardware_interface["write"]) - read_func = getattr(communication_device, communication_device._hardware_interface["read"]) + 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.ros_node_instance, communication_device.ros_node_instance._hardware_interface["write"]) + read_func = getattr(communication_device.ros_node_instance, communication_device.ros_node_instance._hardware_interface["read"]) def _read(): return read_func(*extra_info) @@ -247,9 +252,9 @@ class ROS2ProtocolNode(BaseROS2DeviceNode): return write_func(*extra_info, command) if read_method: - setattr(device, read_method, _read) + setattr(device.driver_instance, read_method, _read) if write_method: - setattr(device, write_method, _write) + setattr(device.driver_instance, write_method, _write) async def _update_resources(self, goal, protocol_kwargs): diff --git a/unilabos/ros/nodes/presets/serial_node.py b/unilabos/ros/nodes/presets/serial_node.py index 6f01a07a..ac9cd59e 100644 --- a/unilabos/ros/nodes/presets/serial_node.py +++ b/unilabos/ros/nodes/presets/serial_node.py @@ -21,6 +21,7 @@ class ROS2SerialNode(BaseROS2DeviceNode): self.hardware_interface = Serial(baudrate=baudrate, port=port) except (OSError, SerialException) as e: # 因为还没调用父类初始化，无法使用日志，直接抛出异常 + # print(f"Failed to connect to serial port {port} at {baudrate} baudrate.") raise RuntimeError(f"Failed to connect to serial port {port} at {baudrate} baudrate.") from e # 初始化BaseROS2DeviceNode，使用自身作为driver_instance