a

robomaster_pi/conn_odom.py

@@ -23,7 +23,8 @@ class OdometryPublisher(Node):
     
         ep_chassis = ep_robot.chassis
         ep_chassis.sub_position(freq=50, callback=self.sub_position_handler)
-        ep_chassis.sub_imu(freq=50, callback=self.sub_imu_info_handler)
+        ep_chassis.sub_attitude(freq=50, callback=sub_attitude_info_handler)
+        #ep_chassis.sub_imu(freq=50, callback=self.sub_imu_info_handler)
         
 
     def sub_imu_info_handler(self, imu_info):
@@ -36,6 +37,9 @@ class OdometryPublisher(Node):
             if(self.zWinkel + (gyro_z /50)* (180 / math.pi) <= -165):
                 self.zWinkel = self.zWinkel + (gyro_z /50)* (180 / math.pi) + 360
         
+    def sub_attitude_info_handler(self, attitude_info):
+        self.yaw, pitch, roll = attitude_info
+        print("chassis attitude: yaw:{0}, pitch:{1}, roll:{2} ".format(self.yaw, pitch, roll))
 
             
         
@@ -44,14 +48,14 @@ class OdometryPublisher(Node):
 
     def sub_position_handler(self, position_info):
         self.x, self.y, self.z = position_info
-        print("chassis position: x:{0}, y:{1}, z:{2}".format(self.x, self.y, self.zWinkel))
+        print("chassis position: x:{0}, y:{1}, z:{2}".format(self.x, self.y, self.yaw))
         self.publish_odometry()
         self.publish_transform()
  
 
     def publish_odometry(self):
         
-        q = self.quaternion_from_euler(0, 0, -self.zWinkel / (180 / math.pi))
+        q = self.quaternion_from_euler(0, 0, -self.yaw / (180 / math.pi))
         msg = Odometry()
         msg.header.stamp = self.get_clock().now().to_msg()
         msg.pose.pose.position.x = self.x
@@ -69,7 +73,7 @@ class OdometryPublisher(Node):
         self.transform_stamped_.transform.translation.x = self.x
         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.zWinkel / (180 / math.pi))
+        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]