a

robomaster_pi/conn_odom.py

+import rclpy
+from rclpy.node import Node
+from nav_msgs.msg import Odometry
+import math
+import numpy as np
+import robomaster
+from robomaster import robot
+
+def quaternion_from_euler(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 publish_odometry(self, position_info):
+        x, y, z = position_info
+        q = quaternion_from_euler(0, 0, -z* math.pi/180)
+        msg = Odometry()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.pose.pose.position.x = x
+        msg.pose.pose.position.y = y
+        msg.pose.pose.position.z = 0
+        msg.pose.pose.orientation.x = 0
+        msg.pose.pose.orientation.y = 0
+        msg.pose.pose.orientation.z = q[2]
+        msg.pose.pose.orientation.w = 0
+        self.publisher_.publish(msg)
+
+class OdometryPublisher(Node):
+    
+    def __init__(self):
+        super().__init__('odometry_publisher')
+        self.publisher_ = self.create_publisher(Odometry, 'odometry', 10)
+        ep_robot = robot.Robot()
+        ep_robot.initialize(conn_type="rndis")
+        ep_chassis = ep_robot.chassis
+        ep_chassis.sub_position(freq=10, callback=publish_odometry)
+
+
+
+
+    
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = OdometryPublisher()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()
\ No newline at end of file

setup.py

@@ -20,6 +20,7 @@ setup(
     tests_require=['pytest'],
     entry_points={
         'console_scripts': [
+            "connection = robomaster_pi.conn_odom:main",
             "connection = robomaster_pi.connection:main",
             "robot_ros_vel = robomaster_pi.robot_ros_vel:main",
             "robot_ros_odom = robomaster_pi.robot_ros_odom:main",