a

robomaster_pi/conn_odom.py

@@ -9,7 +9,6 @@ import tf2_ros
 from geometry_msgs.msg import TransformStamped
 
 class OdometryPublisher(Node):
-    zWinkel = 0.0
     
     def __init__(self):
         super().__init__('odometry_publisher')
@@ -20,6 +19,7 @@ class OdometryPublisher(Node):
         self.transform_stamped_.child_frame_id = "base_footprint"
         ep_robot = robot.Robot()
         ep_robot.initialize(conn_type="rndis")
+        self.yaw = 0.0
     
         ep_chassis = ep_robot.chassis
         ep_chassis.sub_position(freq=50, callback=self.sub_position_handler)
@@ -30,10 +30,6 @@ class OdometryPublisher(Node):
         self.yaw, pitch, roll = attitude_info
 
             
-        
-
-        
-
     def sub_position_handler(self, position_info):
         self.x, self.y, self.z = position_info
         self.publish_odometry()
@@ -61,13 +57,10 @@ class OdometryPublisher(Node):
         self.transform_stamped_.transform.translation.y = -self.y
         self.transform_stamped_.transform.translation.z = 0.0
         q = self.quaternion_from_euler(0.0, 0.0, -self.yaw / (180 / math.pi))
-        
         self.transform_stamped_.transform.rotation.x = q[0]
         self.transform_stamped_.transform.rotation.y = q[1]
         self.transform_stamped_.transform.rotation.z = q[2]
         self.transform_stamped_.transform.rotation.w = q[3]
-
-        
         self.tf_broadcaster_.sendTransform(self.transform_stamped_)