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/ros/nodes/presets/joint_republisher.py
+++ b/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()