a

robomaster_pi/connection.py

@@ -41,28 +41,8 @@ def quaternion_from_euler(ai, aj, ak):
 
     return q
 
-class Robomaster_odom(Node):
-    def __init__(self):
-        self.ep_chassis.sub_position(freq=10, callback=sub_position_handler)
     
 
-    def sub_position_handler(position_info):
-        super().__init__("robomaster_odom")
-        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")
 
 
 class vel_from_ros_node(Node):

robomaster_pi/robot_ros_odom.py

@@ -39,8 +39,8 @@ def quaternion_from_euler(ai, aj, ak):
     return q
 
 class Robomaster_odom(Node):
-    def __init__(self):
-        self.ep_chassis.sub_position(freq=10, callback=sub_position_handler)
+    def __init__(self, ep_chassis):
+        ep_chassis.sub_position(freq=10, callback=self.sub_position_handler)
     
 
     def sub_position_handler(position_info):