a

robomaster_pi/conn_odom.py

@@ -6,6 +6,11 @@ import numpy as np
 import robomaster
 from robomaster import robot
 
+x = 0
+y = 0
+z = 0
+nodeSelf = None
+
 def quaternion_from_euler(ai, aj, ak):
         ai /= 2.0
         aj /= 2.0
@@ -29,12 +34,12 @@ def quaternion_from_euler(ai, aj, ak):
 
         return q
 
-def publish_odometry(position_info):
+def publish_odometry():
         print("ok")
-        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.header.stamp = nodeSelf.get_clock().now().to_msg()
         msg.pose.pose.position.x = x
         msg.pose.pose.position.y = y
         msg.pose.pose.position.z = 0
@@ -42,11 +47,15 @@ def publish_odometry(position_info):
         msg.pose.pose.orientation.y = 0
         msg.pose.pose.orientation.z = q[2]
         msg.pose.pose.orientation.w = 0
-        self.publisher_.publish(msg)
+        nodeSelf.publisher_.publish(msg)
 
 def sub_position_handler(position_info):
-    x, y, z = position_info
+    x = position_info.x
+    y = position_info.y
+    z = position_info.z
     print("chassis position: x:{0}, y:{1}, z:{2}".format(x, y, z))
+    publish_odometry
+
 
 class OdometryPublisher(Node):
     
@@ -57,6 +66,7 @@ class OdometryPublisher(Node):
         ep_robot.initialize(conn_type="rndis")
         ep_chassis = ep_robot.chassis
         ep_chassis.sub_position(freq=10, callback=sub_position_handler)
+        nodeSelf = self