a

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))