a

robomaster_pi/conn_odom.py

@@ -6,12 +6,39 @@ import numpy as np
 import robomaster
 from robomaster import robot
 
-x = 0
-y = 0
-z = 0
-nodeSelf = None
+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(ai, aj, ak):
+    def sub_position_handler(self, position_info):
+        self.x = position_info.x
+        self.y = position_info.y
+        self.z = position_info.z
+        self.publish_odometry()
+
+    def publish_odometry(self):
+        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
+        msg.pose.pose.position.y = self.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)
+
+    def quaternion_from_euler(self, ai, aj, ak):
         ai /= 2.0
         aj /= 2.0
         ak /= 2.0
@@ -34,44 +61,6 @@ def quaternion_from_euler(ai, aj, ak):
 
         return q
 
-def publish_odometry():
-        print("ok")
-        
-        q = quaternion_from_euler(0, 0, -z* math.pi/180)
-        msg = Odometry()
-        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
-        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
-        nodeSelf.publisher_.publish(msg)
-
-def sub_position_handler(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):
-    
-    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=sub_position_handler())
-        nodeSelf = self
-
-
-
-
-    
 
 def main(args=None):
     rclpy.init(args=args)