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

test/experiments/test.json

@@ -9,8 +9,8 @@
             "type": "device",
             "class": "gripper.mock",
             "position": {
-                "x": 1000,
+                "x": 0,
-                "y": 1000,
+                "y": 0,
                 "z": 0
             },
             "config": {

unilabos/app/main.py

@@ -71,7 +71,12 @@ def parse_args():
         default=True,
         help="是否在启动时打开信息页",
     )
-
+    parser.add_argument(
+        "--visual",
+        choices=["rviz", "web","None"],
+        default="rviz",
+        help="选择可视化工具: 'rviz' 或 'web' 或 'None'，默认'rviz'",
+    )
     return parser.parse_args()
 
 
@@ -164,16 +169,63 @@ def main():
         signal.signal(signal.SIGTERM, _exit)
         mqtt_client.start()
 
-    resource_visualization = ResourceVisualization(args_dict["devices_config"], args_dict["resources_config"],registry_dict)
+    if args_dict["visual"] != "None":
-    start_backend(**args_dict)
+        if args_dict["visual"] == "rviz":
-    # print('-'*100)
+            resource_visualization = ResourceVisualization(args_dict["devices_config"], args_dict["resources_config"],registry_dict)
-    # print(resource_visualization.resource_model)
+        elif args_dict["visual"] == "web":
-    # print('-'*100)
+            resource_visualization = ResourceVisualization(args_dict["devices_config"], args_dict["resources_config"],registry_dict,enable_rviz=False )
-    server_thread = threading.Thread(target=start_server)
+        devices_config_add = add_resource_mesh_manager_node(resource_visualization.resource_model, args_dict["resources_config"])
-    server_thread.start()
+        args_dict["devices_config"] = {**args_dict["devices_config"], **devices_config_add}
     
-    resource_visualization.start()
+        server_thread = threading.Thread(target=start_server)
+        server_thread.start()
+        start_backend(**args_dict)
+        resource_visualization.start()
+    else:
+        start_backend(**args_dict)
+        start_server()
 
+def add_resource_mesh_manager_node(
+                                    resource_model, 
+                                    resource_config ,
+                                    mesh_manager_device_id = "resource_mesh_manager",
+                                    joint_publisher_device_id = "joint_republisher"):
+    mesh_manager_config ={
+            "id": mesh_manager_device_id,
+            "name": mesh_manager_device_id,
+            "children": [],
+            "parent": None,
+            "type": "device",
+            "class": "resource.mesh_manager",
+            "position": {
+                "x": 620.6111111111111,
+                "y": 171,
+                "z": 0
+            },
+            "config": {
+                "resource_model": resource_model,
+                "resource_config": resource_config
+            },
+            "data": {
+            }
+        }
+    joint_publisher_config = {
+        "id": joint_publisher_device_id,
+        "name": joint_publisher_device_id,
+        "children": [],
+        "parent": None,
+        "type": "device",
+        "class": "joint_republisher",
+        "position": {
+            "x": 620.6111111111111,
+            "y": 171,
+            "z": 0
+        },
+        "config": {},
+        "data": {}
+    }
+    return {joint_publisher_config["id"]: joint_publisher_config,mesh_manager_config["id"]: mesh_manager_config}
+    # return {joint_publisher_config["id"]: joint_publisher_config}
 
 if __name__ == "__main__":
     main()

unilabos/device_mesh/resource_visalization.py

@@ -119,12 +119,12 @@ class ResourceVisualization:
             }]
         )
 
-        joint_state_publisher_node = nd(
+        # joint_state_publisher_node = nd(
-            package='joint_state_publisher_gui',  # 或 joint_state_publisher
+        #     package='joint_state_publisher_gui',  # 或 joint_state_publisher
-            executable='joint_state_publisher_gui',
+        #     executable='joint_state_publisher_gui',
-            name='joint_state_publisher',
+        #     name='joint_state_publisher',
-            output='screen'
+        #     output='screen'
-        )
+        # )
         # 创建move_group节点
         move_group = nd(
             package='moveit_ros_move_group',
@@ -147,7 +147,7 @@ class ResourceVisualization:
 
         # 将节点添加到launch描述中
         self.launch_description.add_action(robot_state_publisher)
-        self.launch_description.add_action(joint_state_publisher_node)
+        # self.launch_description.add_action(joint_state_publisher_node)
         self.launch_description.add_action(move_group)
 
         # 如果启用RViz,添加RViz节点

unilabos/devices/ros_dev/joint_republisher.py

@@ -3,13 +3,22 @@ from rclpy.node import Node
 from sensor_msgs.msg import JointState 
 from std_msgs.msg import String
 from rclpy.callback_groups import ReentrantCallbackGroup
+from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode
 
-class JointRepublisher(Node):
+class JointRepublisher(BaseROS2DeviceNode):
-    def __init__(self,device_id):
+    def __init__(self,device_id,resource_tracker):
-        super().__init__(device_id)  
+        super().__init__(
+            driver_instance=self,
+            device_id=device_id,
+            status_types={},
+            action_value_mappings={},
+            hardware_interface={},
+            print_publish=False,
+            resource_tracker=resource_tracker,  
+        )  
         
         # print('-'*20,device_id)
-        self.joint_repub = self.create_publisher(String,f'/devices/{device_id}/joint_state_repub',10)
+        self.joint_repub = self.create_publisher(String,f'joint_state_repub',10)
         # 创建订阅者
         self.create_subscription(
             JointState,               

unilabos/devices/ros_dev/resource_mesh_manager.py

@@ -19,17 +19,26 @@ from tf2_ros import TransformBroadcaster, Buffer, TransformListener
 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
 
-class ResourceMeshManager(Node):
+class ResourceMeshManager(BaseROS2DeviceNode):
-    def __init__(self, resource_model: dict, resource_config: list, node_name: str):
+    def __init__(self, resource_model: dict, resource_config: list,resource_tracker, device_id: str = "resource_mesh_manager"):
         """初始化资源网格管理器节点
         
         Args:
             resource_model (dict): 资源模型字典,包含资源的3D模型信息
             resource_config (dict): 资源配置字典,包含资源的配置信息
-            node_name (str): 节点名称
+            device_id (str): 节点名称
         """
-        super().__init__(node_name)
+        super().__init__(
+            driver_instance=self,
+            device_id=device_id,
+            status_types={},
+            action_value_mappings={},
+            hardware_interface={},
+            print_publish=False,
+            resource_tracker=resource_tracker, 
+        ) 
 
         self.resource_model         = resource_model
         self.resource_config_dict   = {item['id']: item for item in resource_config}
@@ -49,7 +58,7 @@ class ResourceMeshManager(Node):
         callback_group = ReentrantCallbackGroup()
         self._get_planning_scene_service = self.create_client(
             srv_type=GetPlanningScene,
-            srv_name="get_planning_scene",
+            srv_name="/get_planning_scene",
             qos_profile=QoSProfile(
                 durability=QoSDurabilityPolicy.VOLATILE,
                 reliability=QoSReliabilityPolicy.RELIABLE,
@@ -62,7 +71,7 @@ class ResourceMeshManager(Node):
         # Create a service for applying the planning scene
         self._apply_planning_scene_service = self.create_client(
             srv_type=ApplyPlanningScene,
-            srv_name="apply_planning_scene",
+            srv_name="/apply_planning_scene",
             qos_profile=QoSProfile(
                 durability=QoSDurabilityPolicy.VOLATILE,
                 reliability=QoSReliabilityPolicy.RELIABLE,
@@ -73,7 +82,7 @@ class ResourceMeshManager(Node):
         )
 
         self.resource_pose_publisher = self.create_publisher(
-            String, f"devices/{node_name}/resource_pose", 10
+            String, f"resource_pose", 10
         )
         self.__collision_object_publisher = self.create_publisher(
             CollisionObject, "/collision_object", 10
@@ -86,7 +95,7 @@ class ResourceMeshManager(Node):
         self._action_server = ActionServer(
             self,
             SendCmd,
-            f'devices/{node_name}/tf_update',
+            f"tf_update",
             self.tf_update,
             callback_group=callback_group
         )
@@ -96,13 +105,15 @@ class ResourceMeshManager(Node):
         self.create_timer(1, self.check_resource_pose_changes)
         
 
+
     def check_move_group_ready(self):
         """检查move_group节点是否已初始化完成"""
 
         # 获取当前可用的节点列表
 
-        tf_ready = self.tf_buffer.can_transform("world", next(iter(self.resource_tf_dict.keys())), rclpy.time.Time())
+        tf_ready = self.tf_buffer.can_transform("world", next(iter(self.resource_tf_dict.keys())), rclpy.time.Time(),rclpy.duration.Duration(seconds=2))
-        # print(tf_ready)
+
+        # 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.add_resource_collision_meshes()
@@ -5022,10 +5033,10 @@ if __name__ == '__main__':
     '''
     resource_config = json.loads(config_s.replace("'",'"'))
     rclpy.init()
-    resource_mesh_manager = ResourceMeshManager(resource_model, resource_config, 'resource_mesh_manager')
+    resource_mesh_manager = ResourceMeshManager(resource_model, resource_config, DeviceNodeResourceTracker())
     # resource_mesh_manager.resource_mesh_setup()
-    resource_mesh_manager.check_move_group_ready()
+    # resource_mesh_manager.check_move_group_ready()
-    resource_mesh_manager.publish_resource_tf()
+    # resource_mesh_manager.publish_resource_tf()
     # resource_mesh_manager.clear_all_collision_objects()
     # print(json.dumps(resource_mesh_manager.resource_tf_dict, indent=4, ensure_ascii=False))
     

unilabos/registry/devices/sim_nodes.yaml

@@ -0,0 +1,11 @@
+joint_republisher:
+  class:
+    module: unilabos.devices.ros_dev.joint_republisher:JointRepublisher
+    type: ros2
+
+resource.mesh_manager:
+  class:
+    module: unilabos.devices.ros_dev.resource_mesh_manager:ResourceMeshManager
+    type: ros2
+
+