From 38ab7d3e783094db2a354c7be665f49dd1d83403 Mon Sep 17 00:00:00 2001 From: Xuwznln <18435084+Xuwznln@users.noreply.github.com> Date: Fri, 31 Oct 2025 21:43:25 +0800 Subject: [PATCH 01/13] fix run async execution error --- unilabos/ros/nodes/base_device_node.py | 39 ++++++++------------------ unilabos/utils/async_util.py | 22 --------------- 2 files changed, 12 insertions(+), 49 deletions(-) delete mode 100644 unilabos/utils/async_util.py diff --git a/unilabos/ros/nodes/base_device_node.py b/unilabos/ros/nodes/base_device_node.py index f1063123..edf41fbd 100644 --- a/unilabos/ros/nodes/base_device_node.py +++ b/unilabos/ros/nodes/base_device_node.py @@ -53,7 +53,7 @@ from unilabos.ros.nodes.resource_tracker import ( ) from unilabos.ros.x.rclpyx import get_event_loop from unilabos.ros.utils.driver_creator import WorkstationNodeCreator, PyLabRobotCreator, DeviceClassCreator -from unilabos.utils.async_util import run_async_func +from rclpy.task import Task from unilabos.utils.import_manager import default_manager from unilabos.utils.log import info, debug, warning, error, critical, logger, trace from unilabos.utils.type_check import get_type_class, TypeEncoder, get_result_info_str @@ -1385,18 +1385,19 @@ class ROS2DeviceNode: 它不继承设备类,而是通过代理模式访问设备类的属性和方法。 """ - # 类变量,用于循环管理 - _loop = None - _loop_running = False - _loop_thread = None - @classmethod - def get_loop(cls): - return cls._loop + def run_async_func(cls, func, trace_error=True, **kwargs) -> Task: + def _handle_future_exception(fut): + try: + fut.result() + except Exception as e: + error(f"异步任务 {func.__name__} 报错了") + error(traceback.format_exc()) - @classmethod - def run_async_func(cls, func, trace_error=True, **kwargs): - return run_async_func(func, loop=cls._loop, trace_error=trace_error, **kwargs) + future = rclpy.get_global_executor().create_task(func(**kwargs)) + if trace_error: + future.add_done_callback(_handle_future_exception) + return future @property def driver_instance(self): @@ -1436,11 +1437,6 @@ class ROS2DeviceNode: print_publish: 是否打印发布信息 driver_is_ros: """ - # 在初始化时检查循环状态 - if ROS2DeviceNode._loop_running and ROS2DeviceNode._loop_thread is not None: - pass - elif ROS2DeviceNode._loop_thread is None: - self._start_loop() # 保存设备类是否支持异步上下文 self._has_async_context = hasattr(driver_class, "__aenter__") and hasattr(driver_class, "__aexit__") @@ -1529,17 +1525,6 @@ class ROS2DeviceNode: except Exception as e: self._ros_node.lab_logger().error(f"设备后初始化失败: {e}") - def _start_loop(self): - def run_event_loop(): - loop = asyncio.new_event_loop() - ROS2DeviceNode._loop = loop - asyncio.set_event_loop(loop) - loop.run_forever() - - ROS2DeviceNode._loop_thread = threading.Thread(target=run_event_loop, daemon=True, name="ROS2DeviceNodeLoop") - ROS2DeviceNode._loop_thread.start() - logger.info(f"循环线程已启动") - class DeviceInfoType(TypedDict): id: str diff --git a/unilabos/utils/async_util.py b/unilabos/utils/async_util.py deleted file mode 100644 index 0f50a730..00000000 --- a/unilabos/utils/async_util.py +++ /dev/null @@ -1,22 +0,0 @@ -import asyncio -import traceback -from asyncio import get_event_loop - -from unilabos.utils.log import error - - -def run_async_func(func, *, loop=None, trace_error=True, **kwargs): - if loop is None: - loop = get_event_loop() - - def _handle_future_exception(fut): - try: - fut.result() - except Exception as e: - error(f"异步任务 {func.__name__} 报错了") - error(traceback.format_exc()) - - future = asyncio.run_coroutine_threadsafe(func(**kwargs), loop) - if trace_error: - future.add_done_callback(_handle_future_exception) - return future From 5331d7bfba4491e56df7a7e26d2593e373575495 Mon Sep 17 00:00:00 2001 From: Xuwznln <18435084+Xuwznln@users.noreply.github.com> Date: Mon, 3 Nov 2025 15:42:12 +0800 Subject: [PATCH 02/13] support sleep and create_task in node --- .../devices/liquid_handling/prcxi/prcxi.py | 395 +++++++++++------- unilabos/ros/nodes/base_device_node.py | 19 +- unilabos/ros/nodes/presets/host_node.py | 3 +- 3 files changed, 257 insertions(+), 160 deletions(-) diff --git a/unilabos/devices/liquid_handling/prcxi/prcxi.py b/unilabos/devices/liquid_handling/prcxi/prcxi.py index d9c04331..a8677f49 100644 --- a/unilabos/devices/liquid_handling/prcxi/prcxi.py +++ b/unilabos/devices/liquid_handling/prcxi/prcxi.py @@ -30,6 +30,7 @@ from pylabrobot.liquid_handling.standard import ( from pylabrobot.resources import Tip, Deck, Plate, Well, TipRack, Resource, Container, Coordinate, TipSpot, Trash from unilabos.devices.liquid_handling.liquid_handler_abstract import LiquidHandlerAbstract +from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode class PRCXIError(RuntimeError): @@ -162,6 +163,10 @@ class PRCXI9300Handler(LiquidHandlerAbstract): ) super().__init__(backend=self._unilabos_backend, deck=deck, simulator=simulator, channel_num=channel_num) + def post_init(self, ros_node: BaseROS2DeviceNode): + super().post_init(ros_node) + self._unilabos_backend.post_init(ros_node) + def set_liquid(self, wells: list[Well], liquid_names: list[str], volumes: list[float]): return super().set_liquid(wells, liquid_names, volumes) @@ -424,6 +429,7 @@ class PRCXI9300Backend(LiquidHandlerBackend): _num_channels = 8 # 默认通道数为 8 _is_reset_ok = False + _ros_node: BaseROS2DeviceNode @property def is_reset_ok(self) -> bool: @@ -456,6 +462,9 @@ class PRCXI9300Backend(LiquidHandlerBackend): self._execute_setup = setup self.debug = debug + def post_init(self, ros_node: BaseROS2DeviceNode): + self._ros_node = ros_node + def create_protocol(self, protocol_name): self.protocol_name = protocol_name self.steps_todo_list = [] @@ -500,7 +509,7 @@ class PRCXI9300Backend(LiquidHandlerBackend): self.api_client.call("IAutomation", "Reset") while not self.is_reset_ok: print("Waiting for PRCXI9300 to reset...") - await asyncio.sleep(1) + await self._ros_node.sleep(1) print("PRCXI9300 reset successfully.") except ConnectionRefusedError as e: raise RuntimeError( @@ -533,7 +542,9 @@ class PRCXI9300Backend(LiquidHandlerBackend): tipspot_index = tipspot.parent.children.index(tipspot) tip_columns.append(tipspot_index // 8) if len(set(tip_columns)) != 1: - raise ValueError("All pickups must be from the same tip column. Found different columns: " + str(tip_columns)) + raise ValueError( + "All pickups must be from the same tip column. Found different columns: " + str(tip_columns) + ) PlateNo = plate_indexes[0] + 1 hole_col = tip_columns[0] + 1 hole_row = 1 @@ -1109,12 +1120,15 @@ class PRCXI9300Api: "LiquidDispensingMethod": liquid_method, } + class DefaultLayout: def __init__(self, product_name: str = "PRCXI9300"): self.labresource = {} if product_name not in ["PRCXI9300", "PRCXI9320"]: - raise ValueError(f"Unsupported product_name: {product_name}. Only 'PRCXI9300' and 'PRCXI9320' are supported.") + raise ValueError( + f"Unsupported product_name: {product_name}. Only 'PRCXI9300' and 'PRCXI9320' are supported." + ) if product_name == "PRCXI9300": self.rows = 2 @@ -1129,25 +1143,93 @@ class DefaultLayout: self.layout = [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16] self.trash_slot = 16 self.waste_liquid_slot = 12 - self.default_layout = {"MatrixId":f"{time.time()}","MatrixName":f"{time.time()}","MatrixCount":16,"WorkTablets": - [{"Number": 1, "Code": "T1", "Material": {"uuid": "57b1e4711e9e4a32b529f3132fc5931f", "materialEnum": 0}}, - {"Number": 2, "Code": "T2", "Material": {"uuid": "57b1e4711e9e4a32b529f3132fc5931f", "materialEnum": 0}}, - {"Number": 3, "Code": "T3", "Material": {"uuid": "57b1e4711e9e4a32b529f3132fc5931f", "materialEnum": 0}}, - {"Number": 4, "Code": "T4", "Material": {"uuid": "57b1e4711e9e4a32b529f3132fc5931f", "materialEnum": 0}}, - {"Number": 5, "Code": "T5", "Material": {"uuid": "57b1e4711e9e4a32b529f3132fc5931f", "materialEnum": 0}}, - {"Number": 6, "Code": "T6", "Material": {"uuid": "57b1e4711e9e4a32b529f3132fc5931f", "materialEnum": 0}}, - {"Number": 7, "Code": "T7", "Material": {"uuid": "57b1e4711e9e4a32b529f3132fc5931f", "materialEnum": 0}}, - {"Number": 8, "Code": "T8", "Material": {"uuid": "57b1e4711e9e4a32b529f3132fc5931f", "materialEnum": 0}}, - {"Number": 9, "Code": "T9", "Material": {"uuid": "57b1e4711e9e4a32b529f3132fc5931f", "materialEnum": 0}}, - {"Number": 10, "Code": "T10", "Material": {"uuid": "57b1e4711e9e4a32b529f3132fc5931f", "materialEnum": 0}}, - {"Number": 11, "Code": "T11", "Material": {"uuid": "57b1e4711e9e4a32b529f3132fc5931f", "materialEnum": 0}}, - {"Number": 12, "Code": "T12", "Material": {"uuid": "730067cf07ae43849ddf4034299030e9", "materialEnum": 0}}, # 这个设置成废液槽,用储液槽表示 - {"Number": 13, "Code": "T13", "Material": {"uuid": "57b1e4711e9e4a32b529f3132fc5931f", "materialEnum": 0}}, - {"Number": 14, "Code": "T14", "Material": {"uuid": "57b1e4711e9e4a32b529f3132fc5931f", "materialEnum": 0}}, - {"Number": 15, "Code": "T15", "Material": {"uuid": "57b1e4711e9e4a32b529f3132fc5931f", "materialEnum": 0}}, - {"Number": 16, "Code": "T16", "Material": {"uuid": "730067cf07ae43849ddf4034299030e9", "materialEnum": 0}} # 这个设置成垃圾桶,用储液槽表示 -] -} + self.default_layout = { + "MatrixId": f"{time.time()}", + "MatrixName": f"{time.time()}", + "MatrixCount": 16, + "WorkTablets": [ + { + "Number": 1, + "Code": "T1", + "Material": {"uuid": "57b1e4711e9e4a32b529f3132fc5931f", "materialEnum": 0}, + }, + { + "Number": 2, + "Code": "T2", + "Material": {"uuid": "57b1e4711e9e4a32b529f3132fc5931f", "materialEnum": 0}, + }, + { + "Number": 3, + "Code": "T3", + "Material": {"uuid": "57b1e4711e9e4a32b529f3132fc5931f", "materialEnum": 0}, + }, + { + "Number": 4, + "Code": "T4", + "Material": {"uuid": "57b1e4711e9e4a32b529f3132fc5931f", "materialEnum": 0}, + }, + { + "Number": 5, + "Code": "T5", + "Material": {"uuid": "57b1e4711e9e4a32b529f3132fc5931f", "materialEnum": 0}, + }, + { + "Number": 6, + "Code": "T6", + "Material": {"uuid": "57b1e4711e9e4a32b529f3132fc5931f", "materialEnum": 0}, + }, + { + "Number": 7, + "Code": "T7", + "Material": {"uuid": "57b1e4711e9e4a32b529f3132fc5931f", "materialEnum": 0}, + }, + { + "Number": 8, + "Code": "T8", + "Material": {"uuid": "57b1e4711e9e4a32b529f3132fc5931f", "materialEnum": 0}, + }, + { + "Number": 9, + "Code": "T9", + "Material": {"uuid": "57b1e4711e9e4a32b529f3132fc5931f", "materialEnum": 0}, + }, + { + "Number": 10, + "Code": "T10", + "Material": {"uuid": "57b1e4711e9e4a32b529f3132fc5931f", "materialEnum": 0}, + }, + { + "Number": 11, + "Code": "T11", + "Material": {"uuid": "57b1e4711e9e4a32b529f3132fc5931f", "materialEnum": 0}, + }, + { + "Number": 12, + "Code": "T12", + "Material": {"uuid": "730067cf07ae43849ddf4034299030e9", "materialEnum": 0}, + }, # 这个设置成废液槽,用储液槽表示 + { + "Number": 13, + "Code": "T13", + "Material": {"uuid": "57b1e4711e9e4a32b529f3132fc5931f", "materialEnum": 0}, + }, + { + "Number": 14, + "Code": "T14", + "Material": {"uuid": "57b1e4711e9e4a32b529f3132fc5931f", "materialEnum": 0}, + }, + { + "Number": 15, + "Code": "T15", + "Material": {"uuid": "57b1e4711e9e4a32b529f3132fc5931f", "materialEnum": 0}, + }, + { + "Number": 16, + "Code": "T16", + "Material": {"uuid": "730067cf07ae43849ddf4034299030e9", "materialEnum": 0}, + }, # 这个设置成垃圾桶,用储液槽表示 + ], + } def get_layout(self) -> Dict[str, Any]: return { @@ -1155,7 +1237,7 @@ class DefaultLayout: "columns": self.columns, "layout": self.layout, "trash_slot": self.trash_slot, - "waste_liquid_slot": self.waste_liquid_slot + "waste_liquid_slot": self.waste_liquid_slot, } def get_trash_slot(self) -> int: @@ -1178,17 +1260,19 @@ class DefaultLayout: reserved_positions = {12, 16} available_positions = [i for i in range(1, 17) if i not in reserved_positions] - # 计算总需求 + # 计算总需求 total_needed = sum(count for _, _, count in needs) if total_needed > len(available_positions): - raise ValueError(f"需要 {total_needed} 个位置,但只有 {len(available_positions)} 个可用位置(排除位置12和16)") + raise ValueError( + f"需要 {total_needed} 个位置,但只有 {len(available_positions)} 个可用位置(排除位置12和16)" + ) # 依次分配位置 current_pos = 0 for reagent_name, material_name, count in needs: - material_uuid = self.labresource[material_name]['uuid'] - material_enum = self.labresource[material_name]['materialEnum'] + material_uuid = self.labresource[material_name]["uuid"] + material_enum = self.labresource[material_name]["materialEnum"] for _ in range(count): if current_pos >= len(available_positions): @@ -1196,17 +1280,18 @@ class DefaultLayout: position = available_positions[current_pos] # 找到对应的tablet并更新 - for tablet in self.default_layout['WorkTablets']: - if tablet['Number'] == position: - tablet['Material']['uuid'] = material_uuid - tablet['Material']['materialEnum'] = material_enum - layout_list.append(dict(reagent_name=reagent_name, material_name=material_name, positions=position)) + for tablet in self.default_layout["WorkTablets"]: + if tablet["Number"] == position: + tablet["Material"]["uuid"] = material_uuid + tablet["Material"]["materialEnum"] = material_enum + layout_list.append( + dict(reagent_name=reagent_name, material_name=material_name, positions=position) + ) break current_pos += 1 return self.default_layout, layout_list - if __name__ == "__main__": # Example usage # 1. 用导出的json,给每个T1 T2板子设定相应的物料,如果是孔板和枪头盒,要对应区分 @@ -1302,10 +1387,7 @@ if __name__ == "__main__": # # # plate2.set_well_liquids(plate_2_liquids) - - - - # handler = PRCXI9300Handler(deck=deck, host="10.181.214.132", port=9999, + # handler = PRCXI9300Handler(deck=deck, host="10.181.214.132", port=9999, # timeout=10.0, setup=False, debug=False, # simulator=True, # matrix_id="71593", @@ -1391,10 +1473,7 @@ if __name__ == "__main__": # # input("Press Enter to continue...") # Wait for user input before proceeding # # print("PRCXI9300Handler initialized with deck and host settings.") - - -### 9320 ### - + ### 9320 ### deck = PRCXI9300Deck(name="PRCXI_Deck", size_x=100, size_y=100, size_z=100) @@ -1412,12 +1491,15 @@ if __name__ == "__main__": new_plate: PRCXI9300Container = PRCXI9300Container.deserialize(well_containers) return new_plate - def get_tip_rack(name: str, child_prefix: str="tip") -> PRCXI9300Container: + def get_tip_rack(name: str, child_prefix: str = "tip") -> PRCXI9300Container: tip_racks = opentrons_96_tiprack_10ul(name).serialize() tip_rack = PRCXI9300Container( - name=name, size_x=50, size_y=50, size_z=10, category="tip_rack", ordering=collections.OrderedDict({ - k: f"{child_prefix}_{k}" for k, v in tip_racks["ordering"].items() - }) + name=name, + size_x=50, + size_y=50, + size_z=10, + category="tip_rack", + ordering=collections.OrderedDict({k: f"{child_prefix}_{k}" for k, v in tip_racks["ordering"].items()}), ) tip_rack_serialized = tip_rack.serialize() tip_rack_serialized["parent_name"] = deck.name @@ -1629,6 +1711,7 @@ if __name__ == "__main__": ) backend: PRCXI9300Backend = handler.backend from pylabrobot.resources import set_volume_tracking + set_volume_tracking(enabled=True) # res = backend.api_client.get_all_materials() asyncio.run(handler.setup()) # Initialize the handler and setup the connection @@ -1640,10 +1723,10 @@ if __name__ == "__main__": for well in plate13.get_all_items(): # well_pos = well.name.split("_")[1] # 走一行 - # if well_pos.startswith("A"): - if well.name.startswith("PlateT13"): # 走整个Plate + # if well_pos.startswith("A"): + if well.name.startswith("PlateT13"): # 走整个Plate asyncio.run(handler.dispense([well], [0.01], [0])) - + # asyncio.run(handler.dispense([plate10.get_item("H12")], [1], [0])) # asyncio.run(handler.dispense([plate13.get_item("A1")], [1], [0])) # asyncio.run(handler.dispense([plate14.get_item("C5")], [1], [0])) @@ -1652,26 +1735,25 @@ if __name__ == "__main__": asyncio.run(handler.run_protocol()) time.sleep(5) os._exit(0) -# 第一种情景:一个孔往多个孔加液 + # 第一种情景:一个孔往多个孔加液 # plate_2_liquids = handler.set_group("water", [plate2.children[0]], [300]) # plate5_liquids = handler.set_group("master_mix", plate5.children[:23], [100]*23) -# 第二个情景:多个孔往多个孔加液(但是个数得对应) - plate_2_liquids = handler.set_group("water", plate2.children[:23], [300]*23) - plate5_liquids = handler.set_group("master_mix", plate5.children[:23], [100]*23) + # 第二个情景:多个孔往多个孔加液(但是个数得对应) + plate_2_liquids = handler.set_group("water", plate2.children[:23], [300] * 23) + plate5_liquids = handler.set_group("master_mix", plate5.children[:23], [100] * 23) # plate11.set_well_liquids([("Water", 100) if (i % 8 == 0 and i // 8 < 6) else (None, 100) for i in range(96)]) # Set liquids for every 8 wells in plate8 # plate11.set_well_liquids([("Water", 100) if (i % 8 == 0 and i // 8 < 6) else (None, 100) for i in range(96)]) # Set liquids for every 8 wells in plate8 -# A = tree_to_list([resource_plr_to_ulab(deck)]) -# # with open("deck.json", "w", encoding="utf-8") as f: -# # json.dump(A, f, indent=4, ensure_ascii=False) + # A = tree_to_list([resource_plr_to_ulab(deck)]) + # # with open("deck.json", "w", encoding="utf-8") as f: + # # json.dump(A, f, indent=4, ensure_ascii=False) -# print(plate11.get_well(0).tracker.get_used_volume()) - # Initialize the backend and setup the connection + # print(plate11.get_well(0).tracker.get_used_volume()) + # Initialize the backend and setup the connection asyncio.run(handler.transfer_group("water", "master_mix", 10)) # Reset tip tracking - # asyncio.run(handler.pick_up_tips([plate8.children[8]],[0])) # print(plate8.children[8]) # asyncio.run(handler.run_protocol()) @@ -1685,121 +1767,118 @@ if __name__ == "__main__": # print(plate1.children[0]) # asyncio.run(handler.discard_tips([0])) -# asyncio.run(handler.add_liquid( -# asp_vols=[10]*7, -# dis_vols=[10]*7, -# reagent_sources=plate11.children[:7], -# targets=plate1.children[2:9], -# use_channels=[0], -# flow_rates=[None] * 7, -# offsets=[Coordinate(0, 0, 0)] * 7, -# liquid_height=[None] * 7, -# blow_out_air_volume=[None] * 2, -# delays=None, -# mix_time=3, -# mix_vol=5, -# spread="custom", -# )) + # asyncio.run(handler.add_liquid( + # asp_vols=[10]*7, + # dis_vols=[10]*7, + # reagent_sources=plate11.children[:7], + # targets=plate1.children[2:9], + # use_channels=[0], + # flow_rates=[None] * 7, + # offsets=[Coordinate(0, 0, 0)] * 7, + # liquid_height=[None] * 7, + # blow_out_air_volume=[None] * 2, + # delays=None, + # mix_time=3, + # mix_vol=5, + # spread="custom", + # )) # asyncio.run(handler.run_protocol()) # Run the protocol + # # # asyncio.run(handler.transfer_liquid( + # # # asp_vols=[10]*2, + # # # dis_vols=[10]*2, + # # # sources=plate11.children[:2], + # # # targets=plate11.children[-2:], + # # # use_channels=[0], + # # # offsets=[Coordinate(0, 0, 0)] * 4, + # # # liquid_height=[None] * 2, + # # # blow_out_air_volume=[None] * 2, + # # # delays=None, + # # # mix_times=3, + # # # mix_vol=5, + # # # spread="wide", + # # # tip_racks=[plate8] + # # # )) + # # # asyncio.run(handler.remove_liquid( + # # # vols=[10]*2, + # # # sources=plate11.children[:2], + # # # waste_liquid=plate11.children[43], + # # # use_channels=[0], + # # # offsets=[Coordinate(0, 0, 0)] * 4, + # # # liquid_height=[None] * 2, + # # # blow_out_air_volume=[None] * 2, + # # # delays=None, + # # # spread="wide" + # # # )) + # # asyncio.run(handler.run_protocol()) + # # # asyncio.run(handler.discard_tips()) + # # # asyncio.run(handler.mix(well_containers.children[:8 + # # # ], mix_time=3, mix_vol=50, height_to_bottom=0.5, offsets=Coordinate(0, 0, 0), mix_rate=100)) + # # #print(json.dumps(handler._unilabos_backend.steps_todo_list, indent=2)) # Print matrix info -# # # asyncio.run(handler.transfer_liquid( -# # # asp_vols=[10]*2, -# # # dis_vols=[10]*2, -# # # sources=plate11.children[:2], -# # # targets=plate11.children[-2:], -# # # use_channels=[0], -# # # offsets=[Coordinate(0, 0, 0)] * 4, -# # # liquid_height=[None] * 2, -# # # blow_out_air_volume=[None] * 2, -# # # delays=None, -# # # mix_times=3, -# # # mix_vol=5, -# # # spread="wide", -# # # tip_racks=[plate8] -# # # )) - -# # # asyncio.run(handler.remove_liquid( -# # # vols=[10]*2, -# # # sources=plate11.children[:2], -# # # waste_liquid=plate11.children[43], -# # # use_channels=[0], -# # # offsets=[Coordinate(0, 0, 0)] * 4, -# # # liquid_height=[None] * 2, -# # # blow_out_air_volume=[None] * 2, -# # # delays=None, -# # # spread="wide" -# # # )) -# # asyncio.run(handler.run_protocol()) - -# # # asyncio.run(handler.discard_tips()) -# # # asyncio.run(handler.mix(well_containers.children[:8 -# # # ], mix_time=3, mix_vol=50, height_to_bottom=0.5, offsets=Coordinate(0, 0, 0), mix_rate=100)) -# # #print(json.dumps(handler._unilabos_backend.steps_todo_list, indent=2)) # Print matrix info - - -# # # asyncio.run(handler.remove_liquid( -# # # vols=[100]*16, -# # # sources=well_containers.children[-16:], -# # # waste_liquid=well_containers.children[:16], # 这个有些奇怪,但是好像也只能这么写 -# # # use_channels=[0, 1, 2, 3, 4, 5, 6, 7], -# # # flow_rates=[None] * 32, -# # # offsets=[Coordinate(0, 0, 0)] * 32, -# # # liquid_height=[None] * 32, -# # # blow_out_air_volume=[None] * 32, -# # # spread="wide", -# # # )) -# # # asyncio.run(handler.transfer_liquid( -# # # asp_vols=[100]*16, -# # # dis_vols=[100]*16, -# # # tip_racks=[tip_rack], -# # # sources=well_containers.children[-16:], -# # # targets=well_containers.children[:16], -# # # use_channels=[0, 1, 2, 3, 4, 5, 6, 7], -# # # offsets=[Coordinate(0, 0, 0)] * 32, -# # # asp_flow_rates=[None] * 16, -# # # dis_flow_rates=[None] * 16, -# # # liquid_height=[None] * 32, -# # # blow_out_air_volume=[None] * 32, -# # # mix_times=3, -# # # mix_vol=50, -# # # spread="wide", -# # # )) -# # print(json.dumps(handler._unilabos_backend.steps_todo_list, indent=2)) # Print matrix info -# # # input("pick_up_tips add step") - #asyncio.run(handler.run_protocol()) # Run the protocol -# # # input("Running protocol...") -# # # input("Press Enter to continue...") # Wait for user input before proceeding -# # # print("PRCXI9300Handler initialized with deck and host settings.") - - -# 一些推荐版位组合的测试样例: - -# 一些推荐版位组合的测试样例: + # # # asyncio.run(handler.remove_liquid( + # # # vols=[100]*16, + # # # sources=well_containers.children[-16:], + # # # waste_liquid=well_containers.children[:16], # 这个有些奇怪,但是好像也只能这么写 + # # # use_channels=[0, 1, 2, 3, 4, 5, 6, 7], + # # # flow_rates=[None] * 32, + # # # offsets=[Coordinate(0, 0, 0)] * 32, + # # # liquid_height=[None] * 32, + # # # blow_out_air_volume=[None] * 32, + # # # spread="wide", + # # # )) + # # # asyncio.run(handler.transfer_liquid( + # # # asp_vols=[100]*16, + # # # dis_vols=[100]*16, + # # # tip_racks=[tip_rack], + # # # sources=well_containers.children[-16:], + # # # targets=well_containers.children[:16], + # # # use_channels=[0, 1, 2, 3, 4, 5, 6, 7], + # # # offsets=[Coordinate(0, 0, 0)] * 32, + # # # asp_flow_rates=[None] * 16, + # # # dis_flow_rates=[None] * 16, + # # # liquid_height=[None] * 32, + # # # blow_out_air_volume=[None] * 32, + # # # mix_times=3, + # # # mix_vol=50, + # # # spread="wide", + # # # )) + # # print(json.dumps(handler._unilabos_backend.steps_todo_list, indent=2)) # Print matrix info + # # # input("pick_up_tips add step") + # asyncio.run(handler.run_protocol()) # Run the protocol + # # # input("Running protocol...") + # # # input("Press Enter to continue...") # Wait for user input before proceeding + # # # print("PRCXI9300Handler initialized with deck and host settings.") + # 一些推荐版位组合的测试样例: + # 一些推荐版位组合的测试样例: with open("prcxi_material.json", "r") as f: material_info = json.load(f) layout = DefaultLayout("PRCXI9320") layout.add_lab_resource(material_info) - MatrixLayout_1, dict_1 = layout.recommend_layout([ - ("reagent_1", "96 细胞培养皿", 3), - ("reagent_2", "12道储液槽", 1), - ("reagent_3", "200μL Tip头", 7), - ("reagent_4", "10μL加长 Tip头", 1), - ]) + MatrixLayout_1, dict_1 = layout.recommend_layout( + [ + ("reagent_1", "96 细胞培养皿", 3), + ("reagent_2", "12道储液槽", 1), + ("reagent_3", "200μL Tip头", 7), + ("reagent_4", "10μL加长 Tip头", 1), + ] + ) print(dict_1) - MatrixLayout_2, dict_2 = layout.recommend_layout([ - ("reagent_1", "96深孔板", 4), - ("reagent_2", "12道储液槽", 1), - ("reagent_3", "200μL Tip头", 1), - ("reagent_4", "10μL加长 Tip头", 1), - ]) + MatrixLayout_2, dict_2 = layout.recommend_layout( + [ + ("reagent_1", "96深孔板", 4), + ("reagent_2", "12道储液槽", 1), + ("reagent_3", "200μL Tip头", 1), + ("reagent_4", "10μL加长 Tip头", 1), + ] + ) # with open("prcxi_material.json", "r") as f: # material_info = json.load(f) diff --git a/unilabos/ros/nodes/base_device_node.py b/unilabos/ros/nodes/base_device_node.py index edf41fbd..2fc7ea7d 100644 --- a/unilabos/ros/nodes/base_device_node.py +++ b/unilabos/ros/nodes/base_device_node.py @@ -53,7 +53,7 @@ from unilabos.ros.nodes.resource_tracker import ( ) from unilabos.ros.x.rclpyx import get_event_loop from unilabos.ros.utils.driver_creator import WorkstationNodeCreator, PyLabRobotCreator, DeviceClassCreator -from rclpy.task import Task +from rclpy.task import Task, Future from unilabos.utils.import_manager import default_manager from unilabos.utils.log import info, debug, warning, error, critical, logger, trace from unilabos.utils.type_check import get_type_class, TypeEncoder, get_result_info_str @@ -555,6 +555,15 @@ class BaseROS2DeviceNode(Node, Generic[T]): rclpy.get_global_executor().add_node(self) self.lab_logger().debug(f"ROS节点初始化完成") + async def sleep(self, rel_time: float, callback_group=None): + if callback_group is None: + callback_group = self.callback_group + await ROS2DeviceNode.async_wait_for(self, rel_time, callback_group) + + @classmethod + async def create_task(cls, func, trace_error=True, **kwargs) -> Task: + return ROS2DeviceNode.run_async_func(func, trace_error, **kwargs) + async def update_resource(self, resources: List["ResourcePLR"]): r = SerialCommand.Request() tree_set = ResourceTreeSet.from_plr_resources(resources) @@ -1399,6 +1408,14 @@ class ROS2DeviceNode: future.add_done_callback(_handle_future_exception) return future + @classmethod + async def async_wait_for(cls, node: Node, wait_time: float, callback_group=None): + future = Future() + timer = node.create_timer(wait_time, lambda : future.set_result(None), callback_group=callback_group, clock=node.get_clock()) + await future + timer.cancel() + node.destroy_timer(timer) + @property def driver_instance(self): return self._driver_instance diff --git a/unilabos/ros/nodes/presets/host_node.py b/unilabos/ros/nodes/presets/host_node.py index 43d16e8d..346cf9c2 100644 --- a/unilabos/ros/nodes/presets/host_node.py +++ b/unilabos/ros/nodes/presets/host_node.py @@ -18,7 +18,8 @@ from unilabos_msgs.srv import ( ResourceDelete, ResourceUpdate, ResourceList, - SerialCommand, ResourceGet, + SerialCommand, + ResourceGet, ) # type: ignore from unilabos_msgs.srv._serial_command import SerialCommand_Request, SerialCommand_Response from unique_identifier_msgs.msg import UUID From bed453034fab0a60d346d2a361d69d9ba1278d2d Mon Sep 17 00:00:00 2001 From: Xuwznln <18435084+Xuwznln@users.noreply.github.com> Date: Mon, 3 Nov 2025 15:49:11 +0800 Subject: [PATCH 03/13] modify devices to use correct executor (sleep, create_task) --- test/registry/example_devices.py | 33 +- unilabos/devices/cnc/grbl_async.py | 11 +- unilabos/devices/cnc/mock.py | 9 +- .../laiyu_liquid/core/laiyu_liquid_main.py | 423 +++++++++--------- .../liquid_handler_abstract.py | 10 +- .../devices/pump_and_valve/runze_async.py | 11 +- .../devices/virtual/virtual_centrifuge.py | 9 +- unilabos/devices/virtual/virtual_column.py | 9 +- unilabos/devices/virtual/virtual_filter.py | 202 ++++----- unilabos/devices/virtual/virtual_heatchill.py | 9 +- unilabos/devices/virtual/virtual_rotavap.py | 273 ++++++----- unilabos/devices/virtual/virtual_separator.py | 11 +- .../devices/virtual/virtual_solenoid_valve.py | 10 +- .../virtual/virtual_solid_dispenser.py | 9 +- unilabos/devices/virtual/virtual_stirrer.py | 11 +- .../devices/virtual/virtual_transferpump.py | 13 +- 16 files changed, 597 insertions(+), 456 deletions(-) diff --git a/test/registry/example_devices.py b/test/registry/example_devices.py index d5b26b2c..d41c7b43 100644 --- a/test/registry/example_devices.py +++ b/test/registry/example_devices.py @@ -3,7 +3,8 @@ """ import asyncio -from typing import Dict, Any, Optional, List +from typing import Dict, Any, List +from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode class SmartPumpController: @@ -14,6 +15,8 @@ class SmartPumpController: 适用于实验室自动化系统中的液体处理任务。 """ + _ros_node: BaseROS2DeviceNode + def __init__(self, device_id: str = "smart_pump_01", port: str = "/dev/ttyUSB0"): """ 初始化智能泵控制器 @@ -30,6 +33,9 @@ class SmartPumpController: self.calibration_factor = 1.0 self.pump_mode = "continuous" # continuous, volume, rate + def post_init(self, ros_node: BaseROS2DeviceNode): + self._ros_node = ros_node + def connect_device(self, timeout: int = 10) -> bool: """ 连接到泵设备 @@ -90,7 +96,7 @@ class SmartPumpController: pump_time = (volume / flow_rate) * 60 # 转换为秒 self.current_flow_rate = flow_rate - await asyncio.sleep(min(pump_time, 3.0)) # 模拟泵送过程 + await self._ros_node.sleep(min(pump_time, 3.0)) # 模拟泵送过程 self.total_volume_pumped += volume self.current_flow_rate = 0.0 @@ -170,6 +176,8 @@ class AdvancedTemperatureController: 适用于需要精确温度控制的化学反应和材料处理过程。 """ + _ros_node: BaseROS2DeviceNode + def __init__(self, controller_id: str = "temp_controller_01"): """ 初始化温度控制器 @@ -185,6 +193,9 @@ class AdvancedTemperatureController: self.pid_enabled = True self.temperature_history: List[Dict] = [] + def post_init(self, ros_node: BaseROS2DeviceNode): + self._ros_node = ros_node + def set_target_temperature(self, temperature: float, rate: float = 10.0) -> bool: """ 设置目标温度 @@ -238,7 +249,7 @@ class AdvancedTemperatureController: } ) - await asyncio.sleep(step_time) + await self._ros_node.sleep(step_time) # 保持历史记录不超过100条 if len(self.temperature_history) > 100: @@ -330,6 +341,8 @@ class MultiChannelAnalyzer: 常用于光谱分析、电化学测量等应用场景。 """ + _ros_node: BaseROS2DeviceNode + def __init__(self, analyzer_id: str = "analyzer_01", channels: int = 8): """ 初始化多通道分析仪 @@ -344,6 +357,9 @@ class MultiChannelAnalyzer: self.is_measuring = False self.sample_rate = 1000 # Hz + def post_init(self, ros_node: BaseROS2DeviceNode): + self._ros_node = ros_node + def configure_channel(self, channel: int, enabled: bool = True, unit: str = "V") -> bool: """ 配置通道 @@ -376,7 +392,7 @@ class MultiChannelAnalyzer: # 模拟数据采集 measurements = [] - for second in range(duration): + for _ in range(duration): timestamp = asyncio.get_event_loop().time() frame_data = {} @@ -391,7 +407,7 @@ class MultiChannelAnalyzer: measurements.append({"timestamp": timestamp, "data": frame_data}) - await asyncio.sleep(1.0) # 每秒采集一次 + await self._ros_node.sleep(1.0) # 每秒采集一次 self.is_measuring = False @@ -465,6 +481,8 @@ class AutomatedDispenser: 集成称重功能,确保分配精度和重现性。 """ + _ros_node: BaseROS2DeviceNode + def __init__(self, dispenser_id: str = "dispenser_01"): """ 初始化自动分配器 @@ -479,6 +497,9 @@ class AutomatedDispenser: self.container_capacity = 1000.0 # mL self.precision_mode = True + def post_init(self, ros_node: BaseROS2DeviceNode): + self._ros_node = ros_node + def move_to_position(self, x: float, y: float, z: float) -> bool: """ 移动到指定位置 @@ -517,7 +538,7 @@ class AutomatedDispenser: if viscosity == "high": dispense_time *= 2 # 高粘度液体需要更长时间 - await asyncio.sleep(min(dispense_time, 5.0)) # 最多等待5秒 + await self._ros_node.sleep(min(dispense_time, 5.0)) # 最多等待5秒 self.dispensed_total += volume diff --git a/unilabos/devices/cnc/grbl_async.py b/unilabos/devices/cnc/grbl_async.py index 7e5ac7f3..3ecd4ba8 100644 --- a/unilabos/devices/cnc/grbl_async.py +++ b/unilabos/devices/cnc/grbl_async.py @@ -12,6 +12,7 @@ from serial import Serial from serial.serialutil import SerialException from unilabos.messages import Point3D +from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode class GrblCNCConnectionError(Exception): @@ -32,6 +33,7 @@ class GrblCNCInfo: class GrblCNCAsync: _status: str = "Offline" _position: Point3D = Point3D(x=0.0, y=0.0, z=0.0) + _ros_node: BaseROS2DeviceNode def __init__(self, port: str, address: str = "1", limits: tuple[int, int, int, int, int, int] = (-150, 150, -200, 0, 0, 60)): self.port = port @@ -58,6 +60,9 @@ class GrblCNCAsync: self._run_future: Optional[Future[Any]] = None self._run_lock = Lock() + def post_init(self, ros_node: BaseROS2DeviceNode): + self._ros_node = ros_node + def _read_all(self): data = self._serial.read_until(b"\n") data_decoded = data.decode() @@ -148,7 +153,7 @@ class GrblCNCAsync: try: await self._query(command) while True: - await asyncio.sleep(0.2) # Wait for 0.5 seconds before polling again + await self._ros_node.sleep(0.2) # Wait for 0.5 seconds before polling again status = await self.get_status() if "Idle" in status: @@ -214,7 +219,7 @@ class GrblCNCAsync: self._pose_number = i self.pose_number_remaining = len(points) - i await self.set_position(point) - await asyncio.sleep(0.5) + await self._ros_node.sleep(0.5) self._step_number = -1 async def stop_operation(self): @@ -235,7 +240,7 @@ class GrblCNCAsync: async def open(self): if self._read_task: raise GrblCNCConnectionError - self._read_task = asyncio.create_task(self._read_loop()) + self._read_task = self._ros_node.create_task(self._read_loop()) try: await self.get_status() diff --git a/unilabos/devices/cnc/mock.py b/unilabos/devices/cnc/mock.py index b8c52f16..ebe96833 100644 --- a/unilabos/devices/cnc/mock.py +++ b/unilabos/devices/cnc/mock.py @@ -2,6 +2,8 @@ import time import asyncio from pydantic import BaseModel +from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode + class Point3D(BaseModel): x: float @@ -14,9 +16,14 @@ def d(a: Point3D, b: Point3D) -> float: class MockCNCAsync: + _ros_node: BaseROS2DeviceNode["MockCNCAsync"] + def __init__(self): self._position: Point3D = Point3D(x=0.0, y=0.0, z=0.0) self._status = "Idle" + + def post_create(self, ros_node): + self._ros_node = ros_node @property def position(self) -> Point3D: @@ -38,5 +45,5 @@ class MockCNCAsync: self._position.x = current_pos.x + (position.x - current_pos.x) / 20 * (i+1) self._position.y = current_pos.y + (position.y - current_pos.y) / 20 * (i+1) self._position.z = current_pos.z + (position.z - current_pos.z) / 20 * (i+1) - await asyncio.sleep(move_time / 20) + await self._ros_node.sleep(move_time / 20) self._status = "Idle" diff --git a/unilabos/devices/laiyu_liquid/core/laiyu_liquid_main.py b/unilabos/devices/laiyu_liquid/core/laiyu_liquid_main.py index 96092556..f369a208 100644 --- a/unilabos/devices/laiyu_liquid/core/laiyu_liquid_main.py +++ b/unilabos/devices/laiyu_liquid/core/laiyu_liquid_main.py @@ -15,108 +15,113 @@ from typing import List, Optional, Dict, Any, Union, Tuple from dataclasses import dataclass from abc import ABC, abstractmethod +from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode + # 基础导入 try: from pylabrobot.resources import Deck, Plate, TipRack, Tip, Resource, Well + PYLABROBOT_AVAILABLE = True except ImportError: # 如果 pylabrobot 不可用,创建基础的模拟类 PYLABROBOT_AVAILABLE = False - + class Resource: def __init__(self, name: str): self.name = name - + class Deck(Resource): pass - + class Plate(Resource): pass - + class TipRack(Resource): pass - + class Tip(Resource): pass - + class Well(Resource): pass + # LaiYu_Liquid 控制器导入 try: - from .controllers.pipette_controller import ( - PipetteController, TipStatus, LiquidClass, LiquidParameters - ) - from .controllers.xyz_controller import ( - XYZController, MachineConfig, CoordinateOrigin, MotorAxis - ) + from .controllers.pipette_controller import PipetteController, TipStatus, LiquidClass, LiquidParameters + from .controllers.xyz_controller import XYZController, MachineConfig, CoordinateOrigin, MotorAxis + CONTROLLERS_AVAILABLE = True except ImportError: CONTROLLERS_AVAILABLE = False + # 创建模拟的控制器类 class PipetteController: def __init__(self, *args, **kwargs): pass - + def connect(self): return True - + def initialize(self): return True - + class XYZController: def __init__(self, *args, **kwargs): pass - + def connect_device(self): return True + logger = logging.getLogger(__name__) class LaiYuLiquidError(RuntimeError): """LaiYu_Liquid 设备异常""" + pass @dataclass class LaiYuLiquidConfig: """LaiYu_Liquid 设备配置""" + port: str = "/dev/cu.usbserial-3130" # RS485转USB端口 address: int = 1 # 设备地址 baudrate: int = 9600 # 波特率 timeout: float = 5.0 # 通信超时时间 - + # 工作台尺寸 deck_width: float = 340.0 # 工作台宽度 (mm) deck_height: float = 250.0 # 工作台高度 (mm) deck_depth: float = 160.0 # 工作台深度 (mm) - + # 移液参数 max_volume: float = 1000.0 # 最大体积 (μL) min_volume: float = 0.1 # 最小体积 (μL) - + # 运动参数 max_speed: float = 100.0 # 最大速度 (mm/s) acceleration: float = 50.0 # 加速度 (mm/s²) - + # 安全参数 safe_height: float = 50.0 # 安全高度 (mm) tip_pickup_depth: float = 10.0 # 吸头拾取深度 (mm) liquid_detection: bool = True # 液面检测 - + # 取枪头相关参数 tip_pickup_speed: int = 30 # 取枪头时的移动速度 (rpm) tip_pickup_acceleration: int = 500 # 取枪头时的加速度 (rpm/s) tip_approach_height: float = 5.0 # 接近枪头时的高度 (mm) tip_pickup_force_depth: float = 2.0 # 强制插入深度 (mm) tip_pickup_retract_height: float = 20.0 # 取枪头后的回退高度 (mm) - + # 丢弃枪头相关参数 tip_drop_height: float = 10.0 # 丢弃枪头时的高度 (mm) tip_drop_speed: int = 50 # 丢弃枪头时的移动速度 (rpm) trash_position: Tuple[float, float, float] = (300.0, 200.0, 0.0) # 垃圾桶位置 (mm) - + # 安全范围配置 deck_width: float = 300.0 # 工作台宽度 (mm) deck_height: float = 200.0 # 工作台高度 (mm) @@ -128,25 +133,25 @@ class LaiYuLiquidConfig: class LaiYuLiquidDeck: """LaiYu_Liquid 工作台管理""" - + def __init__(self, config: LaiYuLiquidConfig): self.config = config self.resources: Dict[str, Resource] = {} self.positions: Dict[str, Tuple[float, float, float]] = {} - + def add_resource(self, name: str, resource: Resource, position: Tuple[float, float, float]): """添加资源到工作台""" self.resources[name] = resource self.positions[name] = position - + def get_resource(self, name: str) -> Optional[Resource]: """获取资源""" return self.resources.get(name) - + def get_position(self, name: str) -> Optional[Tuple[float, float, float]]: """获取资源位置""" return self.positions.get(name) - + def list_resources(self) -> List[str]: """列出所有资源""" return list(self.resources.keys()) @@ -154,8 +159,18 @@ class LaiYuLiquidDeck: class LaiYuLiquidContainer: """LaiYu_Liquid 容器类""" - - def __init__(self, name: str, size_x: float = 0, size_y: float = 0, size_z: float = 0, container_type: str = "", volume: float = 0.0, max_volume: float = 1000.0, lid_height: float = 0.0): + + def __init__( + self, + name: str, + size_x: float = 0, + size_y: float = 0, + size_z: float = 0, + container_type: str = "", + volume: float = 0.0, + max_volume: float = 1000.0, + lid_height: float = 0.0, + ): self.name = name self.size_x = size_x self.size_y = size_y @@ -166,19 +181,19 @@ class LaiYuLiquidContainer: self.max_volume = max_volume self.last_updated = time.time() self.child_resources = {} # 存储子资源 - + @property def is_empty(self) -> bool: return self.volume <= 0.0 - + @property def is_full(self) -> bool: return self.volume >= self.max_volume - + @property def available_volume(self) -> float: return max(0.0, self.max_volume - self.volume) - + def add_volume(self, volume: float) -> bool: """添加体积""" if self.volume + volume <= self.max_volume: @@ -186,7 +201,7 @@ class LaiYuLiquidContainer: self.last_updated = time.time() return True return False - + def remove_volume(self, volume: float) -> bool: """移除体积""" if self.volume >= volume: @@ -194,20 +209,25 @@ class LaiYuLiquidContainer: self.last_updated = time.time() return True return False - + def assign_child_resource(self, resource, location=None): """分配子资源 - 与 PyLabRobot 资源管理系统兼容""" - if hasattr(resource, 'name'): - self.child_resources[resource.name] = { - 'resource': resource, - 'location': location - } + if hasattr(resource, "name"): + self.child_resources[resource.name] = {"resource": resource, "location": location} class LaiYuLiquidTipRack: """LaiYu_Liquid 吸头架类""" - - def __init__(self, name: str, size_x: float = 0, size_y: float = 0, size_z: float = 0, tip_count: int = 96, tip_volume: float = 1000.0): + + def __init__( + self, + name: str, + size_x: float = 0, + size_y: float = 0, + size_z: float = 0, + tip_count: int = 96, + tip_volume: float = 1000.0, + ): self.name = name self.size_x = size_x self.size_y = size_y @@ -216,34 +236,31 @@ class LaiYuLiquidTipRack: self.tip_volume = tip_volume self.tips_available = [True] * tip_count self.child_resources = {} # 存储子资源 - + @property def available_tips(self) -> int: return sum(self.tips_available) - + @property def is_empty(self) -> bool: return self.available_tips == 0 - + def pick_tip(self, position: int) -> bool: """拾取吸头""" if 0 <= position < self.tip_count and self.tips_available[position]: self.tips_available[position] = False return True return False - + def has_tip(self, position: int) -> bool: """检查位置是否有吸头""" if 0 <= position < self.tip_count: return self.tips_available[position] return False - + def assign_child_resource(self, resource, location=None): """分配子资源到指定位置""" - self.child_resources[resource.name] = { - 'resource': resource, - 'location': location - } + self.child_resources[resource.name] = {"resource": resource, "location": location} def get_module_info(): @@ -253,36 +270,32 @@ def get_module_info(): "version": "1.0.0", "description": "LaiYu液体处理工作站模块,提供移液器控制、XYZ轴控制和资源管理功能", "author": "UniLabOS Team", - "capabilities": [ - "移液器控制", - "XYZ轴运动控制", - "吸头架管理", - "板和容器管理", - "资源位置管理" - ], - "dependencies": { - "required": ["serial"], - "optional": ["pylabrobot"] - } + "capabilities": ["移液器控制", "XYZ轴运动控制", "吸头架管理", "板和容器管理", "资源位置管理"], + "dependencies": {"required": ["serial"], "optional": ["pylabrobot"]}, } class LaiYuLiquidBackend: """LaiYu_Liquid 硬件通信后端""" - - def __init__(self, config: LaiYuLiquidConfig, deck: Optional['LaiYuLiquidDeck'] = None): + + _ros_node: BaseROS2DeviceNode + + def __init__(self, config: LaiYuLiquidConfig, deck: Optional["LaiYuLiquidDeck"] = None): self.config = config self.deck = deck # 工作台引用,用于获取资源位置信息 self.pipette_controller = None self.xyz_controller = None self.is_connected = False self.is_initialized = False - + # 状态跟踪 self.current_position = (0.0, 0.0, 0.0) self.tip_attached = False self.current_volume = 0.0 - + + def post_init(self, ros_node: BaseROS2DeviceNode): + self._ros_node = ros_node + def _validate_position(self, x: float, y: float, z: float) -> bool: """验证位置是否在安全范围内""" try: @@ -290,71 +303,71 @@ class LaiYuLiquidBackend: if not (0 <= x <= self.config.deck_width): logger.error(f"X轴位置 {x:.2f}mm 超出范围 [0, {self.config.deck_width}]") return False - + # 检查Y轴范围 if not (0 <= y <= self.config.deck_height): logger.error(f"Y轴位置 {y:.2f}mm 超出范围 [0, {self.config.deck_height}]") return False - + # 检查Z轴范围(负值表示向下,0为工作台表面) if not (-self.config.deck_depth <= z <= self.config.safe_height): logger.error(f"Z轴位置 {z:.2f}mm 超出安全范围 [{-self.config.deck_depth}, {self.config.safe_height}]") return False - + return True except Exception as e: logger.error(f"位置验证失败: {e}") return False - + def _check_hardware_ready(self) -> bool: """检查硬件是否准备就绪""" if not self.is_connected: logger.error("设备未连接") return False - + if CONTROLLERS_AVAILABLE: if self.xyz_controller is None: logger.error("XYZ控制器未初始化") return False - + return True - + async def emergency_stop(self) -> bool: """紧急停止所有运动""" try: logger.warning("执行紧急停止") - + if CONTROLLERS_AVAILABLE and self.xyz_controller: # 停止XYZ控制器 await self.xyz_controller.stop_all_motion() logger.info("XYZ控制器已停止") - + if self.pipette_controller: # 停止移液器控制器 await self.pipette_controller.stop() logger.info("移液器控制器已停止") - + return True except Exception as e: logger.error(f"紧急停止失败: {e}") return False - + async def move_to_safe_position(self) -> bool: """移动到安全位置""" try: if not self._check_hardware_ready(): return False - + safe_position = ( self.config.deck_width / 2, # 工作台中心X self.config.deck_height / 2, # 工作台中心Y - self.config.safe_height # 安全高度Z + self.config.safe_height, # 安全高度Z ) - + if not self._validate_position(*safe_position): logger.error("安全位置无效") return False - + if CONTROLLERS_AVAILABLE and self.xyz_controller: await self.xyz_controller.move_to_work_coord(*safe_position) self.current_position = safe_position @@ -365,33 +378,28 @@ class LaiYuLiquidBackend: self.current_position = safe_position logger.info("模拟移动到安全位置") return True - + except Exception as e: logger.error(f"移动到安全位置失败: {e}") return False - + async def setup(self) -> bool: """设置硬件连接""" try: if CONTROLLERS_AVAILABLE: # 初始化移液器控制器 - self.pipette_controller = PipetteController( - port=self.config.port, - address=self.config.address - ) - + self.pipette_controller = PipetteController(port=self.config.port, address=self.config.address) + # 初始化XYZ控制器 machine_config = MachineConfig() self.xyz_controller = XYZController( - port=self.config.port, - baudrate=self.config.baudrate, - machine_config=machine_config + port=self.config.port, baudrate=self.config.baudrate, machine_config=machine_config ) - + # 连接设备 pipette_connected = await asyncio.to_thread(self.pipette_controller.connect) xyz_connected = await asyncio.to_thread(self.xyz_controller.connect_device) - + if pipette_connected and xyz_connected: self.is_connected = True logger.info("LaiYu_Liquid 硬件连接成功") @@ -404,124 +412,123 @@ class LaiYuLiquidBackend: logger.info("LaiYu_Liquid 运行在模拟模式") self.is_connected = True return True - + except Exception as e: logger.error(f"LaiYu_Liquid 设置失败: {e}") return False - + async def stop(self): """停止设备""" try: - if self.pipette_controller and hasattr(self.pipette_controller, 'disconnect'): + if self.pipette_controller and hasattr(self.pipette_controller, "disconnect"): await asyncio.to_thread(self.pipette_controller.disconnect) - - if self.xyz_controller and hasattr(self.xyz_controller, 'disconnect'): + + if self.xyz_controller and hasattr(self.xyz_controller, "disconnect"): await asyncio.to_thread(self.xyz_controller.disconnect) - + self.is_connected = False self.is_initialized = False logger.info("LaiYu_Liquid 已停止") - + except Exception as e: logger.error(f"LaiYu_Liquid 停止失败: {e}") - + async def move_to(self, x: float, y: float, z: float) -> bool: """移动到指定位置""" try: if not self.is_connected: raise LaiYuLiquidError("设备未连接") - + # 模拟移动 - await asyncio.sleep(0.1) # 模拟移动时间 + await self._ros_node.sleep(0.1) # 模拟移动时间 self.current_position = (x, y, z) logger.debug(f"移动到位置: ({x}, {y}, {z})") return True - + except Exception as e: logger.error(f"移动失败: {e}") return False - + async def pick_up_tip(self, tip_rack: str, position: int) -> bool: """拾取吸头 - 包含真正的Z轴下降控制""" try: # 硬件准备检查 if not self._check_hardware_ready(): return False - + if self.tip_attached: logger.warning("已有吸头附着,无法拾取新吸头") return False - + logger.info(f"开始从 {tip_rack} 位置 {position} 拾取吸头") - + # 获取枪头架位置信息 if self.deck is None: logger.error("工作台未初始化") return False - + tip_position = self.deck.get_position(tip_rack) if tip_position is None: logger.error(f"未找到枪头架 {tip_rack} 的位置信息") return False - + # 计算具体枪头位置(这里简化处理,实际应根据position计算偏移) tip_x, tip_y, tip_z = tip_position - + # 验证所有关键位置的安全性 safe_z = tip_z + self.config.tip_approach_height pickup_z = tip_z - self.config.tip_pickup_force_depth retract_z = tip_z + self.config.tip_pickup_retract_height - - if not (self._validate_position(tip_x, tip_y, safe_z) and - self._validate_position(tip_x, tip_y, pickup_z) and - self._validate_position(tip_x, tip_y, retract_z)): + + if not ( + self._validate_position(tip_x, tip_y, safe_z) + and self._validate_position(tip_x, tip_y, pickup_z) + and self._validate_position(tip_x, tip_y, retract_z) + ): logger.error("枪头拾取位置超出安全范围") return False - + if CONTROLLERS_AVAILABLE and self.xyz_controller: # 真实硬件控制流程 logger.info("使用真实XYZ控制器进行枪头拾取") - + try: # 1. 移动到枪头上方的安全位置 safe_z = tip_z + self.config.tip_approach_height logger.info(f"移动到枪头上方安全位置: ({tip_x:.2f}, {tip_y:.2f}, {safe_z:.2f})") move_success = await asyncio.to_thread( - self.xyz_controller.move_to_work_coord, - tip_x, tip_y, safe_z + self.xyz_controller.move_to_work_coord, tip_x, tip_y, safe_z ) if not move_success: logger.error("移动到枪头上方失败") return False - + # 2. Z轴下降到枪头位置 pickup_z = tip_z - self.config.tip_pickup_force_depth logger.info(f"Z轴下降到枪头拾取位置: {pickup_z:.2f}mm") z_down_success = await asyncio.to_thread( - self.xyz_controller.move_to_work_coord, - tip_x, tip_y, pickup_z + self.xyz_controller.move_to_work_coord, tip_x, tip_y, pickup_z ) if not z_down_success: logger.error("Z轴下降到枪头位置失败") return False - + # 3. 等待一小段时间确保枪头牢固附着 - await asyncio.sleep(0.2) - + await self._ros_node.sleep(0.2) + # 4. Z轴上升到回退高度 retract_z = tip_z + self.config.tip_pickup_retract_height logger.info(f"Z轴上升到回退高度: {retract_z:.2f}mm") z_up_success = await asyncio.to_thread( - self.xyz_controller.move_to_work_coord, - tip_x, tip_y, retract_z + self.xyz_controller.move_to_work_coord, tip_x, tip_y, retract_z ) if not z_up_success: logger.error("Z轴上升失败") return False - + # 5. 更新当前位置 self.current_position = (tip_x, tip_y, retract_z) - + except Exception as move_error: logger.error(f"枪头拾取过程中发生错误: {move_error}") # 尝试移动到安全位置 @@ -529,35 +536,35 @@ class LaiYuLiquidBackend: await self.emergency_stop() await self.move_to_safe_position() return False - + else: # 模拟模式 logger.info("模拟模式:执行枪头拾取动作") - await asyncio.sleep(1.0) # 模拟整个拾取过程的时间 + await self._ros_node.sleep(1.0) # 模拟整个拾取过程的时间 self.current_position = (tip_x, tip_y, tip_z + self.config.tip_pickup_retract_height) - + # 6. 标记枪头已附着 self.tip_attached = True logger.info("吸头拾取成功") return True - + except Exception as e: logger.error(f"拾取吸头失败: {e}") return False - + async def drop_tip(self, location: str = "trash") -> bool: """丢弃吸头 - 包含真正的Z轴控制""" try: # 硬件准备检查 if not self._check_hardware_ready(): return False - + if not self.tip_attached: logger.warning("没有吸头附着,无需丢弃") return True - + logger.info(f"开始丢弃吸头到 {location}") - + # 确定丢弃位置 if location == "trash": # 使用配置中的垃圾桶位置 @@ -567,48 +574,48 @@ class LaiYuLiquidBackend: if self.deck is None: logger.error("工作台未初始化") return False - + drop_position = self.deck.get_position(location) if drop_position is None: logger.error(f"未找到丢弃位置 {location} 的信息") return False drop_x, drop_y, drop_z = drop_position - + # 验证丢弃位置的安全性 safe_z = drop_z + self.config.safe_height drop_height_z = drop_z + self.config.tip_drop_height - - if not (self._validate_position(drop_x, drop_y, safe_z) and - self._validate_position(drop_x, drop_y, drop_height_z)): + + if not ( + self._validate_position(drop_x, drop_y, safe_z) + and self._validate_position(drop_x, drop_y, drop_height_z) + ): logger.error("枪头丢弃位置超出安全范围") return False - + if CONTROLLERS_AVAILABLE and self.xyz_controller: # 真实硬件控制流程 logger.info("使用真实XYZ控制器进行枪头丢弃") - + try: # 1. 移动到丢弃位置上方的安全高度 safe_z = drop_z + self.config.tip_drop_height logger.info(f"移动到丢弃位置上方: ({drop_x:.2f}, {drop_y:.2f}, {safe_z:.2f})") move_success = await asyncio.to_thread( - self.xyz_controller.move_to_work_coord, - drop_x, drop_y, safe_z + self.xyz_controller.move_to_work_coord, drop_x, drop_y, safe_z ) if not move_success: logger.error("移动到丢弃位置上方失败") return False - + # 2. Z轴下降到丢弃高度 logger.info(f"Z轴下降到丢弃高度: {drop_z:.2f}mm") z_down_success = await asyncio.to_thread( - self.xyz_controller.move_to_work_coord, - drop_x, drop_y, drop_z + self.xyz_controller.move_to_work_coord, drop_x, drop_y, drop_z ) if not z_down_success: logger.error("Z轴下降到丢弃位置失败") return False - + # 3. 执行枪头弹出动作(如果有移液器控制器) if self.pipette_controller: try: @@ -617,23 +624,22 @@ class LaiYuLiquidBackend: logger.info("执行枪头弹出命令") except Exception as e: logger.warning(f"枪头弹出命令失败: {e}") - + # 4. 等待一小段时间确保枪头完全脱离 - await asyncio.sleep(0.3) - + await self._ros_node.sleep(0.3) + # 5. Z轴上升到安全高度 logger.info(f"Z轴上升到安全高度: {safe_z:.2f}mm") z_up_success = await asyncio.to_thread( - self.xyz_controller.move_to_work_coord, - drop_x, drop_y, safe_z + self.xyz_controller.move_to_work_coord, drop_x, drop_y, safe_z ) if not z_up_success: logger.error("Z轴上升失败") return False - + # 6. 更新当前位置 self.current_position = (drop_x, drop_y, safe_z) - + except Exception as drop_error: logger.error(f"枪头丢弃过程中发生错误: {drop_error}") # 尝试移动到安全位置 @@ -641,63 +647,63 @@ class LaiYuLiquidBackend: await self.emergency_stop() await self.move_to_safe_position() return False - + else: # 模拟模式 logger.info("模拟模式:执行枪头丢弃动作") - await asyncio.sleep(0.8) # 模拟整个丢弃过程的时间 + await self._ros_node.sleep(0.8) # 模拟整个丢弃过程的时间 self.current_position = (drop_x, drop_y, drop_z + self.config.tip_drop_height) - + # 7. 标记枪头已脱离,清空体积 self.tip_attached = False self.current_volume = 0.0 logger.info("吸头丢弃成功") return True - + except Exception as e: logger.error(f"丢弃吸头失败: {e}") return False - + async def aspirate(self, volume: float, location: str) -> bool: """吸取液体""" try: if not self.is_connected: raise LaiYuLiquidError("设备未连接") - + if not self.tip_attached: raise LaiYuLiquidError("没有吸头附着") - + if volume <= 0 or volume > self.config.max_volume: raise LaiYuLiquidError(f"体积超出范围: {volume}") - + # 模拟吸取 - await asyncio.sleep(0.3) + await self._ros_node.sleep(0.3) self.current_volume += volume logger.debug(f"从 {location} 吸取 {volume} μL") return True - + except Exception as e: logger.error(f"吸取失败: {e}") return False - + async def dispense(self, volume: float, location: str) -> bool: """分配液体""" try: if not self.is_connected: raise LaiYuLiquidError("设备未连接") - + if not self.tip_attached: raise LaiYuLiquidError("没有吸头附着") - + if volume <= 0 or volume > self.current_volume: raise LaiYuLiquidError(f"分配体积无效: {volume}") - + # 模拟分配 - await asyncio.sleep(0.3) + await self._ros_node.sleep(0.3) self.current_volume -= volume logger.debug(f"向 {location} 分配 {volume} μL") return True - + except Exception as e: logger.error(f"分配失败: {e}") return False @@ -705,7 +711,7 @@ class LaiYuLiquidBackend: class LaiYuLiquid: """LaiYu_Liquid 主要接口类""" - + def __init__(self, config: Optional[LaiYuLiquidConfig] = None, **kwargs): # 如果传入了关键字参数,创建配置对象 if kwargs and config is None: @@ -717,37 +723,37 @@ class LaiYuLiquid: self.config = LaiYuLiquidConfig(**config_params) else: self.config = config or LaiYuLiquidConfig() - + # 先创建deck,然后传递给backend self.deck = LaiYuLiquidDeck(self.config) self.backend = LaiYuLiquidBackend(self.config, self.deck) self.is_setup = False - + @property def current_position(self) -> Tuple[float, float, float]: """获取当前位置""" return self.backend.current_position - + @property def current_volume(self) -> float: """获取当前体积""" return self.backend.current_volume - + @property def is_connected(self) -> bool: """获取连接状态""" return self.backend.is_connected - + @property def is_initialized(self) -> bool: """获取初始化状态""" return self.backend.is_initialized - + @property def tip_attached(self) -> bool: """获取吸头附着状态""" return self.backend.tip_attached - + async def setup(self) -> bool: """设置液体处理器""" try: @@ -759,27 +765,28 @@ class LaiYuLiquid: except Exception as e: logger.error(f"LaiYu_Liquid 设置失败: {e}") return False - + async def stop(self): """停止液体处理器""" await self.backend.stop() self.is_setup = False - - async def transfer(self, source: str, target: str, volume: float, - tip_rack: str = "tip_rack_1", tip_position: int = 0) -> bool: + + async def transfer( + self, source: str, target: str, volume: float, tip_rack: str = "tip_rack_1", tip_position: int = 0 + ) -> bool: """液体转移""" try: if not self.is_setup: raise LaiYuLiquidError("设备未设置") - + # 获取源和目标位置 source_pos = self.deck.get_position(source) target_pos = self.deck.get_position(target) tip_pos = self.deck.get_position(tip_rack) - + if not all([source_pos, target_pos, tip_pos]): raise LaiYuLiquidError("位置信息不完整") - + # 执行转移步骤 steps = [ ("移动到吸头架", self.backend.move_to(*tip_pos)), @@ -788,22 +795,22 @@ class LaiYuLiquid: ("吸取液体", self.backend.aspirate(volume, source)), ("移动到目标位置", self.backend.move_to(*target_pos)), ("分配液体", self.backend.dispense(volume, target)), - ("丢弃吸头", self.backend.drop_tip()) + ("丢弃吸头", self.backend.drop_tip()), ] - + for step_name, step_coro in steps: logger.debug(f"执行步骤: {step_name}") success = await step_coro if not success: raise LaiYuLiquidError(f"步骤失败: {step_name}") - + logger.info(f"液体转移完成: {source} -> {target}, {volume} μL") return True - + except Exception as e: logger.error(f"液体转移失败: {e}") return False - + def add_resource(self, name: str, resource_type: str, position: Tuple[float, float, float]): """添加资源到工作台""" if resource_type == "plate": @@ -812,9 +819,9 @@ class LaiYuLiquid: resource = TipRack(name) else: resource = Resource(name) - + self.deck.add_resource(name, resource, position) - + def get_status(self) -> Dict[str, Any]: """获取设备状态""" return { @@ -823,59 +830,59 @@ class LaiYuLiquid: "current_position": self.backend.current_position, "tip_attached": self.backend.tip_attached, "current_volume": self.backend.current_volume, - "resources": self.deck.list_resources() + "resources": self.deck.list_resources(), } def create_quick_setup() -> LaiYuLiquidDeck: """ 创建快速设置的LaiYu液体处理工作站 - + Returns: LaiYuLiquidDeck: 配置好的工作台实例 """ # 创建默认配置 config = LaiYuLiquidConfig() - + # 创建工作台 deck = LaiYuLiquidDeck(config) - + # 导入资源创建函数 try: from .laiyu_liquid_res import ( create_tip_rack_1000ul, create_tip_rack_200ul, create_96_well_plate, - create_waste_container + create_waste_container, ) - + # 添加基本资源 tip_rack_1000 = create_tip_rack_1000ul("tip_rack_1000") tip_rack_200 = create_tip_rack_200ul("tip_rack_200") plate_96 = create_96_well_plate("plate_96") waste = create_waste_container("waste") - + # 添加到工作台 deck.add_resource("tip_rack_1000", tip_rack_1000, (50, 50, 0)) deck.add_resource("tip_rack_200", tip_rack_200, (150, 50, 0)) deck.add_resource("plate_96", plate_96, (250, 50, 0)) deck.add_resource("waste", waste, (50, 150, 0)) - + except ImportError: # 如果资源模块不可用,创建空的工作台 logger.warning("资源模块不可用,创建空的工作台") - + return deck __all__ = [ "LaiYuLiquid", - "LaiYuLiquidBackend", + "LaiYuLiquidBackend", "LaiYuLiquidConfig", "LaiYuLiquidDeck", "LaiYuLiquidContainer", "LaiYuLiquidTipRack", "LaiYuLiquidError", "create_quick_setup", - "get_module_info" -] \ No newline at end of file + "get_module_info", +] diff --git a/unilabos/devices/liquid_handling/liquid_handler_abstract.py b/unilabos/devices/liquid_handling/liquid_handler_abstract.py index 69b757b5..32e370fe 100644 --- a/unilabos/devices/liquid_handling/liquid_handler_abstract.py +++ b/unilabos/devices/liquid_handling/liquid_handler_abstract.py @@ -25,6 +25,8 @@ from pylabrobot.resources import ( Tip, ) +from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode + class LiquidHandlerMiddleware(LiquidHandler): def __init__(self, backend: LiquidHandlerBackend, deck: Deck, simulator: bool = False, channel_num: int = 8): @@ -536,6 +538,7 @@ class LiquidHandlerMiddleware(LiquidHandler): class LiquidHandlerAbstract(LiquidHandlerMiddleware): """Extended LiquidHandler with additional operations.""" support_touch_tip = True + _ros_node: BaseROS2DeviceNode def __init__(self, backend: LiquidHandlerBackend, deck: Deck, simulator: bool=False, channel_num:int = 8): """Initialize a LiquidHandler. @@ -548,8 +551,11 @@ class LiquidHandlerAbstract(LiquidHandlerMiddleware): self.group_info = dict() super().__init__(backend, deck, simulator, channel_num) + def post_init(self, ros_node: BaseROS2DeviceNode): + self._ros_node = ros_node + @classmethod - def set_liquid(self, wells: list[Well], liquid_names: list[str], volumes: list[float]): + def set_liquid(cls, wells: list[Well], liquid_names: list[str], volumes: list[float]): """Set the liquid in a well.""" for well, liquid_name, volume in zip(wells, liquid_names, volumes): well.set_liquids([(liquid_name, volume)]) # type: ignore @@ -1081,7 +1087,7 @@ class LiquidHandlerAbstract(LiquidHandlerMiddleware): print(f"Waiting time: {msg}") print(f"Current time: {time.strftime('%H:%M:%S')}") print(f"Time to finish: {time.strftime('%H:%M:%S', time.localtime(time.time() + seconds))}") - await asyncio.sleep(seconds) + await self._ros_node.sleep(seconds) if msg: print(f"Done: {msg}") print(f"Current time: {time.strftime('%H:%M:%S')}") diff --git a/unilabos/devices/pump_and_valve/runze_async.py b/unilabos/devices/pump_and_valve/runze_async.py index 9b8d649e..7bc11155 100644 --- a/unilabos/devices/pump_and_valve/runze_async.py +++ b/unilabos/devices/pump_and_valve/runze_async.py @@ -8,6 +8,8 @@ import serial.tools.list_ports from serial import Serial from serial.serialutil import SerialException +from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode + class RunzeSyringePumpMode(Enum): Normal = 0 @@ -77,6 +79,8 @@ class RunzeSyringePumpInfo: class RunzeSyringePumpAsync: + _ros_node: BaseROS2DeviceNode + def __init__(self, port: str, address: str = "1", volume: float = 25000, mode: RunzeSyringePumpMode = None): self.port = port self.address = address @@ -102,6 +106,9 @@ class RunzeSyringePumpAsync: self._run_future: Optional[Future[Any]] = None self._run_lock = Lock() + def post_init(self, ros_node: BaseROS2DeviceNode): + self._ros_node = ros_node + def _adjust_total_steps(self): self.total_steps = 6000 if self.mode == RunzeSyringePumpMode.Normal else 48000 self.total_steps_vel = 48000 if self.mode == RunzeSyringePumpMode.AccuratePosVel else 6000 @@ -182,7 +189,7 @@ class RunzeSyringePumpAsync: try: await self._query(command) while True: - await asyncio.sleep(0.5) # Wait for 0.5 seconds before polling again + await self._ros_node.sleep(0.5) # Wait for 0.5 seconds before polling again status = await self.query_device_status() if status == '`': @@ -364,7 +371,7 @@ class RunzeSyringePumpAsync: if self._read_task: raise RunzeSyringePumpConnectionError - self._read_task = asyncio.create_task(self._read_loop()) + self._read_task = self._ros_node.create_task(self._read_loop()) try: await self.query_device_status() diff --git a/unilabos/devices/virtual/virtual_centrifuge.py b/unilabos/devices/virtual/virtual_centrifuge.py index 79f9dce0..afce45a9 100644 --- a/unilabos/devices/virtual/virtual_centrifuge.py +++ b/unilabos/devices/virtual/virtual_centrifuge.py @@ -3,9 +3,13 @@ import logging import time as time_module from typing import Dict, Any, Optional +from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode + class VirtualCentrifuge: """Virtual centrifuge device - 简化版,只保留核心功能""" + + _ros_node: BaseROS2DeviceNode def __init__(self, device_id: Optional[str] = None, config: Optional[Dict[str, Any]] = None, **kwargs): # 处理可能的不同调用方式 @@ -32,6 +36,9 @@ class VirtualCentrifuge: for key, value in kwargs.items(): if key not in skip_keys and not hasattr(self, key): setattr(self, key, value) + + def post_init(self, ros_node: BaseROS2DeviceNode): + self._ros_node = ros_node async def initialize(self) -> bool: """Initialize virtual centrifuge""" @@ -132,7 +139,7 @@ class VirtualCentrifuge: break # 每秒更新一次 - await asyncio.sleep(1.0) + await self._ros_node.sleep(1.0) # 离心完成 self.data.update({ diff --git a/unilabos/devices/virtual/virtual_column.py b/unilabos/devices/virtual/virtual_column.py index 892a320f..539f302a 100644 --- a/unilabos/devices/virtual/virtual_column.py +++ b/unilabos/devices/virtual/virtual_column.py @@ -2,9 +2,13 @@ import asyncio import logging from typing import Dict, Any, Optional +from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode + class VirtualColumn: """Virtual column device for RunColumn protocol 🏛️""" + _ros_node: BaseROS2DeviceNode + def __init__(self, device_id: str = None, config: Dict[str, Any] = None, **kwargs): # 处理可能的不同调用方式 if device_id is None and 'id' in kwargs: @@ -28,6 +32,9 @@ class VirtualColumn: print(f"🏛️ === 虚拟色谱柱 {self.device_id} 已创建 === ✨") print(f"📏 柱参数: 流速={self._max_flow_rate}mL/min | 长度={self._column_length}cm | 直径={self._column_diameter}cm 🔬") + def post_init(self, ros_node: BaseROS2DeviceNode): + self._ros_node = ros_node + async def initialize(self) -> bool: """Initialize virtual column 🚀""" self.logger.info(f"🔧 初始化虚拟色谱柱 {self.device_id} ✨") @@ -101,7 +108,7 @@ class VirtualColumn: step_time = separation_time / steps for i in range(steps): - await asyncio.sleep(step_time) + await self._ros_node.sleep(step_time) progress = (i + 1) / steps * 100 volume_processed = (i + 1) * 5.0 # 假设每步处理5mL diff --git a/unilabos/devices/virtual/virtual_filter.py b/unilabos/devices/virtual/virtual_filter.py index ffd8f549..98effc99 100644 --- a/unilabos/devices/virtual/virtual_filter.py +++ b/unilabos/devices/virtual/virtual_filter.py @@ -4,70 +4,76 @@ import time as time_module from typing import Dict, Any, Optional from unilabos.compile.utils.vessel_parser import get_vessel +from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode class VirtualFilter: """Virtual filter device - 完全按照 Filter.action 规范 🌊""" - + + _ros_node: BaseROS2DeviceNode + def __init__(self, device_id: Optional[str] = None, config: Optional[Dict[str, Any]] = None, **kwargs): - if device_id is None and 'id' in kwargs: - device_id = kwargs.pop('id') - if config is None and 'config' in kwargs: - config = kwargs.pop('config') - + if device_id is None and "id" in kwargs: + device_id = kwargs.pop("id") + if config is None and "config" in kwargs: + config = kwargs.pop("config") + self.device_id = device_id or "unknown_filter" self.config = config or {} self.logger = logging.getLogger(f"VirtualFilter.{self.device_id}") self.data = {} - + # 从config或kwargs中获取配置参数 - self.port = self.config.get('port') or kwargs.get('port', 'VIRTUAL') - self._max_temp = self.config.get('max_temp') or kwargs.get('max_temp', 100.0) - self._max_stir_speed = self.config.get('max_stir_speed') or kwargs.get('max_stir_speed', 1000.0) - self._max_volume = self.config.get('max_volume') or kwargs.get('max_volume', 500.0) - + self.port = self.config.get("port") or kwargs.get("port", "VIRTUAL") + self._max_temp = self.config.get("max_temp") or kwargs.get("max_temp", 100.0) + self._max_stir_speed = self.config.get("max_stir_speed") or kwargs.get("max_stir_speed", 1000.0) + self._max_volume = self.config.get("max_volume") or kwargs.get("max_volume", 500.0) + # 处理其他kwargs参数 - skip_keys = {'port', 'max_temp', 'max_stir_speed', 'max_volume'} + skip_keys = {"port", "max_temp", "max_stir_speed", "max_volume"} for key, value in kwargs.items(): if key not in skip_keys and not hasattr(self, key): setattr(self, key, value) - + + def post_init(self, ros_node: BaseROS2DeviceNode): + self._ros_node = ros_node + async def initialize(self) -> bool: """Initialize virtual filter 🚀""" self.logger.info(f"🔧 初始化虚拟过滤器 {self.device_id} ✨") - + # 按照 Filter.action 的 feedback 字段初始化 - self.data.update({ - "status": "Idle", - "progress": 0.0, # Filter.action feedback - "current_temp": 25.0, # Filter.action feedback - "filtered_volume": 0.0, # Filter.action feedback - "message": "Ready for filtration" - }) - + self.data.update( + { + "status": "Idle", + "progress": 0.0, # Filter.action feedback + "current_temp": 25.0, # Filter.action feedback + "filtered_volume": 0.0, # Filter.action feedback + "message": "Ready for filtration", + } + ) + self.logger.info(f"✅ 过滤器 {self.device_id} 初始化完成 🌊") return True - + async def cleanup(self) -> bool: """Cleanup virtual filter 🧹""" self.logger.info(f"🧹 清理虚拟过滤器 {self.device_id} 🔚") - - self.data.update({ - "status": "Offline" - }) - + + self.data.update({"status": "Offline"}) + self.logger.info(f"✅ 过滤器 {self.device_id} 清理完成 💤") return True - + async def filter( - self, + self, vessel: dict, filtrate_vessel: dict = {}, - stir: bool = False, - stir_speed: float = 300.0, - temp: float = 25.0, - continue_heatchill: bool = False, - volume: float = 0.0 + stir: bool = False, + stir_speed: float = 300.0, + temp: float = 25.0, + continue_heatchill: bool = False, + volume: float = 0.0, ) -> bool: """Execute filter action - 完全按照 Filter.action 参数 🌊""" vessel_id, _ = get_vessel(vessel) @@ -79,59 +85,52 @@ class VirtualFilter: temp = 25.0 # 0度自动设置为室温 self.logger.info(f"🌡️ 温度自动调整: {original_temp}°C → {temp}°C (室温) 🏠") elif temp < 4.0: - temp = 4.0 # 小于4度自动设置为4度 + temp = 4.0 # 小于4度自动设置为4度 self.logger.info(f"🌡️ 温度自动调整: {original_temp}°C → {temp}°C (最低温度) ❄️") - + self.logger.info(f"🌊 开始过滤操作: {vessel_id} → {filtrate_vessel_id} 🚰") self.logger.info(f" 🌪️ 搅拌: {stir} ({stir_speed} RPM)") self.logger.info(f" 🌡️ 温度: {temp}°C") self.logger.info(f" 💧 体积: {volume}mL") self.logger.info(f" 🔥 保持加热: {continue_heatchill}") - + # 验证参数 if temp > self._max_temp or temp < 4.0: error_msg = f"🌡️ 温度 {temp}°C 超出范围 (4-{self._max_temp}°C) ⚠️" self.logger.error(f"❌ {error_msg}") - self.data.update({ - "status": f"Error: 温度超出范围 ⚠️", - "message": error_msg - }) + self.data.update({"status": f"Error: 温度超出范围 ⚠️", "message": error_msg}) return False - + if stir and stir_speed > self._max_stir_speed: error_msg = f"🌪️ 搅拌速度 {stir_speed} RPM 超出范围 (0-{self._max_stir_speed} RPM) ⚠️" self.logger.error(f"❌ {error_msg}") - self.data.update({ - "status": f"Error: 搅拌速度超出范围 ⚠️", - "message": error_msg - }) + self.data.update({"status": f"Error: 搅拌速度超出范围 ⚠️", "message": error_msg}) return False - + if volume > self._max_volume: error_msg = f"💧 过滤体积 {volume} mL 超出范围 (0-{self._max_volume} mL) ⚠️" self.logger.error(f"❌ {error_msg}") - self.data.update({ - "status": f"Error", - "message": error_msg - }) + self.data.update({"status": f"Error", "message": error_msg}) return False - + # 开始过滤 filter_volume = volume if volume > 0 else 50.0 self.logger.info(f"🚀 开始过滤 {filter_volume}mL 液体 💧") - - self.data.update({ - "status": f"Running", - "current_temp": temp, - "filtered_volume": 0.0, - "progress": 0.0, - "message": f"🚀 Starting filtration: {vessel_id} → {filtrate_vessel_id}" - }) - + + self.data.update( + { + "status": f"Running", + "current_temp": temp, + "filtered_volume": 0.0, + "progress": 0.0, + "message": f"🚀 Starting filtration: {vessel_id} → {filtrate_vessel_id}", + } + ) + try: # 过滤过程 - 实时更新进度 start_time = time_module.time() - + # 根据体积和搅拌估算过滤时间 base_time = filter_volume / 5.0 # 5mL/s 基础速度 if stir: @@ -140,78 +139,79 @@ class VirtualFilter: if temp > 50.0: base_time *= 0.7 # 高温加速过滤 self.logger.info(f"🔥 高温加速过滤,预计时间减少30% ⚡") - + filter_time = max(base_time, 10.0) # 最少10秒 self.logger.info(f"⏱️ 预计过滤时间: {filter_time:.1f}秒 ⌛") - + while True: current_time = time_module.time() elapsed = current_time - start_time remaining = max(0, filter_time - elapsed) progress = min(100.0, (elapsed / filter_time) * 100) current_filtered = (progress / 100.0) * filter_volume - + # 更新状态 - 按照 Filter.action feedback 字段 status_msg = f"🌊 过滤中: {vessel}" if stir: status_msg += f" | 🌪️ 搅拌: {stir_speed} RPM" status_msg += f" | 🌡️ {temp}°C | 📊 {progress:.1f}% | 💧 已过滤: {current_filtered:.1f}mL" - - self.data.update({ - "progress": progress, # Filter.action feedback - "current_temp": temp, # Filter.action feedback - "filtered_volume": current_filtered, # Filter.action feedback - "status": "Running", - "message": f"🌊 Filtering: {progress:.1f}% complete, {current_filtered:.1f}mL filtered" - }) - + + self.data.update( + { + "progress": progress, # Filter.action feedback + "current_temp": temp, # Filter.action feedback + "filtered_volume": current_filtered, # Filter.action feedback + "status": "Running", + "message": f"🌊 Filtering: {progress:.1f}% complete, {current_filtered:.1f}mL filtered", + } + ) + # 进度日志(每25%打印一次) if progress >= 25 and progress % 25 < 1: self.logger.info(f"📊 过滤进度: {progress:.0f}% | 💧 {current_filtered:.1f}mL 完成 ✨") - + if remaining <= 0: break - - await asyncio.sleep(1.0) - + + await self._ros_node.sleep(1.0) + # 过滤完成 final_temp = temp if continue_heatchill else 25.0 final_status = f"✅ 过滤完成: {vessel} | 💧 {filter_volume}mL → {filtrate_vessel}" if continue_heatchill: final_status += " | 🔥 继续加热搅拌" self.logger.info(f"🔥 继续保持加热搅拌状态 🌪️") - - self.data.update({ - "status": final_status, - "progress": 100.0, # Filter.action feedback - "current_temp": final_temp, # Filter.action feedback - "filtered_volume": filter_volume, # Filter.action feedback - "message": f"✅ Filtration completed: {filter_volume}mL filtered from {vessel_id}" - }) - + + self.data.update( + { + "status": final_status, + "progress": 100.0, # Filter.action feedback + "current_temp": final_temp, # Filter.action feedback + "filtered_volume": filter_volume, # Filter.action feedback + "message": f"✅ Filtration completed: {filter_volume}mL filtered from {vessel_id}", + } + ) + self.logger.info(f"🎉 过滤完成! 💧 {filter_volume}mL 从 {vessel_id} 过滤到 {filtrate_vessel_id} ✨") self.logger.info(f"📊 最终状态: 温度 {final_temp}°C | 进度 100% | 体积 {filter_volume}mL 🏁") return True - + except Exception as e: error_msg = f"过滤过程中发生错误: {str(e)} 💥" self.logger.error(f"❌ {error_msg}") - self.data.update({ - "status": f"Error", - "message": f"❌ Filtration failed: {str(e)}" - }) + self.data.update({"status": f"Error", "message": f"❌ Filtration failed: {str(e)}"}) return False - + # === 核心状态属性 - 按照 Filter.action feedback 字段 === @property def status(self) -> str: return self.data.get("status", "❓ Unknown") - + @property def progress(self) -> float: """Filter.action feedback 字段 📊""" return self.data.get("progress", 0.0) - + @property def current_temp(self) -> float: """Filter.action feedback 字段 🌡️""" @@ -230,15 +230,15 @@ class VirtualFilter: @property def message(self) -> str: return self.data.get("message", "") - + @property def max_temp(self) -> float: return self._max_temp - + @property def max_stir_speed(self) -> float: return self._max_stir_speed - + @property def max_volume(self) -> float: - return self._max_volume \ No newline at end of file + return self._max_volume diff --git a/unilabos/devices/virtual/virtual_heatchill.py b/unilabos/devices/virtual/virtual_heatchill.py index 2f7e555b..29a9fd28 100644 --- a/unilabos/devices/virtual/virtual_heatchill.py +++ b/unilabos/devices/virtual/virtual_heatchill.py @@ -3,9 +3,13 @@ import logging import time as time_module # 重命名time模块,避免与参数冲突 from typing import Dict, Any +from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode + class VirtualHeatChill: """Virtual heat chill device for HeatChillProtocol testing 🌡️""" + _ros_node: BaseROS2DeviceNode + def __init__(self, device_id: str = None, config: Dict[str, Any] = None, **kwargs): # 处理可能的不同调用方式 if device_id is None and 'id' in kwargs: @@ -35,6 +39,9 @@ class VirtualHeatChill: print(f"🌡️ === 虚拟温控设备 {self.device_id} 已创建 === ✨") print(f"🔥 温度范围: {self._min_temp}°C ~ {self._max_temp}°C | 🌪️ 最大搅拌: {self._max_stir_speed} RPM") + def post_init(self, ros_node: BaseROS2DeviceNode): + self._ros_node = ros_node + async def initialize(self) -> bool: """Initialize virtual heat chill 🚀""" self.logger.info(f"🔧 初始化虚拟温控设备 {self.device_id} ✨") @@ -177,7 +184,7 @@ class VirtualHeatChill: break # 等待1秒后再次检查 - await asyncio.sleep(1.0) + await self._ros_node.sleep(1.0) # 操作完成 final_stir_info = f" | 🌪️ 搅拌: {stir_speed} RPM" if stir else "" diff --git a/unilabos/devices/virtual/virtual_rotavap.py b/unilabos/devices/virtual/virtual_rotavap.py index 23e24b7e..5e85d35c 100644 --- a/unilabos/devices/virtual/virtual_rotavap.py +++ b/unilabos/devices/virtual/virtual_rotavap.py @@ -3,13 +3,19 @@ import logging import time as time_module from typing import Dict, Any, Optional +from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode + + def debug_print(message): """调试输出 🔍""" print(f"🌪️ [ROTAVAP] {message}", flush=True) + class VirtualRotavap: """Virtual rotary evaporator device - 简化版,只保留核心功能 🌪️""" + _ros_node: BaseROS2DeviceNode + def __init__(self, device_id: Optional[str] = None, config: Optional[Dict[str, Any]] = None, **kwargs): # 处理可能的不同调用方式 if device_id is None and "id" in kwargs: @@ -38,56 +44,65 @@ class VirtualRotavap: print(f"🌪️ === 虚拟旋转蒸发仪 {self.device_id} 已创建 === ✨") print(f"🔥 温度范围: 10°C ~ {self._max_temp}°C | 🌀 转速范围: 10 ~ {self._max_rotation_speed} RPM") + def post_init(self, ros_node: BaseROS2DeviceNode): + self._ros_node = ros_node + async def initialize(self) -> bool: """Initialize virtual rotary evaporator 🚀""" self.logger.info(f"🔧 初始化虚拟旋转蒸发仪 {self.device_id} ✨") - + # 只保留核心状态 - self.data.update({ - "status": "🏠 待机中", - "rotavap_state": "Ready", # Ready, Evaporating, Completed, Error - "current_temp": 25.0, - "target_temp": 25.0, - "rotation_speed": 0.0, - "vacuum_pressure": 1.0, # 大气压 - "evaporated_volume": 0.0, - "progress": 0.0, - "remaining_time": 0.0, - "message": "🌪️ Ready for evaporation" - }) - + self.data.update( + { + "status": "🏠 待机中", + "rotavap_state": "Ready", # Ready, Evaporating, Completed, Error + "current_temp": 25.0, + "target_temp": 25.0, + "rotation_speed": 0.0, + "vacuum_pressure": 1.0, # 大气压 + "evaporated_volume": 0.0, + "progress": 0.0, + "remaining_time": 0.0, + "message": "🌪️ Ready for evaporation", + } + ) + self.logger.info(f"✅ 旋转蒸发仪 {self.device_id} 初始化完成 🌪️") - self.logger.info(f"📊 设备规格: 温度范围 10°C ~ {self._max_temp}°C | 转速范围 10 ~ {self._max_rotation_speed} RPM") + self.logger.info( + f"📊 设备规格: 温度范围 10°C ~ {self._max_temp}°C | 转速范围 10 ~ {self._max_rotation_speed} RPM" + ) return True async def cleanup(self) -> bool: """Cleanup virtual rotary evaporator 🧹""" self.logger.info(f"🧹 清理虚拟旋转蒸发仪 {self.device_id} 🔚") - - self.data.update({ - "status": "💤 离线", - "rotavap_state": "Offline", - "current_temp": 25.0, - "rotation_speed": 0.0, - "vacuum_pressure": 1.0, - "message": "💤 System offline" - }) - + + self.data.update( + { + "status": "💤 离线", + "rotavap_state": "Offline", + "current_temp": 25.0, + "rotation_speed": 0.0, + "vacuum_pressure": 1.0, + "message": "💤 System offline", + } + ) + self.logger.info(f"✅ 旋转蒸发仪 {self.device_id} 清理完成 💤") return True async def evaporate( - self, - vessel: str, - pressure: float = 0.1, - temp: float = 60.0, + self, + vessel: str, + pressure: float = 0.1, + temp: float = 60.0, time: float = 180.0, stir_speed: float = 100.0, solvent: str = "", - **kwargs + **kwargs, ) -> bool: """Execute evaporate action - 简化版 🌪️""" - + # 🔧 新增:确保time参数是数值类型 if isinstance(time, str): try: @@ -98,31 +113,31 @@ class VirtualRotavap: elif not isinstance(time, (int, float)): self.logger.error(f"❌ 时间参数类型无效: {type(time)},使用默认值180.0秒") time = 180.0 - + # 确保time是float类型; 并加速 time = float(time) / 10.0 - + # 🔧 简化处理:如果vessel就是设备自己,直接操作 if vessel == self.device_id: debug_print(f"🎯 在设备 {self.device_id} 上直接执行蒸发操作") actual_vessel = self.device_id else: actual_vessel = vessel - + # 参数预处理 if solvent: self.logger.info(f"🧪 识别到溶剂: {solvent}") # 根据溶剂调整参数 solvent_lower = solvent.lower() - if any(s in solvent_lower for s in ['water', 'aqueous']): + if any(s in solvent_lower for s in ["water", "aqueous"]): temp = max(temp, 80.0) pressure = max(pressure, 0.2) self.logger.info(f"💧 水系溶剂:调整参数 → 温度 {temp}°C, 压力 {pressure} bar") - elif any(s in solvent_lower for s in ['ethanol', 'methanol', 'acetone']): + elif any(s in solvent_lower for s in ["ethanol", "methanol", "acetone"]): temp = min(temp, 50.0) pressure = min(pressure, 0.05) self.logger.info(f"⚡ 易挥发溶剂:调整参数 → 温度 {temp}°C, 压力 {pressure} bar") - + self.logger.info(f"🌪️ 开始蒸发操作: {actual_vessel}") self.logger.info(f" 🥽 容器: {actual_vessel}") self.logger.info(f" 🌡️ 温度: {temp}°C") @@ -131,126 +146,140 @@ class VirtualRotavap: self.logger.info(f" 🌀 转速: {stir_speed} RPM") if solvent: self.logger.info(f" 🧪 溶剂: {solvent}") - + # 验证参数 if temp > self._max_temp or temp < 10.0: error_msg = f"🌡️ 温度 {temp}°C 超出范围 (10-{self._max_temp}°C) ⚠️" self.logger.error(f"❌ {error_msg}") - self.data.update({ - "status": f"❌ 错误: 温度超出范围", - "rotavap_state": "Error", - "current_temp": 25.0, - "progress": 0.0, - "evaporated_volume": 0.0, - "message": error_msg - }) + self.data.update( + { + "status": f"❌ 错误: 温度超出范围", + "rotavap_state": "Error", + "current_temp": 25.0, + "progress": 0.0, + "evaporated_volume": 0.0, + "message": error_msg, + } + ) return False if stir_speed > self._max_rotation_speed or stir_speed < 10.0: error_msg = f"🌀 旋转速度 {stir_speed} RPM 超出范围 (10-{self._max_rotation_speed} RPM) ⚠️" self.logger.error(f"❌ {error_msg}") - self.data.update({ - "status": f"❌ 错误: 转速超出范围", - "rotavap_state": "Error", - "current_temp": 25.0, - "progress": 0.0, - "evaporated_volume": 0.0, - "message": error_msg - }) + self.data.update( + { + "status": f"❌ 错误: 转速超出范围", + "rotavap_state": "Error", + "current_temp": 25.0, + "progress": 0.0, + "evaporated_volume": 0.0, + "message": error_msg, + } + ) return False if pressure < 0.01 or pressure > 1.0: error_msg = f"💨 真空度 {pressure} bar 超出范围 (0.01-1.0 bar) ⚠️" self.logger.error(f"❌ {error_msg}") - self.data.update({ - "status": f"❌ 错误: 压力超出范围", - "rotavap_state": "Error", - "current_temp": 25.0, - "progress": 0.0, - "evaporated_volume": 0.0, - "message": error_msg - }) + self.data.update( + { + "status": f"❌ 错误: 压力超出范围", + "rotavap_state": "Error", + "current_temp": 25.0, + "progress": 0.0, + "evaporated_volume": 0.0, + "message": error_msg, + } + ) return False # 开始蒸发 - 🔧 现在time已经确保是float类型 self.logger.info(f"🚀 启动蒸发程序! 预计用时 {time/60:.1f}分钟 ⏱️") - - self.data.update({ - "status": f"🌪️ 蒸发中: {actual_vessel}", - "rotavap_state": "Evaporating", - "current_temp": temp, - "target_temp": temp, - "rotation_speed": stir_speed, - "vacuum_pressure": pressure, - "remaining_time": time, - "progress": 0.0, - "evaporated_volume": 0.0, - "message": f"🌪️ Evaporating {actual_vessel} at {temp}°C, {pressure} bar, {stir_speed} RPM" - }) + + self.data.update( + { + "status": f"🌪️ 蒸发中: {actual_vessel}", + "rotavap_state": "Evaporating", + "current_temp": temp, + "target_temp": temp, + "rotation_speed": stir_speed, + "vacuum_pressure": pressure, + "remaining_time": time, + "progress": 0.0, + "evaporated_volume": 0.0, + "message": f"🌪️ Evaporating {actual_vessel} at {temp}°C, {pressure} bar, {stir_speed} RPM", + } + ) try: # 蒸发过程 - 实时更新进度 start_time = time_module.time() total_time = time last_logged_progress = 0 - + while True: current_time = time_module.time() elapsed = current_time - start_time remaining = max(0, total_time - elapsed) progress = min(100.0, (elapsed / total_time) * 100) - + # 模拟蒸发体积 - 根据溶剂类型调整 - if solvent and any(s in solvent.lower() for s in ['water', 'aqueous']): + if solvent and any(s in solvent.lower() for s in ["water", "aqueous"]): evaporated_vol = progress * 0.6 # 水系溶剂蒸发慢 - elif solvent and any(s in solvent.lower() for s in ['ethanol', 'methanol', 'acetone']): + elif solvent and any(s in solvent.lower() for s in ["ethanol", "methanol", "acetone"]): evaporated_vol = progress * 1.0 # 易挥发溶剂蒸发快 else: evaporated_vol = progress * 0.8 # 默认蒸发量 - + # 🔧 更新状态 - 确保包含所有必需字段 status_msg = f"🌪️ 蒸发中: {actual_vessel} | 🌡️ {temp}°C | 💨 {pressure} bar | 🌀 {stir_speed} RPM | 📊 {progress:.1f}% | ⏰ 剩余: {remaining:.0f}s" - - self.data.update({ - "remaining_time": remaining, - "progress": progress, - "evaporated_volume": evaporated_vol, - "current_temp": temp, - "status": status_msg, - "message": f"🌪️ Evaporating: {progress:.1f}% complete, 💧 {evaporated_vol:.1f}mL evaporated, ⏰ {remaining:.0f}s remaining" - }) - + + self.data.update( + { + "remaining_time": remaining, + "progress": progress, + "evaporated_volume": evaporated_vol, + "current_temp": temp, + "status": status_msg, + "message": f"🌪️ Evaporating: {progress:.1f}% complete, 💧 {evaporated_vol:.1f}mL evaporated, ⏰ {remaining:.0f}s remaining", + } + ) + # 进度日志(每25%打印一次) if progress >= 25 and int(progress) % 25 == 0 and int(progress) != last_logged_progress: - self.logger.info(f"📊 蒸发进度: {progress:.0f}% | 💧 已蒸发: {evaporated_vol:.1f}mL | ⏰ 剩余: {remaining:.0f}s ✨") + self.logger.info( + f"📊 蒸发进度: {progress:.0f}% | 💧 已蒸发: {evaporated_vol:.1f}mL | ⏰ 剩余: {remaining:.0f}s ✨" + ) last_logged_progress = int(progress) - + # 时间到了,退出循环 if remaining <= 0: break - + # 每秒更新一次 - await asyncio.sleep(1.0) - + await self._ros_node.sleep(1.0) + # 蒸发完成 - if solvent and any(s in solvent.lower() for s in ['water', 'aqueous']): + if solvent and any(s in solvent.lower() for s in ["water", "aqueous"]): final_evaporated = 60.0 # 水系溶剂 - elif solvent and any(s in solvent.lower() for s in ['ethanol', 'methanol', 'acetone']): + elif solvent and any(s in solvent.lower() for s in ["ethanol", "methanol", "acetone"]): final_evaporated = 100.0 # 易挥发溶剂 else: final_evaporated = 80.0 # 默认 - - self.data.update({ - "status": f"✅ 蒸发完成: {actual_vessel} | 💧 蒸发量: {final_evaporated:.1f}mL", - "rotavap_state": "Completed", - "evaporated_volume": final_evaporated, - "progress": 100.0, - "current_temp": temp, - "remaining_time": 0.0, - "rotation_speed": 0.0, - "vacuum_pressure": 1.0, - "message": f"✅ Evaporation completed: {final_evaporated}mL evaporated from {actual_vessel}" - }) + + self.data.update( + { + "status": f"✅ 蒸发完成: {actual_vessel} | 💧 蒸发量: {final_evaporated:.1f}mL", + "rotavap_state": "Completed", + "evaporated_volume": final_evaporated, + "progress": 100.0, + "current_temp": temp, + "remaining_time": 0.0, + "rotation_speed": 0.0, + "vacuum_pressure": 1.0, + "message": f"✅ Evaporation completed: {final_evaporated}mL evaporated from {actual_vessel}", + } + ) self.logger.info(f"🎉 蒸发操作完成! ✨") self.logger.info(f"📊 蒸发结果:") @@ -262,24 +291,26 @@ class VirtualRotavap: self.logger.info(f" ⏱️ 总用时: {total_time:.0f}s") if solvent: self.logger.info(f" 🧪 处理溶剂: {solvent} 🏁") - + return True except Exception as e: # 出错处理 error_msg = f"蒸发过程中发生错误: {str(e)} 💥" self.logger.error(f"❌ {error_msg}") - - self.data.update({ - "status": f"❌ 蒸发错误: {str(e)}", - "rotavap_state": "Error", - "current_temp": 25.0, - "progress": 0.0, - "evaporated_volume": 0.0, - "rotation_speed": 0.0, - "vacuum_pressure": 1.0, - "message": f"❌ Evaporation failed: {str(e)}" - }) + + self.data.update( + { + "status": f"❌ 蒸发错误: {str(e)}", + "rotavap_state": "Error", + "current_temp": 25.0, + "progress": 0.0, + "evaporated_volume": 0.0, + "rotation_speed": 0.0, + "vacuum_pressure": 1.0, + "message": f"❌ Evaporation failed: {str(e)}", + } + ) return False # === 核心状态属性 === diff --git a/unilabos/devices/virtual/virtual_separator.py b/unilabos/devices/virtual/virtual_separator.py index e1c46128..0f266ce1 100644 --- a/unilabos/devices/virtual/virtual_separator.py +++ b/unilabos/devices/virtual/virtual_separator.py @@ -2,9 +2,13 @@ import asyncio import logging from typing import Dict, Any, Optional +from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode + class VirtualSeparator: """Virtual separator device for SeparateProtocol testing""" + + _ros_node: BaseROS2DeviceNode def __init__(self, device_id: Optional[str] = None, config: Optional[Dict[str, Any]] = None, **kwargs): # 处理可能的不同调用方式 @@ -35,6 +39,9 @@ class VirtualSeparator: for key, value in kwargs.items(): if key not in skip_keys and not hasattr(self, key): setattr(self, key, value) + + def post_init(self, ros_node: BaseROS2DeviceNode): + self._ros_node = ros_node async def initialize(self) -> bool: """Initialize virtual separator""" @@ -119,14 +126,14 @@ class VirtualSeparator: for repeat in range(repeats): # 搅拌阶段 for progress in range(0, 51, 10): - await asyncio.sleep(simulation_time / (repeats * 10)) + await self._ros_node.sleep(simulation_time / (repeats * 10)) overall_progress = ((repeat * 100) + (progress * 0.5)) / repeats self.data["progress"] = overall_progress self.data["message"] = f"第{repeat+1}次分离 - 搅拌中 ({progress}%)" # 静置分相阶段 for progress in range(50, 101, 10): - await asyncio.sleep(simulation_time / (repeats * 10)) + await self._ros_node.sleep(simulation_time / (repeats * 10)) overall_progress = ((repeat * 100) + (progress * 0.5)) / repeats self.data["progress"] = overall_progress self.data["message"] = f"第{repeat+1}次分离 - 静置分相中 ({progress}%)" diff --git a/unilabos/devices/virtual/virtual_solenoid_valve.py b/unilabos/devices/virtual/virtual_solenoid_valve.py index e0194248..26970cbe 100644 --- a/unilabos/devices/virtual/virtual_solenoid_valve.py +++ b/unilabos/devices/virtual/virtual_solenoid_valve.py @@ -2,11 +2,16 @@ import time import asyncio from typing import Union +from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode + class VirtualSolenoidValve: """ 虚拟电磁阀门 - 简单的开关型阀门,只有开启和关闭两个状态 """ + + _ros_node: BaseROS2DeviceNode + def __init__(self, device_id: str = None, config: dict = None, **kwargs): # 从配置中获取参数,提供默认值 if config is None: @@ -21,6 +26,9 @@ class VirtualSolenoidValve: self._status = "Idle" self._valve_state = "Closed" # "Open" or "Closed" self._is_open = False + + def post_init(self, ros_node: BaseROS2DeviceNode): + self._ros_node = ros_node async def initialize(self) -> bool: """初始化设备""" @@ -63,7 +71,7 @@ class VirtualSolenoidValve: self._status = "Busy" # 模拟阀门响应时间 - await asyncio.sleep(self.response_time) + await self._ros_node.sleep(self.response_time) # 处理不同的命令格式 if isinstance(command, str): diff --git a/unilabos/devices/virtual/virtual_solid_dispenser.py b/unilabos/devices/virtual/virtual_solid_dispenser.py index f8c14a75..63182616 100644 --- a/unilabos/devices/virtual/virtual_solid_dispenser.py +++ b/unilabos/devices/virtual/virtual_solid_dispenser.py @@ -3,6 +3,8 @@ import logging import re from typing import Dict, Any, Optional +from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode + class VirtualSolidDispenser: """ 虚拟固体粉末加样器 - 用于处理 Add Protocol 中的固体试剂添加 ⚗️ @@ -13,6 +15,8 @@ class VirtualSolidDispenser: - 简单反馈:成功/失败 + 消息 📊 """ + _ros_node: BaseROS2DeviceNode + def __init__(self, device_id: str = None, config: Dict[str, Any] = None, **kwargs): self.device_id = device_id or "virtual_solid_dispenser" self.config = config or {} @@ -32,6 +36,9 @@ class VirtualSolidDispenser: print(f"⚗️ === 虚拟固体分配器 {self.device_id} 创建成功! === ✨") print(f"📊 设备规格: 最大容量 {self.max_capacity}g | 精度 {self.precision}g 🎯") + def post_init(self, ros_node: BaseROS2DeviceNode): + self._ros_node = ros_node + async def initialize(self) -> bool: """初始化固体加样器 🚀""" self.logger.info(f"🔧 初始化固体分配器 {self.device_id} ✨") @@ -263,7 +270,7 @@ class VirtualSolidDispenser: for i in range(steps): progress = (i + 1) / steps * 100 - await asyncio.sleep(step_time) + await self._ros_node.sleep(step_time) if i % 2 == 0: # 每隔一步显示进度 self.logger.debug(f"📊 加样进度: {progress:.0f}% | {amount_emoji} 正在分配 {reagent}...") diff --git a/unilabos/devices/virtual/virtual_stirrer.py b/unilabos/devices/virtual/virtual_stirrer.py index cccf61ea..8e95617f 100644 --- a/unilabos/devices/virtual/virtual_stirrer.py +++ b/unilabos/devices/virtual/virtual_stirrer.py @@ -3,9 +3,13 @@ import logging import time as time_module from typing import Dict, Any +from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode + class VirtualStirrer: """Virtual stirrer device for StirProtocol testing - 功能完整版 🌪️""" + _ros_node: BaseROS2DeviceNode + def __init__(self, device_id: str = None, config: Dict[str, Any] = None, **kwargs): # 处理可能的不同调用方式 if device_id is None and 'id' in kwargs: @@ -34,6 +38,9 @@ class VirtualStirrer: print(f"🌪️ === 虚拟搅拌器 {self.device_id} 已创建 === ✨") print(f"🔧 速度范围: {self._min_speed} ~ {self._max_speed} RPM | 📱 端口: {self.port}") + def post_init(self, ros_node: BaseROS2DeviceNode): + self._ros_node = ros_node + async def initialize(self) -> bool: """Initialize virtual stirrer 🚀""" self.logger.info(f"🔧 初始化虚拟搅拌器 {self.device_id} ✨") @@ -134,7 +141,7 @@ class VirtualStirrer: if remaining <= 0: break - await asyncio.sleep(1.0) + await self._ros_node.sleep(1.0) self.logger.info(f"✅ 搅拌阶段完成! 🌪️ {stir_speed} RPM × {stir_time}s") @@ -176,7 +183,7 @@ class VirtualStirrer: if remaining <= 0: break - await asyncio.sleep(1.0) + await self._ros_node.sleep(1.0) self.logger.info(f"✅ 沉降阶段完成! 🛑 静置 {settling_time}s") diff --git a/unilabos/devices/virtual/virtual_transferpump.py b/unilabos/devices/virtual/virtual_transferpump.py index 1187db5f..7b8eea86 100644 --- a/unilabos/devices/virtual/virtual_transferpump.py +++ b/unilabos/devices/virtual/virtual_transferpump.py @@ -4,6 +4,8 @@ from enum import Enum from typing import Union, Optional import logging +from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode + class VirtualPumpMode(Enum): Normal = 0 @@ -14,6 +16,8 @@ class VirtualPumpMode(Enum): class VirtualTransferPump: """虚拟转移泵类 - 模拟泵的基本功能,无需实际硬件 🚰""" + _ros_node: BaseROS2DeviceNode + def __init__(self, device_id: str = None, config: dict = None, **kwargs): """ 初始化虚拟转移泵 @@ -53,6 +57,9 @@ class VirtualTransferPump: print(f"💨 快速模式: {'启用' if self._fast_mode else '禁用'} | 移动时间: {self._fast_move_time}s | 喷射时间: {self._fast_dispense_time}s") print(f"📊 最大容量: {self.max_volume}mL | 端口: {self.port}") + def post_init(self, ros_node: BaseROS2DeviceNode): + self._ros_node = ros_node + async def initialize(self) -> bool: """初始化虚拟泵 🚀""" self.logger.info(f"🔧 初始化虚拟转移泵 {self.device_id} ✨") @@ -104,7 +111,7 @@ class VirtualTransferPump: async def _simulate_operation(self, duration: float): """模拟操作延时 ⏱️""" self._status = "Busy" - await asyncio.sleep(duration) + await self._ros_node.sleep(duration) self._status = "Idle" def _calculate_duration(self, volume: float, velocity: float = None) -> float: @@ -223,7 +230,7 @@ class VirtualTransferPump: # 等待一小步时间 if i < steps and step_duration > 0: - await asyncio.sleep(step_duration) + await self._ros_node.sleep(step_duration) else: # 移动距离很小,直接完成 self._position = target_position @@ -341,7 +348,7 @@ class VirtualTransferPump: # 短暂停顿 self.logger.debug("⏸️ 短暂停顿...") - await asyncio.sleep(0.1) + await self._ros_node.sleep(0.1) # 排液 await self.dispense(volume, dispense_velocity) From 6d934e354cd6d47967da9f521e1a2ddccbd8f413 Mon Sep 17 00:00:00 2001 From: Xuwznln <18435084+Xuwznln@users.noreply.github.com> Date: Mon, 3 Nov 2025 16:31:37 +0800 Subject: [PATCH 04/13] adjust with_children param --- unilabos/ros/nodes/base_device_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/unilabos/ros/nodes/base_device_node.py b/unilabos/ros/nodes/base_device_node.py index 2fc7ea7d..19503fdc 100644 --- a/unilabos/ros/nodes/base_device_node.py +++ b/unilabos/ros/nodes/base_device_node.py @@ -656,7 +656,7 @@ class BaseROS2DeviceNode(Node, Generic[T]): ].call_async( SerialCommand.Request( command=json.dumps( - {"data": {"data": resources_uuid, "with_children": False}, "action": "get"} + {"data": {"data": resources_uuid, "with_children": True if action == "add" else "update"}, "action": "get"} ) ) ) # type: ignore From 86c7880b5c11b7d5c5052a38a3d9c6680ed10d04 Mon Sep 17 00:00:00 2001 From: Xuwznln <18435084+Xuwznln@users.noreply.github.com> Date: Fri, 7 Nov 2025 14:46:27 +0800 Subject: [PATCH 05/13] disable slave connect websocket --- unilabos/app/main.py | 25 +++++++++++++------------ 1 file changed, 13 insertions(+), 12 deletions(-) diff --git a/unilabos/app/main.py b/unilabos/app/main.py index c646518f..db15e2c6 100644 --- a/unilabos/app/main.py +++ b/unilabos/app/main.py @@ -375,22 +375,23 @@ def main(): args_dict["bridges"] = [] - # 获取通信客户端(仅支持WebSocket) - comm_client = get_communication_client() - - if "websocket" in args_dict["app_bridges"]: - args_dict["bridges"].append(comm_client) if "fastapi" in args_dict["app_bridges"]: args_dict["bridges"].append(http_client) - if "websocket" in args_dict["app_bridges"]: + # 获取通信客户端(仅支持WebSocket) + if BasicConfig.is_host_mode: + comm_client = get_communication_client() + if "websocket" in args_dict["app_bridges"]: + args_dict["bridges"].append(comm_client) + def _exit(signum, frame): + comm_client.stop() + sys.exit(0) - def _exit(signum, frame): - comm_client.stop() - sys.exit(0) + signal.signal(signal.SIGINT, _exit) + signal.signal(signal.SIGTERM, _exit) + comm_client.start() + else: + print_status("SlaveMode跳过Websocket连接") - signal.signal(signal.SIGINT, _exit) - signal.signal(signal.SIGTERM, _exit) - comm_client.start() args_dict["resources_mesh_config"] = {} args_dict["resources_edge_config"] = resource_edge_info # web visiualize 2D From 83854a741df4befebae46029a38c95690545f901 Mon Sep 17 00:00:00 2001 From: Xuwznln <18435084+Xuwznln@users.noreply.github.com> Date: Fri, 7 Nov 2025 14:52:09 +0800 Subject: [PATCH 06/13] correct remove_resource stats --- unilabos/ros/nodes/resource_tracker.py | 41 ++++++++++++++++++++------ 1 file changed, 32 insertions(+), 9 deletions(-) diff --git a/unilabos/ros/nodes/resource_tracker.py b/unilabos/ros/nodes/resource_tracker.py index c958fe7b..fc3ca7a0 100644 --- a/unilabos/ros/nodes/resource_tracker.py +++ b/unilabos/ros/nodes/resource_tracker.py @@ -2,7 +2,7 @@ import traceback import uuid from pydantic import BaseModel, field_serializer, field_validator from pydantic import Field -from typing import List, Tuple, Any, Dict, Literal, Optional, cast, TYPE_CHECKING +from typing import List, Tuple, Any, Dict, Literal, Optional, cast, TYPE_CHECKING, Union from unilabos.utils.log import logger @@ -927,7 +927,7 @@ class DeviceNodeResourceTracker(object): """ 递归遍历资源树,更新所有节点的uuid - Args:0 + Args: resource: 资源对象(可以是dict或实例) uuid_map: uuid映射字典,{old_uuid: new_uuid} @@ -952,6 +952,27 @@ class DeviceNodeResourceTracker(object): return self._traverse_and_process(resource, process) + def loop_gather_uuid(self, resource) -> List[str]: + """ + 递归遍历资源树,收集所有节点的uuid + + Args: + resource: 资源对象(可以是dict或实例) + + Returns: + 收集到的uuid列表 + """ + uuid_list = [] + + def process(res): + current_uuid = self._get_resource_attr(res, "uuid", "unilabos_uuid") + if current_uuid: + uuid_list.append(current_uuid) + return 0 + + self._traverse_and_process(resource, process) + return uuid_list + def _collect_uuid_mapping(self, resource): """ 递归收集资源的 uuid 映射到 uuid_to_resources @@ -972,7 +993,7 @@ class DeviceNodeResourceTracker(object): self._traverse_and_process(resource, process) - def _remove_uuid_mapping(self, resource): + def _remove_uuid_mapping(self, resource) -> int: """ 递归清除资源的 uuid 映射 @@ -985,9 +1006,10 @@ class DeviceNodeResourceTracker(object): if current_uuid and current_uuid in self.uuid_to_resources: self.uuid_to_resources.pop(current_uuid) logger.debug(f"移除资源UUID映射: {current_uuid} -> {res}") + return 1 return 0 - self._traverse_and_process(resource, process) + return self._traverse_and_process(resource, process) def parent_resource(self, resource): if id(resource) in self.resource2parent_resource: @@ -1042,13 +1064,12 @@ class DeviceNodeResourceTracker(object): removed = True break - if not removed: + # 递归清除uuid映射 + count = self._remove_uuid_mapping(resource) + if not count: logger.warning(f"尝试移除不存在的资源: {resource}") return False - # 递归清除uuid映射 - self._remove_uuid_mapping(resource) - # 清除 resource2parent_resource 中与该资源相关的映射 # 需要清除:1) 该资源作为 key 的映射 2) 该资源作为 value 的映射 keys_to_remove = [] @@ -1071,7 +1092,9 @@ class DeviceNodeResourceTracker(object): self.uuid_to_resources.clear() self.resource2parent_resource.clear() - def figure_resource(self, query_resource, try_mode=False): + def figure_resource( + self, query_resource: Union[List[Union[dict, "PLRResource"]], dict, "PLRResource"], try_mode=False + ) -> Union[List[Union[dict, "PLRResource", List[Union[dict, "PLRResource"]]]], dict, "PLRResource"]: if isinstance(query_resource, list): return [self.figure_resource(r, try_mode) for r in query_resource] elif ( From baa107c2300579a1f64aa678b47f9e68b0845486 Mon Sep 17 00:00:00 2001 From: Xuwznln <18435084+Xuwznln@users.noreply.github.com> Date: Fri, 7 Nov 2025 15:04:53 +0800 Subject: [PATCH 07/13] change uuid logger to trace level --- .../devices/liquid_handling/liquid_handler_abstract.py | 10 +++++----- unilabos/ros/nodes/resource_tracker.py | 8 +++++--- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/unilabos/devices/liquid_handling/liquid_handler_abstract.py b/unilabos/devices/liquid_handling/liquid_handler_abstract.py index 32e370fe..6aefaf31 100644 --- a/unilabos/devices/liquid_handling/liquid_handler_abstract.py +++ b/unilabos/devices/liquid_handling/liquid_handler_abstract.py @@ -1,11 +1,11 @@ from __future__ import annotations -import re -import traceback -from typing import List, Sequence, Optional, Literal, Union, Iterator, Dict, Any, Callable, Set, cast -from collections import Counter + import asyncio import time -import pprint as pp +import traceback +from collections import Counter +from typing import List, Sequence, Optional, Literal, Union, Iterator, Dict, Any, Callable, Set, cast + from pylabrobot.liquid_handling import LiquidHandler, LiquidHandlerBackend, LiquidHandlerChatterboxBackend, Strictness from pylabrobot.liquid_handling.liquid_handler import TipPresenceProbingMethod from pylabrobot.liquid_handling.standard import GripDirection diff --git a/unilabos/ros/nodes/resource_tracker.py b/unilabos/ros/nodes/resource_tracker.py index fc3ca7a0..085ce028 100644 --- a/unilabos/ros/nodes/resource_tracker.py +++ b/unilabos/ros/nodes/resource_tracker.py @@ -894,7 +894,7 @@ class DeviceNodeResourceTracker(object): new_uuid = name_to_uuid_map[resource_name] self.set_resource_uuid(res, new_uuid) self.uuid_to_resources[new_uuid] = res - logger.debug(f"设置资源UUID: {resource_name} -> {new_uuid}") + logger.trace(f"设置资源UUID: {resource_name} -> {new_uuid}") return 1 return 0 @@ -917,7 +917,8 @@ class DeviceNodeResourceTracker(object): if resource_name and resource_name in name_to_extra_map: extra = name_to_extra_map[resource_name] self.set_resource_extra(res, extra) - logger.debug(f"设置资源Extra: {resource_name} -> {extra}") + if len(extra): + logger.debug(f"设置资源Extra: {resource_name} -> {extra}") return 1 return 0 @@ -986,9 +987,10 @@ class DeviceNodeResourceTracker(object): if current_uuid: old = self.uuid_to_resources.get(current_uuid) self.uuid_to_resources[current_uuid] = res - logger.debug( + logger.trace( f"收集资源UUID映射: {current_uuid} -> {res} {'' if old is None else f'(覆盖旧值: {old})'}" ) + return 1 return 0 self._traverse_and_process(resource, process) From af2fb7f34a65eed7fe68c8e01c6cde56b6559c52 Mon Sep 17 00:00:00 2001 From: Xuwznln <18435084+Xuwznln@users.noreply.github.com> Date: Fri, 7 Nov 2025 16:11:27 +0800 Subject: [PATCH 08/13] enable slave mode --- unilabos/app/backend.py | 4 +- unilabos/app/register.py | 5 +- unilabos/ros/main_slave_run.py | 130 +++++++++++++----------- unilabos/ros/nodes/presets/host_node.py | 21 ++-- unilabos/ros/nodes/resource_tracker.py | 2 +- unilabos/utils/log.py | 3 +- 6 files changed, 90 insertions(+), 75 deletions(-) diff --git a/unilabos/app/backend.py b/unilabos/app/backend.py index d43b9544..b2bc0af2 100644 --- a/unilabos/app/backend.py +++ b/unilabos/app/backend.py @@ -13,7 +13,7 @@ def start_backend( graph=None, controllers_config: dict = {}, bridges=[], - without_host: bool = False, + is_slave: bool = False, visual: str = "None", resources_mesh_config: dict = {}, **kwargs, @@ -32,7 +32,7 @@ def start_backend( raise ValueError(f"Unsupported backend: {backend}") backend_thread = threading.Thread( - target=main if not without_host else slave, + target=main if not is_slave else slave, args=( devices_config, resources_config, diff --git a/unilabos/app/register.py b/unilabos/app/register.py index f456183d..633df98f 100644 --- a/unilabos/app/register.py +++ b/unilabos/app/register.py @@ -1,11 +1,12 @@ import json import time +from typing import Optional, Tuple, Dict, Any from unilabos.utils.log import logger from unilabos.utils.type_check import TypeEncoder -def register_devices_and_resources(lab_registry): +def register_devices_and_resources(lab_registry, gather_only=False) -> Optional[Tuple[Dict[str, Any], Dict[str, Any]]]: """ 注册设备和资源到服务器(仅支持HTTP) """ @@ -28,6 +29,8 @@ def register_devices_and_resources(lab_registry): resources_to_register[resource_info["id"]] = resource_info logger.debug(f"[UniLab Register] 收集资源: {resource_info['id']}") + if gather_only: + return devices_to_register, resources_to_register # 注册设备 if devices_to_register: try: diff --git a/unilabos/ros/main_slave_run.py b/unilabos/ros/main_slave_run.py index d9ad3682..1ded6da1 100644 --- a/unilabos/ros/main_slave_run.py +++ b/unilabos/ros/main_slave_run.py @@ -6,11 +6,12 @@ from typing import Optional, Dict, Any, List import rclpy from unilabos_msgs.srv._serial_command import SerialCommand_Response +from unilabos.app.register import register_devices_and_resources from unilabos.ros.nodes.presets.resource_mesh_manager import ResourceMeshManager from unilabos.ros.nodes.resource_tracker import DeviceNodeResourceTracker, ResourceTreeSet from unilabos.devices.ros_dev.liquid_handler_joint_publisher import LiquidHandlerJointPublisher from unilabos_msgs.srv import SerialCommand # type: ignore -from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor +from rclpy.executors import MultiThreadedExecutor from rclpy.node import Node from rclpy.timer import Timer @@ -108,66 +109,51 @@ def slave( rclpy_init_args: List[str] = ["--log-level", "debug"], ) -> None: """从节点函数""" + # 1. 初始化 ROS2 if not rclpy.ok(): rclpy.init(args=rclpy_init_args) executor = rclpy.__executor if not executor: executor = rclpy.__executor = MultiThreadedExecutor() - devices_instances = {} - for device_config in devices_config.root_nodes: - device_id = device_config.res_content.id - if device_config.res_content.type != "device": - d = initialize_device_from_dict(device_id, device_config.get_nested_dict()) - devices_instances[device_id] = d - # 默认初始化 - # if d is not None and isinstance(d, Node): - # executor.add_node(d) - # else: - # print(f"Warning: Device {device_id} could not be initialized or is not a valid Node") - n = Node(f"slaveMachine_{BasicConfig.machine_name}", parameter_overrides=[]) - executor.add_node(n) - - if visual != "disable": - from unilabos.ros.nodes.presets.joint_republisher import JointRepublisher - - resource_mesh_manager = ResourceMeshManager( - resources_mesh_config, - resources_config, # type: ignore FIXME - resource_tracker=DeviceNodeResourceTracker(), - device_id="resource_mesh_manager", - ) - joint_republisher = JointRepublisher("joint_republisher", DeviceNodeResourceTracker()) - - executor.add_node(resource_mesh_manager) - executor.add_node(joint_republisher) + # 1.5 启动 executor 线程 thread = threading.Thread(target=executor.spin, daemon=True, name="slave_executor_thread") thread.start() + # 2. 创建 Slave Machine Node + n = Node(f"slaveMachine_{BasicConfig.machine_name}", parameter_overrides=[]) + executor.add_node(n) + + # 3. 向 Host 报送节点信息和物料,获取 UUID 映射 + uuid_mapping = {} if not BasicConfig.slave_no_host: + # 3.1 报送节点信息 sclient = n.create_client(SerialCommand, "/node_info_update") sclient.wait_for_service() + registry_config = {} + devices_to_register, resources_to_register = register_devices_and_resources(lab_registry, True) + registry_config.update(devices_to_register) + registry_config.update(resources_to_register) request = SerialCommand.Request() request.command = json.dumps( { "machine_name": BasicConfig.machine_name, "type": "slave", "devices_config": devices_config.dump(), - "registry_config": lab_registry.obtain_registry_device_info(), + "registry_config": registry_config, }, ensure_ascii=False, cls=TypeEncoder, ) - response = sclient.call_async(request).result() + sclient.call_async(request).result() logger.info(f"Slave node info updated.") - # 使用新的 c2s_update_resource_tree 服务 - rclient = n.create_client(SerialCommand, "/c2s_update_resource_tree") - rclient.wait_for_service() - - # 序列化 ResourceTreeSet 为 JSON + # 3.2 报送物料树,获取 UUID 映射 if resources_config: + rclient = n.create_client(SerialCommand, "/c2s_update_resource_tree") + rclient.wait_for_service() + request = SerialCommand.Request() request.command = json.dumps( { @@ -180,35 +166,61 @@ def slave( }, ensure_ascii=False, ) - tree_response: SerialCommand_Response = rclient.call_async(request).result() + tree_response: SerialCommand_Response = rclient.call(request) uuid_mapping = json.loads(tree_response.response) - # 创建反向映射:new_uuid -> old_uuid - reverse_uuid_mapping = {new_uuid: old_uuid for old_uuid, new_uuid in uuid_mapping.items()} - for node in resources_config.root_nodes: - if node.res_content.type == "device": - for sub_node in node.children: - # 只有二级子设备 - if sub_node.res_content.type != "device": - device_tracker = devices_instances[node.res_content.id].resource_tracker - # sub_node.res_content.uuid 已经是新UUID,需要用旧UUID去查找 - old_uuid = reverse_uuid_mapping.get(sub_node.res_content.uuid) - if old_uuid: - # 找到旧UUID,使用UUID查找 - resource_instance = device_tracker.figure_resource({"uuid": old_uuid}) - else: - # 未找到旧UUID,使用name查找 - resource_instance = device_tracker.figure_resource({"name": sub_node.res_content.name}) - device_tracker.loop_update_uuid(resource_instance, uuid_mapping) + logger.info(f"Slave resource tree added. UUID mapping: {len(uuid_mapping)} nodes") + + # 3.3 使用 UUID 映射更新 resources_config 的 UUID(参考 client.py 逻辑) + old_uuids = {node.res_content.uuid: node for node in resources_config.all_nodes} + for old_uuid, node in old_uuids.items(): + if old_uuid in uuid_mapping: + new_uuid = uuid_mapping[old_uuid] + node.res_content.uuid = new_uuid + # 更新所有子节点的 parent_uuid + for child in node.children: + child.res_content.parent_uuid = new_uuid else: - logger.error("Slave模式不允许新增非设备节点下的物料") - continue - if tree_response: - logger.info(f"Slave resource tree added. Response: {tree_response.response}") - else: - logger.warning("Slave resource tree add response is None") + logger.warning(f"资源UUID未更新: {old_uuid}") else: logger.info("No resources to add.") + # 4. 初始化所有设备实例(此时 resources_config 的 UUID 已更新) + devices_instances = {} + for device_config in devices_config.root_nodes: + device_id = device_config.res_content.id + if device_config.res_content.type == "device": + d = initialize_device_from_dict(device_id, device_config.get_nested_dict()) + if d is not None: + devices_instances[device_id] = d + logger.info(f"Device {device_id} initialized.") + else: + logger.warning(f"Device {device_id} initialization failed.") + + # 5. 如果启用可视化,创建可视化相关节点 + if visual != "disable": + from unilabos.ros.nodes.presets.joint_republisher import JointRepublisher + + # 将 ResourceTreeSet 转换为 list 用于 visual 组件 + resources_list = ( + [node.res_content.model_dump(by_alias=True) for node in resources_config.all_nodes] + if resources_config + else [] + ) + resource_mesh_manager = ResourceMeshManager( + resources_mesh_config, + resources_list, + resource_tracker=DeviceNodeResourceTracker(), + device_id="resource_mesh_manager", + ) + joint_republisher = JointRepublisher("joint_republisher", DeviceNodeResourceTracker()) + lh_joint_pub = LiquidHandlerJointPublisher( + resources_config=resources_list, resource_tracker=DeviceNodeResourceTracker() + ) + executor.add_node(resource_mesh_manager) + executor.add_node(joint_republisher) + executor.add_node(lh_joint_pub) + + # 7. 保持运行 while True: time.sleep(1) diff --git a/unilabos/ros/nodes/presets/host_node.py b/unilabos/ros/nodes/presets/host_node.py index 346cf9c2..5c43a5d0 100644 --- a/unilabos/ros/nodes/presets/host_node.py +++ b/unilabos/ros/nodes/presets/host_node.py @@ -878,11 +878,10 @@ class HostNode(BaseROS2DeviceNode): success = False uuid_mapping = {} if len(self.bridges) > 0: - from unilabos.app.web.client import HTTPClient + from unilabos.app.web.client import HTTPClient, http_client - client: HTTPClient = self.bridges[-1] resource_start_time = time.time() - uuid_mapping = client.resource_tree_add(resource_tree_set, mount_uuid, first_add) + uuid_mapping = http_client.resource_tree_add(resource_tree_set, mount_uuid, first_add) success = True resource_end_time = time.time() self.lab_logger().info( @@ -990,9 +989,10 @@ class HostNode(BaseROS2DeviceNode): """ 更新节点信息回调 """ - self.lab_logger().info(f"[Host Node] Node info update request received: {request}") + # self.lab_logger().info(f"[Host Node] Node info update request received: {request}") try: from unilabos.app.communication import get_communication_client + from unilabos.app.web.client import HTTPClient, http_client info = json.loads(request.command) if "SYNC_SLAVE_NODE_INFO" in info: @@ -1001,10 +1001,10 @@ class HostNode(BaseROS2DeviceNode): edge_device_id = info["edge_device_id"] self.device_machine_names[edge_device_id] = machine_name else: - comm_client = get_communication_client() - registry_config = info["registry_config"] - for device_config in registry_config: - comm_client.publish_registry(device_config["id"], device_config) + devices_config = info.pop("devices_config") + registry_config = info.pop("registry_config") + if registry_config: + http_client.resource_registry({"resources": registry_config}) self.lab_logger().debug(f"[Host Node] Node info update: {info}") response.response = "OK" except Exception as e: @@ -1030,10 +1030,9 @@ class HostNode(BaseROS2DeviceNode): success = False if len(self.bridges) > 0: # 边的提交待定 - from unilabos.app.web.client import HTTPClient + from unilabos.app.web.client import HTTPClient, http_client - client: HTTPClient = self.bridges[-1] - r = client.resource_add(add_schema(resources)) + r = http_client.resource_add(add_schema(resources)) success = bool(r) response.success = success diff --git a/unilabos/ros/nodes/resource_tracker.py b/unilabos/ros/nodes/resource_tracker.py index 085ce028..ce23a5be 100644 --- a/unilabos/ros/nodes/resource_tracker.py +++ b/unilabos/ros/nodes/resource_tracker.py @@ -1007,7 +1007,7 @@ class DeviceNodeResourceTracker(object): current_uuid = self._get_resource_attr(res, "uuid", "unilabos_uuid") if current_uuid and current_uuid in self.uuid_to_resources: self.uuid_to_resources.pop(current_uuid) - logger.debug(f"移除资源UUID映射: {current_uuid} -> {res}") + logger.trace(f"移除资源UUID映射: {current_uuid} -> {res}") return 1 return 0 diff --git a/unilabos/utils/log.py b/unilabos/utils/log.py index 74442a62..3894233b 100644 --- a/unilabos/utils/log.py +++ b/unilabos/utils/log.py @@ -191,7 +191,8 @@ def configure_logger(loglevel=None): # 添加处理器到根日志记录器 root_logger.addHandler(console_handler) - + logging.getLogger("asyncio").setLevel(logging.INFO) + logging.getLogger("urllib3").setLevel(logging.INFO) # 配置日志系统 configure_logger() From 5f859917d48779b3cc83e7cc8331413267c0f726 Mon Sep 17 00:00:00 2001 From: Xuwznln <18435084+Xuwznln@users.noreply.github.com> Date: Fri, 31 Oct 2025 19:57:16 +0800 Subject: [PATCH 09/13] support name change during materials change --- unilabos/ros/nodes/base_device_node.py | 221 ++++++++++++++++++------- 1 file changed, 160 insertions(+), 61 deletions(-) diff --git a/unilabos/ros/nodes/base_device_node.py b/unilabos/ros/nodes/base_device_node.py index 19503fdc..febc95f2 100644 --- a/unilabos/ros/nodes/base_device_node.py +++ b/unilabos/ros/nodes/base_device_node.py @@ -638,6 +638,147 @@ class BaseROS2DeviceNode(Node, Generic[T]): - remove: 从资源树中移除资源 """ from pylabrobot.resources.resource import Resource as ResourcePLR + + def _handle_add( + plr_resources: List[ResourcePLR], tree_set: ResourceTreeSet, additional_add_params: Dict[str, Any] + ) -> Dict[str, Any]: + """ + 处理资源添加操作的内部函数 + + Args: + plr_resources: PLR资源列表 + tree_set: 资源树集合 + additional_add_params: 额外的添加参数 + + Returns: + 操作结果字典 + """ + for plr_resource, tree in zip(plr_resources, tree_set.trees): + self.resource_tracker.add_resource(plr_resource) + self.transfer_to_new_resource(plr_resource, tree, additional_add_params) + + func = getattr(self.driver_instance, "resource_tree_add", None) + if callable(func): + func(plr_resources) + + return {"success": True, "action": "add"} + + def _handle_remove(resources_uuid: List[str]) -> Dict[str, Any]: + """ + 处理资源移除操作的内部函数 + + Args: + resources_uuid: 要移除的资源UUID列表 + + Returns: + 操作结果字典,包含移除的资源列表 + """ + found_resources: List[List[Union[ResourcePLR, dict]]] = self.resource_tracker.figure_resource( + [{"uuid": uid} for uid in resources_uuid], try_mode=True + ) + found_plr_resources = [] + other_plr_resources = [] + + for found_resource in found_resources: + for resource in found_resource: + if issubclass(resource.__class__, ResourcePLR): + found_plr_resources.append(resource) + else: + other_plr_resources.append(resource) + + # 调用driver的remove回调 + func = getattr(self.driver_instance, "resource_tree_remove", None) + if callable(func): + func(found_plr_resources) + + # 从parent卸载并从tracker移除 + for plr_resource in found_plr_resources: + if plr_resource.parent is not None: + plr_resource.parent.unassign_child_resource(plr_resource) + self.resource_tracker.remove_resource(plr_resource) + self.lab_logger().info(f"移除物料 {plr_resource} 及其子节点") + + for other_plr_resource in other_plr_resources: + self.resource_tracker.remove_resource(other_plr_resource) + self.lab_logger().info(f"移除物料 {other_plr_resource} 及其子节点") + + return { + "success": True, + "action": "remove", + "removed_plr": found_plr_resources, + "removed_other": other_plr_resources, + } + + def _handle_update( + plr_resources: List[ResourcePLR], tree_set: ResourceTreeSet, additional_add_params: Dict[str, Any] + ) -> Dict[str, Any]: + """ + 处理资源更新操作的内部函数 + + Args: + plr_resources: PLR资源列表(包含新状态) + tree_set: 资源树集合 + additional_add_params: 额外的参数 + + Returns: + 操作结果字典 + """ + for plr_resource, tree in zip(plr_resources, tree_set.trees): + states = plr_resource.serialize_all_state() + original_instance: ResourcePLR = self.resource_tracker.figure_resource( + {"uuid": tree.root_node.res_content.uuid}, try_mode=False + ) + + # Update操作中包含改名:需要先remove再add + if original_instance.name != plr_resource.name: + old_name = original_instance.name + new_name = plr_resource.name + self.lab_logger().info(f"物料改名操作:{old_name} -> {new_name}") + + # 收集所有相关的uuid(包括子节点) + all_uuids = self.resource_tracker.loop_gather_uuid(original_instance) + _handle_remove(all_uuids) + original_instance.name = new_name + _handle_add([original_instance], tree_set, additional_add_params) + + self.lab_logger().info(f"物料改名完成:{old_name} -> {new_name}") + continue + + # 常规更新:不涉及改名 + original_parent_resource = original_instance.parent + original_parent_resource_uuid = getattr(original_parent_resource, "unilabos_uuid", None) + target_parent_resource_uuid = tree.root_node.res_content.uuid_parent + + self.lab_logger().info( + f"物料{original_instance} 原始父节点{original_parent_resource_uuid} " + f"目标父节点{target_parent_resource_uuid} 更新" + ) + + # 更新extra + if getattr(plr_resource, "unilabos_extra", None) is not None: + original_instance.unilabos_extra = getattr(plr_resource, "unilabos_extra") # type: ignore # noqa: E501 + + # 如果父节点变化,需要重新挂载 + if ( + original_parent_resource_uuid != target_parent_resource_uuid + and original_parent_resource is not None + ): + self.transfer_to_new_resource(original_instance, tree, additional_add_params) + + # 加载状态 + original_instance.load_all_state(states) + child_count = len(original_instance.get_all_children()) + self.lab_logger().info( + f"更新了资源属性 {plr_resource}[{tree.root_node.res_content.uuid}] " f"及其子节点 {child_count} 个" + ) + + # 调用driver的update回调 + func = getattr(self.driver_instance, "resource_tree_update", None) + if callable(func): + func(plr_resources) + + return {"success": True, "action": "update"} + try: data = json.loads(req.command) results = [] @@ -664,68 +805,20 @@ class BaseROS2DeviceNode(Node, Generic[T]): tree_set = ResourceTreeSet.from_raw_list(raw_nodes) try: if action == "add": - # 添加资源到资源跟踪器 + if tree_set is None: + raise ValueError("tree_set不能为None") plr_resources = tree_set.to_plr_resources() - for plr_resource, tree in zip(plr_resources, tree_set.trees): - self.resource_tracker.add_resource(plr_resource) - self.transfer_to_new_resource(plr_resource, tree, additional_add_params) - func = getattr(self.driver_instance, "resource_tree_add", None) - if callable(func): - func(plr_resources) - results.append({"success": True, "action": "add"}) + result = _handle_add(plr_resources, tree_set, additional_add_params) + results.append(result) elif action == "update": - # 更新资源 + if tree_set is None: + raise ValueError("tree_set不能为None") plr_resources = tree_set.to_plr_resources() - for plr_resource, tree in zip(plr_resources, tree_set.trees): - states = plr_resource.serialize_all_state() - original_instance: ResourcePLR = self.resource_tracker.figure_resource( - {"uuid": tree.root_node.res_content.uuid}, try_mode=False - ) - original_parent_resource = original_instance.parent - original_parent_resource_uuid = getattr(original_parent_resource, "unilabos_uuid", None) - target_parent_resource_uuid = tree.root_node.res_content.uuid_parent - self.lab_logger().info( - f"物料{original_instance} 原始父节点{original_parent_resource_uuid} 目标父节点{target_parent_resource_uuid} 更新" - ) - # todo: 对extra进行update - if getattr(plr_resource, "unilabos_extra", None) is not None: - original_instance.unilabos_extra = getattr(plr_resource, "unilabos_extra") - if original_parent_resource_uuid != target_parent_resource_uuid and original_parent_resource is not None: - self.transfer_to_new_resource(original_instance, tree, additional_add_params) - original_instance.load_all_state(states) - self.lab_logger().info( - f"更新了资源属性 {plr_resource}[{tree.root_node.res_content.uuid}] 及其子节点 {len(original_instance.get_all_children())} 个" - ) - - func = getattr(self.driver_instance, "resource_tree_update", None) - if callable(func): - func(plr_resources) - results.append({"success": True, "action": "update"}) + result = _handle_update(plr_resources, tree_set, additional_add_params) + results.append(result) elif action == "remove": - # 移除资源 - found_resources: List[List[Union[ResourcePLR, dict]]] = self.resource_tracker.figure_resource( - [{"uuid": uid} for uid in resources_uuid], try_mode=True - ) - found_plr_resources = [] - other_plr_resources = [] - for found_resource in found_resources: - for resource in found_resource: - if issubclass(resource.__class__, ResourcePLR): - found_plr_resources.append(resource) - else: - other_plr_resources.append(resource) - func = getattr(self.driver_instance, "resource_tree_remove", None) - if callable(func): - func(found_plr_resources) - for plr_resource in found_plr_resources: - if plr_resource.parent is not None: - plr_resource.parent.unassign_child_resource(plr_resource) - self.resource_tracker.remove_resource(plr_resource) - self.lab_logger().info(f"移除物料 {plr_resource} 及其子节点") - for other_plr_resource in other_plr_resources: - self.resource_tracker.remove_resource(other_plr_resource) - self.lab_logger().info(f"移除物料 {other_plr_resource} 及其子节点") - results.append({"success": True, "action": "remove"}) + result = _handle_remove(resources_uuid) + results.append(result) except Exception as e: error_msg = f"Error processing {action} operation: {str(e)}" self.lab_logger().error(f"[Resource Tree Update] {error_msg}") @@ -1004,9 +1097,14 @@ class BaseROS2DeviceNode(Node, Generic[T]): # 通过资源跟踪器获取本地实例 final_resources = queried_resources if is_sequence else queried_resources[0] - final_resources = self.resource_tracker.figure_resource({"name": final_resources.name}, try_mode=False) if not is_sequence else [ - self.resource_tracker.figure_resource({"name": res.name}, try_mode=False) for res in queried_resources - ] + final_resources = ( + self.resource_tracker.figure_resource({"name": final_resources.name}, try_mode=False) + if not is_sequence + else [ + self.resource_tracker.figure_resource({"name": res.name}, try_mode=False) + for res in queried_resources + ] + ) action_kwargs[k] = final_resources except Exception as e: @@ -1227,6 +1325,7 @@ class BaseROS2DeviceNode(Node, Generic[T]): raise JsonCommandInitError( f"执行动作时JSON缺少function_name或function_args: {ex}\n原JSON: {string}\n{traceback.format_exc()}" ) + def _convert_resource_sync(self, resource_data: Dict[str, Any]): """同步转换资源数据为实例""" # 创建资源查询请求 From 8b5653d8013fc8b544263d05dd8317d5c5e4e5f3 Mon Sep 17 00:00:00 2001 From: Xuwznln <18435084+Xuwznln@users.noreply.github.com> Date: Fri, 31 Oct 2025 22:12:54 +0800 Subject: [PATCH 10/13] fix json dumps --- unilabos/ros/nodes/base_device_node.py | 10 +++----- unilabos/ros/nodes/presets/host_node.py | 33 ++++++++----------------- 2 files changed, 14 insertions(+), 29 deletions(-) diff --git a/unilabos/ros/nodes/base_device_node.py b/unilabos/ros/nodes/base_device_node.py index febc95f2..e52ea80d 100644 --- a/unilabos/ros/nodes/base_device_node.py +++ b/unilabos/ros/nodes/base_device_node.py @@ -705,8 +705,8 @@ class BaseROS2DeviceNode(Node, Generic[T]): return { "success": True, "action": "remove", - "removed_plr": found_plr_resources, - "removed_other": other_plr_resources, + # "removed_plr": found_plr_resources, + # "removed_other": other_plr_resources, } def _handle_update( @@ -736,13 +736,11 @@ class BaseROS2DeviceNode(Node, Generic[T]): self.lab_logger().info(f"物料改名操作:{old_name} -> {new_name}") # 收集所有相关的uuid(包括子节点) - all_uuids = self.resource_tracker.loop_gather_uuid(original_instance) - _handle_remove(all_uuids) + _handle_remove([original_instance.unilabos_uuid]) original_instance.name = new_name _handle_add([original_instance], tree_set, additional_add_params) self.lab_logger().info(f"物料改名完成:{old_name} -> {new_name}") - continue # 常规更新:不涉及改名 original_parent_resource = original_instance.parent @@ -827,7 +825,7 @@ class BaseROS2DeviceNode(Node, Generic[T]): # 返回处理结果 result_json = {"results": results, "total": len(data)} - res.response = json.dumps(result_json, ensure_ascii=False) + res.response = json.dumps(result_json, ensure_ascii=False, cls=TypeEncoder) self.lab_logger().info(f"[Resource Tree Update] Completed processing {len(data)} operations") except json.JSONDecodeError as e: diff --git a/unilabos/ros/nodes/presets/host_node.py b/unilabos/ros/nodes/presets/host_node.py index 5c43a5d0..7a8806d4 100644 --- a/unilabos/ros/nodes/presets/host_node.py +++ b/unilabos/ros/nodes/presets/host_node.py @@ -165,29 +165,16 @@ class HostNode(BaseROS2DeviceNode): # resources_config 的 root node 是 # # 创建反向映射:new_uuid -> old_uuid # reverse_uuid_mapping = {new_uuid: old_uuid for old_uuid, new_uuid in uuid_mapping.items()} - # for tree in resources_config.trees: - # node = tree.root_node - # if node.res_content.type == "device": - # if node.res_content.id == "host_node": - # continue - # # slave节点走c2s更新接口,拿到add自行update uuid - # device_tracker = self.devices_instances[node.res_content.id].resource_tracker - # old_uuid = reverse_uuid_mapping.get(node.res_content.uuid) - # if old_uuid: - # # 找到旧UUID,使用UUID查找 - # resource_instance = device_tracker.uuid_to_resources.get(old_uuid) - # else: - # # 未找到旧UUID,使用name查找 - # resource_instance = device_tracker.figure_resource( - # {"name": node.res_content.name} - # ) - # device_tracker.loop_update_uuid(resource_instance, uuid_mapping) - # else: - # try: - # for plr_resource in ResourceTreeSet([tree]).to_plr_resources(): - # self.resource_tracker.add_resource(plr_resource) - # except Exception as ex: - # self.lab_logger().warning("[Host Node-Resource] 根节点物料序列化失败!") + for tree in resources_config.trees: + node = tree.root_node + if node.res_content.type == "device": + continue + else: + try: + for plr_resource in ResourceTreeSet([tree]).to_plr_resources(): + self._resource_tracker.add_resource(plr_resource) + except Exception as ex: + self.lab_logger().warning(f"[Host Node-Resource] 根节点物料{tree}序列化失败!") except Exception as ex: logger.error(f"[Host Node-Resource] 添加物料出错!\n{traceback.format_exc()}") # 初始化Node基类,传递空参数覆盖列表 From 200105f647d9f7288fea7abda9160a34859e2bc8 Mon Sep 17 00:00:00 2001 From: dijkstra402 Date: Sat, 8 Nov 2025 13:35:47 +0800 Subject: [PATCH 11/13] Add debug prints to create_orders and add resource_tree_transfer method --- .../bioyond_cell/2025092701.xlsx | Bin 18159 -> 18162 bytes .../bioyond_cell/bioyond_cell_workstation.py | 71 +++++++++++++++++- .../workstation/bioyond_studio/config.py | 6 +- .../workstation/bioyond_studio/station.py | 13 +++- .../workstation/coin_cell_assembly.zip | Bin 19794 -> 0 bytes .../coin_cell_assembly/coin_cell_assembly.py | 6 +- .../coin_cell_assembly_1105.csv | 64 ++++++++++++++++ .../resources/bioyond/YB_bottle_carriers.py | 4 +- unilabos/resources/bioyond/YB_bottles.py | 2 +- 9 files changed, 152 insertions(+), 14 deletions(-) delete mode 100644 unilabos/devices/workstation/coin_cell_assembly.zip create mode 100644 unilabos/devices/workstation/coin_cell_assembly/coin_cell_assembly_1105.csv diff --git a/unilabos/devices/workstation/bioyond_studio/bioyond_cell/2025092701.xlsx b/unilabos/devices/workstation/bioyond_studio/bioyond_cell/2025092701.xlsx index 4147095ed2784e56ccc6f637aec02c2a575c2cc8..9d0696191d2c76ff1788e485ed89d498ebf0f503 100644 GIT binary patch delta 162 zcmaFg%lN66aYMfR)60|-knO!jk?+Pq!Wg^kg8@*izEW&=IL$k|HHLn=(6)7AJijMz78J`iDh1 z;pR`ab`58gojlpqhVjzm3qZ1dGOwEj9Znlh*CbziRF-lK9?54(+=)wR502!e# A2LJ#7 delta 163 zcmey=%lN*RaYMflRTwRyX$3mc=^A)Q1f1v3L1{W3cn{bW0vWQ8JIB{yRwg%Vq(;*ugIJDV~)qsgxNI*fjs zi}VkRa>6Z`Z08!zC^dPSs}1A%$(Mm-&13;L3&xL=J>6^>`zN=%*)d8?KIW#z7U#kM F1OQjPG1CA5 diff --git a/unilabos/devices/workstation/bioyond_studio/bioyond_cell/bioyond_cell_workstation.py b/unilabos/devices/workstation/bioyond_studio/bioyond_cell/bioyond_cell_workstation.py index ee825791..4b27572c 100644 --- a/unilabos/devices/workstation/bioyond_studio/bioyond_cell/bioyond_cell_workstation.py +++ b/unilabos/devices/workstation/bioyond_studio/bioyond_cell/bioyond_cell_workstation.py @@ -3,6 +3,7 @@ from cgi import print_arguments from doctest import debug from typing import Dict, Any, List, Optional import requests +from pylabrobot.resources.resource import Resource as ResourcePLR from pathlib import Path import pandas as pd import time @@ -254,7 +255,7 @@ class BioyondCellWorkstation(BioyondWorkstation): def auto_feeding4to3( self, # ★ 修改点:默认模板路径 - xlsx_path: Optional[str] = "C:/ML/GitHub/Uni-Lab-OS/unilabos/devices/workstation/bioyond_studio/bioyond_cell/material_template.xlsx", + xlsx_path: Optional[str] = "/Users/sml/work/Unilab/Uni-Lab-OS/unilabos/devices/workstation/bioyond_studio/bioyond_cell/material_template.xlsx", # ---------------- WH4 - 加样头面 (Z=1, 12个点位) ---------------- WH4_x1_y1_z1_1_materialName: str = "", WH4_x1_y1_z1_1_quantity: float = 0.0, WH4_x2_y1_z1_2_materialName: str = "", WH4_x2_y1_z1_2_quantity: float = 0.0, @@ -306,7 +307,7 @@ class BioyondCellWorkstation(BioyondWorkstation): # ---------- 模式 1: Excel 导入 ---------- if xlsx_path: - path = Path(xlsx_path) + path = Path(__file__).parent / Path(xlsx_path) if path.exists(): # ★ 修改点:路径存在才加载 try: df = pd.read_excel(path, sheet_name=0, header=None, engine="openpyxl") @@ -471,14 +472,23 @@ class BioyondCellWorkstation(BioyondWorkstation): - totalMass 自动计算为所有物料质量之和 - createTime 缺失或为空时自动填充为当前日期(YYYY/M/D) """ - path = Path(xlsx_path) + default_path = Path("/Users/sml/work/Unilab/Uni-Lab-OS/unilabos/devices/workstation/bioyond_studio/bioyond_cell/2025092701.xlsx") + path = Path(xlsx_path) if xlsx_path else default_path + print(f"[create_orders] 使用 Excel 路径: {path}") + if path != default_path: + print("[create_orders] 来源: 调用方传入自定义路径") + else: + print("[create_orders] 来源: 使用默认模板路径") + if not path.exists(): + print(f"[create_orders] ⚠️ Excel 文件不存在: {path}") raise FileNotFoundError(f"未找到 Excel 文件:{path}") try: df = pd.read_excel(path, sheet_name=0, engine="openpyxl") except Exception as e: raise RuntimeError(f"读取 Excel 失败:{e}") + print(f"[create_orders] Excel 读取成功,行数: {len(df)}, 列: {list(df.columns)}") # 列名容错:返回可选列名,找不到则返回 None def _pick(col_names: List[str]) -> Optional[str]: @@ -495,9 +505,20 @@ class BioyondCellWorkstation(BioyondWorkstation): col_pouch = _pick(["软包组装分液体积", "pouchCellInfo"]) col_cond = _pick(["电导测试分液体积", "conductivityInfo"]) col_cond_cnt = _pick(["电导测试分液瓶数", "conductivityBottleCount"]) + print("[create_orders] 列匹配结果:", { + "order_name": col_order_name, + "create_time": col_create_time, + "bottle_type": col_bottle_type, + "mix_time": col_mix_time, + "load": col_load, + "pouch": col_pouch, + "conductivity": col_cond, + "conductivity_bottle_count": col_cond_cnt, + }) # 物料列:所有以 (g) 结尾 material_cols = [c for c in df.columns if isinstance(c, str) and c.endswith("(g)")] + print(f"[create_orders] 识别到的物料列: {material_cols}") if not material_cols: raise KeyError("未发现任何以“(g)”结尾的物料列,请检查表头。") @@ -545,6 +566,9 @@ class BioyondCellWorkstation(BioyondWorkstation): if mass > 0: mats.append({"name": mcol.replace("(g)", ""), "mass": mass}) total_mass += mass + else: + if mass < 0: + print(f"[create_orders] 第 {idx+1} 行物料 {mcol} 数值为负数: {mass}") order_data = { "batchId": batch_id, @@ -559,11 +583,22 @@ class BioyondCellWorkstation(BioyondWorkstation): "materialInfos": mats, "totalMass": round(total_mass, 4) # 自动汇总 } + print(f"[create_orders] 第 {idx+1} 行解析结果: orderName={order_data['orderName']}, " + f"loadShedding={order_data['loadSheddingInfo']}, pouchCell={order_data['pouchCellInfo']}, " + f"conductivity={order_data['conductivityInfo']}, totalMass={order_data['totalMass']}, " + f"material_count={len(mats)}") + + if order_data["totalMass"] <= 0: + print(f"[create_orders] ⚠️ 第 {idx+1} 行总质量 <= 0,可能导致 LIMS 校验失败") + if not mats: + print(f"[create_orders] ⚠️ 第 {idx+1} 行未找到有效物料") + orders.append(order_data) + print(f"[create_orders] 即将提交订单数量: {len(orders)}") response = self._post_lims("/api/lims/order/orders", orders) - print(response) + print(f"[create_orders] 接口返回: {response}") # 等待任务报送成功 data_list = response.get("data", []) if data_list: @@ -1014,7 +1049,35 @@ class BioyondCellWorkstation(BioyondWorkstation): "create_result": create_result, "inbound_result": inbound_result, } + def resource_tree_transfer(self, old_parent: ResourcePLR, plr_resource: ResourcePLR, parent_resource: ResourcePLR): + # ROS2DeviceNode.run_async_func(self._ros_node.resource_tree_transfer, True, **{ + # "old_parent": old_parent, + # "plr_resource": plr_resource, + # "parent_resource": parent_resource, + # }) + print("resource_tree_transfer", plr_resource, parent_resource) + if hasattr(plr_resource, "unilabos_extra") and plr_resource.unilabos_extra: + if "update_resource_site" in plr_resource.unilabos_extra: + site = plr_resource.unilabos_extra["update_resource_site"] + plr_model = plr_resource.model + board_type = None + for key, (moudle_name,moudle_uuid) in MATERIAL_TYPE_MAPPINGS.items(): + if plr_model == moudle_name: + board_type = key + break + if board_type is None: + pass + bottle1 = plr_resource.children[0] + bottle_moudle = bottle1.model + bottle_type = None + for key, (moudle_name, moudle_uuid) in MATERIAL_TYPE_MAPPINGS.items(): + if bottle_moudle == moudle_name: + bottle_type = key + break + self.create_sample(plr_resource.name, board_type,bottle_type,site) + return + self.lab_logger().warning(f"无库位的上料,不处理,{plr_resource} 挂载到 {parent_resource}") def create_sample( self, diff --git a/unilabos/devices/workstation/bioyond_studio/config.py b/unilabos/devices/workstation/bioyond_studio/config.py index b594125b..adb2d720 100644 --- a/unilabos/devices/workstation/bioyond_studio/config.py +++ b/unilabos/devices/workstation/bioyond_studio/config.py @@ -8,8 +8,8 @@ import os # BioyondCellWorkstation 默认配置(包含所有必需参数) API_CONFIG = { # API 连接配置 - # "api_host": os.getenv("BIOYOND_API_HOST", "http://172.21.32.65:44389"),#实机 - "api_host": os.getenv("BIOYOND_API_HOST", "http://172.16.10.169:44388"),# 仿真机 + # "api_host": os.getenv("BIOYOND_API_HOST", "http://172.16.1.143:44389"),#实机 + "api_host": os.getenv("BIOYOND_API_HOST", "http://172.16.7.149:44388"),# 仿真机 "api_key": os.getenv("BIOYOND_API_KEY", "8A819E5C"), "timeout": int(os.getenv("BIOYOND_TIMEOUT", "30")), @@ -17,7 +17,7 @@ API_CONFIG = { "report_token": os.getenv("BIOYOND_REPORT_TOKEN", "CHANGE_ME_TOKEN"), # HTTP 服务配置 - "HTTP_host": os.getenv("BIOYOND_HTTP_HOST", "172.21.32.115"), # HTTP服务监听地址,监听计算机飞连ip地址 + "HTTP_host": os.getenv("BIOYOND_HTTP_HOST", "172.16.2.140"), # HTTP服务监听地址,监听计算机飞连ip地址 "HTTP_port": int(os.getenv("BIOYOND_HTTP_PORT", "8080")), "debug_mode": False,# 调试模式 } diff --git a/unilabos/devices/workstation/bioyond_studio/station.py b/unilabos/devices/workstation/bioyond_studio/station.py index d6fe4870..44f9cf19 100644 --- a/unilabos/devices/workstation/bioyond_studio/station.py +++ b/unilabos/devices/workstation/bioyond_studio/station.py @@ -177,7 +177,18 @@ class BioyondWorkstation(WorkstationBase): ROS2DeviceNode.run_async_func(self._ros_node.update_resource, True, **{ "resources": [self.deck] }) - + def resource_tree_transfer(self, old_parent: ResourcePLR, plr_resource: ResourcePLR, parent_resource: ResourcePLR): + # ROS2DeviceNode.run_async_func(self._ros_node.resource_tree_transfer, True, **{ + # "old_parent": old_parent, + # "plr_resource": plr_resource, + # "parent_resource": parent_resource, + # }) + print("resource_tree_transfer", plr_resource, parent_resource) + if hasattr(plr_resource, "unilabos_data") and plr_resource.unilabos_data: + if "update_resource_site" in plr_resource.unilabos_data: + site = plr_resource.unilabos_data["update_resource_site"] + return + self.lab_logger().warning(f"无库位的上料,不处理,{plr_resource} 挂载到 {parent_resource}") def transfer_resource_to_another(self, resource: List[ResourceSlot], mount_resource: List[ResourceSlot], sites: List[str], mount_device_id: DeviceSlot): ROS2DeviceNode.run_async_func(self._ros_node.transfer_resource_to_another, True, **{ "plr_resources": resource, diff --git a/unilabos/devices/workstation/coin_cell_assembly.zip b/unilabos/devices/workstation/coin_cell_assembly.zip deleted file mode 100644 index b95b7f4a3f731c9d33a00a977343af3c5962ded5..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 19794 zcmZsiLv$|;?51nmwr#t;wQbwBZQHhO+s3c$EpBZa^Ua+9IkT9_BFQG}H_4MI%7B8Q z0sU{Yh||^iKgs_%)c>N1gO$C3iJ7gffsu=gnVqq%7o+0;6hHx;RO;yHw%|Ae;{XAL zhX4W5{r?M$-CSKA>=_)ruJNs0w#A-&EItTBu^@2qOioGW zh|IYl%{#+!S+L`bX`=?)`PxC8c=Dhn0HMj{LJd!W7}_9Dk}zd*E{g$!1w_qx3SZ!V z36|?ylWWNs=w+AXjc@z;|895O_W9k`khsFSZ?0u7<*nU9*QcD4U#X-%k zVolzl@$}UqK3^=PUT2#sp8HWD-If|^t+Fg5j_yC8Eq>i5BJJIx9vjS4+UtS*o*~9a%NOlcXMX&^E$Ox= z7Fb7RSRng+y-C`qbg=w`&(&{$!;qOxE`=x^$I5vnD3F0NW}G~zSt5QjlB{3nEcBAJ z@-m%(r>OL}%IWac>SZY}9oxCa6j_9>3TwW|s8$CPTwTB`?Qzf%@vEOxZ}468b4?i_ z=fB*0kN@82^`ZH=uk~iwuxmeyCqmfYCo5pq9vLrI#6E-lc;Q!c*{V3 zOM(%UJt)4EkAnJ=HRZX5wX%TGKDlj3BpyYU5Q5txH4x7lB>GH2jS;S5QzK(KnPf?7 zO4kj&y_u)pl=Y`sTWBk^gAS69%~;*q{#UhsN+pC&Ixol9M4!Y9MwY{lP>|(5noi;=F7S9ektRt$W4GJMX+^V!r_4uJvR2xR}#FiWPeXrZh3~p%B|KkP?fcY z$g-&5hv=P*TDXj0HEUA2d3VQ=K)B3wS?iaKU3X^$9oAVY$;BLda-KE0MGbwFhu~^5 z1G0?M!0%87i;wMdB+*WPH$2`N9*Wr76Sj@^s^>jT>t-pBH^nLihH~WAtG$Z7_g3`DHqn z-w(wdGRGc$1^ym3hcz27r@`SQgxMk%j1}%yc1KWt88(Z6_EgCU2}LTCyG1oS!r!W1 z7or;V;NumLflF(^&cKMGvZ&9~>2#Xw_l%}LDYeQjw7Q_*w7_7u^JDMw;QBaET)EBn zMD7|?o#Qdj#^}4HtJ{V+4_evx6UWaAyWQt>Rq@d!GY2ck&QOn3PedxzGWXBp%H}dB zLtzUO^Qw%Ss;MDBr$9IGWNwOaDs#p2!k5zC<#RvP@0tUMQyr0G;B9ry@2R3V1X_Do zRe0z8Sk~uQ)8#&y|5fI(<@uvY5a>3*$=PU9i@oPByIgl(T1%ZJhI741f!H zLQS_5-fUulRBBhObMhB(dSF%-@y<(UTlH`3E#)d(|u$q@-I{ z(U|xcwUtjgt1H%89q!oguGF4$; z-CrAi5=x#x_QQkUM!~vrr`+dv(fj)i<=f9{_s6#W=Mp@TNfL}ffuo`!5bdufZ=cu6 z{`iAhac<=Iyp#_jbRtU`*(O9aab}UcS*x6|%s-dOv9vhBeND%1uj_>I4|bN4xMTE6 zf~}s@{*9cT><7VO>U(i3x>|t2m)H}r?0kN9`>Fh9g!5JB1X zfgUke`|s6azb6SUDimkoIwoylMshwX@-oUYI6(nhcazf6V(*hjXpJ#65iSl#B5>A- zZ@b*QpD`i z4&T}aeSx7XQ;*Mk@hfAY=lXnd8En)rUxwqZ0;RefGF`r-dEl3)4jUE?+;4$I*0p(o ztOAoLB-xQ%?=}xA31UXLOAf}RLL5iWPPS4ddFTIr{Ehy3opA@^nv`g?)*H|cMj){g z$|`0`Ys~~v0@&R_tF4XGHi|k?9|;wh4m^Gl?!pfW9chRV8gK6H6)8fY7athsy9|P$ zoA1Ut%0dv3@{?{#QbHz9XOz;mkn*E=?HL0Q8~t@na3T!-1{pi7YE5LY={MGO9nUN_ zsnB$Vzk1axFP3h&n`iHLT(DGK7n;g|Xh$x`UK*e`iS&Vf4~by^fFW^$G7%=F7!|&) z1|M8>)W_&STu*ykQrd4z43PNtc|C|fu6JS7AxYnkS*zjDA{2R{zOT8TnjbMIN>NBb zqi86pD)IEJWT~Qoix79Bl7k>AL^k8OR*o$tp0mDOz)0dJzJ)f@(}OR975*kAIikTG z-VPnaXE&a!l9SU-{v#cXkhG@x%2$B4k=l-@-7RusFJcg^C!3TPCQqP6UwX#6pv*#JPeAnal#n^1NEb84Bz@IgeYS< z7pg;#b{&SRPQ%M!%52WDB` zgMK=)OGLry7P%ng7OOvTNgCO4vTQL~-S1rFWR7jy{i%|E5O*crTJJebnJ;Fdw{hr! zFH?VDf1`7bL9-)bmZQ+hYyp+841=|U#XP7#DiftkiYm?>)I}AC$(bZ8^;f0DqsNC= zk~bGtwB#0aOSmIa5FPok*iai<;RMp7Ux+v&%nijkkX$?h>dEgN7OHad6mht#7%>_M z#w!m){Qwlc-Io=XTH?7S1qh}bQxi(D?Yr?qk4tfID_irH%osHFBBuF`xl)@x(+Tuu z38GJC!;^y_;u-Knf+1vghg+PVz!tCqMCi+`j;8XOQ$|$BVHs zXQu=XP8>`Y%UVe3WeoCoADn4kB7UNBEh5kNW#@4GqU@tiE%97vKC`}` z@@wVf^dL`EI(Q-Yp75}?;*pE_Rz3_{Jkd@<{P}RoMK7`;vJvgspa`9x=p9eORQHfe zMD%l=a?~u8l4vxEuH{Mx1o1*3Hb)}yc9N7&Y;_D>Nd9oPohjl?`rj$KRwU!W&Tyzj zxhkAWX=AG#8ULF$AmRf9nL_`SDs=aU?1J`5d2N8hq-=l9Nhh+hTW$DdYDm4Vy0B)I z#WXw6eg_~x;)K*%NO=;a9Y_RW3GG<+$pq1d4917*0*YeLgB57JGKbp*bbH{hJWR46 zVhP>afGKX?qkHpu>x1wD)L>UYjdh&ShHM8?EXOr{8=lV5S;Iy^5y2u1*0eQ#iZPf8Bd$S6Rijl&ugX3eu4X|3|L zMMxGyUl>^oi+D*MS!={<)}4P)k@j%tgu20uBT(cSb)kfN?}Q=;Mi7!t0fG(o8*f5s zs1w*J|82d(YRlrO#vg@WoNtOH0pXpQ3@5%kAkt(P1XSwZHyGR-oc(mD&M_nT3;{mv za-tbU94=;=7&ju54z!MX4=@e~pRTd2x-+#OcOKv}r-8yd?*smd(n$Ha6Sy=>+=wUIsIex@F3!^}bzt6ar%Lz3I;uCZXzJJlI{}a=F!5<% z3yPpEkhy$GAWcKGKa#EKq^m96Y;ukD^Payo!&hL7g$409lAFkLn@hlIfyM|LM^{|* zjm8joZ4Kphl^GRWZ_I>&wavmf)J-SLB76&D?5drT8v8IS#5i@LtQ8%QZRSRiZjECl04N z5$ucjzb{G;>I@T!hfS^g#IPsE6O2)t86C(k^)$2vjZo1VLU|mEA=$X4U^)k1Poshf z2O6j#yY@e@5ggMk4Q4D_+~Ot&L<`U2%H;>jEozRrzi+VGuwR^$3PxFHmGYD#2V?qS_i}H|2Ajk@ZT!%oY+u!`d>+_SW#Zr%lMGLef}{fD3kW-n`HC+X#ZLaj4g`%@h396p(c`lWc}) zV7Z`lNS=#0^Y_8QfY`fvC?N0L#S^52UQXQS7U&1wttD#=f<}2+or_bW`MS-(wL{yu z%dwqj>F6F#$37ACESt z@agF=%Pq9FRCbUu$Q*AP24n)YzTB~U?znRX)K{28_G zyYY{D2~^@yA$+?jMTm*r)}ai^U89i}m0td6!$&AWm7gLr--$6rFo3Gm!a3-K7$L@d_>*m@vRU?jXW7s8%J2NShJo) zVh@&!>t-dtlSBfLV&?Ve=7yR9fpge}`j-CtJbF`bbQv#P!Z-?drZc%0m%JRrHgG8? z(}9#E0s~$UeM~_q2RtUV($usZ6T;QGmyi8=Ka^FAMQ&ukuQ%=6u=eo*>0V5WRM>$L(KQVJS?_jDCT|Jo$anC zckRsH0D7a&N=CWg99pQlLMm1AEQ9IhTB>U_ATe!YCuDaE7ijX*Byl1f9A|R^+R1C2}sZoIM}X#=V~t zXpvBY4F=K(O|ZFE)x*=~$D`(}iyX~gn@hYv85%>K6JdTynQr(pdO(1G<+PdT@7B_* zIVg<$f(k2)6u`Y^NJm(246e^5A#t24mZm#;ZC5Kf;ut?En7)+i_PhnfLzeS)#YI|Y3rjfR z1>th&JrPdB=twzVl0x^2SPh0Ylu~7FLJxwBLsNjc@HnTDk+E+|Eh*vfHLxyOcq@$a~8&j91DZ`RU6M^u>)_fO(MP}O4P z1{QX6R};*Y$BYzRLJG*GY2C+h|3Cxf`5Cp9ra#1h$Xs4kYk?|0PHaMLO&aNPN05)Txx&WAq`(uzZj08vEbOVTY) z!C#7>_)ebYxah{ZdZOSIksn(*3z5}Lp>2@uA{uYJ@Y>-Kpg<|rE<&0pV1Vubh?f}| zArgPc`RmBs*gxDF!9YJ@qq}W7^jr65R-NQw1S`ertc!_mV7bi^aI<<5d8Y+um5<~d zCgKYCdZ%e0w7EhPi|||~C5cwXhsyriU*>7(EcHROX6h$IK@ALvFD0ATpjc|(04)AN z5yl|t+QRnMs?+-zL~nvelHM_TX4(3S8%~78n06-Tyo*4tN5SPSm-n%s9gjI-P|s~V4ZlFY@uQe9tgg}6 zhBG@vD-O~7an@A{Ii}EU$L-X!M_KQz8gTFa)5KxbMhxh%$}kZ}*E8WZ5NM$^QFY0G z)98%|S1y$2E+EuLzcNK=+>VG0YdA)K}hux|J3#$v4na`+$ z%(L*sM!2{|-~6hevBk2|ehRJAfjHkyxS>ya*k)y*R5Rui&}X75gf0pRCIcuT5JuhX zqs@C_{`lew2C>J{^%O&ZLMzWLN$6vTmumc>jXR67@90x`7d9l{#`(ca3L@He;LGM` z>i-3Oqo*9hSh}@QGK8G49(iG~8e@09L)>n!6y;+!cj!iBC5wsM0P>4dj>*K;d(VLH zY&qO~M@*;PUy)(b{1Ou{{zjcz7i!}YLWMg6EwQyESDNy?)Tt9Ku#j91_uwWcy_70= za>_6o56Zd7oC+1-;A?gl*~*w=3eX)|oMjo`kt3+!4OyRrODEe*_5s~ZD11d3#14}b z0};+C3`6fgzC_7_LLeYY|2*r!`B_gfzCD+-@FWeMFa8DGP?BbteKv}p7-Tcud)^NU~F<11;w zy!?hJx;@Z?z2Rz?W~0a{rd!mt^tZzhbWr9;5TqdY0;IB%xIH@L1NhF!B=pDW!&AEk zr^`YT*?u&ml%0(9_{LX3{&wvteuB5Bo44Di=f&;G&FvEZv-{)a8W)hU68`EoWBiZC zekbL8$^+i472!_&41bsZvo+rDlZ78&%a4nbFL$NqgNI!!m%;zT_`}4H3VtCNG^2=h zPicmxrgX9>Jw^ZyQ#g4PsQAH98pjduXOiF{o9pv9IZFa@+$^u0Y%{eCQE6*%KM0LI zCw!c4hOVolyc$Da(_S+e!a?gG5Jy($oNR;#WU{1V2LTE$f#FVgsnzvK684HiSZ(cR zf*>S`XHju88J9(Zc3cw~qw6hGorB;LN)Sx!DkeU}=O4s3O-^K&=&r{Fo8{ztqXSQXOU88`rJm0!DdL4cH7^d@!ZHgSO zR7z$G&z)Ojz%8nPR(uI)!v~^8CrfR?1cB#~EpsvXQbh zEjs#ubZex)#aP-o2^$-%&*Mto^30ZMnFMB8OLKKrc4c?g-UfUjjX)Lw}^MGc6v;g zDqY%4^?UTzhnzA~HqA@Y#EsB&Q|dhlj$9_qec~# zX2?^%x{T=C4C1qLn618iRab6p>deKFracdbdYvj1JuP}08$B+&oNd0N2>sCSy9mxc z$+MY>?As_xR8bNaJ{XZL-0;sVt~v6Na^0rm$&jeX*wzV+`}m!=!dUfR*Mk$cTF@9sll z9Y~WkCoh!B%*1h+Fg#UkdNsBBaa3um-r0Zv<5RR(sPU1+28=ajGMtvUnD5YMSQo%r z$%!b(uxU2@Y@JR*z0|Aw^4O-QA=y<-3C0WL@moY%s0eYD|7)%PQL_6!@AU(*-3^$4 z33_ao^;tBT3M%rE*s`GnWswI;fYT(pj8-LPa}bmpK<;s1bbV}^Ta8})aGdH2&8ll9IT;-PqdmE%`nycI2<@C{)s9>(mFi&fYJo~9 z#T9a+E1NcF)q_dp_fgUB-sXE6{QBifu;cVXW?;`TNnF zlupnQM|Ko2$8W5L`dSK0 zBBwiV=@9CFGKttwU$C^OU%k!O-V?0wbBaz!wT0SJMwRy0O{0V<1e->!hH)t+nt;bS zTnT-mDAbsy>Dt)GmEcmwNulo`n5wZyl%wgPDs;AlRHU81$XNZ4J!hm+lj|g9OA-vX zi~k$NHrGSw)wdM7J8~jXIJOUeQuhV&)O?CA8*~n8nsc}X3ve zqLk)cmD<2eYA033Swb4CVWKy*J!2Cgn-1(kLUbSujsd4I|NdE1r_WiyB*{j)gOp1G z;sWQ^Bsus=xf>-Vd*XoS3%Ox&iOw6AiZUWllG`2I-H)=@jQtj*zvnYvs$mp2kz{aT zC)Qv{JtJ$-o2hA5e3#A8uMO>vBLMGxZ<#J-Gb>`g!bKGnD-Z$uDk#t0Qud&leh2aH zP6NyUMvV=125DqFf8@F)%!*>gpw|(k$W(gNQ&1yvM_omQZI38zf9g6TQRx=?2F9$IK>}!02w9mi3Th4zn@kDJ%+EvQvWC;fuFHZ7>#w!a-NOqMtXKnEl-x;8Uiq3L= zeHP9%1_C_a1zXp`z7fU++2CIIv*DXCOF({3U;i}bM1(wYQVHh3`E>@A?jc?KOm)26 zK_}Tw@PS2;?dB?cToEr0s?HN|73g2$`+AkwvTTHDFxLJ0HqoN_G0Bc5wFX54Gmtho9HG@^1lWqv7rpoC04$xyf+g_48*wzE-Ob4})&g-p z6fDKtxq90W9dJ+mrE>vY{T7U&F;A!+tzq1L_m_~PoYC=o%ZT2`b4eP8q-S?jov+g8m`naCL7<{+5r6HMpCyX3o8A5X z6l^0B{&e{ncK)h)Wqd7MYb8*?m-l6d+&Nbz=G0eWDYA`%3$SN?8{~peR~h^W58Q79 zE_MO;v~4DwLdLzvUMnrp`6%@3y&!0LGPBI|^ zQfMMxG3vJ?j+Ys(_`Pf=J>IXjpUbeqUWrNENP(0dB9^?(Cg9CYx|21TOZ<+t<%ZoU7y6`$v=_#eO%v% z3i7rRPGVbNWETwrl3brjQGfSdezwMIxpB#}PL*LfCXzTQRx8q%=|RQYvlFh+Bw{a7 z3TQkjh6GZMMl)B}!e8PvV58o2A}~BfSZCw88{SXKfo}(<6-1yCoJfE$+q}n z2JHUS{RedNDGm4UJf7CHR(7{PP(3_eVvh2wOn1#jD>V_2zWtui%WOSifBC2^O3e{m_Q-Y?mer-~H#*?0!r= zeAyM@7H^5R}${ow&|eb?)Kc>LUrOIpBl%SZkUJ=c=DeSA`z zm`Z=C9Ajdt%k5EaTaE_lvW%!G=!+vJ_+g0H%NGi$0i_?79o(RP6!}jQWfr-en7`$v zxZ^}^6i_Oq6{?*m0V@oLn3p0oa3bd`cNK)y0+OLUI4J{@0YHDfOULq z>AVH&3fXBY*=ABw6P!r8+GNcf6PaARh76g)(rETEleQf#dFEDQE(cA{0%?yZ6nkpp zD+}r|50mjr)}qOOn-({Ss6ev#>3lH@k=i^Ree+iY${^YCeoZ>`{ZvV_Y$edmt0Sg( z{-I01iF1U6$;K|2o|mk&OhS~R$PX$26rPGWv3>w=A$qhx5b}RCY&N^mUx~D34K|xQ z=nI6^0&(~eH4^dM!XIKq4qj}M9d`0{^*AL6!0Nz^NU_-KCKY%eCvY^>itQ(hWCiT^ z`u^u+)-P9Y(mOz8;&E3kl3xEUWP+6>5($^ScM9rRu)6vx z%%Yt}edOE0?vfCj{dI1eR1ZRH(z+hJd+`}hE~XF~88MrulgB!VFCCJ;9QCTFsoi<_ zq0pwK<0&izFy!*Qpe_dq*p9k6JG&zj#ETqtk^gVz->*|X$op%=CrIS+iQ5sRKXM?` z_V08(oe}q(!DR;ik+K&;+Ff#U9g(*al$Mpl)zJV?xR9LmZiH|vnM4OdqNl)0ZnS$- zz`a z@I3;pO7Evy1?T1#-*bA?8Ng(Oge?dOB(`jOO_(mx(b}3LxkTj0S%Sj{c0QG^yk-mD zkxe`8iF&yESH(aZ?C{{`Ab(rAR4+An9u(Qy(nc_HKK-EIPDkA7pIhte8vG)Rw}Uv1 znMOODjUQL`My3#Ce<0Rn&-2=U5@7bS zo^MsS=zVomI_>)u%~SAIKK+h};kn&&xj?(`JxR^RFQ{ZALnMzj8mRvfQb62b2qET` zm9@sD{?i04R$C|9$oeY(o33GiYnP|px+g(+Z*{5p7qaE^ub2ro;dS$;`7b?o04wqE z_D#-RPByc|u&UcK=AYQ>#*owJy@u8$Y!I;E#Sz}1${uoFGlKP3(ICicsN&xar;UWJ zLF&`PzXJ9=hi%7QfN4TUM|GY24TRj#I&^9l79P0%e!sW0ZZ~s8Mpeyt-a0U^QtfN>T>>hF>S>;u_CE z06z!wn<`^C8ORy15(WX6O;^>=uQ|h7ZQWhR!NvPz@yoYG+ls-31cOgQKCaB9XZ;=X z3ClH$oyEUn1c+mXyjpr)$Q7xplS~jyyw;hSJN(2(va_|Iq7G*Gv)#B-)Q6#%@_B4h zO=5)Nqy`}KAIWlq9yrcEq<_kE^6d%Mgd@9ug39qp0g8AhyZe^E`w+TpSN&C@!<8Ii z9v)!iJA{b12Y=7;uLbi6;cE>u}jOgxW}BCcPPYN zP*)uG@}?aGTkgJXpr6kpdyhI~!*R~W=tef{{$zxrM~b4o&T&kW{+zm_CK8l1rjU$#(`eGkTTM~wDe)umeUde!O=Z}IbVd^#XMi~~xFk_LM1~WP1;`AM2!HU1 zO_Mis7Y^Uwk_mTVaR(-6d-v}eaHwF*iiPBO9A`9}H=+<`3go~c1)}l<;H7eDo z+eti&#jv3$y1$G2NJ9HVMkPcckNJzH6U#OtEBw+%G&*5%6)eE`LSOL9o7)|1X>C45Nwfu1?d_gWq5S(>^ZC+J#;;qT`Ihl3LV&uxgKSAC$#*2Qd^-R5 zS}>GAh(IA?lxW^5tQW_>`Ik{1T3s!hjnwbgPsj=~K}+$xWHXPFkRv@^g<q`j*qJ}@_Hdwie-wKi920A^DdG2peSMn=(XK~J(d$yK969r=!0BGHPI2C;2?!G+!VhTpt{=?o5nu06Yyrn5zHdUg0yzIrnPGh0_+T6PU=ooBxG!}B;1j>gRs={-ghduFip!6?k;@{8HL zXPyQ>-2GEnjK|;J>F=?v*5>PeDMJ2w7Zsoc1==3ewY*t>BS3FB{i@+M(5^pEV%g(C zuvd4lI z2a9honwowaTJ<-(%r%VI;+3dEr0R_xEf?ASl(5Fr2la zH2~56xaVy&-8cyt6`iga(CYC@RO8eq1dn_|r5VDKxWoB`%CDC(7ZLC3-M*Jj!Y)+c z{*0ozm39}j^UXCU6qFh-5uu#tWEZOFDN+t8Kw9AO{#l5l&5JAYl#~!jL@*fCwQw1B z;ouCTly43F6tjVm%G7|WuA*@J%tHQ?h9B=LIUJ8gl#RbIQwoxz9de#`2NHpnWbd8$ zH*l7NLU0zNMOpyi7HpVPj4x0llZ&$8jtBK+7kk}$%nEL!e%{*S#p_*fbPMXD=q;we zXvCd6<7+8&~;xnv9YfQiDhiE|hU1-mLlr`&f@t2f*`swcs6dUE zVh2&qRJ=8L%XwEk6-AXWYwgVg3>!*p1QLr>4}B^G$FJCHL& zb`IYuAKlvpaOk@V1Y5-g1>zs_EBGbVhFW}9#kX1eUmh_kUR_I+`fm`KEA)?-1OTGX z5GE2$7F?v6^V);hD2I!~Z4mz5$F3Tb)Z&yVe!+h&0zHWICCPrlw}7U@+yjJu=b5p~ z%ZAVS(J{HtN58(J9P*u`@sx?v_d#lq(3i)ZzX`3+##-M*nUo!rxcA->Dm+S(D;q+m z|LeW7aL0Cu_Pp%_flJ^Zs?D~tdF*Q?0NDjlxhOmvJ(<^Obb`*K0OXg19waFHciut;fGv&z{^?N2`+rCWj>>ctt6<_)Z_j9giEdr*I38>N>s%cSjH_1F&7FcBmvcpb1T0oFN zTN3^9??<=%g=JjEOyXpZMzRW?wcu9nM?fBig^bGn=s5(3Z#?i^=?^H7y}mv*x1CXInb z!|0faBMk)9h7xf5p30dOOyzxJ9rWzM@Hr;y-rjWMk1*JEwI8t$3v<#2qj)x%B)f5P zStDqC_Wh(~urG;G2HoJ4O)zR%2nk5@f6_3Nt8l;9`H=d(9tJ!Vjq^9V$}Efq=LS1O zr6=z^xuN-6qBXx?atl&3oveD#saFdL9l5VyD3}*qC3E2R zg}=;cIkPKnB4WESB-fEion}nKmk|rtghZJY(+AeZ=hj23{QlKoJN4^u)+!ck$&%%D zHqG<7m2eVX@;!{`=f_gf4_@{nj(E>2M<8Jj?_caX2r&oIfR0uQS#H$FtgSCxRsD8$ z^q;nTR58xA`Bj0NT}P;+3x!ebyk z!o|8}mR_7)G5y(z`O_Sa@6Y?H{n8w-74@Fy2AJwJya9IGw-jozNpo`mzTB6+yOovL$&cNf#~z~R&;e@c%PKM6#VNEx%4)<@ z&;f$+%}AhdX;kCs3<(LIi93kO&j+b_>1AVh{i>y8Na-^-GHO%M55Jd=d92O1M}6%Zfnfr!cx(Mo!I$5J=v*VwFwbI+q%q0y+>1 zGS=ZQQzp(!@8vE>z;P9gIp`$%Vo!Y5j7mbiNz$E|G*NY7!8cwz!qD0H&{okDb>pie z;<6E68%Qp>LgpV^-Jlf1W?#SzE8X=mHu1IDNEIWS6FDrDkk6!Aq{mn{j~`nWk&YTL zPUmrf={Fl%M>ELNMe=3}bYCwz^-gbFQvP9Hak3pZbr|#nznmy^ezeH=bsyEq&{R7IxOhpxzPwFLM-k=SS6hi-af$N@OpGnv zx47U66u4?g#O2->L_6Nibr2u?WBfx*A<-lP=Cg;bFx+WAiz7%wyxm>0+-7*8Yv2#P zt8GWfw!8&?H&~EdH>Q$7FWA-(;9?_?NzT_YU35rY)jhQFZ)vlNoP~@oIif_U`yZe1 z#bmWSE{b;k)b|3T>!a=j2ugTbB3Cj+XT5@L*cxXZN34|g0Q6B%(TR#*Abh=nFVarW z)r#eT9zz!bXiVM5e$@Fe?abh^K21+UMof(L8-K{o2ifu=FdDknBKT~~9QYFQC%JF6 z?3k$d`z_WR-smBeYTKcvK`k}Y39ij)p4x)m(gN;NS*`4xHHIHB5@_n&c-wr)9E!Dk zv*rQSDxWllvGM`ZvJ3?qD+_afo2~aci!%p4nF0K)58;+)&g`avq*}jf-Su+~oetjjr6;S!_mkK(pgjo)k>Z)-gzlLle-wuB|rMv}D# z{Z$74dcqLc;DyCMNaTV$6$&0MgREM6F1muAR^M;b)~P}CimmM*?7Gdjf#NmiKJm4? zJcamtF+vFW5+mL~CBnVv<%2_uA?;xl&trdZc$OFTK=R~yi)9xr0s;+D#^kP&)+@?X z&8-Zm*eCqk)!hT901wUWu2R1bu$srh24B9z4H+8yE&eYSo1417wrYZ`GDS*wg~4{t zct7}maTtmI3B|q0mPMmrC67l=cv$=W=9V-+=i0Yp+co$>*x|EZ)(n6&w}OoAazD3? z=`ht1mo{7rE=;^9LD5QBqRrtTj4j~6^0vX*tU8O+V+WGHF%-)S&)n5`+2_q6W5!Vu zsSfxLDWz$ozj>1*DP4^;)w-|4aQlG$3m<}uEirY<0(o2j1_V;8#)@_DMzn-L&bOnJ zVTHRtQ$5^6^fo_X9aHSm00BqfgLf9g{kZ&hQ5f!eu6&)LlFDvRW_0O1ax!(STXZOo z=F?}7D;L^o>8Zu)mXQl5zB?8S$GD_`+@F4L0*3bn@nD}*6bcn8!^^6eNOtMqoiU0m z4gdW>)P=2$1MZ4;Ip7=8rC}T*WtL%dlCrC39&vZxunl^bZSDGZ zl*1965&$UOzQNxiZI)_|vH5y)EZF?Z}uL z^4^+#HSQ8MrVKXiT22h4F%d&LYJ>3yKbnxzgJ$?D`gOu69gOtL%_{p_5Cd0j?z;rE zK~KKxQ%l8bg?|RO9fG$gun4#@c)1G1EYD1qpNS_R1%UwT?QabRJO>hA5*d=%Vw7oq zz`MIp!z|ka8^-o#A~LswrQ;~W_R7V;Y^faDF=60I2Qy5LHb%xa4$p034NTa^!rI2} zS#XSR&yBx{PL9vLX2#`!bJ+JU*zWAu{}+F4j{seto}OY`&N@qj_&vh(*}^!yYl&yEn~l~t;hKIA*Oie?;g)Y%zv5-HYduKgqcJ()3vb&< zwwoUc`#2Cz4gM|th#MAzq!w5#wgEBLTJKocN_DC*UNceb9vsfoY_^<W$2^9>MRpsGFUsG!St@ehEpNcRV6| zfcTkQtu2%)X`g~9yW7OIB=5yo%$Z038E7uT`I}jb1AkToMkyI=!5-z7X?HwQ_ z9HR0AO97JJ`<1eO`OFU<<2OfBb?)l6&(ZTfZZ|-FO2zQh2oWWc-?9BWSJZ`!Du*Ku zduQewT2gzSY@bJk%C@ISTOVbb75XU!_04&H*RX+%-8JY6zw)J!@rWnecCh;qwbZ(Q zkt&rIyQOzS0{)+boVL!NZ@c(p2;usmaZ~biD4|EMD{N0THrwl~5!6!(PaB^H)5uL~ zJnyFVV>_RRpoq(aX!sRq8q`fFL7-4~O!cNmL=4_6J8EFc1@W{D%9?i_Pk z8TB13CBbH}{_HWR3r5W=Lp1jjG&jX)A{KmT4IDQf94s&no0w7$jzZBg*Kid@d) zspgTXChQbTe}gp{lV?~=Yl#8NXEV{1x0{UG z%h{6D=#|Dc4VqZt(sk*j~@T_ z-A1aj^53B#n-_{4vVgQY@b$~UG&1%EDFmhWKuNeX&8DHEa?h3N6K2$=@(Hh0_f;1; zVMJK?P#a(l4{3~iSG{qXo`KCteO^=3*~4gA%?ZV0p$)HDphWskRjILOvRh)KD0Xp+ zHd%CQD7UTWgHB>$-e6E|SHV~cfVIKgJ#uwp664l2!3&d2-2oQjA$-k5-GyfXCb+$b zu~*hEZ|%a-^W{NfT(Y)vSJ;&ZzNPug_0~(i}G%ZaBP7w?%w7?yi}503@k5 zQ+5v-FYNoKIU*Hq%%dt7b$2bzHv$j0LH=Yn3zLnB;Yu8Bw~9Ik{A4MW)>4`cUDqlTt@1lSi2z(Y>QC6$R9X7bQf~IQ84rB(KoGeln)xF ziThy1E9wL;BpaC~rhjrp%8@z~;gdWZrg=PuPOG*?-H8gYhPnxap>vX~1V}ud@#f5u z4I`hkWS5yd{9csT<1%Euff5#Ea93aNUH!hR%NU_7v(prPRsWznRB(VOjX&3ej89k_yIS47YI)=!4)e14x+IczSd!kY(Kh~gn}laafIf-bVII`1b!?~|41%m2GD zT8<3(@0(RRbbPg`Ue!avT!ruF>M1#kcP`Rrqr+(LrcI0t{97Pe2lgr9ruY1U05Zntz3R*v!n+}`}CN3k;X z^iz^lT{xXNS(L4WTStis;AWpZZN}BD8EXrlq~4fnC@^?`By~4xq5U@VoXuQev5OTw znuoNV!>AEwnZ-Zzs1RTK+_gItY^>P%u&SakLT(e>9&BtJ2DQxA4JDKJJyrRCHF6$M zO(kmpjsYox^aX@~^db=i3lM_z-jO1RB3(j>XbeRGqf%C+gh(jDLu%+nKna0GlqyXS zfzYG~p+$-W1O;C3xx0AOw|nN?JN^Ie%$+kc=gdEU$61PeMm|IPY=rt0WSi%2MX)`a z0wXv`Lm4dq;KvF8$oyMHFh?g@m+Qy`tO5LuI#W2U4GdCg#PnV} zlGA1mPOWY&U*c2cPIJo9!AYoHxqTI_tGc>reO&D$}nZ^Nyf zZ7i*iy2Y63E!^^;{Yh(_nYHa{iI)M6oXH zbQYh4ufp%NUJ-1S%|lAK{i0ea zY}`sdUYYx{4KvLEt_ssC_DGYVTpwaAohXu@Jke+873+Cg;95)sR0k7eB*mmHuh3Lv zoy;}qp_kY1E3jf5l#&QXJ`r_#RNOmy(Tj|du@yDpJ$^-9Ao-WC zENR7Q!+??Qet)DS!A3DpeAZZq&*>1&Qi+M6J@QPp!JvKkgx;kx0>ZxN z0DY6*7jPOT;)^rRg;aJ~lx)nXN5KyRGOiru*pWd6o4;A08B96esgtVJFOTK#FUubt zD|WqrORsBT>GG3P9!&@q;z1G(w%obJO*!SGVNsF^b!&9lgf;hOyzsIsg~>)?JzmnI z634{PdUT{d2)~`L=BqH>nMG@E@!mk<5Y&zlvyFlum%ZqKN02Bp-RqyLhEexegt*M# z8$ll$O=LU9nzBx3*MX$Cic8iu2WInH1DUtwVJva&Gvo|2n~_jQHZ_huXT0w;|A_ z{s;&5;q=nF50CWplExywT!q#6kv5`jy`FtOU)YS3P3HDp>Aq_#$kkrsJP^FF)%z^K zBa>9wh#2XDZ8XvMZ3k9yvW}nEgSTx*MpKRpS<2pNm3IF4X0$U0p2k$Z0o>PB`xvJmZ-iT+Z>w=iT#x^WA9q+fJ{EvS>Y z-J4dAH=ZJX8BX(8A|G7ETtimG%*{|fKqp@7-dy$D^zjBsW2xeAl3TWUVr@z9pqD7k z%ZsmGJ)xAOV-m@s>!NPr=RJHV#aW`e2fm;9(sJfK0;m%ov-U4FV13>EzuIGQf&039 zdYp8Tgi-=_{>@h}G!Swt>GG)h4EmpA8M$|+uw8-wnwt?VRv_#YFk#c0g7 zK#xf}N@%m;1zEKQNMq7o$tpIQIOeO;fGj&+>;&R$=fK+L-1DioIht58JNJ%{*cj-YA8|c(slF%$zl;_wmThGe4w#%e9VT#D; z52QbwKLs`p2!;%BVhp-?kUUXV0o0kVB)wGa610Xw?&Vavq%_=Aw-J@-&y%=xE9A!Y zmC1EdVrhM7_4*vSsnqsAp+TQ_+TUy}SBU*5(r$6Zc@5{!dZ^6$BEH;yJvHX-$4;Ul z%sL3{Mja9k8l$zKkxcJwzQG;(<<++COd(en^q>x-*!mInbgHEYLMmqg%B*=}M6kP; zYhBH~>d9ag^~x>HMgqk)MAnLi2c}Xe!16JpChCcnN_q5btL_b(-I6%R#D)7g78(33KYX2y8jDuc6HR``VLw@X4;vtp zAwK4M{S-EEFJO4(_q}x0L6C{-pONBw5^wu^w~aFT*to_x#EVwFvJmM$pDc%_JHyGS zTUCd>9SHUzW5EzmkW@CqfUP@ed;EIN<3-7jxra1%qma5WXsRnfTxFUY-_?O=!LnVlvyKV$ zYC_(JqFEYrA-WJ)ug38XmU$dexGzZ{E6ubr3yw5b>txAjUgVua2nz&P+}-j-GMsjA z(Z?~(%{~(7Gpo?blULD&kaO-{ND>&KMg?4u50H?am_LXO+ox3e%Ciy3rR`qd7dO7nkJh~{Um+0&aNw$J7UmX^Ntp1 z8(wW(e68j)U;Hx7-t2XVxmveOV^z}qII#RXv&5aHK)d0V; zJSFx`4%;{XC$bI!guZ4T0Pq6(3E_H6dLswEmHx~78}c)2RewVEpFn bool: + def func_allpack_cmd(self, elec_num, elec_use_num, elec_vol:int=50, assembly_type:int=7, assembly_pressure:int=4200, file_path: str="/Users/sml/work") -> bool: elec_num, elec_use_num, elec_vol, assembly_type, assembly_pressure = int(elec_num), int(elec_use_num), int(elec_vol), int(assembly_type), int(assembly_pressure) summary_csv_file = os.path.join(file_path, "duandian.csv") # 如果断点文件存在,先读取之前的进度 diff --git a/unilabos/devices/workstation/coin_cell_assembly/coin_cell_assembly_1105.csv b/unilabos/devices/workstation/coin_cell_assembly/coin_cell_assembly_1105.csv new file mode 100644 index 00000000..3f7b357f --- /dev/null +++ b/unilabos/devices/workstation/coin_cell_assembly/coin_cell_assembly_1105.csv @@ -0,0 +1,64 @@ +Name,DataType,InitValue,Comment,Attribute,DeviceType,Address, +COIL_SYS_START_CMD,BOOL,,,,coil,9010, +COIL_SYS_STOP_CMD,BOOL,,,,coil,9020, +COIL_SYS_RESET_CMD,BOOL,,,,coil,9030, +COIL_SYS_HAND_CMD,BOOL,,,,coil,9040, +COIL_SYS_AUTO_CMD,BOOL,,,,coil,9050, +COIL_SYS_INIT_CMD,BOOL,,,,coil,9060, +COIL_UNILAB_SEND_MSG_SUCC_CMD,BOOL,,,,coil,9700, +COIL_UNILAB_REC_MSG_SUCC_CMD,BOOL,,,,coil,9710,unilab_rec_msg_succ_cmd +COIL_SYS_START_STATUS,BOOL,,,,coil,9210, +COIL_SYS_STOP_STATUS,BOOL,,,,coil,9220, +COIL_SYS_RESET_STATUS,BOOL,,,,coil,9230, +COIL_SYS_HAND_STATUS,BOOL,,,,coil,9240, +COIL_SYS_AUTO_STATUS,BOOL,,,,coil,9250, +COIL_SYS_INIT_STATUS,BOOL,,,,coil,9260, +COIL_REQUEST_REC_MSG_STATUS,BOOL,,,,coil,9500, +COIL_REQUEST_SEND_MSG_STATUS,BOOL,,,,coil,9510,request_send_msg_status +REG_MSG_ELECTROLYTE_USE_NUM,INT16,,,,hold_register,17000, +REG_MSG_ELECTROLYTE_NUM,INT16,,,,hold_register,17002,unilab_send_msg_electrolyte_num +REG_MSG_ELECTROLYTE_VOLUME,INT16,,,,hold_register,17004,unilab_send_msg_electrolyte_vol +REG_MSG_ASSEMBLY_TYPE,INT16,,,,hold_register,17006,unilab_send_msg_assembly_type +REG_MSG_ASSEMBLY_PRESSURE,INT16,,,,hold_register,17008,unilab_send_msg_assembly_pressure +REG_DATA_ASSEMBLY_COIN_CELL_NUM,INT16,,,,hold_register,16000,data_assembly_coin_cell_num +REG_DATA_OPEN_CIRCUIT_VOLTAGE,FLOAT32,,,,hold_register,16002,data_open_circuit_voltage +REG_DATA_AXIS_X_POS,FLOAT32,,,,hold_register,16004, +REG_DATA_AXIS_Y_POS,FLOAT32,,,,hold_register,16006, +REG_DATA_AXIS_Z_POS,FLOAT32,,,,hold_register,16008, +REG_DATA_POLE_WEIGHT,FLOAT32,,,,hold_register,16010,data_pole_weight +REG_DATA_ASSEMBLY_PER_TIME,FLOAT32,,,,hold_register,16012,data_assembly_time +REG_DATA_ASSEMBLY_PRESSURE,INT16,,,,hold_register,16014,data_assembly_pressure +REG_DATA_ELECTROLYTE_VOLUME,INT16,,,,hold_register,16016,data_electrolyte_volume +REG_DATA_COIN_NUM,INT16,,,,hold_register,16018,data_coin_num +REG_DATA_ELECTROLYTE_CODE,STRING,,,,hold_register,16020,data_electrolyte_code() +REG_DATA_COIN_CELL_CODE,STRING,,,,hold_register,16030,data_coin_cell_code() +REG_DATA_STACK_VISON_CODE,STRING,,,,hold_register,18004,data_stack_vision_code() +REG_DATA_GLOVE_BOX_PRESSURE,FLOAT32,,,,hold_register,16050,data_glove_box_pressure +REG_DATA_GLOVE_BOX_WATER_CONTENT,FLOAT32,,,,hold_register,16052,data_glove_box_water_content +REG_DATA_GLOVE_BOX_O2_CONTENT,FLOAT32,,,,hold_register,16054,data_glove_box_o2_content +UNILAB_SEND_ELECTROLYTE_BOTTLE_NUM,BOOL,,,,coil,9720, +UNILAB_RECE_ELECTROLYTE_BOTTLE_NUM,BOOL,,,,coil,9520, +REG_MSG_ELECTROLYTE_NUM_USED,INT16,,,,hold_register,17496, +REG_DATA_ELECTROLYTE_USE_NUM,INT16,,,,hold_register,16000, +UNILAB_SEND_FINISHED_CMD,BOOL,,,,coil,9730, +UNILAB_RECE_FINISHED_CMD,BOOL,,,,coil,9530, +REG_DATA_ASSEMBLY_TYPE,INT16,,,,hold_register,16018,ASSEMBLY_TYPE7or8 +COIL_ALUMINUM_FOIL,BOOL,,使用铝箔垫,,coil,9340, +REG_MSG_NE_PLATE_MATRIX,INT16,,负极片矩阵点位,,hold_register,17440, +REG_MSG_SEPARATOR_PLATE_MATRIX,INT16,,隔膜矩阵点位,,hold_register,17450, +REG_MSG_TIP_BOX_MATRIX,INT16,,移液枪头矩阵点位,,hold_register,17480, +REG_MSG_NE_PLATE_NUM,INT16,,负极片盘数,,hold_register,17443, +REG_MSG_SEPARATOR_PLATE_NUM,INT16,,隔膜盘数,,hold_register,17453, +REG_MSG_PRESS_MODE,BOOL,,压制模式(false:压力检测模式,True:距离模式),,coil,9360,电池压制模式 +,,,,,,, +,BOOL,,视觉对位(false:使用,true:忽略),,coil,9300,视觉对位 +,BOOL,,复检(false:使用,true:忽略),,coil,9310,视觉复检 +,BOOL,,手套箱_左仓(false:使用,true:忽略),,coil,9320,手套箱左仓 +,BOOL,,手套箱_右仓(false:使用,true:忽略),,coil,9420,手套箱右仓 +,BOOL,,真空检知(false:使用,true:忽略),,coil,9350,真空检知 +,BOOL,,电解液添加模式(false:单次滴液,true:二次滴液),,coil,9370,滴液模式 +,BOOL,,正极片称重(false:使用,true:忽略),,coil,9380,正极片称重 +,BOOL,,正负极片组装方式(false:正装,true:倒装),,coil,9390,正负极反装 +,BOOL,,压制清洁(false:使用,true:忽略),,coil,9400,压制清洁 +,BOOL,,物料盘摆盘方式(false:水平摆盘,true:堆叠摆盘),,coil,9410,负极片摆盘方式 +REG_MSG_BATTERY_CLEAN_IGNORE,BOOL,,忽略电池清洁(false:使用,true:忽略),,coil,9460, diff --git a/unilabos/resources/bioyond/YB_bottle_carriers.py b/unilabos/resources/bioyond/YB_bottle_carriers.py index 01d64c52..5284f0af 100644 --- a/unilabos/resources/bioyond/YB_bottle_carriers.py +++ b/unilabos/resources/bioyond/YB_bottle_carriers.py @@ -355,7 +355,7 @@ def YB_6x5ml_DispensingVialCarrier(name: str) -> BottleCarrier: size_y=carrier_size_y, size_z=carrier_size_z, sites=sites, - model="6x5ml_DispensingVialCarrier", + model="YB_6x5ml_DispensingVialCarrier", ) carrier.num_items_x = 4 carrier.num_items_y = 2 @@ -554,7 +554,7 @@ def YB_jia_yang_tou_da_1X1_carrier(name: str) -> BottleCarrier: size_y=carrier_size_y, size_z=carrier_size_z, sites=sites, - model="6x_LargeDispenseHeadCarrier", + model="YB_6x_LargeDispenseHeadCarrier", ) carrier.num_items_x = 1 carrier.num_items_y = 1 diff --git a/unilabos/resources/bioyond/YB_bottles.py b/unilabos/resources/bioyond/YB_bottles.py index 8ebdb415..432383ec 100644 --- a/unilabos/resources/bioyond/YB_bottles.py +++ b/unilabos/resources/bioyond/YB_bottles.py @@ -87,7 +87,7 @@ def YB_fen_ye_5ml_Bottle( height=height, max_volume=max_volume, barcode=barcode, - model="Separation_Bottle_5ml", + model="YB_fen_ye_5ml_Bottle", ) """20ml分液瓶""" From 48d429ae0038a11495333addcb1c4101efcb3272 Mon Sep 17 00:00:00 2001 From: Xuwznln <18435084+Xuwznln@users.noreply.github.com> Date: Fri, 31 Oct 2025 22:14:43 +0800 Subject: [PATCH 12/13] fix resource_get param --- unilabos/ros/nodes/base_device_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/unilabos/ros/nodes/base_device_node.py b/unilabos/ros/nodes/base_device_node.py index e52ea80d..7025e775 100644 --- a/unilabos/ros/nodes/base_device_node.py +++ b/unilabos/ros/nodes/base_device_node.py @@ -795,7 +795,7 @@ class BaseROS2DeviceNode(Node, Generic[T]): ].call_async( SerialCommand.Request( command=json.dumps( - {"data": {"data": resources_uuid, "with_children": True if action == "add" else "update"}, "action": "get"} + {"data": {"data": resources_uuid, "with_children": True if action == "add" else False}, "action": "get"} ) ) ) # type: ignore From 7af32b379bb90aa40e84a64ade4910d772c15073 Mon Sep 17 00:00:00 2001 From: dijkstra402 Date: Sat, 8 Nov 2025 14:53:25 +0800 Subject: [PATCH 13/13] Add YB_ prefix to bottle carrier model names --- unilabos/resources/bioyond/YB_bottle_carriers.py | 16 ++++++++-------- unilabos/resources/bioyond/YB_bottles.py | 12 ++++++------ 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/unilabos/resources/bioyond/YB_bottle_carriers.py b/unilabos/resources/bioyond/YB_bottle_carriers.py index 5284f0af..d61a327f 100644 --- a/unilabos/resources/bioyond/YB_bottle_carriers.py +++ b/unilabos/resources/bioyond/YB_bottle_carriers.py @@ -233,7 +233,7 @@ def YB_1BottleCarrier(name: str) -> BottleCarrier: resource_size_y=beaker_diameter, name_prefix=name, ), - model="1BottleCarrier", + model="YB_1BottleCarrier", ) carrier.num_items_x = 1 carrier.num_items_y = 1 @@ -270,7 +270,7 @@ def YB_1GaoNianYeBottleCarrier(name: str) -> BottleCarrier: resource_size_y=beaker_diameter, name_prefix=name, ), - model="1GaoNianYeBottleCarrier", + model="YB_1GaoNianYeBottleCarrier", ) carrier.num_items_x = 1 carrier.num_items_y = 1 @@ -307,7 +307,7 @@ def YB_1Bottle100mlCarrier(name: str) -> BottleCarrier: resource_size_y=beaker_diameter, name_prefix=name, ), - model="1Bottle100mlCarrier", + model="YB_1Bottle100mlCarrier", ) carrier.num_items_x = 1 carrier.num_items_y = 1 @@ -405,7 +405,7 @@ def YB_6x20ml_DispensingVialCarrier(name: str) -> BottleCarrier: size_y=carrier_size_y, size_z=carrier_size_z, sites=sites, - model="6x20ml_DispensingVialCarrier", + model="YB_6x20ml_DispensingVialCarrier", ) carrier.num_items_x = 4 carrier.num_items_y = 2 @@ -455,7 +455,7 @@ def YB_6x_SmallSolutionBottleCarrier(name: str) -> BottleCarrier: size_y=carrier_size_y, size_z=carrier_size_z, sites=sites, - model="6x_SmallSolutionBottleCarrier", + model="YB_6x_SmallSolutionBottleCarrier", ) carrier.num_items_x = 4 carrier.num_items_y = 2 @@ -505,7 +505,7 @@ def YB_4x_LargeSolutionBottleCarrier(name: str) -> BottleCarrier: size_y=carrier_size_y, size_z=carrier_size_z, sites=sites, - model="4x_LargeSolutionBottleCarrier", + model="YB_4x_LargeSolutionBottleCarrier", ) carrier.num_items_x = 2 carrier.num_items_y = 2 @@ -591,7 +591,7 @@ def YB_AdapterBlock(name: str) -> BottleCarrier: resource_size_y=adapter_diameter, name_prefix=name, ), - model="AdapterBlock", + model="YB_AdapterBlock", ) carrier.num_items_x = 1 carrier.num_items_y = 1 @@ -639,7 +639,7 @@ def YB_TipBox(name: str) -> BottleCarrier: size_y=carrier_size_y, size_z=carrier_size_z, sites=sites, - model="TipBox", + model="YB_TipBox", ) carrier.num_items_x = 12 carrier.num_items_y = 8 diff --git a/unilabos/resources/bioyond/YB_bottles.py b/unilabos/resources/bioyond/YB_bottles.py index 432383ec..83b73e46 100644 --- a/unilabos/resources/bioyond/YB_bottles.py +++ b/unilabos/resources/bioyond/YB_bottles.py @@ -15,7 +15,7 @@ def YB_jia_yang_tou_da( height=height, max_volume=max_volume, barcode=barcode, - model="Solid_Stock", + model="YB_jia_yang_tou_da_1X1_carrier", ) """液1x1""" @@ -51,7 +51,7 @@ def YB_ye_100ml_Bottle( height=height, max_volume=max_volume, barcode=barcode, - model="Liquid_Bottle_100ml", + model="YB_1Bottle100mlCarrier", ) """高粘液""" @@ -105,7 +105,7 @@ def YB_fen_ye_20ml_Bottle( height=height, max_volume=max_volume, barcode=barcode, - model="Separation_Bottle_20ml", + model="YB_fen_ye_20ml_Bottle", ) """配液瓶(小)""" @@ -123,7 +123,7 @@ def YB_pei_ye_xiao_Bottle( height=height, max_volume=max_volume, barcode=barcode, - model="Mixing_Bottle_Small", + model="YB_pei_ye_xiao_Bottle", ) """配液瓶(大)""" @@ -141,7 +141,7 @@ def YB_pei_ye_da_Bottle( height=height, max_volume=max_volume, barcode=barcode, - model="Mixing_Bottle_Large", + model="YB_pei_ye_da_Bottle", ) """枪头""" @@ -159,5 +159,5 @@ def YB_Pipette_Tip( height=height, max_volume=max_volume, barcode=barcode, - model="Pipette_Tip", + model="YB_Pipette_Tip", )