Device visualization (#14)

* 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中 --------- Co-authored-by: zhangshixiang <@zhangshixiang> Co-authored-by: wznln <18435084+Xuwznln@users.noreply.github.com>
2025-12-17 13:01:12 +00:00 · 2025-05-06 09:37:57 +08:00
parent 752442cb37
commit a52133b7d0
53 changed files with 2654 additions and 25 deletions
--- a/unilabos/devices/ros_dev/init.py
+++ b/unilabos/devices/ros_dev/init.py
--- a/unilabos/devices/ros_dev/liquid_handler_joint_publisher.py
+++ b/unilabos/devices/ros_dev/liquid_handler_joint_publisher.py
@@ -0,0 +1,208 @@
+import copy
+import rclpy
+import json
+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 rclpy.action.server import ServerGoalHandle
+from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode
+from tf_transformations import quaternion_from_euler
+from tf2_ros import TransformBroadcaster, Buffer, TransformListener 
+
+
+class LiquidHandlerJointPublisher(BaseROS2DeviceNode):
+    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,
+            status_types={},
+            action_value_mappings={},
+            hardware_interface={},
+            print_publish=False,
+            resource_tracker=resource_tracker,  
+        )  
+
+        # 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.joint_config   = joint_config
+        self.j_msg.position = [0.0 for i in range(len(joint_config['joint_names']))]
+        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.lh_joint_pub_callback)
+        self.j_action       = ActionServer(
+            self,
+            SendCmd,
+            "joint",
+            self.lh_joint_action_callback,
+            result_timeout=5000
+        )
+
+    def lh_joint_action_callback(self,goal_handle: ServerGoalHandle):
+        """Move a single joint
+
+        Args:
+            command: A JSON-formatted string that includes joint_name, speed, position
+
+                    joint_name (str): The name of the joint to move
+                    speed (float): The speed of the movement, speed > 0
+                    position (float): The position to move to
+
+        Returns:
+            None
+        """
+        result = SendCmd.Result()
+        cmd_str = str(goal_handle.request.command).replace('\'','\"')
+        # goal_handle.execute()
+
+        try:
+            cmd_dict = json.loads(cmd_str)
+            self.move_joints(**cmd_dict)
+            result.success = True
+            goal_handle.succeed()
+            
+        except Exception as e:
+            print(e)
+            goal_handle.abort()
+            result.success = False
+
+        return result
+    def inverse_kinematics(self, x, y, z, 
+                           x_joint:dict, 
+                           y_joint:dict, 
+                           z_joint:dict   ):
+        """
+        将x、y、z坐标转换为对应关节的位置
+        
+        Args:
+            x (float): x坐标
+            y (float): y坐标
+            z (float): z坐标
+            x_joint (dict): x轴关节配置，包含plus和offset
+            y_joint (dict): y轴关节配置，包含plus和offset
+            z_joint (dict): z轴关节配置，包含plus和offset
+            
+        Returns:
+            dict: 关节名称和对应位置的字典
+        """
+        joint_positions = copy.deepcopy(self.j_msg.position)
+        
+        # 处理x轴关节
+        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 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():
+
+    pass
+
+if __name__ == '__main__':
+    main()