a

robomaster_pi/conn_odom.py

@@ -38,16 +38,16 @@ class OdometryPublisher(Node):
 
     def publish_odometry(self):
         
-        q = self.quaternion_from_euler(0, 0, -self.yaw / (180 / math.pi))
+        q = self.quaternion_from_euler(0.0, 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
         msg.pose.pose.position.y = -self.y
         msg.pose.pose.position.z = 0.0
-        msg.pose.pose.orientation.x = 0.0
-        msg.pose.pose.orientation.y = 0.0
+        msg.pose.pose.orientation.x = q[0]
+        msg.pose.pose.orientation.y = q[1]
         msg.pose.pose.orientation.z = q[2]
-        msg.pose.pose.orientation.w = 0.0
+        msg.pose.pose.orientation.w = q[3]
         
         self.publisher_.publish(msg)