Device visualization (#22)

* add 3d visualization

* 完成在main中启动设备可视化

完成在main中启动设备可视化，并输出物料ID：mesh的对应关系resource_model

添加物料模型管理类，遍历物料与resource_model，完成TF数据收集

* 完成TF发布

* 修改模型方向，在yaml中添加变换属性

* 添加物料tf变化时，发送topic到前端

另外修改了物料初始化的方法，防止在tf还未发布时提前建立物料模型与发布话题

* 添加关节发布节点与物料可视化节点进入unilab

* 使用json启动plr与3D模型仿真

* 完成启动OT并联动rviz

* add 3d visualization

* 完成在main中启动设备可视化

完成在main中启动设备可视化，并输出物料ID：mesh的对应关系resource_model

添加物料模型管理类，遍历物料与resource_model，完成TF数据收集

* 完成TF发布

* 修改模型方向，在yaml中添加变换属性

* 添加物料tf变化时，发送topic到前端

另外修改了物料初始化的方法，防止在tf还未发布时提前建立物料模型与发布话题

* 添加关节发布节点与物料可视化节点进入unilab

* 使用json启动plr与3D模型仿真

* 完成启动OT并联动rviz

* 修复rviz位置问题,

修复rviz位置问题,
在无tf变动时减缓发送频率
在backend中添加物料跟随方法

* fix: running logic

* fix: running logic

* fix: missing ot

* 在main中直接初始化republisher和物料的mesh节点

* 将joint_republisher和resource_mesh_manager添加进 main_slave_run.py中

* 编写mesh添加action

* add action

* fix

* fix: browser on rviz

* fix: cloud bridge error fallback to local

* fix: salve auto run rviz

* 初始化两个plate

---------

Co-authored-by: zhangshixiang <@zhangshixiang>
Co-authored-by: wznln <18435084+Xuwznln@users.noreply.github.com>

test/experiments/test_copy.json

@@ -99,7 +99,7 @@
             "position": {
                 "x": 10.87,
                 "y": 70.77,
-                "z": 10
+                "z": 9.47
             },
             "config": {
                 "type": "TipSpot",

unilabos/app/main.py

@@ -191,7 +191,9 @@ def main():
             resource_visualization = ResourceVisualization(devices_and_resources, args_dict["resources_config"] ,enable_rviz=enable_rviz)
             args_dict["resources_mesh_config"] = resource_visualization.resource_model
             start_backend(**args_dict)
-            server_thread = threading.Thread(target=start_server, args=(not args_dict["disable_browser"],))
+            server_thread = threading.Thread(target=start_server, kwargs=dict(
+                open_browser=not args_dict["disable_browser"]
+            ))
             server_thread.start()
             asyncio.set_event_loop(asyncio.new_event_loop())
             resource_visualization.start()

unilabos/device_mesh/resource_visalization.py

@@ -10,7 +10,7 @@ from unilabos.registry.registry import lab_registry
 
 
 class ResourceVisualization:
-    def __init__(self, device: dict, resource: list, enable_rviz: bool = True):
+    def __init__(self, device: dict, resource: dict, enable_rviz: bool = True):
         """初始化资源可视化类
         
         该类用于将设备和资源的3D模型可视化展示。通过解析设备和资源的配置信息,
@@ -18,13 +18,13 @@ class ResourceVisualization:
         
         Args:
             device (dict): 设备配置字典,包含设备的类型、位置等信息
-            resource (list): 资源配置列表,包含资源的类型、位置等信息
+            resource (dict): 资源配置字典,包含资源的类型、位置等信息 
             registry (dict): 注册表字典,包含设备和资源类型的注册信息
             enable_rviz (bool, optional): 是否启用RViz可视化. Defaults to True.
         """
         self.launch_service = LaunchService()
         self.launch_description = LaunchDescription()
-        self.resource_list = resource
+        self.resource_dict = resource
         self.resource_model = {}
         self.resource_type = ['deck', 'plate', 'container']
         self.mesh_path = Path(__file__).parent.absolute()
@@ -64,11 +64,12 @@ class ResourceVisualization:
                         self.resource_model[node['id']] = {
                             'mesh': f"{str(self.mesh_path)}/resources/{model_config['mesh']}",
                             'mesh_tf': model_config['mesh_tf']}
-                        if model_config['children_mesh'] is not None:
+                        if 'children_mesh' in model_config:
-                            self.resource_model[f"{node['id']}_"] = {
+                            if model_config['children_mesh'] is not None:
-                                'mesh': f"{str(self.mesh_path)}/resources/{model_config['children_mesh']}",
+                                self.resource_model[f"{node['id']}_"] = {
-                                'mesh_tf': model_config['children_mesh_tf']
+                                    'mesh': f"{str(self.mesh_path)}/resources/{model_config['children_mesh']}",
-                            }
+                                    'mesh_tf': model_config['children_mesh_tf']
+                                }
                     elif model_config['type'] == 'device':
                         new_include = etree.SubElement(self.root, f"{{{xacro_uri}}}include")
                         new_include.set("filename", f"{str(self.mesh_path)}/devices/{model_config['mesh']}/macro_device.xacro")
@@ -76,6 +77,7 @@ class ResourceVisualization:
                         new_dev.set("parent_link", "world")
                         new_dev.set("mesh_path", str(self.mesh_path))
                         new_dev.set("device_name", node["id"]+"_")
+                        new_dev.set("station_name", node["parent"]+'_')
                         new_dev.set("x",str(float(node["position"]["x"])/1000))
                         new_dev.set("y",str(float(node["position"]["y"])/1000))
                         new_dev.set("z",str(float(node["position"]["z"])/1000))

unilabos/devices/ros_dev/liquid_handler_joint_publisher.py

@@ -5,7 +5,7 @@ import time
 from rclpy.executors import MultiThreadedExecutor
 from rclpy.action import ActionServer
 from sensor_msgs.msg import JointState
-from ilabos_msgs.action import SendCmd
+from unilabos_msgs.action import SendCmd
 from rclpy.action.server import ServerGoalHandle
 from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode
 from tf_transformations import quaternion_from_euler

unilabos/registry/resources/opentrons/plates.yaml

@@ -39,6 +39,10 @@ nest_96_wellplate_2ml_deep:
   class: 
     module: pylabrobot.resources.opentrons.plates:nest_96_wellplate_2ml_deep
     type: pylabrobot
+  model: 
+    type: resource
+    mesh: tecan_nested_tip_rack/meshes/plate.stl
+    mesh_tf: [0.064, 0.043, 0, -1.5708, 0, 1.5708]
 
 nest_96_wellplate_200ul_flat:
   description: Nest 96 wellplate 200ul flat

unilabos/registry/resources/opentrons/tip_racks.yaml

@@ -68,7 +68,7 @@ opentrons_96_filtertiprack_1000ul:
     mesh: tecan_nested_tip_rack/meshes/plate.stl
     mesh_tf: [0.064, 0.043, 0, -1.5708, 0, 1.5708]
     children_mesh: generic_labware_tube_10_75/meshes/0_base.stl
-    children_mesh_tf: [0.0018, 0.0018, 0, -1.5708,0, 0]
+    children_mesh_tf: [0.0085, 0.006, 0, -1.5708,0, 0]
     
 opentrons_96_filtertiprack_20ul:
   description: Opentrons 96 filtertiprack 20ul

unilabos/resources/graphio.py

@@ -394,14 +394,14 @@ def resource_ulab_to_plr(resource: dict, plr_model=False) -> "ResourcePLR":
     return resource_plr
 
 
-def resource_plr_to_ulab(resource_plr: "ResourcePLR"):
+def resource_plr_to_ulab(resource_plr: "ResourcePLR", parent_name: str = None):
     def resource_plr_to_ulab_inner(d: dict, all_states: dict) -> dict:
         r = {
             "id": d["name"],
             "name": d["name"],
             "sample_id": None,
             "children": [resource_plr_to_ulab_inner(child, all_states) for child in d["children"]],
-            "parent": d["parent_name"] if d["parent_name"] else None,
+            "parent": d["parent_name"] if d["parent_name"] else parent_name if parent_name else None,
             "type": "device",  # FIXME plr自带的type是python class name
             "class": d.get("class", ""),
             "position": (
@@ -417,6 +417,7 @@ def resource_plr_to_ulab(resource_plr: "ResourcePLR"):
     d = resource_plr.serialize()
     all_states = resource_plr.serialize_all_state()
     r = resource_plr_to_ulab_inner(d, all_states)
+
     return r
 
 
@@ -451,7 +452,8 @@ def initialize_resource(resource_config: dict, lab_registry: dict) -> list[dict]
 
         if resource_class_config["type"] == "pylabrobot":
             resource_plr = RESOURCE(name=resource_config["name"])
-            r = resource_plr_to_ulab(resource_plr=resource_plr)
+            r = resource_plr_to_ulab(resource_plr=resource_plr, parent_name=resource_config.get("parent", None))
+            # r = resource_plr_to_ulab(resource_plr=resource_plr)
             if resource_config.get("position") is not None:
                 r["position"] = resource_config["position"]
             r = tree_to_list([r])
@@ -475,8 +477,9 @@ def initialize_resources(resources_config) -> list[dict]:
     """
 
     from unilabos.registry.registry import lab_registry
-
     resources = []
     for resource_config in resources_config:
+
         resources.extend(initialize_resource(resource_config, lab_registry))
+
     return resources

unilabos/ros/main_slave_run.py

@@ -120,6 +120,21 @@ def slave(
     n = Node(f"slaveMachine_{BasicConfig.machine_name}", parameter_overrides=[])
     executor.add_node(n)
 
+    if visual != "disable":
+        resource_mesh_manager = ResourceMeshManager(
+            resources_mesh_config,
+            resources_config,
+            resource_tracker= DeviceNodeResourceTracker(),
+            device_id = 'resource_mesh_manager',
+        )
+        joint_republisher = JointRepublisher(
+            'joint_republisher',
+            DeviceNodeResourceTracker()
+        )
+
+        executor.add_node(resource_mesh_manager)
+        executor.add_node(joint_republisher)
+        
     thread = threading.Thread(target=executor.spin, daemon=True, name="slave_executor_thread")
     thread.start()
 

unilabos/ros/nodes/presets/host_node.py

@@ -643,7 +643,8 @@ class HostNode(BaseROS2DeviceNode):
                 self.lab_logger().debug(f"[Host Node-Resource] Retrieved from bridge: {len(r)} resources")
             except Exception as e:
                 self.lab_logger().error(f"[Host Node-Resource] Error retrieving from bridge: {str(e)}")
-                r = []
+                r = [resource for resource in self.resources_config if resource.get("id") == request.id]
+                self.lab_logger().warning(f"[Host Node-Resource] Retrieved from local: {len(r)} resources")
         else:
             # 本地物料服务，根据 id 查询物料
             r = [resource for resource in self.resources_config if resource.get("id") == request.id]

unilabos/ros/nodes/presets/resource_mesh_manager.py

@@ -1,3 +1,4 @@
+from pathlib import Path
 import time
 import rclpy,json
 from rclpy.node import Node
@@ -6,7 +7,7 @@ import numpy as np
 from moveit_msgs.srv import GetPlanningScene, ApplyPlanningScene
 from rclpy.callback_groups import ReentrantCallbackGroup
 from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSReliabilityPolicy, QoSHistoryPolicy
-from moveit_msgs.msg import CollisionObject, AttachedCollisionObject, AllowedCollisionEntry
+from moveit_msgs.msg import CollisionObject, AttachedCollisionObject, AllowedCollisionEntry, RobotState, PlanningScene
 from shape_msgs.msg import Mesh, MeshTriangle, SolidPrimitive
 from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion, TransformStamped
 from rclpy.callback_groups import ReentrantCallbackGroup
@@ -20,6 +21,8 @@ from rclpy.action import ActionServer
 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.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", rate=50):
@@ -51,10 +54,10 @@ class ResourceMeshManager(BaseROS2DeviceNode):
         self.zero_count             = 0
 
         self.old_resource_pose      = {}
-        self.__planning_scene       = None
+        self.__planning_scene       = PlanningScene()
         self.__old_planning_scene   = None
         self.__old_allowed_collision_matrix = None
-        
+        self.mesh_path = Path(__file__).parent.parent.parent.parent.absolute()
         
         callback_group = ReentrantCallbackGroup()
         self._get_planning_scene_service = self.create_client(
@@ -101,11 +104,18 @@ class ResourceMeshManager(BaseROS2DeviceNode):
             callback_group=callback_group
         )
 
-        self.resource_mesh_setup()
+        # 创建一个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
+        )
+
+        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)
-        
-
 
     def check_move_group_ready(self):
         """检查move_group节点是否已初始化完成"""
@@ -113,33 +123,74 @@ class ResourceMeshManager(BaseROS2DeviceNode):
         # 获取当前可用的节点列表
 
         tf_ready = self.tf_buffer.can_transform("world", next(iter(self.resource_tf_dict.keys())), rclpy.time.Time(),rclpy.duration.Duration(seconds=2))
-
+        
         # if tf_ready:
         if self._get_planning_scene_service.service_is_ready() and self._apply_planning_scene_service.service_is_ready() and tf_ready:
             self.move_group_ready = True
             self.publish_resource_tf()
-            self.add_resource_collision_meshes()
+            self.add_resource_collision_meshes(self.resource_tf_dict)
             
         # time.sleep(1)
+       
+    def add_resource_mesh_callback(self, goal_handle : ServerGoalHandle):
+        tf_update_msg = goal_handle.request
+        try:    
+            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):
+        """刷新资源配置"""
 
+        registry = lab_registry
+        resource_config = json.loads(resource_config_str)
         
+        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 model_config['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}
+        tf_dict = self.resource_mesh_setup(resource_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):
+
+    def resource_mesh_setup(self, resource_config_dict:dict):
         """move_group初始化完成后的设置"""
         self.get_logger().info('开始设置资源网格管理器')
         #遍历resource_config中的资源配置，判断panent是否在resource_model中，
-
+        resource_tf_dict = {}
-        for resource_id, resource_config in self.resource_config_dict.items():
+        for resource_id, resource_config in resource_config_dict.items():
 
             parent = resource_config['parent']
             parent_link = 'world'
             if parent in self.resource_model:
                 parent_link = parent
             elif parent is None and resource_id in self.resource_model:
+
                 pass
-            elif parent not in self.resource_model and parent is not None and resource_config['class'] != '':
+            elif parent is not None and resource_id in self.resource_model:
                 parent_link = f"{self.resource_config_dict[parent]['parent']}_{parent}_device_link".replace("None","")
+
             else:
+
                 continue
             # 提取位置信息并转换单位
             position = {
@@ -163,12 +214,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
                 float(rotation_dict['y']), 
                 float(rotation_dict['z'])
             )
-            # print("-"*20)
+
-            # print(f"resource_id: {resource_id}")
-            # print(f"parent: {parent}")
-            # print(f"resource_config: {resource_config}")
-            # print(f"parent_link: {parent_link}")
-            # print("-"*20)
             rotation = {
                 "x": q[0],
                 "y": q[1],
@@ -177,12 +223,14 @@ class ResourceMeshManager(BaseROS2DeviceNode):
             }
 
             # 更新资源TF字典
-            self.resource_tf_dict[resource_id] = {
+            resource_tf_dict[resource_id] = {
                 "parent": parent_link,
                 "position": position,
                 "rotation": rotation
             }
 
+        return resource_tf_dict
+
 
     def publish_resource_tf(self):
         """
@@ -376,7 +424,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
         return SendCmd.Result(success=True)
 
 
-    def add_resource_collision_meshes(self):
+    def add_resource_collision_meshes(self,resource_tf_dict:dict):
         """
         遍历资源配置字典，为每个在resource_model中有对应模型的资源添加碰撞网格
         
@@ -384,8 +432,11 @@ class ResourceMeshManager(BaseROS2DeviceNode):
         如果有，则调用add_collision_mesh方法将其添加到碰撞环境中。
         """
         self.get_logger().info('开始添加资源碰撞网格')
-        
+
-        for resource_id, tf_info in self.resource_tf_dict.items():
+        self.__planning_scene = self._get_planning_scene_service.call(
+            GetPlanningScene.Request()
+        ).scene
+        for resource_id, tf_info in resource_tf_dict.items():
             
             if resource_id in self.resource_model:
                 # 获取位置信息
@@ -405,14 +456,14 @@ class ResourceMeshManager(BaseROS2DeviceNode):
                 )
                 
                 # 添加碰撞网格
-                self.add_collision_mesh(
+                collision_object = self.get_collision_mesh(
                     filepath=self.resource_model[resource_id]['mesh'],
                     id=resource_id,
                     position=position,
                     quat_xyzw=q,
                     frame_id=resource_id
                 )
-
+                self.__planning_scene.world.collision_objects.append(collision_object)
             elif f"{tf_info['parent']}_" in self.resource_model:
                 # 获取资源的父级框架ID
                 id_ = f"{tf_info['parent']}_"
@@ -433,15 +484,21 @@ class ResourceMeshManager(BaseROS2DeviceNode):
                 )
                 
                 # 添加碰撞网格
-                self.add_collision_mesh(
+                collision_object = self.get_collision_mesh(
                     filepath=self.resource_model[id_]['mesh'],
                     id=resource_id,
                     position=position,
                     quat_xyzw=q,
                     frame_id=resource_id
                 )
-            time.sleep(0.03)
 
+                self.__planning_scene.world.collision_objects.append(collision_object)
+
+        req = ApplyPlanningScene.Request()
+        req.scene = self.__planning_scene
+        self._apply_planning_scene_service.call_async(req)
+            
+        
         self.get_logger().info('资源碰撞网格添加完成')
 
 
@@ -637,6 +694,116 @@ class ResourceMeshManager(BaseROS2DeviceNode):
             operation=operation,
         )
 
+    def get_collision_mesh(
+        self,
+        filepath: Optional[str],
+        id: str,
+        pose: Optional[Union[PoseStamped, Pose]] = None,
+        position: Optional[Union[Point, Tuple[float, float, float]]] = None,
+        quat_xyzw: Optional[
+            Union[Quaternion, Tuple[float, float, float, float]]
+        ] = None,
+        frame_id: Optional[str] = None,
+        operation: int = CollisionObject.ADD,
+        scale: Union[float, Tuple[float, float, float]] = 1.0,
+        mesh: Optional[Any] = None,
+    ):
+        """
+        Add collision object with a mesh geometry. Either `filepath` must be
+        specified or `mesh` must be provided.
+        Note: This function required 'trimesh' Python module to be installed.
+        """
+
+        # Load the mesh
+        try:
+            import trimesh
+        except ImportError as err:
+            raise ImportError(
+                "Python module 'trimesh' not found! Please install it manually in order "
+                "to add collision objects into the MoveIt 2 planning scene."
+            ) from err
+
+        # Check the parameters
+        if (pose is None) and (position is None or quat_xyzw is None):
+            raise ValueError(
+                "Either `pose` or `position` and `quat_xyzw` must be specified!"
+            )
+        if (filepath is None and mesh is None) or (
+            filepath is not None and mesh is not None
+        ):
+            raise ValueError("Exactly one of `filepath` or `mesh` must be specified!")
+        if mesh is not None and not isinstance(mesh, trimesh.Trimesh):
+            raise ValueError("`mesh` must be an instance of `trimesh.Trimesh`!")
+
+        if isinstance(pose, PoseStamped):
+            pose_stamped = pose
+        elif isinstance(pose, Pose):
+            pose_stamped = PoseStamped(
+                header=Header(
+                    stamp=self.get_clock().now().to_msg(),
+                    frame_id=(
+                        frame_id if frame_id is not None else self.__base_link_name
+                    ),
+                ),
+                pose=pose,
+            )
+        else:
+            if not isinstance(position, Point):
+                position = Point(
+                    x=float(position[0]), y=float(position[1]), z=float(position[2])
+                )
+            if not isinstance(quat_xyzw, Quaternion):
+                quat_xyzw = Quaternion(
+                    x=float(quat_xyzw[0]),
+                    y=float(quat_xyzw[1]),
+                    z=float(quat_xyzw[2]),
+                    w=float(quat_xyzw[3]),
+                )
+            pose_stamped = PoseStamped(
+                header=Header(
+                    stamp=self.get_clock().now().to_msg(),
+                    frame_id=(
+                        frame_id if frame_id is not None else self.__base_link_name
+                    ),
+                ),
+                pose=Pose(position=position, orientation=quat_xyzw),
+            )
+
+        msg = CollisionObject(
+            header=pose_stamped.header,
+            id=id,
+            operation=operation,
+            pose=pose_stamped.pose,
+        )
+
+        if filepath is not None:
+            mesh = trimesh.load(filepath)
+
+        # Scale the mesh
+        if isinstance(scale, float):
+            scale = (scale, scale, scale)
+        if not (scale[0] == scale[1] == scale[2] == 1.0):
+            # If the mesh was passed in as a parameter, make a copy of it to
+            # avoid transforming the original.
+            if filepath is not None:
+                mesh = mesh.copy()
+            # Transform the mesh
+            transform = np.eye(4)
+            np.fill_diagonal(transform, scale)
+            mesh.apply_transform(transform)
+
+        msg.meshes.append(
+            Mesh(
+                triangles=[MeshTriangle(vertex_indices=face) for face in mesh.faces],
+                vertices=[
+                    Point(x=vert[0], y=vert[1], z=vert[2]) for vert in mesh.vertices
+                ],
+            )
+        )
+        
+        # self.__collision_object_publisher.publish(msg)
+        return msg
+
     def add_collision_mesh(
         self,
         filepath: Optional[str],
@@ -743,7 +910,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
                 ],
             )
         )
-
+        
         self.__collision_object_publisher.publish(msg)
 
     def remove_collision_object(self, id: str):

unilabos/ros/utils/driver_creator.py

@@ -6,6 +6,7 @@
 """
 import asyncio
 import inspect
+import json
 import traceback
 from abc import abstractmethod
 from typing import Type, Any, Dict, Optional, TypeVar, Generic