a

robomaster_pi/conn_vel.py

@@ -10,7 +10,7 @@ from geometry_msgs.msg import TransformStamped
 from geometry_msgs.msg import Twist
 from tf2_ros import TransformBroadcaster
 import os
-
+import timer
 
 from rclpy.node import Node
 
@@ -48,16 +48,25 @@ def quaternion_from_euler(ai, aj, ak):
 
 class vel_from_ros_node(Node):
     def __init__(self, my_ep_chassis):
+        self.hostname = "192.168.1.1"
+        
         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)
-    
+        
+        Timer(1, self.check_connection)
+        
+            
+
+
+    def check_connection():
+        self.response = os.system("ping -c 1 " + self.hostname)
+
     def pose_callback(self, msg: Twist):
-        hostname = "192.168.1.1"
-        response = os.system("ping -c 1 " + hostname)
         
-        if((msg.angular.z == 0.0 and msg.linear.x == 0.0 and msg.linear.y == 0.0) or response != 0):
+        
+        if((msg.angular.z == 0.0 and msg.linear.x == 0.0 and msg.linear.y == 0.0) or self.response != 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))