a

robomaster_pi/Odom.py

@@ -2,7 +2,9 @@ import rclpy
 from rclpy.node import Node
 from nav_msgs.msg import Odometry
 import robomaster
+import math
 from robomaster import robot
+import numpy as np
 
 class OdometryPublisher(Node):
     def __init__(self):
@@ -14,15 +16,59 @@ class OdometryPublisher(Node):
     
         ep_chassis = ep_robot.chassis
         ep_chassis.sub_velocity(freq=50, callback=self.sub_velocity_handler)
+        ep_chassis.sub_attitude(freq=50, callback=self.sub_attitude_info_handler)
+        ep_chassis.sub_position(freq=50, callback=self.sub_position_handler)
+
+
+    def sub_attitude_info_handler(self, attitude_info):
+        self.yaw, pitch, roll = attitude_info
+
+            
+    def sub_position_handler(self, position_info):
+        self.x, self.y, self.z = position_info
+        self.publish_odometry()
+        self.publish_transform()
 
     def sub_velocity_handler(self, velocity_info):
         a, b, c, x, y, z = velocity_info
+        q = self.quaternion_from_euler(0, 0, -self.yaw / (180 / math.pi))
         self.msg.header.stamp = self.get_clock().now().to_msg()
         self.msg.twist.twist.linear.x = x
         self.msg.twist.twist.linear.y = y
+        self.msg.twist.twist.linear.z = 0.0
+        self.msg.pose.pose.position.x = self.x
+        self.msg.pose.pose.position.y = -self.y
+        self.msg.pose.pose.position.z = 0.0
+        self.msg.pose.pose.orientation.x = 0.0
+        self.msg.pose.pose.orientation.y = 0.0
+        self.msg.pose.pose.orientation.z = q[2]
+        self.msg.pose.pose.orientation.w = 0.0
 
         self.publisher_.publish(self.msg)
 
+    def quaternion_from_euler(self, ai, aj, ak):
+        ai /= 2.0
+        aj /= 2.0
+        ak /= 2.0
+        ci = math.cos(ai)
+        si = math.sin(ai)
+        cj = math.cos(aj)
+        sj = math.sin(aj)
+        ck = math.cos(ak)
+        sk = math.sin(ak)
+        cc = ci*ck
+        cs = ci*sk
+        sc = si*ck
+        ss = si*sk
+
+        q = np.empty((4, ))
+        q[0] = cj*sc - sj*cs
+        q[1] = cj*ss + sj*cc
+        q[2] = cj*cs - sj*sc
+        q[3] = cj*cc + sj*ss
+
+        return q
+
 def main(args=None):
     rclpy.init(args=args)
     node = OdometryPublisher()