add 3d visualization

unilabos/devices/ros_dev/joint_republisher.py

@@ -0,0 +1,51 @@
+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
+
+class JointRepublisher(Node):
+    def __init__(self,device_id):
+        super().__init__(device_id)  
+        
+        # print('-'*20,device_id)
+        self.joint_repub = self.create_publisher(String,f'/devices/{device_id}/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()