修复rviz位置问题,

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

test/experiments/test.json

@@ -36,6 +36,108 @@
             },
             "data": {
             }
+        },
+        {
+            "id": "ot_joint_publisher",
+            "name": "ot_joint_publisher",
+            "sample_id": null,
+            "children": [
+                
+            ],
+            "parent": null,
+            "type": "device",
+            "class": "lh_joint_publisher",
+            "position": {
+                "x": 0,
+                "y": 0,
+                "z": 0
+            },
+            "config": {
+                "lh_id":"deck",
+                "joint_config":
+                {
+                    "joint_names":[
+                        "first_joint",
+                        "second_joint",
+                        "third_joint",
+                        "fourth_joint"
+                    ],
+                    "y":{
+                        "first_joint":{
+                        "factor":-1,
+                        "offset":0.0
+                        }
+                    },
+                    "x":{
+                        "second_joint":{
+                        "factor":-1,
+                        "offset":0.0
+                        }
+                    },
+                    "z":{
+                        "third_joint":{
+                        "factor":1,
+                        "offset":0.0
+                        },
+                        "fourth_joint":{
+                        "factor":1,
+                        "offset":0.0
+                        }
+                    }
+                }
+            },
+            "data": {}
+        },
+        {
+            "id": "ot_joint_publisher",
+            "name": "ot_joint_publisher",
+            "sample_id": null,
+            "children": [
+                
+            ],
+            "parent": null,
+            "type": "device",
+            "class": "lh_joint_publisher",
+            "position": {
+                "x": 0,
+                "y": 0,
+                "z": 0
+            },
+            "config": {
+                "lh_id":"deck",
+                "joint_config":
+                {
+                    "joint_names":[
+                        "first_joint",
+                        "second_joint",
+                        "third_joint",
+                        "fourth_joint"
+                    ],
+                    "y":{
+                        "first_joint":{
+                        "factor":-1,
+                        "offset":0.0
+                        }
+                    },
+                    "x":{
+                        "second_joint":{
+                        "factor":-1,
+                        "offset":0.0
+                        }
+                    },
+                    "z":{
+                        "third_joint":{
+                        "factor":1,
+                        "offset":0.0
+                        },
+                        "fourth_joint":{
+                        "factor":1,
+                        "offset":0.0
+                        }
+                    }
+                }
+            },
+            "data": {}
         }
     ],
     "links": [

test/experiments/test_copy.json

@@ -68,8 +68,8 @@
             "type": "plate",
             "class": "opentrons_96_filtertiprack_1000ul",
             "position": {
-                "x": 132.5,
+                "x": 0,
-                "y": 271.5,
+                "y": 0,
                 "z": 69
             },
             "config": {
@@ -99,7 +99,7 @@
             "position": {
                 "x": 10.87,
                 "y": 70.77,
-                "z": 3.03
+                "z": 10
             },
             "config": {
                 "type": "TipSpot",

unilabos/device_mesh/view_robot.rviz

@@ -360,10 +360,10 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.4347957372665405
+      Pitch: 0.40479573607444763
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 6.020748138427734
+      Yaw: 6.090748310089111
     Saved: ~
 Window Geometry:
   Displays:
@@ -383,5 +383,5 @@ Window Geometry:
   Views:
     collapsed: true
   Width: 2518
-  X: 140
+  X: 385
-  Y: 145
+  Y: 120

unilabos/devices/ros_dev/liquid_handler_joint_publisher.py

@@ -1,3 +1,4 @@
+import copy
 import rclpy
 import json
 import time
@@ -12,7 +13,7 @@ from tf2_ros import TransformBroadcaster, Buffer, TransformListener
 
 
 class LiquidHandlerJointPublisher(BaseROS2DeviceNode):
-    def __init__(self,device_id:str, joint_config:dict,resource_tracker):
+    def __init__(self,device_id:str, joint_config:dict, lh_id:str,resource_tracker, rate=50):
         super().__init__(
             driver_instance=self,
             device_id=device_id,
@@ -22,26 +23,61 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode):
             print_publish=False,
             resource_tracker=resource_tracker,  
         )  
-        # self.station_name   = station_name
+
-        # self.dev_name       = dev_name
+        # joint_config_dict = {
+        #     "joint_names":[
+        #         "first_joint",
+        #         "second_joint",
+        #         "third_joint",
+        #         "fourth_joint"
+        #     ],
+        #     "y":{
+        #         "first_joint":{
+        #         "factor":-1,
+        #         "offset":0.0
+        #         }
+        #     },
+        #     "x":{
+        #         "second_joint":{
+        #         "factor":-1,
+        #         "offset":0.0
+        #         }
+        #     },
+        #     "z":{
+        #         "third_joint":{
+        #         "factor":1,
+        #         "offset":0.0
+        #         },
+        #         "fourth_joint":{
+        #         "factor":1,
+        #         "offset":0.0
+        #         }
+        #     }
+        # }
+
         self.j_msg          = JointState()
+        self.lh_id          = lh_id
         # self.j_msg.name     = joint_names
-        self.j_msg.name     = [device_id+x for x in joint_config.keys()]
+        self.joint_config   = joint_config
-        self.rate           = 50
+        self.j_msg.position = [0.0 for i in range(len(joint_config['joint_names']))]
-        self.j_msg.position = [0.0 for i in range(len(joint_config.keys()))]
+        self.j_msg.name     = [f"{self.lh_id}_{x}" for x in joint_config['joint_names']]
+        # self.joint_config   = joint_config_dict
+        # self.j_msg.position = [0.0 for i in range(len(joint_config_dict['joint_names']))]
+        # self.j_msg.name     = [f"{self.lh_id}_{x}" for x in joint_config_dict['joint_names']]
+        self.rate           = rate
         self.tf_buffer      = Buffer()
         self.tf_listener    = TransformListener(self.tf_buffer, self)
         self.j_pub          = self.create_publisher(JointState,'/joint_states',10)
-        self.create_timer(0.02,self.sim_joint_pub_callback)
+        self.create_timer(0.02,self.lh_joint_pub_callback)
         self.j_action       = ActionServer(
             self,
             SendCmd,
             "joint",
-            self.sim_joint_action_callback,
+            self.lh_joint_action_callback,
             result_timeout=5000
         )
 
-    def sim_joint_action_callback(self,goal_handle: ServerGoalHandle):
+    def lh_joint_action_callback(self,goal_handle: ServerGoalHandle):
         """Move a single joint
 
         Args:
@@ -60,7 +96,7 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode):
 
         try:
             cmd_dict = json.loads(cmd_str)
-            self.move_joint(**cmd_dict)
+            self.move_joints(**cmd_dict)
             result.success = True
             goal_handle.succeed()
             
@@ -70,44 +106,103 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode):
             result.success = False
 
         return result
-
+    def inverse_kinematics(self, x, y, z, 
-
+                           x_joint:dict, 
-    def move_joint(self, joint_name, speed, position):
+                           y_joint:dict, 
+                           z_joint:dict   ):
+        """
+        将x、y、z坐标转换为对应关节的位置
         
-        # joint_index = self.j_msg.name.index(self.station_name+self.dev_name+joint_name)
+        Args:
-        joint_index = self.j_msg.name.index(joint_name)
+            x (float): x坐标
-        distance = position - self.j_msg.position[joint_index]
+            y (float): y坐标
-        if distance == 0:
+            z (float): z坐标
-            return
+            x_joint (dict): x轴关节配置，包含plus和offset
-        flag = abs(distance)/distance
+            y_joint (dict): y轴关节配置，包含plus和offset
-        
+            z_joint (dict): z轴关节配置，包含plus和offset
-        while abs(distance)>speed/self.rate:
             
-            self.j_msg.position[joint_index] += flag*speed/self.rate
+        Returns:
-            self.sim_joint_pub_callback()
+            dict: 关节名称和对应位置的字典
-            time.sleep(0.02)
+        """
-            distance = position - self.j_msg.position[joint_index]
+        joint_positions = copy.deepcopy(self.j_msg.position)
-
+        
-        self.j_msg.position[joint_index] = position
+        # 处理x轴关节
-        self.sim_joint_pub_callback()
+        for joint_name, config in x_joint.items():
+            index = self.j_msg.name.index(f"{self.lh_id}_{joint_name}")
+            joint_positions[index] = x * config["factor"] + config["offset"]
+            
+        # 处理y轴关节
+        for joint_name, config in y_joint.items():
+            index = self.j_msg.name.index(f"{self.lh_id}_{joint_name}")
+            joint_positions[index] = y * config["factor"] + config["offset"]
+            
+        # 处理z轴关节
+        for joint_name, config in z_joint.items():
+            index = self.j_msg.name.index(f"{self.lh_id}_{joint_name}")
+            joint_positions[index] = z * config["factor"] + config["offset"]
+            
+        
+        return joint_positions
 
 
-    def sim_joint_pub_callback(self):
+    def move_joints(self, resource_name, link_name, speed, x_joint=None, y_joint=None, z_joint=None):
+        
+        transform = self.tf_buffer.lookup_transform(
+                    link_name,
+                    resource_name,
+                    rclpy.time.Time()
+                )
+        x,y,z = transform.transform.translation.x, transform.transform.translation.y, transform.transform.translation.z
+        if x_joint is None:
+            x_joint_config = next(iter(self.joint_config['x'].items()))
+        elif x_joint in self.joint_config['x']:
+            x_joint_config = self.joint_config['x'][x_joint]
+        else:
+            raise ValueError(f"x_joint {x_joint} not in joint_config['x']")
+        if y_joint is None:
+            y_joint_config = next(iter(self.joint_config['y'].items()))
+        elif y_joint in self.joint_config['y']:
+            y_joint_config = self.joint_config['y'][y_joint]
+        else:
+            raise ValueError(f"y_joint {y_joint} not in joint_config['y']")
+        if z_joint is None:
+            z_joint_config = next(iter(self.joint_config['z'].items()))
+        elif z_joint in self.joint_config['z']:
+            z_joint_config = self.joint_config['z'][z_joint]
+        else:
+            raise ValueError(f"z_joint {z_joint} not in joint_config['z']")
+        joint_positions_target = self.inverse_kinematics(x,y,z,x_joint_config,y_joint_config,z_joint_config)
+
+        loop_flag = 0
+
+
+        while loop_flag < len(self.joint_config['joint_names']):
+            loop_flag = 0
+            for i in range(len(self.joint_config['joint_names'])):
+                distance = joint_positions_target[i] - self.j_msg.position[i]
+                if distance == 0:
+                    loop_flag += 1
+                    continue
+                minus_flag = distance/abs(distance)
+                if abs(distance) > speed/self.rate:
+                    self.j_msg.position[i] += minus_flag * speed/self.rate
+                else :
+                    self.j_msg.position[i] = joint_positions_target[i]
+                    loop_flag += 1
+                    
+
+            # 发布关节状态
+            self.lh_joint_pub_callback()
+            time.sleep(1/self.rate)
+
+
+    def lh_joint_pub_callback(self):
         self.j_msg.header.stamp = self.get_clock().now().to_msg()
         self.j_pub.publish(self.j_msg)
 
 def main():
 
-    joint_json:dict = json.load(open("device_data.json", encoding='utf-8'))
+    pass
-    joint_action_list = {}
-    rclpy.init()
-    executor = MultiThreadedExecutor()
-    for station_name,dev_dict in joint_json.items():
-        for dev_name,joint_names in dev_dict.items():
-            joint_action_list[station_name+'_'+dev_name] = SimJointPublisher(station_name,dev_name,joint_names)
-            executor.add_node(joint_action_list[station_name+'_'+dev_name])
-
-    executor.spin()
 
 if __name__ == '__main__':
     main()

unilabos/devices/ros_dev/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.rate, self.publish_resource_tf)
-        self.create_timer(1, self.check_resource_pose_changes)
+        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()
+        if changed_poses != {}:
-        changed_poses_msg.data = json.dumps(changed_poses)
+            self.zero_count = 0
-        self.resource_pose_publisher.publish(changed_poses_msg)
+            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):
         """

unilabos/registry/devices/sim_nodes.yaml

@@ -8,4 +8,8 @@ resource.mesh_manager:
     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
+    type: ros2
 

unilabos/ros/nodes/presets/joint_republisher.py

@@ -0,0 +1,60 @@
+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()