修改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

@@ -71,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)
@@ -770,6 +771,7 @@ class LiquidHandlerAbstract(LiquidHandlerMiddleware):
         simulator: bool = False,
         channel_num: int = 8,
         total_height: float = 310,
+        **kwargs,
     ):
         """Initialize a LiquidHandler.
 
@@ -809,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

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,
@@ -46,8 +61,10 @@ class ResourceMeshManager(BaseROS2DeviceNode):
             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)
@@ -77,7 +94,6 @@ 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._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)
-        self.create_timer(1/self.rate, self.publish_resource_tf)
-        self.create_timer(1/self.rate, self.check_resource_pose_changes)
+        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,9 +165,106 @@ 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:
+            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}")
@@ -151,45 +273,48 @@ class ResourceMeshManager(BaseROS2DeviceNode):
         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':
+        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 'children_mesh' in model_config.keys():
+                        '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']
+                            '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: