Telos4
/
RoboRally
1
0
Fork 1
Code Issues Pull Requests Releases Wiki Activity

implemented control from multiple robots

master
Simon Pirkelmann 2020-11-14 16:06:57 +01:00
parent 47f652fd62
commit a3af40b001
4 changed files with 119 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

56
remote_control/controller.py Normal file
View File

@ -0,0 +1,56 @@
import threading
import time
class ControllerBase:
def __init__(self, control_rate=0.1):
self.robot = None
self.controlling = False
self.target_pos = None
self.control_thread = None
self.control_rate = control_rate
def control_loop(self):
while self.controlling:
if self.target_pos is not None:
state = self.get_measured_position()
control = self.compute_control(state)
self.apply_control(control)
time.sleep(self.control_rate)
self.apply_control((0.0,0.0)) # stop robot
def set_target_position(self, target_pos):
self.target_pos = target_pos
def get_measured_position(self):
if self.robot is not None:
return self.robot.get_measurement()
else:
raise Exception("error: controller cannot get measurement!\n there is no robot attached to the controller!")
def compute_control(self, state):
return (0.0, 0.0)
def apply_control(self, control):
if self.robot is not None:
self.robot.send_cmd(u1=control[0], u2=control[1])
else:
raise Exception("error: controller cannot apply control!\n there is no robot attached to the controller!")
def attach_robot(self, robot):
self.robot = robot
def start(self):
self.controlling = True
# start control thread
self.control_thread = threading.Thread(target=self.control_loop)
self.control_thread.start()
def stop(self):
# pause controller
self.controlling = False
if self.control_thread is not None:
self.control_thread.join()

10
remote_control/event_listener.py
View File

@ -7,7 +7,9 @@ import json
class EventListener:
def __init__(self, event_server):
self.event_thread = threading.Thread(target=self.receive_events)
self.event_thread.daemon = True # mark thread as daemon -> it terminates automatically when program shuts down
self.event_queue = queue.Queue()
self.receiving = False
# connect to event server
print(f"connecting to event server on {event_server} ...")
@ -33,8 +35,8 @@ class EventListener:
print(f"error: could not connect to event server at {event_server}.")
def receive_events(self):
receiving = True
while receiving:
self.receiving = True
while self.receiving:
received = str(self.event_socket.recv(1024), "utf-8")
if len(received) > 0:
events = received.split('\n')
@ -43,5 +45,5 @@ class EventListener:
event = json.loads(event_json)
self.event_queue.put(event)
else:
receiving = False
print("event server seems to have shut down -> stop listening")
self.receiving = False
print("event server seems to have shut down -> stop listening")

2
remote_control/measurement_server.py
View File

@ -50,7 +50,7 @@ class MeasurementServer(socketserver.ThreadingMixIn, socketserver.TCPServer):
print("an error occurred -> terminating connection")
if __name__ == "__main__":
aruco_estimator = ArucoEstimator(use_realsense=False, robot_marker_ids=[11, 12, 13, 14])
aruco_estimator = ArucoEstimator(use_realsense=True, robot_marker_ids=[11, 12, 13, 14])
estimator_thread = threading.Thread(target=aruco_estimator.run_tracking, kwargs={'invert_grayscale': True})
estimator_thread.start()

66
remote_control/robot.py
View File

@ -13,6 +13,7 @@ class Robot:
self.angle = None
self.ip = ip
self.port = 1234
self.socket = socket.socket()
# currently active control
@ -24,17 +25,19 @@ class Robot:
self.measurement_server = measurement_server
self.measurement_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) # TCP socket
self.measurement_thread = threading.Thread(target=self.receive_measurements)
self.measurement_thread.daemon = True # mark thread as daemon -> it terminates automatically when program shuts down
self.receiving = False
def connect(self):
# connect to robot
try:
print("connecting to robot {} with ip {} ...".format(self.id, self.ip))
self.socket.connect((self.ip, 1234)) # connect to robot
print(f"connecting to robot {self.ip} at {self.ip}:{self.port} ...")
self.socket.connect((self.ip, self.port)) # connect to robot
print("connected!")
self.connected = True
except socket.error:
print("error: could not connect to robot {} with ip {}".format(self.id, self.ip))
print(f"error: could not connect to robot {self.id} at {self.ip}:{self.port}")
# connect to measurement server
print(f"connecting to measurement server on {self.measurement_server} ...")
@ -47,7 +50,7 @@ class Robot:
# check if we receive data from the measurement server
response = self.measurement_socket.recv(1024)
if not 'error' in str(response):
print("... connected! -> start listening for events")
print("... connected! -> start listening for measurements")
self.measurement_socket.settimeout(None)
# if so we start the measurement thread
self.measurement_thread.start()
@ -58,9 +61,8 @@ class Robot:
except ConnectionRefusedError:
print(f"error: could not connect to measurement server at {self.measurement_server}.")
def send_cmd(self, u1=0.0, u2=0.0):
if self.socket:
if self.socket and self.connected:
try:
self.socket.send(f'({u1},{u2})\n'.encode())
except BrokenPipeError:
@ -69,20 +71,64 @@ class Robot:
except ConnectionResetError:
print(f"error: connection to robot {self.id} with ip {self.ip} lost")
pass
else:
print(f"error: robot {self.id} is not connected to {self.ip}")
def receive_measurements(self):
receiving = True
while receiving:
self.receiving = True
while self.receiving:
received = str(self.measurement_socket.recv(1024), "utf-8")
if len(received) > 0:
measurement = json.loads(received)
last_received = received.split('\n')[-2]
measurement = json.loads(last_received)
self.t_last_measurement = measurement['t']
self.x = measurement['x']
self.y = measurement['y']
self.angle = measurement['angle']
else:
receiving = False
self.receiving = False
print(f"measurement server stopped sending data for robot {self.id}")
def stop_measurement_thread(self):
self.receiving = False
self.measurement_thread.join()
def get_measurement(self):
return (self.t_last_measurement, self.x, self.y, self.angle)
def __str__(self):
connection_state = '' if self.connected else 'not'
state = self.get_measurement()
last_measurement = f'last measurement = {state}' if not None in state else 'no measurements available'
return f"Robot {self.id}: ip = {self.ip}:{self.port} ({connection_state} connected) " + last_measurement
def __repr__(self):
return str(self)
class ControlledRobot(Robot):
def __init__(self, marker_id, ip):
super().__init__(marker_id, ip)
self.controller = None
def start_control(self):
if self.controller is not None:
self.controller.start()
else:
raise Exception("Error: Cannot start control: there is not controller attached to the robot!")
def stop_control(self):
if self.controller is not None:
self.controller.stop()
else:
raise Exception("Error: Cannot stop control: there is not controller attached to the robot!")
def attach_controller(self, controller):
self.controller = controller
self.controller.attach_robot(self)
def move_to_pos(self, target_pos):
if self.controller is not None:
self.controller.set_target_position(target_pos)
else:
raise Exception("Error: Cannot move to position: there is not controller attached to the robot!")