diff --git a/remote_control/aruco_estimator.py b/remote_control/aruco_estimator.py index b5795de..55e04c9 100644 --- a/remote_control/aruco_estimator.py +++ b/remote_control/aruco_estimator.py @@ -141,6 +141,7 @@ class ArucoEstimator: {'name': 'Grid rows', 'type': 'int', 'value': self.grid_rows, 'tip': "Number of rows for the grid"}, {'name': 'Display mode', 'type': 'list', 'values': ['color', 'grayscale'], 'value': 'color', 'tip': "Display mode for the video"}, {'name': 'Autoexposure', 'type': 'bool', 'value': True}, + {'name': 'Controlled robot', 'type': 'list', 'values': self.robot_marker_ids, 'tip': 'Robot to control'}, RobotMarkerGroup(name="Robot markers", children=robot_marker_group), CornerMarkerGroup(name="Corner markers", children=corner_marker_group), ] @@ -156,6 +157,8 @@ class ArucoEstimator: self.params.param('Robot marker size').sigValueChanged.connect(lambda _, v: self.__setattr__('robot_marker_size', v)) self.params.param('Show FPS').sigValueChanged.connect(lambda _, v: self.fps_overlay.show() if v else self.fps_overlay.hide()) self.params.param('Autoexposure').sigValueChanged.connect(lambda _, v: self.set_autoexposure(v)) + self.params.param('Controlled robot').sigValueChanged.connect(lambda _, v: self.event_queue.put(('controlled_robot', + {'robot_id': v}))) self.paramtree = ParameterTree() self.paramtree.setParameters(self.params, showTop=False) diff --git a/remote_control/casadi_opt.py b/remote_control/casadi_opt.py index 8c74b55..4508a35 100644 --- a/remote_control/casadi_opt.py +++ b/remote_control/casadi_opt.py @@ -143,7 +143,7 @@ class OpenLoopSolver: def solve(self, x0, target): - + target = np.asarray(target) angles_unwrapped = np.unwrap([x0[2], target[2]]) # unwrap angle to avoid jump in data x0[2] = angles_unwrapped[0] target[2] = angles_unwrapped[1] diff --git a/remote_control/mpc_test.py b/remote_control/mpc_test.py index 1ff7c60..1f8328c 100644 --- a/remote_control/mpc_test.py +++ b/remote_control/mpc_test.py @@ -2,25 +2,12 @@ import sys import socket import threading import time +import json from robot import Robot, ControlledRobot from mpc_controller import MPCController -# HOST, PORT = "localhost", 42424 -# -# robot_id = 12 -# -# sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) -# sock.connect((HOST, PORT)) -# sock.sendall(f"{robot_id}\n".encode()) # request data for robot with given id -# #sock.sendall(f"events\n".encode()) # request events -# receiving = True -# while receiving: -# received = str(sock.recv(1024), "utf-8") -# print("Received: {}".format(received)) -# receiving = len(received) > 0 - class RemoteController: def __init__(self): @@ -28,35 +15,84 @@ class RemoteController: #self.robots = [Robot(11, '192.168.1.11')] #, Robot(13, '192.168.1.13'), Robot(14, '192.168.1.14')] #self.robots = [Robot(12, '10.10.11.91'), Robot(13, '10.10.11.90'), Robot(14, '10.10.11.89')] #self.robots = [Robot(14, '10.10.11.89')] - self.robots = [ControlledRobot(13, '192.168.1.13')] + self.robots = [ControlledRobot(13, '192.168.1.13'), ControlledRobot(12, '192.168.1.12')] self.robot_ids = {} for r in self.robots: self.robot_ids[r.id] = r - - # socket for movement commands - self.comm_socket = socket.socket() - self.comm_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + self.controlled_robot = self.robots[0].id for robot in self.robots: robot.connect() - self.comm_socket.bind(('', 1337)) - self.comm_socket.listen(5) + # thread for handling events received by measurement server (from GUI) + self.event_thread = threading.Thread(target=self.handle_events) + self.event_thread.daemon = True + + # connect to socket for events from GUI + try: + HOST, PORT = "localhost", 42424 + self.event_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + self.event_socket.connect((HOST, PORT)) + self.event_socket.sendall("events\n".encode()) + self.event_socket.settimeout(0.1) + # check if we receive data from the measurement server + response = self.event_socket.recv(1024) + if 'error' not in str(response): + print("... connected! -> start listening for measurements") + self.event_socket.settimeout(None) + # if so we start the measurement thread + self.event_thread.start() + else: + print(f"error: cannot communicate with the measurement server.\n The response was: {response}") + except socket.timeout: + print(f"error: the measurement server did not respond with data.") + + + #sock.sendall(f"{robot_id}\n".encode()) # request data for robot with given id + # sock.sendall(f"events\n".encode()) # request events + # receiving = True + # while receiving: + # received = str(sock.recv(1024), "utf-8") + # print("Received: {}".format(received)) + # receiving = len(received) > 0 self.t = time.time() # start thread for marker position detection - self.controller = MPCController() - self.robots[0].attach_controller(self.controller) + self.controllers = [] + for r in self.robots: + c = MPCController() + self.controllers.append(c) + r.attach_controller(c) + + def handle_events(self): + receiving = True + while receiving: + received = str(self.event_socket.recv(1024), "utf-8") + if len(received) > 0: + last_received = received.split('\n')[-2] + event_type, payload = json.loads(last_received) + print(event_type, payload) + + if event_type == 'click': + target_pos = (payload['x'], payload['y'], payload['angle']) + self.robot_ids[self.controlled_robot].move_to_pos(target_pos=target_pos) + elif event_type == 'controlled_robot': + self.controlled_robot = payload['robot_id'] + else: + receiving = False + print(f"measurement server stopped sending event data") def run(self): + #print("waiting until all markers are detected...") #while not (self.estimator.all_corners_detected() and self.estimator.all_robots_detected()): # pass print("starting control") #self.controller.interactive_control(robots=self.robots) - self.controller.start() + for c in self.controllers: + c.start() pass @@ -70,10 +106,9 @@ if __name__ == '__main__': # to use the program, first start measurement_server.py, which launches the Aruco marker detection GUI and streams # marker positions and GUI events via socket -# next, launch this program (mpc_test.py) in debug mode and set a breakpoint after self.controller.start() -# you can now set the target position for the controlled robot via move_to_pos(), e.g. -# self.robots[0].move_to_pos([0.0,0,0], True) +# next, launch this program (mpc_test.py) +# you can now direct the robots via the GUI (assuming four corner markers for position reference have been detected) +# click any position on the screen and the current robot will drive there +# you can also switch which robot should be controlled in the GUI -# TODO -# figure out how to get clicked positions from measurement server and drive robot there -# also document and improve program structure \ No newline at end of file +# TODO document and improve program structure \ No newline at end of file diff --git a/remote_control/robot.py b/remote_control/robot.py index ad13ad8..605ad41 100644 --- a/remote_control/robot.py +++ b/remote_control/robot.py @@ -95,10 +95,10 @@ class Robot: return self.t_last_measurement, self.x, self.y, self.angle def __str__(self): - connection_state = '' if self.connected else 'not' + connection_state = '' if self.connected else ' not' state = self.get_measurement() last_measurement = f'last measurement = {state}' if None not in state else 'no measurements available' - return f"Robot {self.id}: ip = {self.ip}:{self.port} ({connection_state} connected) " + last_measurement + return f"Robot {self.id}: ip = {self.ip}:{self.port} ({connection_state} connected ) " + last_measurement def __repr__(self): return str(self)