a

robomaster_pi/robot_ros_odom.py

@@ -39,7 +39,7 @@ def quaternion_from_euler(ai, aj, ak):
     return q
 
 class Robomaster_odom(Node):
-    
+
     def sub_position_handler(position_info):
         print("handler")
         tf_broadcaster = TransformBroadcaster()
@@ -60,7 +60,24 @@ class Robomaster_odom(Node):
 
     def __init__(self, ep_chassis):
         super().__init__("robomaster_odom")
-        ep_chassis.sub_position(freq=10, callback=self.sub_position_handler)
+        def sub_position_handler(position_info):
+            print("handler")
+            tf_broadcaster = TransformBroadcaster()
+            t = TransformStamped()
+            t.header.stampep_chassis = time.get_clock().now().to_msg()
+            t.header.frame_id = 'odom'
+            t.child_frame_id = "base_footprint"
+            t.transform.translation.x = position_info.x
+            t.transform.translation.y = -position_info.y
+            t.transform.translation.z = 0.0
+            q = quaternion_from_euler(0, 0, -position_info.z* math.pi/180)
+            t.transform.rotation.x = q[0]
+            t.transform.rotation.y = q[1]
+            t.transform.rotation.z = q[2]
+            t.transform.rotation.w = q[3]
+            tf_broadcaster.sendTransform(t)
+            print("ok")
+        ep_chassis.sub_position(freq=10, callback=sub_position_handler)
         print("sub")