From 6288e37464e5a2d8facf11bc9e6588909d0de01b Mon Sep 17 00:00:00 2001 From: q434343 <554662886@qq.com> Date: Thu, 14 May 2026 17:49:48 +0800 Subject: [PATCH] =?UTF-8?q?=E6=BC=94=E7=A4=BA=E6=97=B6=E4=BF=AE=E6=94=B9?= =?UTF-8?q?=E7=9A=84=E9=83=A8=E5=88=86=E4=BB=A3=E7=A0=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../liquid_handler_abstract.py | 54 +++++- .../devices/liquid_handling/prcxi/prcxi.py | 164 ++++++++++++++++-- unilabos/labware_manager/labware_db.json | 2 +- unilabos/ros/nodes/base_device_node.py | 2 +- unilabos/ros/nodes/presets/host_node.py | 159 +++++++++++++++++ .../test/experiments/prcxi_9320_slim.json | 4 +- 6 files changed, 361 insertions(+), 24 deletions(-) diff --git a/unilabos/devices/liquid_handling/liquid_handler_abstract.py b/unilabos/devices/liquid_handling/liquid_handler_abstract.py index 99e85bd7..ad0b8267 100644 --- a/unilabos/devices/liquid_handling/liquid_handler_abstract.py +++ b/unilabos/devices/liquid_handling/liquid_handler_abstract.py @@ -61,6 +61,7 @@ class TransferLiquidReturn(TypedDict): class LiquidHandlerMiddleware(LiquidHandler): + _ros_node: ROS2DeviceNode def __init__( self, backend: LiquidHandlerBackend, deck: Deck, simulator: bool = False, channel_num: int = 8, **kwargs ): @@ -79,6 +80,11 @@ class LiquidHandlerMiddleware(LiquidHandler): self._simulate_handler = LiquidHandlerAbstract(self._simulate_backend, deck, False) super().__init__(backend, deck) + def post_init(self, ros_node: BaseROS2DeviceNode): + self._ros_node = ros_node + if getattr(self, "_simulator", False) and getattr(self, "_simulate_handler", None) is not None: + self._simulate_handler._ros_node = ros_node + async def setup(self, **backend_kwargs): if self._simulator: return await self._simulate_handler.setup(**backend_kwargs) @@ -152,6 +158,14 @@ class LiquidHandlerMiddleware(LiquidHandler): if self._simulator: return await self._simulate_handler.pick_up_tips(tip_spots, use_channels, offsets, **backend_kwargs) + if hasattr(self, "_ros_node") and self._ros_node is not None: + task = ROS2DeviceNode.run_async_func(self._ros_node.update_resource, True, **{"resources": tip_spots}) + submit_time = time.time() + while not task.done(): + if time.time() - submit_time > 10: + self._ros_node.lab_logger().info(f"pick_up_tips {tip_spots} 超时") + break + time.sleep(0.01) return await super().pick_up_tips(tip_spots, use_channels, offsets, **backend_kwargs) async def drop_tips( @@ -360,6 +374,16 @@ class LiquidHandlerMiddleware(LiquidHandler): EXTRA_SAMPLE_UUID: sample_uuid_value, "volume": volume, } + + if hasattr(self, "_ros_node") and self._ros_node is not None: + task = ROS2DeviceNode.run_async_func(self._ros_node.update_resource, True, **{"resources": resources}) + submit_time = time.time() + while not task.done(): + if time.time() - submit_time > 10: + self._ros_node.lab_logger().info(f"aspirate {resources} 超时") + break + time.sleep(0.01) + return SimpleReturn(samples=res_samples, volumes=res_volumes) async def dispense( @@ -495,6 +519,15 @@ class LiquidHandlerMiddleware(LiquidHandler): res_samples.append({"name": resource.name, EXTRA_SAMPLE_UUID: res_uuid}) res_volumes.append(volume) + if hasattr(self, "_ros_node") and self._ros_node is not None: + task = ROS2DeviceNode.run_async_func(self._ros_node.update_resource, True, **{"resources": resources}) + submit_time = time.time() + while not task.done(): + if time.time() - submit_time > 10: + self._ros_node.lab_logger().info(f"dispense {resources} 超时") + break + time.sleep(0.01) + return SimpleReturn(samples=res_samples, volumes=res_volumes) async def transfer( @@ -880,7 +913,7 @@ class LiquidHandlerAbstract(LiquidHandlerMiddleware): super().__init__(backend_type, deck, simulator, channel_num, total_height=total_height, **kwargs) def post_init(self, ros_node: BaseROS2DeviceNode): - self._ros_node = ros_node + super().post_init(ros_node) ROS2DeviceNode.run_async_func(self._ros_node.update_resource, True, **{ "resources": [self.deck] }) @@ -1036,13 +1069,14 @@ class LiquidHandlerAbstract(LiquidHandlerMiddleware): well.set_liquids([(liquid_name, safe_volume)]) # type: ignore res_volumes.append(safe_volume) - task = ROS2DeviceNode.run_async_func(self._ros_node.update_resource, True, **{"resources": wells}) - submit_time = time.time() - while not task.done(): - if time.time() - submit_time > 10: - self._ros_node.lab_logger().info(f"set_liquid_from_plate {plate} 超时") - break - time.sleep(0.01) + if hasattr(self, "_ros_node") and self._ros_node is not None: + task = ROS2DeviceNode.run_async_func(self._ros_node.update_resource, True, **{"resources": wells}) + submit_time = time.time() + while not task.done(): + if time.time() - submit_time > 10: + self._ros_node.lab_logger().info(f"set_liquid_from_plate {plate} 超时") + break + time.sleep(0.01) return SetLiquidFromPlateReturn( plate=ResourceTreeSet.from_plr_resources([plate], known_newly_created=False).dump(), # type: ignore @@ -1449,6 +1483,7 @@ class LiquidHandlerAbstract(LiquidHandlerMiddleware): mix_rate: Optional[int] = None, mix_liquid_height: Optional[float] = None, delays: Optional[List[int]] = None, + pre_aspirate_from_target: Optional[float] = None, none_keys: List[str] = [], ) -> TransferLiquidReturn: """Transfer liquid with automatic mode detection. @@ -1600,6 +1635,8 @@ class LiquidHandlerAbstract(LiquidHandlerMiddleware): kwargs['mix_liquid_height'] = safe_get(mix_liquid_height, i, wrap=False) if delays is not None: kwargs['delays'] = safe_get(delays, i) + if pre_aspirate_from_target is not None: + kwargs['pre_aspirate_from_target'] = safe_get(pre_aspirate_from_target, i) cur_source = sources[i % num_sources] cur_target = targets[i % num_targets] @@ -1659,6 +1696,7 @@ class LiquidHandlerAbstract(LiquidHandlerMiddleware): mix_rate = kwargs.get('mix_rate') mix_liquid_height = kwargs.get('mix_liquid_height') delays = kwargs.get('delays') + pre_aspirate_from_target = kwargs.get('pre_aspirate_from_target') tip = [] if pick_up: diff --git a/unilabos/devices/liquid_handling/prcxi/prcxi.py b/unilabos/devices/liquid_handling/prcxi/prcxi.py index 0e7d0c8e..95731fcd 100644 --- a/unilabos/devices/liquid_handling/prcxi/prcxi.py +++ b/unilabos/devices/liquid_handling/prcxi/prcxi.py @@ -149,6 +149,40 @@ class PRCXI9300Deck(Deck): pos = self.sites[idx]["position"] return Coordinate(pos["x"], pos["y"], pos["z"]) + def get_slot_location(self, slot: Union[int, str]) -> Coordinate: + """根据 slot 标识返回该 slot 的坐标。 + + 支持的输入： + - int: 1-based slot 序号（与 ``assign_child_at_slot`` 一致），1 → sites[0] + - str: 纯数字字符串 ``"3"``，或带前缀的 label ``"T3"``（不区分大小写） + + Raises: + ValueError: slot 解析失败或越界 + """ + idx: Optional[int] = None + if isinstance(slot, int): + idx = slot - 1 + elif isinstance(slot, str): + s = slot.strip() + if not s: + raise ValueError(f"空 slot 标识") + digits = s[1:] if s[0].isalpha() else s + try: + idx = int(digits) - 1 + except ValueError: + # 退而求其次：直接按 label 全等匹配 + for i, site in enumerate(self.sites): + if site.get("label") == s: + idx = i + break + if idx is None: + raise ValueError(f"无法解析 slot 标识: {slot!r}") + if idx < 0 or idx >= len(self.sites): + raise ValueError( + f"slot {slot!r} 超出范围 [1, {len(self.sites)}] (解析为 idx={idx})" + ) + return self._get_site_location(idx) + def _get_site_resource(self, idx: int) -> Optional[Resource]: site_loc = self._get_site_location(idx) for child in self.children: @@ -444,7 +478,7 @@ class PRCXI9300Trash(Trash): if name != "trash": print(f"Warning: PRCXI9300Trash usually expects name='trash' for backend logic, but got '{name}'.") - super().__init__(name, size_x, size_y, size_z, **kwargs) + super().__init__(name, size_x, size_y, size_z, category=category, **kwargs) self._unilabos_state = {} # 初始化时注入 UUID if material_info: @@ -533,12 +567,16 @@ class PRCXI9300TubeRack(TubeRack): # 根据情况传递不同的参数 if items_to_pass is not None: - super().__init__(name, size_x, size_y, size_z, ordered_items=items_to_pass, model=model, **kwargs) + super().__init__( + name, size_x, size_y, size_z, ordered_items=items_to_pass, category=category, model=model, **kwargs + ) elif ordering_param is not None: # 传递 ordering 参数，让 TubeRack 自己创建 Tube 对象 - super().__init__(name, size_x, size_y, size_z, ordering=ordering_param, model=model, **kwargs) + super().__init__( + name, size_x, size_y, size_z, ordering=ordering_param, category=category, model=model, **kwargs + ) else: - super().__init__(name, size_x, size_y, size_z, model=model, **kwargs) + super().__init__(name, size_x, size_y, size_z, category=category, model=model, **kwargs) self._unilabos_state = {} if material_info: @@ -716,6 +754,7 @@ class PRCXI9300PlateAdapter(PlateAdapter): adapter_hole_size_x=adapter_hole_size_x, adapter_hole_size_y=adapter_hole_size_y, adapter_hole_size_z=adapter_hole_size_z, + category=category, model=model, **kwargs, ) @@ -803,6 +842,8 @@ class PRCXI9300Handler(LiquidHandlerAbstract): self.xy_coupling = xy_coupling self._slot_prcxi_positions: Dict[int, Tuple[float, float]] = {} self.calibration_labware_type = calibration_labware_type + self.max_z_pipetting = 185 + self.max_z_claw = 170 if calibration_points is not None: self.calibrate_from_points(calibration_points, labware_type=self.calibration_labware_type) @@ -839,12 +880,65 @@ class PRCXI9300Handler(LiquidHandlerAbstract): ) super().__init__(backend=self._unilabos_backend, deck=deck, simulator=simulator, channel_num=channel_num) self._first_transfer_done = False + # backend 在做槽位反查时若拿不到 deck，需要回退到 handler.deck，这里建立反向引用 + self._unilabos_backend._handler = self @staticmethod def _get_slot_number(resource) -> Optional[int]: """从 resource 的 unilabos_extra["update_resource_site"]（如 "T13"）或位置反算槽位号。""" return _get_slot_number(resource) + def _top_level_consumable(self, resource): + """从任意 PLR 资源沿 parent 向上找"放在 deck 上的那一层耗材"。""" + if resource is None: + return None + cur = resource + while cur is not None: + parent = getattr(cur, "parent", None) + if isinstance(parent, PRCXI9300Deck): + return cur + if parent is None: + # 已到顶；若 cur 本身就是 deck，没有"耗材"层 + if isinstance(cur, PRCXI9300Deck): + return None + return cur + cur = parent + return None + + def _attach_resources_to_deck_if_needed(self, items: Sequence[Resource]) -> None: + """把通过 _resolve_to_plr_resources 拿回的"游离"耗材自动挂到 self.deck。 + + - 已经在 PRCXI9300Deck 上（含 name 同名）的跳过； + - 优先按 ``unilabos_extra.update_resource_site`` 的 Tn 解析槽位； + - 否则交给 ``Deck.assign_child_resource`` 找空槽。 + - 任意失败仅打印告警，不中断主流程（backend 仍可走名字兜底）。 + """ + deck = getattr(self, "deck", None) + if not isinstance(deck, PRCXI9300Deck): + return + existing_names = {getattr(c, "name", None) for c in deck.children} + for item in items: + top = self._top_level_consumable(item) + if top is None or not isinstance(top, Resource): + continue + if isinstance(getattr(top, "parent", None), PRCXI9300Deck): + continue + top_name = getattr(top, "name", None) + if top_name in existing_names: + continue + spot_idx: Optional[int] = None + extra = getattr(top, "unilabos_extra", {}) or {} + site = str(extra.get("update_resource_site", "")) + if site: + digits = "".join(c for c in site if c.isdigit()) + if digits: + spot_idx = int(digits) - 1 + try: + deck.assign_child_resource(top, spot=spot_idx, reassign=False) + existing_names.add(top_name) + except Exception as e: + print(f"[PRCXI] 自动挂载到 deck 失败: name={top_name}, site={site or '?'}, err={e}") + def _match_and_create_matrix(self): """首次 transfer_liquid 时，根据 deck 上的 resource 自动匹配耗材并创建 WorkTabletMatrix。""" backend = self._unilabos_backend @@ -962,7 +1056,7 @@ class PRCXI9300Handler(LiquidHandlerAbstract): slot_pos = self._slot_prcxi_positions[number] pos.x = slot_pos[0] - child.get_size_x() / 2 + self.left_2_claw.x pos.y = slot_pos[1] - child.get_size_y() / 2 + self.left_2_claw.y - claw_positions.append({"Number": number, "XPos": pos.x, "YPos": pos.y, "ZPos": pos.z}) + claw_positions.append({"Number": number, "XPos": pos.x, "YPos": pos.y, "ZPos": max(min(pos.z, self.max_z_claw),0)}) if child.children: pip_pos = self.plr_pos_to_prcxi(child.children[0]) @@ -978,13 +1072,13 @@ class PRCXI9300Handler(LiquidHandlerAbstract): "Number": number, "XPos": pip_pos.x, "YPos": pip_pos.y, - "ZPos": pip_pos.z, + "ZPos": max(min(pip_pos.z, self.max_z_pipetting),0), "X_Left": half_x, "X_Right": half_x, "ZAgainstTheWall": pip_pos.z - z_wall, "X2Pos": pip_pos.x + self.right_2_left.x, "Y2Pos": pip_pos.y + self.right_2_left.y, - "Z2Pos": pip_pos.z + self.right_2_left.z, + "Z2Pos": max(min(pip_pos.z + self.right_2_left.z, self.max_z_pipetting),0), "X2_Left": half_x, "X2_Right": half_x, "ZAgainstTheWall2": pip_pos.z - z_wall, @@ -1241,6 +1335,7 @@ class PRCXI9300Handler(LiquidHandlerAbstract): mix_rate: Optional[int] = None, mix_liquid_height: Optional[float] = None, delays: Optional[List[int]] = None, + pre_aspirate_from_target: Optional[float] = None, none_keys: List[str] = [], ) -> TransferLiquidReturn: if not self._first_transfer_done: @@ -1254,6 +1349,8 @@ class PRCXI9300Handler(LiquidHandlerAbstract): sources = await self._resolve_to_plr_resources(sources) targets = await self._resolve_to_plr_resources(targets) tip_racks = list(await self._resolve_to_plr_resources(tip_racks)) + # 远端解析回来的 PLR 实例可能未挂到 self.deck，主动绑定一次，避免 backend 取 plate.parent==None + self._attach_resources_to_deck_if_needed(list(sources) + list(targets) + list(tip_racks)) if isinstance(tip_racks[0], TipRack): tip_rack = tip_racks[0] else: @@ -1320,6 +1417,7 @@ class PRCXI9300Handler(LiquidHandlerAbstract): mix_rate=mix_rate, mix_liquid_height=mix_liquid_height, delays=delays, + pre_aspirate_from_target=pre_aspirate_from_target, none_keys=none_keys, ) if self.step_mode: @@ -1475,6 +1573,7 @@ class PRCXI9300Backend(LiquidHandlerBackend): _num_channels = 8 # 默认通道数为 8 _is_reset_ok = False _ros_node: BaseROS2DeviceNode + _handler: Optional["PRCXI9300Handler"] = None # 由 PRCXI9300Handler.__init__ 注入 @property def is_reset_ok(self) -> bool: @@ -1508,13 +1607,52 @@ class PRCXI9300Backend(LiquidHandlerBackend): self.debug = debug self.axis = "Left" - @staticmethod - def _deck_plate_slot_no(plate, deck) -> int: - """台面板位槽号（1–16）：与 PRCXI9300Handler._get_slot_number 一致；无法解析时退回 deck 子项顺序 +1。""" + def _resolve_deck(self, plate, deck=None) -> Optional["PRCXI9300Deck"]: + """定位 plate 所属的 PRCXI9300Deck：按 deck 入参 → plate 的祖先链 → handler.deck 顺序回退。""" + if isinstance(deck, PRCXI9300Deck): + return deck + cur = plate + while cur is not None: + if isinstance(cur, PRCXI9300Deck): + return cur + cur = getattr(cur, "parent", None) + if self._handler is not None: + handler_deck = getattr(self._handler, "deck", None) + if isinstance(handler_deck, PRCXI9300Deck): + return handler_deck + return None + + def _deck_plate_slot_no(self, plate, deck=None) -> int: + """台面板位槽号（1–16）：优先 _get_slot_number；否则沿父链/handler.deck 找到 deck 后取序号+1。""" sn = PRCXI9300Handler._get_slot_number(plate) if sn is not None: return sn - return deck.children.index(plate) + 1 + actual_deck = self._resolve_deck(plate, deck) + if actual_deck is None: + raise RuntimeError( + f"无法定位 {getattr(plate, 'name', '?')} 所在的 PRCXI9300Deck：" + "请确认 tip_rack/plate 已挂到 self.deck，或在 unilabos_extra 中提供 update_resource_site=Tn。" + ) + if plate in actual_deck.children: + index = actual_deck.children.index(plate) + plate_new = actual_deck.children[index] + sn = PRCXI9300Handler._get_slot_number(plate_new) + if sn is not None: + return sn + else: + raise RuntimeError( + f"无法定位 {getattr(plate_new, 'name', '?')} 所在的 PRCXI9300Deck：" + f"x: {plate_new.location}" + ) + # 名字兜底（远端解析回来的实例与 deck 上的不是同一对象） + plate_name = getattr(plate, "name", None) + if plate_name is not None: + for i, c in enumerate(actual_deck.children): + if getattr(c, "name", None) == plate_name: + return i + 1 + raise RuntimeError( + f"{getattr(plate, 'name', '?')} 不在 deck.children 中且无可解析的槽位号" + ) @staticmethod def _resource_num_items_y(resource) -> int: @@ -1977,7 +2115,9 @@ class PRCXI9300Backend(LiquidHandlerBackend): assist_fun1 = "" if ops[0].blow_out_air_volume is not None: - assist_fun1 = f"吹样({float(min(max(ops[0].blow_out_air_volume,0),10))}ul)" + assist_fun1 = f"吹样({float(min(max(ops[0].blow_out_air_volume,5),10))}ul)" + else : + assist_fun1 = f"吹样({5.0}ul)" step = self.api_client.Tapping( axis=axis, diff --git a/unilabos/labware_manager/labware_db.json b/unilabos/labware_manager/labware_db.json index b559a8f7..19a39084 100644 --- a/unilabos/labware_manager/labware_db.json +++ b/unilabos/labware_manager/labware_db.json @@ -632,7 +632,7 @@ "size_y": 85.8, "size_z": 42.66, "model": "PRCXI_EP_Adapter", - "category": null, + "category": "tube_rack", "plate_type": null, "material_info": { "uuid": "e146697c395e4eabb3d6b74f0dd6aaf7", diff --git a/unilabos/ros/nodes/base_device_node.py b/unilabos/ros/nodes/base_device_node.py index 43b8ebfb..0362a227 100644 --- a/unilabos/ros/nodes/base_device_node.py +++ b/unilabos/ros/nodes/base_device_node.py @@ -529,7 +529,7 @@ class BaseROS2DeviceNode(Node, Generic[T]): ) # 调整了液体以及Deck之后要重新Assign # noinspection PyUnresolvedReferences - rts_with_parent = ResourceTreeSet.from_plr_resources([parent_resource]) + rts_with_parent = ResourceTreeSet.from_plr_resources([plr_instance]) if rts_with_parent.root_nodes[0].res_content.uuid_parent is None: rts_with_parent.root_nodes[0].res_content.parent_uuid = self.uuid request.command = json.dumps( diff --git a/unilabos/ros/nodes/presets/host_node.py b/unilabos/ros/nodes/presets/host_node.py index 369ed5ae..c93ab052 100644 --- a/unilabos/ros/nodes/presets/host_node.py +++ b/unilabos/ros/nodes/presets/host_node.py @@ -570,6 +570,102 @@ class HostNode(BaseROS2DeviceNode): responses.append(response.response) return responses + def _lookup_deck_for_slot(self, device_id: str, deck_id: str): + """根据 device_id / deck_id 查找 deck PLR 实例，找不到返回 None。 + + 优先级： + 1. ``devices_instances[device_id]`` 上对应 driver 的 ``deck`` 属性（PLR LiquidHandler 的标准属性） + 2. driver / wrapper / _ros_node 各级 resource_tracker.figure_resource({"id": deck_id}) + 3. host 自己的 ``_resource_tracker`` + """ + log = self.lab_logger() + + def _try_tracker(tracker, src_desc: str): + if tracker is None: + log.debug(f"[Host Node] _lookup_deck: {src_desc} tracker 为 None，跳过") + return None + try: + matches = tracker.figure_resource({"id": deck_id}, try_mode=True) + except Exception as e: + log.warning(f"[Host Node] _lookup_deck: {src_desc}.figure_resource({deck_id}) 失败: {e}") + return None + if isinstance(matches, list) and matches: + obj = next((m for m in matches if not isinstance(m, dict)), matches[0]) + if obj is not None and not isinstance(obj, dict): + log.debug(f"[Host Node] _lookup_deck: 命中 via {src_desc} -> {type(obj).__name__}") + return obj + log.debug(f"[Host Node] _lookup_deck: {src_desc} figure_resource 未命中 (matches={matches!r})") + return None + + # 1) 先按 device_id 拿 driver 上的 deck + candidate_ids = [] + if device_id: + candidate_ids.append(device_id) + stripped = device_id.lstrip("/") + if stripped and stripped != device_id: + candidate_ids.append(stripped) + tail = device_id.split("/")[-1] + if tail and tail not in candidate_ids: + candidate_ids.append(tail) + + d = None + for did in candidate_ids: + d = self.devices_instances.get(did) + if d is not None: + break + if d is None: + log.warning( + f"[Host Node] _lookup_deck: devices_instances 找不到 device_id={device_id!r} " + f"(尝试过 {candidate_ids}); 当前已知: {list(self.devices_instances.keys())}" + ) + else: + # 真正的 driver 在 wrapper 的 _driver_instance / _ros_node.driver_instance 上 + driver_candidates = [] + for attr_path in ("_driver_instance", "_ros_node.driver_instance", "driver_instance"): + obj = d + for part in attr_path.split("."): + obj = getattr(obj, part, None) + if obj is None: + break + if obj is not None and obj not in driver_candidates: + driver_candidates.append(obj) + + for drv in driver_candidates: + deck = getattr(drv, "deck", None) + if deck is not None: + deck_name = getattr(deck, "name", None) + if deck_name == deck_id: + log.debug( + f"[Host Node] _lookup_deck: 命中 via {type(drv).__name__}.deck (name={deck_name})" + ) + return deck + log.debug( + f"[Host Node] _lookup_deck: {type(drv).__name__}.deck.name={deck_name!r} 与 {deck_id!r} 不一致" + ) + + # 退化：从 wrapper / _ros_node 的 resource_tracker 找 + tracker_paths = ( + "resource_tracker", + "_ros_node.resource_tracker", + ) + for attr_path in tracker_paths: + tracker = d + for part in attr_path.split("."): + tracker = getattr(tracker, part, None) + if tracker is None: + break + obj = _try_tracker(tracker, f"device({device_id}).{attr_path}") + if obj is not None: + return obj + + # 2) host 自己的 tracker（一般为空，因为 init 时 device 树被 continue 了） + host_tracker = getattr(self, "resource_tracker", None) or getattr(self, "_resource_tracker", None) + obj = _try_tracker(host_tracker, "host._resource_tracker") + if obj is not None: + return obj + + return None + async def create_resource( self, device_id: DeviceSlot, @@ -583,6 +679,30 @@ class HostNode(BaseROS2DeviceNode): slot_on_deck: str = "", ) -> CreateResourceReturn: # 暂不支持多对同名父子同时存在 + # 如果 slot_on_deck 不是空，并且 bind_locations 全为 0，则尝试通过 deck 的 slot 信息推算真实坐标 + if slot_on_deck and ( + (not hasattr(bind_locations, "x") or bind_locations.x == 0) + and (not hasattr(bind_locations, "y") or bind_locations.y == 0) + and (not hasattr(bind_locations, "z") or bind_locations.z == 0) + ): + # 尝试通过 parent (deck) 查找 slot 坐标，parent 应是deck的id + deck_id = parent.split("/")[-1] + deck_obj = self._lookup_deck_for_slot(device_id, deck_id) + if deck_obj is not None and hasattr(deck_obj, "get_slot_location"): + try: + slot_location = deck_obj.get_slot_location(slot_on_deck) + bind_locations.x = slot_location.x + bind_locations.y = slot_location.y + bind_locations.z = slot_location.z + except Exception as e: + self.lab_logger().warning( + f"[Host Node] 无法通过deck({deck_id})获取slot({slot_on_deck})位置: {e}" + ) + else: + self.lab_logger().warning( + f"[Host Node] 找不到deck对象({deck_id})或其不支持get_slot_location, 无法修正bind_locations" + ) + res_creation_input = { "id": res_id.split("/")[-1], "name": res_id.split("/")[-1], @@ -608,6 +728,45 @@ class HostNode(BaseROS2DeviceNode): } ) init_new_res = initialize_resource(res_creation_input) # flatten的格式 + + # 若 init_new_res 中节点的 pose.position 与 pose.position3d 同时全为 0， + # 用上面通过 deck slot 反查得到的 bind_locations 覆盖（位置仍可能是默认 0） + bind_xyz = { + "x": float(getattr(bind_locations, "x", 0) or 0), + "y": float(getattr(bind_locations, "y", 0) or 0), + "z": float(getattr(bind_locations, "z", 0) or 0), + } + if any(v != 0.0 for v in bind_xyz.values()): + def _is_zero_xyz(p): + if not isinstance(p, dict): + return False + return ( + float(p.get("x", 0) or 0) == 0.0 + and float(p.get("y", 0) or 0) == 0.0 + and float(p.get("z", 0) or 0) == 0.0 + ) + + def _patch_node(node): + if not isinstance(node, dict): + return + pose = node.get("pose") + if not isinstance(pose, dict): + return + pos = pose.get("position") + pos3d = pose.get("position3d") + if _is_zero_xyz(pos) and _is_zero_xyz(pos3d): + pose["position"] = dict(bind_xyz) + pose["position3d"] = dict(bind_xyz) + + def _walk(obj): + if isinstance(obj, dict): + _patch_node(obj) + elif isinstance(obj, list): + for item in obj: + _walk(item) + + _walk(init_new_res) + if len(init_new_res) > 1: # 一个物料，多个子节点 init_new_res = [init_new_res] resources: List[Resource] | List[List[Resource]] = init_new_res # initialize_resource已经返回list[dict] diff --git a/unilabos/test/experiments/prcxi_9320_slim.json b/unilabos/test/experiments/prcxi_9320_slim.json index 9af1c5bd..4c4ea49f 100644 --- a/unilabos/test/experiments/prcxi_9320_slim.json +++ b/unilabos/test/experiments/prcxi_9320_slim.json @@ -26,11 +26,11 @@ "is_9320": true, "timeout": 10, "matrix_id": "", - "simulator": false, + "simulator": true, "channel_num": 2, "step_mode": false, "calibration_points": { - "line_1": [[452.07,21.19], [313.88,21.19], [176.87,21.19], [39.08,21.19]], + "line_1": [[452.07,21.19], [313.88,21.19], [177.17,21.19], [39.08,21.19]], "line_2": [[451.37,116.68], [313.28,116.88], [176.58,116.69], [38.58,117.18]], "line_3": [[450.87,212.18], [312.98,212.38], [176.08,212.68], [38.08,213.18]], "line_4": [[450.08,307.68], [312.18,307.89], [175.18,308.18], [37.58,309.18]]