a

robomaster_pi/connection.py

@@ -16,7 +16,6 @@ from rclpy.node import Node
 
 from rclpy.node import Node
 x, y, z = 0, 0, 0
-ep_chassis = None
 
 
 def quaternion_from_euler(ai, aj, ak):
@@ -60,33 +59,18 @@ def sub_position_handler(position_info):
 
 
 class vel_from_ros_node(Node):
-    
-    def __init__(self):
+    def __init__(self, my_ep_chassis):
         super().__init__("vel_from_ros")
+        self.my_ep_chassis = my_ep_chassis
         self.get_vel_from_ros_ = self.create_subscription(
             Twist, "/cmd_vel", self.pose_callback, 10)
     
     def pose_callback(self, msg: Twist):
         
         if(msg.angular.z == 0.0 and msg.linear.x == 0.0 and msg.linear.y == 0.0):
-            ep_chassis.drive_speed(x = 0, y = 0, z = 0.5)
+            self.my_ep_chassis.drive_speed(x = 0, y = 0, z = 0.5)
         else:
-            ep_chassis.drive_speed(x = msg.linear.x, y = msg.linear.y, z = msg.angular.z)
-
-
-if __name__ == '__main__':
-    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)
-
-    rclpy.init()
-    node = vel_from_ros_node()
-    rclpy.spin(node)
-    rclpy.shutdown()
-
-    ep_robot.close()
+            self.my_ep_chassis.drive_speed(x = msg.linear.x, y = msg.linear.y, z = msg.angular.z)
 
 def main():
     ep_robot = robot.Robot()
@@ -96,7 +80,7 @@ def main():
     ep_chassis.sub_position(freq=10, callback=sub_position_handler)
 
     rclpy.init()
-    node = vel_from_ros_node()
+    node = vel_from_ros_node(ep_chassis)
     rclpy.spin(node)
     rclpy.shutdown()