From a3af40b001fe8971e48173bac21b118928f97f45 Mon Sep 17 00:00:00 2001 From: Simon Pirkelmann Date: Sat, 14 Nov 2020 16:06:57 +0100 Subject: [PATCH] implemented control from multiple robots --- remote_control/controller.py | 56 +++++++++++++++++++++++ remote_control/event_listener.py | 10 +++-- remote_control/measurement_server.py | 2 +- remote_control/robot.py | 66 +++++++++++++++++++++++----- 4 files changed, 119 insertions(+), 15 deletions(-) create mode 100644 remote_control/controller.py diff --git a/remote_control/controller.py b/remote_control/controller.py new file mode 100644 index 0000000..80baee6 --- /dev/null +++ b/remote_control/controller.py @@ -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() diff --git a/remote_control/event_listener.py b/remote_control/event_listener.py index 8b2e92f..48db580 100644 --- a/remote_control/event_listener.py +++ b/remote_control/event_listener.py @@ -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") \ No newline at end of file + self.receiving = False + print("event server seems to have shut down -> stop listening") diff --git a/remote_control/measurement_server.py b/remote_control/measurement_server.py index 25e6f16..3954e66 100644 --- a/remote_control/measurement_server.py +++ b/remote_control/measurement_server.py @@ -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() diff --git a/remote_control/robot.py b/remote_control/robot.py index 24d4e5d..811116b 100644 --- a/remote_control/robot.py +++ b/remote_control/robot.py @@ -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!")