修改rviz显示逻辑与joint_publisher，添加moveit2相关节点描述

unilabos/device_mesh/resource_visalization.py

@@ -201,17 +201,42 @@ class ResourceVisualization:
                 self.moveit_controllers_yaml['moveit_simple_controller_manager'][f"{name}_{controller_name}"] = moveit_dict['moveit_simple_controller_manager'][controller_name]
 
 
+    @staticmethod
+    def _ensure_ros2_env() -> dict:
+        """确保 ROS2 环境变量正确设置，返回可用于子进程的 env dict"""
+        import sys
+        env = dict(os.environ)
+        conda_prefix = os.path.dirname(os.path.dirname(sys.executable))
+
+        if "AMENT_PREFIX_PATH" not in env or not env["AMENT_PREFIX_PATH"].strip():
+            candidate = os.pathsep.join([conda_prefix, os.path.join(conda_prefix, "Library")])
+            env["AMENT_PREFIX_PATH"] = candidate
+            os.environ["AMENT_PREFIX_PATH"] = candidate
+
+        extra_bin_dirs = [
+            os.path.join(conda_prefix, "Library", "bin"),
+            os.path.join(conda_prefix, "Library", "lib"),
+            os.path.join(conda_prefix, "Scripts"),
+            conda_prefix,
+        ]
+        current_path = env.get("PATH", "")
+        for d in extra_bin_dirs:
+            if d not in current_path:
+                current_path = d + os.pathsep + current_path
+        env["PATH"] = current_path
+        os.environ["PATH"] = current_path
+
+        return env
+
     def create_launch_description(self) -> LaunchDescription:
         """
         创建launch描述，包含robot_state_publisher和move_group节点
 
-        Args:
-            urdf_str: URDF文本
-
         Returns:
             LaunchDescription: launch描述对象
         """
-        # 检查ROS 2环境变量
+        launch_env = self._ensure_ros2_env()
+
         if "AMENT_PREFIX_PATH" not in os.environ:
             raise OSError(
                 "ROS 2环境未正确设置。需要设置 AMENT_PREFIX_PATH 环境变量。\n"
@@ -290,7 +315,7 @@ class ResourceVisualization:
                         {"robot_description": robot_description},
                         ros2_controllers,
                     ],
-                    env=dict(os.environ)
+                    env=launch_env,
                 )
             )
             for controller in self.moveit_controllers_yaml['moveit_simple_controller_manager']['controller_names']:
@@ -300,7 +325,7 @@ class ResourceVisualization:
                         executable="spawner",
                         arguments=[f"{controller}", "--controller-manager", f"controller_manager"],
                         output="screen",
-                        env=dict(os.environ)
+                        env=launch_env,
                     )
                 )
             controllers.append(
@@ -309,7 +334,7 @@ class ResourceVisualization:
                         executable="spawner",
                         arguments=["joint_state_broadcaster", "--controller-manager", f"controller_manager"],
                         output="screen",
-                        env=dict(os.environ)
+                        env=launch_env,
                 )
             )
             for i in controllers:
@@ -317,7 +342,6 @@ class ResourceVisualization:
         else:
             ros2_controllers = None
 
-        # 创建robot_state_publisher节点
         robot_state_publisher = nd(
             package='robot_state_publisher',
             executable='robot_state_publisher',
@@ -327,9 +351,8 @@ class ResourceVisualization:
                 'robot_description': robot_description,
                 'use_sim_time': False
             },
-            # kinematics_dict
             ],
-            env=dict(os.environ)
+            env=launch_env,
         )
 
 
@@ -361,7 +384,7 @@ class ResourceVisualization:
             executable='move_group',
             output='screen',
             parameters=moveit_params,
-            env=dict(os.environ)
+            env=launch_env,
         )
 
 
@@ -379,13 +402,11 @@ class ResourceVisualization:
                 arguments=['-d', f"{str(self.mesh_path)}/view_robot.rviz"],
                 output='screen',
                 parameters=[
-                    {'robot_description_kinematics': kinematics_dict,
-                     },
+                    {'robot_description_kinematics': kinematics_dict},
                     robot_description_planning,
                     planning_pipelines,
-
                 ],
-                env=dict(os.environ)
+                env=launch_env,
             )
             self.launch_description.add_action(rviz_node)
 

unilabos/devices/liquid_handling/liquid_handler_abstract.py

@@ -10,6 +10,7 @@ from pylabrobot.liquid_handling import LiquidHandler, LiquidHandlerBackend, Liqu
 from pylabrobot.liquid_handling.liquid_handler import TipPresenceProbingMethod
 from pylabrobot.liquid_handling.standard import GripDirection
 from pylabrobot.resources.errors import TooLittleLiquidError, TooLittleVolumeError
+from pylabrobot.resources.volume_tracker import no_volume_tracking
 from pylabrobot.resources import (
     Resource,
     TipRack,
@@ -70,7 +71,8 @@ class LiquidHandlerMiddleware(LiquidHandler):
         if simulator:
             if joint_config:
                 self._simulate_backend = UniLiquidHandlerRvizBackend(
-                    channel_num, kwargs["total_height"], joint_config=joint_config, lh_device_id=deck.name
+                    channel_num, kwargs["total_height"], joint_config=joint_config, lh_device_id=deck.name,
+                    simulate_rviz=kwargs.get("simulate_rviz", True)
                 )
             else:
                 self._simulate_backend = LiquidHandlerChatterboxBackend(channel_num)
@@ -207,54 +209,66 @@ class LiquidHandlerMiddleware(LiquidHandler):
         offsets: Optional[List[Coordinate]] = None,
         liquid_height: Optional[List[Optional[float]]] = None,
         blow_out_air_volume: Optional[List[Optional[float]]] = None,
-        spread: Literal["wide", "tight", "custom"] = "wide",
+        spread: Literal["wide", "tight", "custom"] = "custom",
         **backend_kwargs,
     ):
         if spread == "":
-            spread = "wide"
+            spread = "custom"
 
-        def _safe_aspirate_volumes(_resources: Sequence[Container], _vols: List[float]) -> List[float]:
-            """将 aspirate 体积裁剪到源容器当前液量范围内，避免 volume tracker 报错。"""
-            safe: List[float] = []
-            for res, vol in zip(_resources, _vols):
-                req = max(float(vol), 0.0)
-                used_volume = None
+        for res in resources:
+            tracker = getattr(res, "tracker", None)
+            if tracker is None or getattr(tracker, "is_disabled", False):
+                continue
+            history = getattr(tracker, "liquid_history", None)
+            if tracker.get_used_volume() <= 0 and isinstance(history, list) and len(history) == 0:
+                fill_vol = tracker.max_volume if tracker.max_volume > 0 else 50000
                 try:
-                    tracker = getattr(res, "tracker", None)
-                    if bool(getattr(tracker, "is_disabled", False)):
-                        # tracker 关闭时（例如预吸空气），不按液体体积裁剪
-                        safe.append(req)
-                        continue
-                    get_used = getattr(tracker, "get_used_volume", None)
-                    if callable(get_used):
-                        used_volume = get_used()
+                    tracker.add_liquid(fill_vol)
                 except Exception:
-                    used_volume = None
+                    tracker.liquid_history.append(("auto_init", fill_vol))
 
-                if isinstance(used_volume, (int, float)):
-                    req = min(req, max(float(used_volume), 0.0))
-                safe.append(req)
-            return safe
-
-        actual_vols = _safe_aspirate_volumes(resources, vols)
-        if actual_vols != vols and hasattr(self, "_ros_node") and self._ros_node is not None:
-            self._ros_node.lab_logger().warning(f"[aspirate] volume adjusted, requested_vols={vols}, actual_vols={actual_vols}")
         if self._simulator:
-            return await self._simulate_handler.aspirate(
-                resources,
-                actual_vols,
-                use_channels,
-                flow_rates,
-                offsets,
-                liquid_height,
-                blow_out_air_volume,
-                spread,
-                **backend_kwargs,
-            )
+            try:
+                return await self._simulate_handler.aspirate(
+                    resources,
+                    vols,
+                    use_channels,
+                    flow_rates,
+                    offsets,
+                    liquid_height,
+                    blow_out_air_volume,
+                    spread,
+                    **backend_kwargs,
+                )
+            except (TooLittleLiquidError, TooLittleVolumeError) as e:
+                tracker_info = []
+                for r in resources:
+                    t = r.tracker
+                    tracker_info.append(
+                        f"{r.name}(used={t.get_used_volume():.1f}, "
+                        f"free={t.get_free_volume():.1f}, max={r.max_volume})"
+                    )
+                if hasattr(self, "_ros_node") and self._ros_node is not None:
+                    self._ros_node.lab_logger().warning(
+                        f"[aspirate] volume tracker error, bypassing tracking. "
+                        f"error={e}, vols={vols}, trackers={tracker_info}"
+                    )
+                with no_volume_tracking():
+                    return await self._simulate_handler.aspirate(
+                        resources,
+                        vols,
+                        use_channels,
+                        flow_rates,
+                        offsets,
+                        liquid_height,
+                        blow_out_air_volume,
+                        spread,
+                        **backend_kwargs,
+                    )
         try:
             await super().aspirate(
                 resources,
-                actual_vols,
+                vols,
                 use_channels,
                 flow_rates,
                 offsets,
@@ -267,7 +281,7 @@ class LiquidHandlerMiddleware(LiquidHandler):
             if "Resource is too small to space channels" in str(e) and spread != "custom":
                 await super().aspirate(
                     resources,
-                    actual_vols,
+                    vols,
                     use_channels,
                     flow_rates,
                     offsets,
@@ -278,35 +292,15 @@ class LiquidHandlerMiddleware(LiquidHandler):
                 )
             else:
                 raise
-        except TooLittleLiquidError:
-            # 再兜底一次：按实时可用液量重算后重试，避免状态更新竞争导致的瞬时不足
-            retry_vols = _safe_aspirate_volumes(resources, actual_vols)
-            if any(v > 0 for v in retry_vols):
-                await super().aspirate(
-                    resources,
-                    retry_vols,
-                    use_channels,
-                    flow_rates,
-                    offsets,
-                    liquid_height,
-                    blow_out_air_volume,
-                    spread,
-                    **backend_kwargs,
-                )
-                actual_vols = retry_vols
-            else:
-                actual_vols = retry_vols
 
         res_samples = []
         res_volumes = []
-        # 处理 use_channels 为 None 的情况（通常用于单通道操作）
         if use_channels is None:
-            # 对于单通道操作，推断通道为 [0]
             channels_to_use = [0] * len(resources)
         else:
             channels_to_use = use_channels
 
-        for resource, volume, channel in zip(resources, actual_vols, channels_to_use):
+        for resource, volume, channel in zip(resources, vols, channels_to_use):
             sample_uuid_value = getattr(resource, "unilabos_extra", {}).get(EXTRA_SAMPLE_UUID, None)
             res_samples.append({"name": resource.name, "sample_uuid": sample_uuid_value})
             res_volumes.append(volume)
@@ -353,17 +347,43 @@ class LiquidHandlerMiddleware(LiquidHandler):
         actual_vols = _safe_dispense_volumes(resources, vols)
 
         if self._simulator:
-            return await self._simulate_handler.dispense(
-                resources,
-                actual_vols,
-                use_channels,
-                flow_rates,
-                offsets,
-                liquid_height,
-                blow_out_air_volume,
-                spread,
-                **backend_kwargs,
-            )
+            try:
+                return await self._simulate_handler.dispense(
+                    resources,
+                    actual_vols,
+                    use_channels,
+                    flow_rates,
+                    offsets,
+                    liquid_height,
+                    blow_out_air_volume,
+                    spread,
+                    **backend_kwargs,
+                )
+            except (TooLittleLiquidError, TooLittleVolumeError) as e:
+                tracker_info = []
+                for r in resources:
+                    t = r.tracker
+                    tracker_info.append(
+                        f"{r.name}(used={t.get_used_volume():.1f}, "
+                        f"free={t.get_free_volume():.1f}, max={r.max_volume})"
+                    )
+                if hasattr(self, "_ros_node") and self._ros_node is not None:
+                    self._ros_node.lab_logger().warning(
+                        f"[dispense] volume tracker error, bypassing tracking. "
+                        f"error={e}, vols={actual_vols}, trackers={tracker_info}"
+                    )
+                with no_volume_tracking():
+                    return await self._simulate_handler.dispense(
+                        resources,
+                        actual_vols,
+                        use_channels,
+                        flow_rates,
+                        offsets,
+                        liquid_height,
+                        blow_out_air_volume,
+                        spread,
+                        **backend_kwargs,
+                    )
         try:
             await super().dispense(
                 resources,
@@ -751,6 +771,7 @@ class LiquidHandlerAbstract(LiquidHandlerMiddleware):
         simulator: bool = False,
         channel_num: int = 8,
         total_height: float = 310,
+        **kwargs,
     ):
         """Initialize a LiquidHandler.
 
@@ -790,14 +811,20 @@ class LiquidHandlerAbstract(LiquidHandlerMiddleware):
                         except Exception:
                             backend_cls = None
                 if backend_cls is not None and isinstance(backend_cls, type):
-                    backend_type = backend_cls(**backend_dict)  # pass the rest of dict as kwargs
+                    if simulator:
+                        backend_type = LiquidHandlerChatterboxBackend(channel_num)
+                    else:
+                        init_kwargs = dict(backend_dict)
+                        init_kwargs["total_height"] = total_height
+                        init_kwargs.update(kwargs)
+                        backend_type = backend_cls(**init_kwargs)
             except Exception as exc:
                 raise RuntimeError(f"Failed to convert backend type '{type_str}' to class: {exc}")
         else:
             backend_type = backend
         self._simulator = simulator
         self.group_info = dict()
-        super().__init__(backend_type, deck, simulator, channel_num)
+        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
@@ -847,6 +874,14 @@ class LiquidHandlerAbstract(LiquidHandlerMiddleware):
                     local = cast(Union[Container, TipRack], plr)
                 if hasattr(plr, "unilabos_extra") and hasattr(local, "unilabos_extra"):
                     local.unilabos_extra = getattr(plr, "unilabos_extra", {}).copy()
+                if local is not plr and hasattr(plr, "tracker") and hasattr(local, "tracker"):
+                    local_tracker = local.tracker
+                    plr_tracker = plr.tracker
+                    local_history = getattr(local_tracker, "liquid_history", None)
+                    plr_history = getattr(plr_tracker, "liquid_history", None)
+                    if (isinstance(local_history, list) and len(local_history) == 0
+                            and isinstance(plr_history, list) and len(plr_history) > 0):
+                        local_tracker.liquid_history = list(plr_history)
                 resolved.append(local)
             if len(resolved) != len(uuids):
                 raise ValueError(
@@ -856,8 +891,18 @@ class LiquidHandlerAbstract(LiquidHandlerMiddleware):
             resolved = _resolve_from_local_by_uuids()
 
         result = list(items)
-        for (idx, _), plr in zip(dict_items, resolved):
-            result[idx] = plr
+        for (idx, orig_dict), res in zip(dict_items, resolved):
+            if isinstance(orig_dict, dict) and hasattr(res, "tracker"):
+                tracker = res.tracker
+                local_history = getattr(tracker, "liquid_history", None)
+                if isinstance(local_history, list) and len(local_history) == 0:
+                    data = orig_dict.get("data") or {}
+                    dict_history = data.get("liquid_history")
+                    if isinstance(dict_history, list) and len(dict_history) > 0:
+                        tracker.liquid_history = [
+                            (name, float(vol)) for name, vol in dict_history
+                        ]
+            result[idx] = res
         return result
 
     @classmethod
@@ -1058,7 +1103,7 @@ class LiquidHandlerAbstract(LiquidHandlerMiddleware):
                     for _ in range(len(sources)):
                         tip = []
                         for __ in range(len(use_channels)):
-                            tip.extend(self._get_next_tip())
+                            tip.append(self._get_next_tip())
                         await self.pick_up_tips(tip)
                         await self.aspirate(
                             resources=[sources[_]],
@@ -1098,7 +1143,7 @@ class LiquidHandlerAbstract(LiquidHandlerMiddleware):
                     for i in range(0, len(sources), 8):
                         tip = []
                         for _ in range(len(use_channels)):
-                            tip.extend(self._get_next_tip())
+                            tip.append(self._get_next_tip())
                         await self.pick_up_tips(tip)
                         current_targets = waste_liquid[i : i + 8]
                         current_reagent_sources = sources[i : i + 8]
@@ -1192,7 +1237,7 @@ class LiquidHandlerAbstract(LiquidHandlerMiddleware):
                 for _ in range(len(targets)):
                     tip = []
                     for x in range(len(use_channels)):
-                        tip.extend(self._get_next_tip())
+                        tip.append(self._get_next_tip())
                     await self.pick_up_tips(tip)
 
                     await self.aspirate(
@@ -1244,7 +1289,7 @@ class LiquidHandlerAbstract(LiquidHandlerMiddleware):
                 for i in range(0, len(targets), 8):
                     tip = []
                     for _ in range(len(use_channels)):
-                        tip.extend(self._get_next_tip())
+                        tip.append(self._get_next_tip())
                     await self.pick_up_tips(tip)
                     current_targets = targets[i : i + 8]
                     current_reagent_sources = reagent_sources[i : i + 8]
@@ -1432,6 +1477,7 @@ class LiquidHandlerAbstract(LiquidHandlerMiddleware):
 
         if len(use_channels) != 8:
             max_len = max(num_sources, num_targets)
+            prev_dropped = True  # 循环开始前通道上无 tip
             for i in range(max_len):
 
                 # 辅助函数：
@@ -1491,6 +1537,29 @@ class LiquidHandlerAbstract(LiquidHandlerMiddleware):
                 if delays is not None:
                     kwargs['delays'] = safe_get(delays, i)
 
+                cur_source = sources[i % num_sources]
+                cur_target = targets[i % num_targets]
+
+                # drop: 仅当下一轮的 source 和 target 都相同时才保留 tip（下一轮可以复用）
+                drop_tip = True
+                if i < max_len - 1:
+                    next_source = sources[(i + 1) % num_sources]
+                    next_target = targets[(i + 1) % num_targets]
+                    if cur_target is next_target and cur_source is next_source:
+                        drop_tip = False
+
+                # pick_up: 仅当上一轮保留了 tip（未 drop）且 source 相同时才复用
+                pick_up_tip = True
+                if i > 0 and not prev_dropped:
+                    prev_source = sources[(i - 1) % num_sources]
+                    if cur_source is prev_source:
+                        pick_up_tip = False
+
+                prev_dropped = drop_tip
+
+                kwargs['pick_up'] = pick_up_tip
+                kwargs['drop'] = drop_tip
+
                 await self._transfer_base_method(**kwargs)
 
 
@@ -1506,6 +1575,8 @@ class LiquidHandlerAbstract(LiquidHandlerMiddleware):
         use_channels: List[int],
         asp_vols: List[float],
         dis_vols: List[float],
+        pick_up: bool = True,
+        drop: bool = True,
         **kwargs
     ):
 
@@ -1526,8 +1597,9 @@ class LiquidHandlerAbstract(LiquidHandlerMiddleware):
         delays = kwargs.get('delays')
 
         tip = []
-        tip.extend(self._get_next_tip())
-        await self.pick_up_tips(tip)
+        if pick_up:
+            tip.append(self._get_next_tip())
+            await self.pick_up_tips(tip)
         blow_out_air_volume_before_vol = 0.0
         if blow_out_air_volume_before is not None and len(blow_out_air_volume_before) > 0:
             blow_out_air_volume_before_vol = float(blow_out_air_volume_before[0] or 0.0)
@@ -1609,7 +1681,8 @@ class LiquidHandlerAbstract(LiquidHandlerMiddleware):
         if delays is not None and len(delays) > 1:
             await self.custom_delay(seconds=delays[0])
         await self.touch_tip(targets[0])
-        await self.discard_tips(use_channels=use_channels)
+        if drop:
+            await self.discard_tips(use_channels=use_channels)
 
     # except Exception as e:
     #     traceback.print_exc()
@@ -1729,23 +1802,77 @@ class LiquidHandlerAbstract(LiquidHandlerMiddleware):
     def iter_tips(self, tip_racks: Sequence[TipRack]) -> Iterator[Resource]:
         """Yield tips from a list of TipRacks one-by-one until depleted."""
         for rack in tip_racks:
-            for tip in rack:
-                yield tip
-        # raise RuntimeError("Out of tips!")
+            if isinstance(rack, TipSpot):
+                yield rack
+            elif isinstance(rack, TipRack):
+                for item in rack:
+                    if isinstance(item, list):
+                        yield from item
+                    else:
+                        yield item
 
     def _get_next_tip(self):
         """从 current_tip 迭代器获取下一个 tip，耗尽时抛出明确错误而非 StopIteration"""
         try:
             return next(self.current_tip)
         except StopIteration as e:
-            raise RuntimeError("Tip rack exhausted: no more tips available for transfer") from e
+            diag_parts = []
+            tip_racks = getattr(self, 'tip_racks', None)
+            if tip_racks is not None:
+                for idx, rack in enumerate(tip_racks):
+                    r_name = getattr(rack, 'name', '?')
+                    r_type = type(rack).__name__
+                    is_tr = isinstance(rack, TipRack)
+                    is_ts = isinstance(rack, TipSpot)
+                    n_children = len(getattr(rack, 'children', []))
+                    diag_parts.append(
+                        f"rack[{idx}] name={r_name}, type={r_type}, "
+                        f"is_TipRack={is_tr}, is_TipSpot={is_ts}, children={n_children}"
+                    )
+            else:
+                diag_parts.append("tip_racks=None")
+            by_type = getattr(self, '_tip_racks_by_type', {})
+            diag_parts.append(f"_tip_racks_by_type keys={list(by_type.keys())}")
+            raise RuntimeError(
+                f"Tip rack exhausted: no more tips available for transfer. "
+                f"Diagnostics: {'; '.join(diag_parts)}"
+            ) from e
 
     def set_tiprack(self, tip_racks: Sequence[TipRack]):
-        """Set the tip racks for the liquid handler."""
+        """Set the tip racks for the liquid handler.
+
+        Groups tip racks by type name (``type(rack).__name__``).
+        - Only actual TipRack / TipSpot instances are registered.
+        - If a rack has already been registered (by ``name``), it is skipped.
+        - If a rack is new and its type already exists, it is appended to that type's list.
+        - If the type is new, a new key-value pair is created.
+
+        If the current ``tip_racks`` contain no valid TipRack/TipSpot (e.g. a
+        Plate was passed by mistake), the iterator falls back to all previously
+        registered racks.
+        """
+        if not hasattr(self, '_tip_racks_by_type'):
+            self._tip_racks_by_type: Dict[str, List[TipRack]] = {}
+            self._seen_rack_names: Set[str] = set()
+
+        for rack in tip_racks:
+            if not isinstance(rack, (TipRack, TipSpot)):
+                continue
+            rack_name = rack.name if hasattr(rack, 'name') else str(id(rack))
+            if rack_name in self._seen_rack_names:
+                continue
+            self._seen_rack_names.add(rack_name)
+            type_key = type(rack).__name__
+            if type_key not in self._tip_racks_by_type:
+                self._tip_racks_by_type[type_key] = []
+            self._tip_racks_by_type[type_key].append(rack)
+
+        valid_racks = [r for r in tip_racks if isinstance(r, (TipRack, TipSpot))]
+        if not valid_racks:
+            valid_racks = [r for racks in self._tip_racks_by_type.values() for r in racks]
 
         self.tip_racks = tip_racks
-        tip_iter = self.iter_tips(tip_racks)
-        self.current_tip = tip_iter
+        self.current_tip = self.iter_tips(valid_racks)
 
     async def move_to(self, well: Well, dis_to_top: float = 0, channel: int = 0):
         """

unilabos/devices/liquid_handling/prcxi/prcxi.py

@@ -103,7 +103,7 @@ class PRCXI9300Deck(Deck):
 
     def __init__(self, name: str, size_x: float, size_y: float, size_z: float,
                  sites: Optional[List[Dict[str, Any]]] = None, **kwargs):
-        super().__init__(name, size_x, size_y, size_z)
+        super().__init__( size_x, size_y, size_z, name=name)
         if sites is not None:
             self.sites: List[Dict[str, Any]] = [dict(s) for s in sites]
         else:

unilabos/devices/liquid_handling/rviz_backend.py

@@ -59,6 +59,7 @@ class UniLiquidHandlerRvizBackend(LiquidHandlerBackend):
     self.total_height = total_height
     self.joint_config = kwargs.get("joint_config", None)
     self.lh_device_id = kwargs.get("lh_device_id", "lh_joint_publisher")
+    self.simulate_rviz = kwargs.get("simulate_rviz", False)
     if not rclpy.ok():
         rclpy.init()
     self.joint_state_publisher = None
@@ -69,7 +70,7 @@ class UniLiquidHandlerRvizBackend(LiquidHandlerBackend):
     self.joint_state_publisher = LiquidHandlerJointPublisher(
                                 joint_config=self.joint_config,
                                 lh_device_id=self.lh_device_id,
-                                simulate_rviz=True)
+                                simulate_rviz=self.simulate_rviz)
 
     # 启动ROS executor
     self.executor = rclpy.executors.MultiThreadedExecutor()

unilabos/devices/ros_dev/liquid_handler_joint_publisher_node.py

@@ -42,6 +42,7 @@ class LiquidHandlerJointPublisher(Node):
             while self.resource_action is None:
                 self.resource_action = self.check_tf_update_actions()
                 time.sleep(1)
+                self.get_logger().info(f'Waiting for TfUpdate server: {self.resource_action}')
 
             self.resource_action_client = ActionClient(self, SendCmd, self.resource_action)
             while not self.resource_action_client.wait_for_server(timeout_sec=1.0):

unilabos/ros/main_slave_run.py

@@ -51,6 +51,7 @@ def main(
     bridges: List[Any] = [],
     visual: str = "disable",
     resources_mesh_config: dict = {},
+    resources_mesh_resource_list: list = [],
     rclpy_init_args: List[str] = ["--log-level", "debug"],
     discovery_interval: float = 15.0,
 ) -> None:
@@ -77,12 +78,12 @@ def main(
     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 []
-        )
+        # 优先使用从 main.py 传入的完整资源列表（包含所有子资源）
+        if resources_mesh_resource_list:
+            resources_list = resources_mesh_resource_list
+        else:
+            # fallback: 从 ResourceTreeSet 获取
+            resources_list = [node.res_content.model_dump(by_alias=True) for node in resources_config.all_nodes]
         resource_mesh_manager = ResourceMeshManager(
             resources_mesh_config,
             resources_list,
@@ -90,7 +91,7 @@ def main(
             device_id="resource_mesh_manager",
             device_uuid=str(uuid.uuid4()),
         )
-        joint_republisher = JointRepublisher("joint_republisher", host_node.resource_tracker)
+        joint_republisher = JointRepublisher("joint_republisher","joint_republisher", host_node.resource_tracker)
         # lh_joint_pub = LiquidHandlerJointPublisher(
         #     resources_config=resources_list, resource_tracker=host_node.resource_tracker
         # )
@@ -114,6 +115,7 @@ def slave(
     bridges: List[Any] = [],
     visual: str = "disable",
     resources_mesh_config: dict = {},
+    resources_mesh_resource_list: list = [],
     rclpy_init_args: List[str] = ["--log-level", "debug"],
 ) -> None:
     """从节点函数"""
@@ -208,12 +210,12 @@ def slave(
     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 []
-        )
+        # 优先使用从 main.py 传入的完整资源列表（包含所有子资源）
+        if resources_mesh_resource_list:
+            resources_list = resources_mesh_resource_list
+        else:
+            # fallback: 从 ResourceTreeSet 获取
+            resources_list = [node.res_content.model_dump(by_alias=True) for node in resources_config.all_nodes]
         resource_mesh_manager = ResourceMeshManager(
             resources_mesh_config,
             resources_list,

unilabos/ros/nodes/presets/resource_mesh_manager.py

@@ -23,17 +23,32 @@ from unilabos_msgs.action import SendCmd
 from rclpy.action.server import ServerGoalHandle
 from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode,DeviceNodeResourceTracker
 from unilabos.resources.graphio import initialize_resources
+from unilabos.resources.resource_tracker import EXTRA_CLASS
 from unilabos.registry.registry import lab_registry
 
 class ResourceMeshManager(BaseROS2DeviceNode):
-    def __init__(self, resource_model: dict, resource_config: list,resource_tracker, device_id: str = "resource_mesh_manager", registry_name: str = "", rate=50, **kwargs):
+    def __init__(
+        self,
+        resource_model: Optional[dict] = None,
+        resource_config: Optional[list] = None,
+        resource_tracker=None,
+        device_id: str = "resource_mesh_manager",
+        registry_name: str = "",
+        rate=50,
+        **kwargs,
+    ):
         """初始化资源网格管理器节点
-        
+
         Args:
-            resource_model (dict): 资源模型字典,包含资源的3D模型信息
-            resource_config (dict): 资源配置字典,包含资源的配置信息
-            device_id (str): 节点名称
+            resource_model: 资源模型字典（可选，为 None 时自动从 registry 构建）
+            resource_config: 资源配置列表（可选，为 None 时启动后通过 ActionServer 或 load_from_resource_tree 加载）
+            resource_tracker: 资源追踪器
+            device_id: 节点名称
+            rate: TF 发布频率
         """
+        if resource_tracker is None:
+            resource_tracker = DeviceNodeResourceTracker()
+
         super().__init__(
             driver_instance=self,
             device_id=device_id,
@@ -42,12 +57,14 @@ class ResourceMeshManager(BaseROS2DeviceNode):
             action_value_mappings={},
             hardware_interface={},
             print_publish=False,
-            resource_tracker=resource_tracker, 
+            resource_tracker=resource_tracker,
             device_uuid=kwargs.get("uuid", str(uuid.uuid4())),
-        ) 
+        )
 
-        self.resource_model         = resource_model
-        self.resource_config_dict   = {item['uuid']: item for item in resource_config}
+        self.resource_model         = resource_model if resource_model is not None else {}
+        self.resource_config_dict   = (
+            {item['uuid']: item for item in resource_config} if resource_config else {}
+        )
         self.move_group_ready       = False
         self.resource_tf_dict       = {}
         self.tf_broadcaster         = TransformBroadcaster(self)
@@ -63,7 +80,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
         self.mesh_path = Path(__file__).parent.parent.parent.parent.absolute()
         self.msg_type = 'resource_status'
         self.resource_status_dict = {}
-        
+
         callback_group = ReentrantCallbackGroup()
         self._get_planning_scene_service = self.create_client(
             srv_type=GetPlanningScene,
@@ -76,8 +93,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
             ),
             callback_group=callback_group,
         )
-        
-        # Create a service for applying the planning scene
+
         self._apply_planning_scene_service = self.create_client(
             srv_type=ApplyPlanningScene,
             srv_name="/apply_planning_scene",
@@ -103,27 +119,36 @@ class ResourceMeshManager(BaseROS2DeviceNode):
             AttachedCollisionObject, "/attached_collision_object", 0
         )
 
-        # 创建一个Action Server用于修改resource_tf_dict
         self._action_server = ActionServer(
             self,
             SendCmd,
             f"tf_update",
             self.tf_update,
-            callback_group=callback_group
+            callback_group=callback_group,
         )
 
-        # 创建一个Action Server用于添加新的资源模型与resource_tf_dict
         self._add_resource_mesh_action_server = ActionServer(
             self,
             SendCmd,
             f"add_resource_mesh",
             self.add_resource_mesh_callback,
-            callback_group=callback_group
+            callback_group=callback_group,
         )
 
-        self.resource_tf_dict = self.resource_mesh_setup(self.resource_config_dict)
-        self.create_timer(1/self.rate, self.publish_resource_tf)
-        self.create_timer(1/self.rate, self.check_resource_pose_changes)
+        self._reload_resource_mesh_action_server = ActionServer(
+            self,
+            SendCmd,
+            f"reload_resource_mesh",
+            self._reload_resource_mesh_callback,
+            callback_group=callback_group,
+        )
+
+        if self.resource_config_dict:
+            self.resource_tf_dict = self.resource_mesh_setup(self.resource_config_dict)
+        else:
+            self.get_logger().info("未提供 resource_config，将通过 ActionServer 或 load_from_resource_tree 加载")
+        self.create_timer(1 / self.rate, self.publish_resource_tf)
+        self.create_timer(1 / self.rate, self.check_resource_pose_changes)
 
     def check_move_group_ready(self):
         """检查move_group节点是否已初始化完成"""
@@ -140,56 +165,156 @@ class ResourceMeshManager(BaseROS2DeviceNode):
             self.add_resource_collision_meshes(self.resource_tf_dict)
 
        
-    def add_resource_mesh_callback(self, goal_handle : ServerGoalHandle):
+    def _build_resource_model_for_config(self, resource_config_dict: dict):
+        """从 registry 中为给定的资源配置自动构建 resource_model（mesh 信息）"""
+        registry = lab_registry
+        for _uuid, res_cfg in resource_config_dict.items():
+            resource_id = res_cfg.get('id', '')
+            resource_class = res_cfg.get('class', '')
+            if not resource_class:
+                continue
+            if resource_class not in registry.resource_type_registry:
+                continue
+            reg_entry = registry.resource_type_registry[resource_class]
+            if 'model' not in reg_entry:
+                continue
+            model_config = reg_entry['model']
+            if model_config.get('type') != 'resource':
+                continue
+            if resource_id in self.resource_model:
+                continue
+            self.resource_model[resource_id] = {
+                'mesh': f"{str(self.mesh_path)}/device_mesh/resources/{model_config['mesh']}",
+                'mesh_tf': model_config['mesh_tf'],
+            }
+            if model_config.get('children_mesh') is not None:
+                self.resource_model[f"{resource_id}_"] = {
+                    'mesh': f"{str(self.mesh_path)}/device_mesh/resources/{model_config['children_mesh']}",
+                    'mesh_tf': model_config['children_mesh_tf'],
+                }
+
+    def load_from_resource_tree(self):
+        """从 resource_tracker 中读取资源树，自动构建 resource_config_dict / resource_model 并刷新 TF"""
+        new_config_dict: dict = {}
+
+        def _collect_plr_resource(res, parent_uuid: Optional[str] = None):
+            res_uuid = getattr(res, 'unilabos_uuid', None)
+            if not res_uuid:
+                res_uuid = str(uuid.uuid4())
+            extra = getattr(res, 'unilabos_extra', {}) or {}
+            resource_class = extra.get(EXTRA_CLASS, '')
+
+            location = getattr(res, 'location', None)
+            pos_x = float(location.x) if location else 0.0
+            pos_y = float(location.y) if location else 0.0
+            pos_z = float(location.z) if location else 0.0
+
+            rotation = extra.get('rotation', {'x': 0, 'y': 0, 'z': 0})
+
+            new_config_dict[res_uuid] = {
+                'id': res.name,
+                'uuid': res_uuid,
+                'class': resource_class,
+                'parent_uuid': parent_uuid,
+                'pose': {
+                    'position': {'x': pos_x, 'y': pos_y, 'z': pos_z},
+                    'rotation': rotation,
+                },
+            }
+
+            for child in getattr(res, 'children', []) or []:
+                _collect_plr_resource(child, res_uuid)
+
+        for resource in self.resource_tracker.resources:
+            root_parent_uuid = None
+            plr_parent = getattr(resource, 'parent', None)
+            if plr_parent is not None:
+                root_parent_uuid = getattr(plr_parent, 'unilabos_uuid', None)
+            _collect_plr_resource(resource, root_parent_uuid)
+
+        if not new_config_dict:
+            self.get_logger().warning("resource_tracker 中没有找到任何资源")
+            return
+
+        self.resource_config_dict = {**self.resource_config_dict, **new_config_dict}
+        self._build_resource_model_for_config(new_config_dict)
+
+        tf_dict = self.resource_mesh_setup(new_config_dict)
+        self.resource_tf_dict = {**self.resource_tf_dict, **tf_dict}
+        self.publish_resource_tf()
+        if self.move_group_ready:
+            self.add_resource_collision_meshes(tf_dict)
+        self.get_logger().info(f"从资源树加载了 {len(new_config_dict)} 个资源")
+
+    def _reload_resource_mesh_callback(self, goal_handle: ServerGoalHandle):
+        """ActionServer 回调：重新从资源树加载所有 mesh"""
+        try:
+            self.load_from_resource_tree()
+        except Exception as e:
+            self.get_logger().error(f"重新加载资源失败: {e}")
+            goal_handle.abort()
+            return SendCmd.Result(success=False)
+        goal_handle.succeed()
+        return SendCmd.Result(success=True)
+
+    def add_resource_mesh_callback(self, goal_handle: ServerGoalHandle):
         tf_update_msg = goal_handle.request
-        try:    
-            self.add_resource_mesh(tf_update_msg.command)
+        try:
+            parsed = json.loads(tf_update_msg.command.replace("'", '"'))
+            if 'resources' in parsed:
+                for res_config in parsed['resources']:
+                    self.add_resource_mesh(json.dumps(res_config))
+            else:
+                self.add_resource_mesh(tf_update_msg.command)
         except Exception as e:
             self.get_logger().error(f"添加资源失败: {e}")
             goal_handle.abort()
             return SendCmd.Result(success=False)
         goal_handle.succeed()
         return SendCmd.Result(success=True)
-    
-    def add_resource_mesh(self,resource_config_str:str):
-        """刷新资源配置"""
 
+    def add_resource_mesh(self, resource_config_str: str):
+        """添加单个资源的 mesh 配置"""
         registry = lab_registry
-        resource_config = json.loads(resource_config_str.replace("'",'"'))
-        
+        resource_config = json.loads(resource_config_str.replace("'", '"'))
+
         if resource_config['id'] in self.resource_config_dict:
             self.get_logger().info(f'资源 {resource_config["id"]} 已存在')
             return
-        if resource_config['class'] in registry.resource_type_registry.keys():
-            model_config = registry.resource_type_registry[resource_config['class']]['model']
-            if model_config['type'] == 'resource':
-                self.resource_model[resource_config['id']] = {
-                    'mesh': f"{str(self.mesh_path)}/device_mesh/resources/{model_config['mesh']}",
-                    'mesh_tf': model_config['mesh_tf']}
-                if 'children_mesh' in model_config.keys():
-                    self.resource_model[f"{resource_config['id']}_"] = {
-                        'mesh': f"{str(self.mesh_path)}/device_mesh/resources/{model_config['children_mesh']}",
-                        'mesh_tf': model_config['children_mesh_tf']
+        resource_class = resource_config.get('class', '')
+        if resource_class and resource_class in registry.resource_type_registry:
+            reg_entry = registry.resource_type_registry[resource_class]
+            if 'model' in reg_entry:
+                model_config = reg_entry['model']
+                if model_config.get('type') == 'resource':
+                    self.resource_model[resource_config['id']] = {
+                        'mesh': f"{str(self.mesh_path)}/device_mesh/resources/{model_config['mesh']}",
+                        'mesh_tf': model_config['mesh_tf'],
                     }
+                    if model_config.get('children_mesh') is not None:
+                        self.resource_model[f"{resource_config['id']}_"] = {
+                            'mesh': f"{str(self.mesh_path)}/device_mesh/resources/{model_config['children_mesh']}",
+                            'mesh_tf': model_config['children_mesh_tf'],
+                        }
         resources = initialize_resources([resource_config])
         resource_dict = {item['id']: item for item in resources}
-        self.resource_config_dict = {**self.resource_config_dict,**resource_dict}
+        self.resource_config_dict = {**self.resource_config_dict, **resource_dict}
         tf_dict = self.resource_mesh_setup(resource_dict)
-        self.resource_tf_dict = {**self.resource_tf_dict,**tf_dict}
+        self.resource_tf_dict = {**self.resource_tf_dict, **tf_dict}
         self.publish_resource_tf()
         self.add_resource_collision_meshes(tf_dict)
 
-
-    def resource_mesh_setup(self, resource_config_dict:dict):
-        """move_group初始化完成后的设置"""
+    def resource_mesh_setup(self, resource_config_dict: dict):
+        """根据资源配置字典设置 TF 关系"""
         self.get_logger().info('开始设置资源网格管理器')
-        #遍历resource_config中的资源配置，判断panent是否在resource_model中，
         resource_tf_dict = {}
         for resource_uuid, resource_config in resource_config_dict.items():
             parent = None
             resource_id = resource_config['id']
-            if resource_config['parent_uuid'] is not None and resource_config['parent_uuid'] != "":
-                parent = resource_config_dict[resource_config['parent_uuid']]['id']
+            parent_uuid = resource_config.get('parent_uuid')
+            if parent_uuid is not None and parent_uuid != "":
+                parent_entry = resource_config_dict.get(parent_uuid) or self.resource_config_dict.get(parent_uuid)
+                parent = parent_entry['id'] if parent_entry else None
 
             parent_link = 'world'
             if parent in self.resource_model:

unilabos/workflow/common.py

@@ -412,6 +412,8 @@ def build_protocol_graph(
         param=None,
     )
 
+    trash_create_node_id = None  # 记录 trash 的 create_resource 节点
+
     # 为每个唯一的 slot 创建 create_resource 节点
     for slot, info in slots_info.items():
         node_id = str(uuid.uuid4())
@@ -445,6 +447,8 @@ def build_protocol_graph(
         slot_to_create_resource[slot] = node_id
         if object_type == "tiprack":
             resource_last_writer[info["labware_id"]] = f"{node_id}:labware"
+        if object_type == "trash":
+            trash_create_node_id = node_id
         # create_resource 之间不需要 ready 连接
 
     # ==================== 第二步：为每个 reagent 创建 set_liquid_from_plate 节点 ====================
@@ -516,8 +520,8 @@ def build_protocol_graph(
         # set_liquid_from_plate 的输出 output_wells 用于连接 transfer_liquid
         resource_last_writer[labware_id] = f"{node_id}:output_wells"
 
-    # transfer_liquid 之间通过 ready 串联，从 None 开始
-    last_control_node_id = None
+    # transfer_liquid 之间通过 ready 串联；若存在 trash 节点，第一个 transfer_liquid 从 trash 的 ready 开始
+    last_control_node_id = trash_create_node_id
 
     # 端口名称映射：JSON 字段名 -> 实际 handle key
     INPUT_PORT_MAPPING = {