diff --git a/robomaster_pi/conn_vel.py b/robomaster_pi/conn_vel.py index 1aa880d41b4c005c1e05fe1bbfca15e360ce441a..54d18756854bfdd42734abdc493ebea85beabd81 100644 --- a/robomaster_pi/conn_vel.py +++ b/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))