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