added more commands and improved error checking
parent
c98760a2c7
commit
e7ab92dfe2
|
|
@ -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)
|
||||
|
|
|
|||
Loading…
Reference in New Issue