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

added more commands and improved error checking

Browse Source
This commit is contained in:
Simon Pirkelmann 2020-09-09 20:21:34 +02:00
parent c98760a2c7
commit e7ab92dfe2
1 changed files with 58 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

68
remote_control/roborally.py
View File

@ -77,9 +77,11 @@ class Robot:
def connect(self): def connect(self):
# connect to robot # connect to robot
try: try:
print("connecting to robot {} with ip {} ...".format(self.id, self.ip))
self.socket.connect((self.ip, 1234)) # connect to robot self.socket.connect((self.ip, 1234)) # connect to robot
print("connected!")
except socket.error: 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) sys.exit(1)
def f_ode(t, x, u): def f_ode(t, x, u):
@ -109,7 +111,8 @@ class RemoteController:
self.anim_stopped = False 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(15, '192.168.1.102')]
#self.robots = [Robot(id, ip)] #self.robots = [Robot(id, ip)]
@ -117,7 +120,7 @@ class RemoteController:
for r in self.robots: for r in self.robots:
self.robot_ids[r.id] = r 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 # socket for movement commands
self.comm_socket = socket.socket() self.comm_socket = socket.socket()
@ -317,6 +320,7 @@ class RemoteController:
running = True running = True
while running: while running:
(clientsocket, address) = self.comm_socket.accept() (clientsocket, address) = self.comm_socket.accept()
clientsocket.settimeout(None)
connected = True connected = True
while connected: while connected:
@ -324,22 +328,54 @@ class RemoteController:
data = myreceive(clientsocket) data = myreceive(clientsocket)
print(data) print(data)
try: inputs = data.split(b',')
robot_id, cmd = data.split(',') cmd = inputs[0]
robot_id = int(robot_id)
cmd = cmd.strip() cmd = cmd.strip()
if robot_id in self.robot_ids and cmd in self.valid_cmds: 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) self.mpc_control(robot_id, cmd)
elif cmd == 'quit': 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() clientsocket.close()
self.comm_socket.close() self.comm_socket.close()
connected = False connected = False
running = False running = False
else: 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: except RuntimeError:
print("disconnected") print("disconnected")
@ -352,12 +388,12 @@ class RemoteController:
# compute new grid position and orientation # compute new grid position and orientation
if cmd == 'forward': if cmd == 'forward':
new_x = grid_pos[0] + 1 * np.cos(grid_pos[2]) new_x = int(grid_pos[0] + 1 * np.cos(grid_pos[2]))
new_y = grid_pos[1] + 1 * np.sin(grid_pos[2]) new_y = int(grid_pos[1] + 1 * np.sin(grid_pos[2]))
new_angle = grid_pos[2] new_angle = grid_pos[2]
elif cmd == 'backward': elif cmd == 'backward':
new_x = grid_pos[0] - 1 * np.cos(grid_pos[2]) new_x = int(grid_pos[0] - 1 * np.cos(grid_pos[2]))
new_y = grid_pos[1] - 1 * np.sin(grid_pos[2]) new_y = int(grid_pos[1] - 1 * np.sin(grid_pos[2]))
new_angle = grid_pos[2] new_angle = grid_pos[2]
elif cmd == 'turn left': elif cmd == 'turn left':
new_x = grid_pos[0] new_x = grid_pos[0]
@ -367,6 +403,10 @@ class RemoteController:
new_x = grid_pos[0] new_x = grid_pos[0]
new_y = grid_pos[1] new_y = grid_pos[1]
new_angle = np.unwrap([0, grid_pos[2] - np.pi / 2])[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: else:
print("unknown command!") print("unknown command!")
sys.exit(1) sys.exit(1)