From e7ab92dfe2b56b64735ff59414d2ffdeb6326e32 Mon Sep 17 00:00:00 2001 From: spirkelmann Date: Wed, 9 Sep 2020 20:21:34 +0200 Subject: [PATCH] added more commands and improved error checking --- remote_control/roborally.py | 76 ++++++++++++++++++++++++++++--------- 1 file changed, 58 insertions(+), 18 deletions(-) diff --git a/remote_control/roborally.py b/remote_control/roborally.py index 2fddcab..bcae352 100644 --- a/remote_control/roborally.py +++ b/remote_control/roborally.py @@ -77,9 +77,11 @@ class Robot: def connect(self): # connect to robot try: - self.socket.connect((self.ip, 1234)) # connect to robot + print("connecting to robot {} with ip {} ...".format(self.id, self.ip)) + self.socket.connect((self.ip, 1234)) # connect to robot + print("connected!") except socket.error: - print("could not connect to socket") + print("could not connect to robot {} with ip {}".format(self.id, self.ip)) sys.exit(1) def f_ode(t, x, u): @@ -109,7 +111,8 @@ class RemoteController: self.anim_stopped = False - self.robots = [Robot(11, '192.168.1.101'), Robot(14, '192.168.1.103')] + #self.robots = [Robot(11, '192.168.1.101'), Robot(14, '192.168.1.103')] + self.robots = [Robot(11, '192.168.1.101')] #self.robots = [Robot(15, '192.168.1.102')] #self.robots = [Robot(id, ip)] @@ -117,7 +120,7 @@ class RemoteController: for r in self.robots: self.robot_ids[r.id] = r - self.valid_cmds = ['forward', 'backward', 'turn left', 'turn right'] + self.valid_cmds = ['forward', 'backward', 'turn left', 'turn right', 'turn around', 'get position', 'set position'] # socket for movement commands self.comm_socket = socket.socket() @@ -317,6 +320,7 @@ class RemoteController: running = True while running: (clientsocket, address) = self.comm_socket.accept() + clientsocket.settimeout(None) connected = True while connected: @@ -324,22 +328,54 @@ class RemoteController: data = myreceive(clientsocket) print(data) - try: - robot_id, cmd = data.split(',') - robot_id = int(robot_id) - cmd = cmd.strip() + inputs = data.split(b',') + cmd = inputs[0] + cmd = cmd.strip() - if robot_id in self.robot_ids and cmd in self.valid_cmds: - self.mpc_control(robot_id, cmd) - elif cmd == 'quit': + if len(inputs) > 1: + if cmd in self.valid_cmds: + try: + robot_id = int(inputs[1]) + except ValueError: + print("could not read robot id!") + clientsocket.sendall(b'Could not read robot id!\n') + + if robot_id in self.robot_ids: + if cmd == 'get position': + clientsocket.sendall(bytes('{}\n'.format(self.robot_ids[robot_id].grid_pos))) + elif cmd == 'set position': + try: + pos_data = ",".join(inputs[2:]) + new_grid_pos = tuple(map(lambda x: int(x[1]) if x[0] < 2 else float(x[1]), enumerate(pos_data.strip().strip('()').split(',')))) + self.robot_ids[robot_id].grid_pos = new_grid_pos + except IndexError as e: + print("could not set grid position!") + clientsocket.sendall(bytes( + 'could not set grid position! (invalid format)\n'.format( + self.robot_ids[robot_id].grid_pos))) + except ValueError as e: + print("could not set grid position!") + clientsocket.sendall(bytes('could not set grid position! (invalid format)\n'.format(self.robot_ids[robot_id].grid_pos))) + else: + self.mpc_control(robot_id, cmd) + clientsocket.sendall(b'OK\n') + else: + print("invalid robot id!") + clientsocket.sendall(b'Invalid robot id!\n') + + else: + clientsocket.sendall(b'Invalid command!\n') + else: # len(inputs) <= 1 + if 'quit' in inputs[0]: clientsocket.close() self.comm_socket.close() connected = False running = False else: - print("invalid command or robot id!") - except ValueError: - print("could not process command!") + print("could not process command!") + clientsocket.sendall(b'Could not process request!\n') + + except RuntimeError: print("disconnected") @@ -352,12 +388,12 @@ class RemoteController: # compute new grid position and orientation if cmd == 'forward': - new_x = grid_pos[0] + 1 * np.cos(grid_pos[2]) - new_y = grid_pos[1] + 1 * np.sin(grid_pos[2]) + new_x = int(grid_pos[0] + 1 * np.cos(grid_pos[2])) + new_y = int(grid_pos[1] + 1 * np.sin(grid_pos[2])) new_angle = grid_pos[2] elif cmd == 'backward': - new_x = grid_pos[0] - 1 * np.cos(grid_pos[2]) - new_y = grid_pos[1] - 1 * np.sin(grid_pos[2]) + new_x = int(grid_pos[0] - 1 * np.cos(grid_pos[2])) + new_y = int(grid_pos[1] - 1 * np.sin(grid_pos[2])) new_angle = grid_pos[2] elif cmd == 'turn left': new_x = grid_pos[0] @@ -367,6 +403,10 @@ class RemoteController: new_x = grid_pos[0] new_y = grid_pos[1] new_angle = np.unwrap([0, grid_pos[2] - np.pi / 2])[1] + elif cmd == 'turn around': + new_x = grid_pos[0] + new_y = grid_pos[1] + new_angle = np.unwrap([0, grid_pos[2] + np.pi])[1] else: print("unknown command!") sys.exit(1)