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

unilabos/app/main.py

@@ -10,6 +10,9 @@ import yaml
 from copy import deepcopy
 import threading
 
+import rclpy
+from unilabos.ros.nodes.resource_tracker import DeviceNodeResourceTracker
+
 # 首先添加项目根目录到路径
 current_dir = os.path.dirname(os.path.abspath(__file__))
 ilabos_dir = os.path.dirname(os.path.dirname(current_dir))
@@ -19,6 +22,10 @@ if ilabos_dir not in sys.path:
 from unilabos.config.config import load_config, BasicConfig, _update_config_from_env
 from unilabos.utils.banner_print import print_status, print_unilab_banner
 from unilabos.device_mesh.resource_visalization import ResourceVisualization
+from unilabos.ros.nodes.presets.joint_republisher import JointRepublisher
+from unilabos.ros.nodes.presets.resource_mesh_manager import ResourceMeshManager
+from rclpy.executors import MultiThreadedExecutor
+
 
 def parse_args():
     """解析命令行参数"""
@@ -184,8 +191,29 @@ def main():
         elif args_dict["visual"] == "web":
             enable_rviz=False
         resource_visualization = ResourceVisualization(devices_and_resources, args_dict["resources_config"] ,enable_rviz=enable_rviz)
-        devices_config_add = add_resource_mesh_manager_node(resource_visualization.resource_model, args_dict["resources_config"])
-        args_dict["devices_config"] = {**args_dict["devices_config"], **devices_config_add}
+
+        # 如果没有初始化，则初始化ros，并创建一个多线程执行器
+        # 在main_slave_run.py中也会初始化ros，并创建一个多线程执行器
+        # 所以这里需要判断是否已经初始化，如果已经初始化，则不重复初始化    
+        if not rclpy.ok():
+            rclpy.init(args = ["--log-level", "debug"],)
+        executor = rclpy.__executor
+        if not executor:
+            executor = rclpy.__executor = MultiThreadedExecutor()
+
+        resource_mesh_manager = ResourceMeshManager(
+            resource_visualization.resource_model,
+            args_dict["resources_config"],
+            resource_tracker= DeviceNodeResourceTracker(),
+            device_id = 'resource_mesh_manager',
+        )
+        joint_republisher = JointRepublisher(
+            'joint_republisher',
+            DeviceNodeResourceTracker()
+        )
+
+        rclpy.__executor.add_node(resource_mesh_manager)
+        rclpy.__executor.add_node(joint_republisher)
         start_backend(**args_dict)
         server_thread = threading.Thread(target=start_server)
         server_thread.start()
@@ -197,47 +225,6 @@ def main():
         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

@@ -52,10 +52,6 @@ class ResourceVisualization:
                 device_class = node['class']
                 # 检查设备类型是否在注册表中
                 if device_class not in registry.device_type_registry.keys():
-                    print("="*20)
-                    print(device_class)
-                    print(registry.device_type_registry.keys())
-                    print("="*20)
                     raise ValueError(f"设备类型 {device_class} 未在注册表中注册")
             elif node['type'] in self.resource_type:
                 # print(registry.resource_type_registry)

unilabos/devices/ros_dev/joint_republisher.py

@@ -1,60 +0,0 @@
-import rclpy,json
-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(BaseROS2DeviceNode):
-    def __init__(self,device_id,resource_tracker):
-        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'joint_state_repub',10)
-        # 创建订阅者
-        self.create_subscription(
-            JointState,               
-            '/joint_states',         
-            self.listener_callback,  
-            10,
-            callback_group=ReentrantCallbackGroup()
-        )
-        self.msg = String()
-
-    def listener_callback(self, msg:JointState):
-        
-        try:
-            json_dict = {}
-            json_dict["name"]           = list(msg.name)
-            json_dict["position"]       = list(msg.position)
-            json_dict["velocity"]       = list(msg.velocity)
-            json_dict["effort"]         = list(msg.effort)
-
-            self.msg.data = str(json_dict)
-            self.joint_repub.publish(self.msg)
-            # print('-'*20)
-            # print(self.msg.data)
-            
-        except Exception as e:
-            print(e)
-
-
-def main():
-
-    rclpy.init()
-    subscriber = JointRepublisher()
-    rclpy.spin(subscriber)
-    subscriber.destroy_node()
-    rclpy.shutdown()
-
-
-if __name__ == '__main__':
-    main()

unilabos/registry/devices/sim_nodes.yaml

@@ -1,13 +1,3 @@
-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
-
 lh_joint_publisher:
   class:
     module: unilabos.devices.ros_dev.liquid_handler_joint_publisher:LiquidHandlerJointPublisher

unilabos/ros/main_slave_run.py

@@ -49,9 +49,11 @@ def main(
     discovery_interval: float = 5.0,
 ) -> None:
     """主函数"""
-    rclpy.init(args=args)
-    rclpy.__executor = executor = MultiThreadedExecutor()
-
+    if not rclpy.ok():
+        rclpy.init(args=args)
+    executor = rclpy.__executor
+    if not executor:
+        executor = rclpy.__executor = MultiThreadedExecutor()
     # 创建主机节点
     host_node = HostNode(
         "host_node",
@@ -79,8 +81,11 @@ def slave(
     args: List[str] = ["--log-level", "debug"],
 ) -> None:
     """从节点函数"""
-    rclpy.init(args=args)
-    rclpy.__executor = executor = MultiThreadedExecutor()
+    if not rclpy.ok():
+        rclpy.init(args=args)
+    executor = rclpy.__executor
+    if not executor:
+        executor = rclpy.__executor = MultiThreadedExecutor()
     devices_config_copy = copy.deepcopy(devices_config)
     for device_id, device_config in devices_config.items():
         d = initialize_device_from_dict(device_id, device_config)

unilabos/ros/nodes/presets/resource_mesh_manager.py

@@ -22,7 +22,7 @@ from rclpy.action.server import ServerGoalHandle
 from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode,DeviceNodeResourceTracker
 
 class ResourceMeshManager(BaseROS2DeviceNode):
-    def __init__(self, resource_model: dict, resource_config: list,resource_tracker, device_id: str = "resource_mesh_manager"):
+    def __init__(self, resource_model: dict, resource_config: list,resource_tracker, device_id: str = "resource_mesh_manager", rate=50):
         """初始化资源网格管理器节点
         
         Args:
@@ -47,7 +47,8 @@ class ResourceMeshManager(BaseROS2DeviceNode):
         self.tf_broadcaster         = TransformBroadcaster(self)
         self.tf_buffer              = Buffer()
         self.tf_listener            = TransformListener(self.tf_buffer, self)
-
+        self.rate                   = rate
+        self.zero_count             = 0
 
         self.old_resource_pose      = {}
         self.__planning_scene       = None
@@ -101,8 +102,8 @@ class ResourceMeshManager(BaseROS2DeviceNode):
         )
 
         self.resource_mesh_setup()
-        self.create_timer(0.02, self.publish_resource_tf)
-        self.create_timer(1, self.check_resource_pose_changes)
+        self.create_timer(1/self.rate, self.publish_resource_tf)
+        self.create_timer(1/self.rate, self.check_resource_pose_changes)
         
 
 
@@ -116,6 +117,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
         # 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()
             
         # time.sleep(1)
@@ -246,7 +248,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
                 transform = self.tf_buffer.lookup_transform(
                     "world",
                     resource_id,
-                    rclpy.time.Time(),
+                    rclpy.time.Time(seconds=0),
                     rclpy.duration.Duration(seconds=5)
                 )
                 
@@ -280,13 +282,24 @@ class ResourceMeshManager(BaseROS2DeviceNode):
                         
             except Exception as e:
                 self.get_logger().warning(f"获取资源 {resource_id} 的世界坐标变换失败: {e}")
-                
-        changed_poses_msg = String()
-        changed_poses_msg.data = json.dumps(changed_poses)
-        self.resource_pose_publisher.publish(changed_poses_msg)
+        
+        if changed_poses != {}:
+            self.zero_count = 0
+            changed_poses_msg = String()
+            changed_poses_msg.data = json.dumps(changed_poses)
+            self.resource_pose_publisher.publish(changed_poses_msg)
+        else:
+            if self.zero_count > self.rate:
+                self.zero_count = 0
+                changed_poses_msg = String()
+                changed_poses_msg.data = json.dumps(changed_poses)
+                self.resource_pose_publisher.publish(changed_poses_msg)
+            self.zero_count += 1
+            
+        
         
     
-    def _is_pose_equal(self, pose1, pose2, tolerance=1e-5):
+    def _is_pose_equal(self, pose1, pose2, tolerance=1e-7):
         """
         比较两个位姿是否相等（考虑浮点数精度）
         
@@ -328,7 +341,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
                 transform = self.tf_buffer.lookup_transform(
                     target_parent,
                     resource_id,
-                    rclpy.time.Time()
+                    rclpy.time.Time(seconds=0)
                 )
                 
                 # 提取转换中的位置和旋转信息
@@ -350,7 +363,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
                     "position": position,
                     "rotation": rotation
                 }
-
+                
                 self.attach_collision_object(id=resource_id,link_name=target_parent)
 
             self.publish_resource_tf()
@@ -362,48 +375,6 @@ class ResourceMeshManager(BaseROS2DeviceNode):
         goal_handle.succeed()
         return SendCmd.Result(success=True)
 
-    def publish_resource_tf(self):
-        """
-        发布资源之间的TF关系
-        
-        遍历self.resource_tf_dict中的每个元素，根据key，parent，以及position和rotation，
-        发布key和parent之间的tf关系
-        """
-
-        transforms = []
-        
-        # 遍历资源TF字典
-        resource_tf_dict = copy.deepcopy(self.resource_tf_dict)
-        for resource_id, tf_info in resource_tf_dict.items():
-            parent = tf_info['parent']
-            position = tf_info['position']
-            rotation = tf_info['rotation']
-            
-            # 创建静态变换消息
-            
-            transform = TransformStamped()
-            transform.header.stamp = self.get_clock().now().to_msg()
-            transform.header.frame_id = parent
-            transform.child_frame_id = resource_id
-            
-            # 设置位置
-            transform.transform.translation.x = float(position['x'])
-            transform.transform.translation.y = float(position['y'])
-            transform.transform.translation.z = float(position['z'])
-            
-            # 设置旋转
-            transform.transform.rotation.x = rotation['x']
-            transform.transform.rotation.y = rotation['y']
-            transform.transform.rotation.z = rotation['z']
-            transform.transform.rotation.w = rotation['w']
-            
-            transforms.append(transform)
-            
-        # 一次性发布所有静态变换
-        if transforms:
-            self.tf_broadcaster.sendTransform(transforms)
-
-            # self.get_logger().info(f'已发布 {len(transforms)} 个资源TF关系')
 
     def add_resource_collision_meshes(self):
         """