From 6eb2e48463b68946b25667d7d5079b325934b454 Mon Sep 17 00:00:00 2001
From: Emil Harlan <eharlan@uni-bremen.de>
Date: Thu, 1 Jun 2023 11:16:31 +0200
Subject: [PATCH] a
---
robomaster_pi/conn_vel.py | 19 ++++++++++++++-----
1 file changed, 14 insertions(+), 5 deletions(-)
diff --git a/robomaster_pi/conn_vel.py b/robomaster_pi/conn_vel.py
index 54d1875..33af968 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))
--
GitLab