mirror of
https://github.com/deepmodeling/Uni-Lab-OS
synced 2026-05-23 01:59:59 +00:00
演示时修改的部分代码
This commit is contained in:
@@ -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:
|
||||
|
||||
@@ -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,
|
||||
|
||||
@@ -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",
|
||||
|
||||
@@ -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(
|
||||
|
||||
@@ -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]
|
||||
|
||||
@@ -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]]
|
||||
|
||||
Reference in New Issue
Block a user