From 134dc9b99e0ba604662826bb0afa34320f9cf68d Mon Sep 17 00:00:00 2001
From: Emil Harlan <eharlan@uni-bremen.de>
Date: Thu, 1 Jun 2023 13:36:50 +0200
Subject: [PATCH] a
---
robomaster_pi/conn_vel.py | 58 ++++++++++++++++-----------------------
1 file changed, 23 insertions(+), 35 deletions(-)
diff --git a/robomaster_pi/conn_vel.py b/robomaster_pi/conn_vel.py
index d67ab1b..e0b26e8 100644
--- a/robomaster_pi/conn_vel.py
+++ b/robomaster_pi/conn_vel.py
@@ -11,63 +11,51 @@ from geometry_msgs.msg import TransformStamped
from geometry_msgs.msg import Twist
from tf2_ros import TransformBroadcaster
import os
-
-from rclpy.node import Node
-
-
-from rclpy.node import Node
-x, y, z = 0, 0, 0
+import socket
-def quaternion_from_euler(ai, aj, ak):
- ai /= 2.0
- aj /= 2.0
- ak /= 2.0
- ci = math.cos(ai)
- si = math.sin(ai)
- cj = math.cos(aj)
- sj = math.sin(aj)
- ck = math.cos(ak)
- sk = math.sin(ak)
- cc = ci*ck
- cs = ci*sk
- sc = si*ck
- ss = si*sk
- q = np.empty((4, ))
- q[0] = cj*sc - sj*cs
- q[1] = cj*ss + sj*cc
- q[2] = cj*cs - sj*sc
- q[3] = cj*cc + sj*ss
- return q
-
-
-
class vel_from_ros_node(Node):
def __init__(self, my_ep_chassis):
super().__init__("vel_from_ros")
self.hostname = "192.168.1.77"
- self.response = 1
+ self.response = False
Timer(1, self.check_connection)
self.my_ep_chassis = my_ep_chassis
self.get_vel_from_ros_ = self.create_subscription(Twist, "/cmd_vel", self.pose_callback, 10)
+
+
+ def check_connection():
+ self.response = self.isConnected()
+ if(self.response =! True):
+ self.my_ep_chassis.drive_speed(x = 0, y = 0, z = 0.5)
+
+
+ def isConnected():
+ try:
+ # connect to the host -- tells us if the host is actually
+ # reachable
+ sock = socket.create_connection(("www.google.com", 80))
+ if sock is not None:
+ print('Clossing socket')
+ sock.close
+ return True
+ except OSError:
+ pass
+ return False
-
- def check_connection():
- self.response = os.system("ping -c 1 " + self.hostname)
-
def pose_callback(self, msg: Twist):
- if((msg.angular.z == 0.0 and msg.linear.x == 0.0 and msg.linear.y == 0.0) or self.response == 0):
+ if((msg.angular.z == 0.0 and msg.linear.x == 0.0 and msg.linear.y == 0.0) or response != True):
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