a

robomaster_pi/conn_vel.py

@@ -14,16 +14,8 @@ import os
 import socket
 import ping3
 
-
-
-
-
-    
-
 class vel_from_ros_node(Node):
     def __init__(self, my_ep_chassis):
-
-
         super().__init__("vel_from_ros")
         self.response = False
         self.timer = threading.Timer(1, self.check_connection)
@@ -31,32 +23,31 @@ class vel_from_ros_node(Node):
         self.my_ep_chassis = my_ep_chassis
         self.get_vel_from_ros_ = self.create_subscription(Twist, "/cmd_vel", self.pose_callback, 10)
 
-
     def check_connection(self):
-        self.response = self.check_ip_reachability()
-        if(self.response != True):
-            self.my_ep_chassis.drive_speed(x = 0, y = 0, z = 0.5)
-
+        if self.isConnected():
+            self.response = True
+        else:
+            self.response = False
+            self.my_ep_chassis.drive_speed(x=0, y=0, z=0.5)
+        
+        self.timer = threading.Timer(1, self.check_connection)
+        self.timer.start()
 
-    def check_ip_reachability(self):
+    def isConnected(self):
         response = ping3.ping("192.168.1.77")
         if response is not None:
             return True
         else:
             return False
-        
-        
-        
-            
 
     def pose_callback(self, msg: Twist):
-        print("kek")
-        
-        
-        if((msg.angular.z == 0.0 and msg.linear.x == 0.0 and msg.linear.y == 0.0) or self.response != True):
-            self.my_ep_chassis.drive_speed(x = 0, y = 0, z = 0.5)
+        if self.response:
+            if((msg.angular.z == 0.0 and msg.linear.x == 0.0 and msg.linear.y == 0.0)):
+                self.my_ep_chassis.drive_speed(x=0, y=0, z=0.5)
+            else:
+                self.my_ep_chassis.drive_speed(x=msg.linear.x, y=-msg.linear.y, z=-msg.angular.z * (180 / math.pi))
         else:
-            self.my_ep_chassis.drive_speed(x = msg.linear.x, y = -msg.linear.y, z = -msg.angular.z * (180 / math.pi))
+            self.my_ep_chassis.drive_speed(x=0, y=0, z=0.5)
 
 def main():
     ep_robot = robot.Robot()
@@ -70,4 +61,6 @@ def main():
     rclpy.shutdown()
 
     ep_robot.close()
-    
\ No newline at end of file
+
+if __name__ == '__main__':
+    main()