a

robomaster_pi/conn_vel.py

@@ -9,6 +9,7 @@ from tf2_ros import TransformBroadcaster
 from geometry_msgs.msg import TransformStamped
 from geometry_msgs.msg import Twist
 from tf2_ros import TransformBroadcaster
+import os
 
 
 from rclpy.node import Node
@@ -53,8 +54,10 @@ class vel_from_ros_node(Node):
             Twist, "/cmd_vel", self.pose_callback, 10)
     
     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):
+        if((msg.angular.z == 0.0 and msg.linear.x == 0.0 and msg.linear.y == 0.0) or 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))