a

robomaster_pi/conn_odom.py

-import asyncio
-import math
-import numpy as np
 import rclpy
-from nav_msgs.msg import Odometry
 from rclpy.node import Node
+from nav_msgs.msg import Odometry
+import math
+import numpy as np
 import robomaster
 from robomaster import robot
 
-
 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=self.sub_position_handler)
         self.x = 0
         self.y = 0
         self.z = 0
 
-    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 sub_position_handler(self, position_info):
+        self.x, self.y, self.z = position_info
+        self.publish_odometry()
+        print("ok")
 
     def publish_odometry(self):
-        print("publishing odometry")
-        q = self.quaternion_from_euler(0, 0, -self.z * math.pi / 180)
+        print("ja")
+        q = self.quaternion_from_euler(0, 0, -self.z* math.pi/180)
         msg = Odometry()
         msg.header.stamp = self.get_clock().now().to_msg()
         msg.pose.pose.position.x = self.x
@@ -54,27 +38,36 @@ class OdometryPublisher(Node):
         msg.pose.pose.orientation.w = 0
         self.publisher_.publish(msg)
 
-    async def sub_position_handler(self, position_info):
-        self.x = position_info.x
-        self.y = position_info.y
-        self.z = position_info.z
-        print("chassis position: x:{0}, y:{1}, z:{2}".format(self.x, self.y, self.z))
-        self.publish_odometry()
+    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
 
-    async def start(self):
-        ep_robot = robot.Robot()
-        ep_robot.initialize(conn_type="rndis")
-        ep_chassis = ep_robot.chassis
-        await ep_chassis.sub_position(freq=10, callback=self.sub_position_handler)
+        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()
-    asyncio.run(node.start())
+    rclpy.spin(node)
     node.destroy_node()
     rclpy.shutdown()
 
-
 if __name__ == '__main__':
     main()
\ No newline at end of file