From 8e7f54f5e4dc9bf13734831a5f03ab1d083da1f1 Mon Sep 17 00:00:00 2001
From: Emil Harlan <eharlan@uni-bremen.de>
Date: Fri, 2 Jun 2023 09:12:52 +0200
Subject: [PATCH] a
---
robomaster_pi/conn_vel.py | 43 ++++++++++++++++-----------------------
1 file changed, 18 insertions(+), 25 deletions(-)
diff --git a/robomaster_pi/conn_vel.py b/robomaster_pi/conn_vel.py
index 4718850..139d81c 100644
--- a/robomaster_pi/conn_vel.py
+++ b/robomaster_pi/conn_vel.py
@@ -14,16 +14,8 @@ import os
import socket
import ping3
-
-
-
-
-
-
class vel_from_ros_node(Node):
def __init__(self, my_ep_chassis):
-
-
super().__init__("vel_from_ros")
self.response = False
self.timer = threading.Timer(1, self.check_connection)
@@ -31,32 +23,31 @@ class vel_from_ros_node(Node):
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):
- self.response = self.check_ip_reachability()
- if(self.response != True):
- self.my_ep_chassis.drive_speed(x = 0, y = 0, z = 0.5)
-
+ if self.isConnected():
+ self.response = True
+ else:
+ self.response = False
+ self.my_ep_chassis.drive_speed(x=0, y=0, z=0.5)
+
+ self.timer = threading.Timer(1, self.check_connection)
+ self.timer.start()
- def check_ip_reachability(self):
+ def isConnected(self):
response = ping3.ping("192.168.1.77")
if response is not None:
return True
else:
return False
-
-
-
-
def pose_callback(self, msg: Twist):
- print("kek")
-
-
- if((msg.angular.z == 0.0 and msg.linear.x == 0.0 and msg.linear.y == 0.0) or self.response != True):
- self.my_ep_chassis.drive_speed(x = 0, y = 0, z = 0.5)
+ if self.response:
+ if((msg.angular.z == 0.0 and msg.linear.x == 0.0 and msg.linear.y == 0.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))
else:
- self.my_ep_chassis.drive_speed(x = msg.linear.x, y = -msg.linear.y, z = -msg.angular.z * (180 / math.pi))
+ self.my_ep_chassis.drive_speed(x=0, y=0, z=0.5)
def main():
ep_robot = robot.Robot()
@@ -70,4 +61,6 @@ def main():
rclpy.shutdown()
ep_robot.close()
-
\ No newline at end of file
+
+if __name__ == '__main__':
+ main()
--
GitLab