RoboRally/remote_control/roborally.py

234 lines
8.8 KiB
Python

import sys
import socket
import threading
import time
from mpc_controller import MPCController
import opencv_viewer_example
MSGLEN = 64
def myreceive(sock):
chunks = []
bytes_recd = 0
while bytes_recd < MSGLEN:
chunk = sock.recv(1)
if chunk == b'':
raise RuntimeError("socket connection broken")
chunks.append(chunk)
bytes_recd = bytes_recd + len(chunk)
if chunks[-1] == b'\n':
break
return b''.join(chunks)
class Robot:
# dictionary mapping the current orientation and a turn command to the resulting orientation
resulting_orientation = {
'^': {'turn left': '<', 'turn right': '>', 'turn around': 'v'},
'>': {'turn left': '^', 'turn right': 'v', 'turn around': '<'},
'v': {'turn left': '>', 'turn right': '<', 'turn around': '^'},
'<': {'turn left': 'v', 'turn right': '^', 'turn around': '>'},
}
# dictionary mapping an orientation to its opposite
opposites = {'^': 'v', '>': '<', 'v': '^', '<': '>'}
def __init__(self, id, ip, x=0, y=0, orientation='>'):
self.x = x
self.y = y
self.orientation = orientation
self.id = id
self.pos = None
self.euler = None
self.ip = ip
self.socket = socket.socket()
# currently active control
self.u1 = 0.0
self.u2 = 0.0
def get_neighbor_coordinates(self, direction):
# get the coordinates of the neighboring tile in the given direction
if direction == '^':
return (self.x, self.y - 1)
elif direction == '>':
return (self.x + 1, self.y)
elif direction == 'v':
return (self.x, self.y + 1)
elif direction == '<':
return (self.x - 1, self.y)
else:
print("error: unknown direction")
sys.exit(1)
def move(self, type):
if type == 'forward':
target_tile = self.get_neighbor_coordinates(self.orientation)
self.x = target_tile[0]
self.y = target_tile[1]
elif type == 'backward':
opposite_orientation = Robot.opposites[self.orientation]
target_tile = self.get_neighbor_coordinates(opposite_orientation)
self.x = target_tile[0]
self.y = target_tile[1]
elif 'turn' in type:
self.orientation = Robot.resulting_orientation[self.orientation][type]
else:
print("error: invalid move")
sys.exit(1)
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("connected!")
except socket.error:
print("could not connect to robot {} with ip {}".format(self.id, self.ip))
sys.exit(1)
def send_cmd(self, u1=0.0, u2=0.0):
if self.socket:
try:
self.socket.send(f'({u1},{u2})\n'.encode())
except BrokenPipeError:
#print(f"error: connection to robot {self.id} with ip {self.ip} lost")
pass
def __str__(self):
return self.__repr__()
def __repr__(self):
return f"x: {self.x}, y: {self.y}, orienation: {self.orientation}"
class RemoteController:
def __init__(self):
# self.robots = #[Robot(11, '192.168.1.11', (6, -3, np.pi)), Robot(12, '192.168.1.12', (6, -3, np.pi)),
# Robot(13, '192.168.1.13', (6, -3, np.pi)), Robot(14, '192.168.1.14', (6, -2, np.pi))]
#self.robots = [Robot(13, '192.168.1.13', (6, -3, np.pi))]
#self.robots = []
#self.robots = [Robot(11, '192.168.1.11', (6,-3,0)), Robot(14, '192.168.1.14', (6,3,0))]
#self.robots = [Robot(11, '192.168.1.11'), Robot(14, '192.168.1.14')]
self.robots = [Robot(12, '192.168.1.12')]
self.robot_ids = {}
for r in self.robots:
self.robot_ids[r.id] = r
self.valid_cmds = ['forward', 'backward', 'turn left', 'turn right', 'turn around', 'get position', 'set position']
# socket for movement commands
self.comm_socket = socket.socket()
self.comm_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
for robot in self.robots:
robot.connect()
self.comm_socket.bind(('', 1337))
self.comm_socket.listen(5)
self.t = time.time()
# start thread for marker position detection
self.estimator = opencv_viewer_example.ArucoEstimator(self.robot_ids.keys())
self.estimator_thread = threading.Thread(target=self.estimator.run_tracking)
self.estimator_thread.start()
self.controller = MPCController(self.estimator)
def run(self):
print("waiting until all markers are detected...")
while not self.estimator.all_corners_detected():
pass
print("starting control")
running = True
while running:
(clientsocket, address) = self.comm_socket.accept()
clientsocket.settimeout(None)
connected = True
while connected:
try:
data = myreceive(clientsocket)
print("data received: ", data)
inputs = data.split(b',')
cmd = inputs[0]
cmd = cmd.strip().decode()
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 == b'get position':
clientsocket.sendall(bytes('{}\n'.format(self.robot_ids[robot_id].grid_pos)))
elif cmd == b'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
clientsocket.sendall(b'OK\n')
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.grid_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 b'quit' in inputs[0]:
clientsocket.close()
self.comm_socket.close()
connected = False
running = False
else:
print("could not process command!")
clientsocket.sendall(b'Could not process request!\n')
except RuntimeError:
print("disconnected")
connected = False
clientsocket.close()
def grid_control(self, robot_id, cmd):
robot = self.robot_ids[robot_id] # get robot to be controlled
print("robot grid pos before move: ", robot)
robot.move(cmd)
print("robot grid pos after move: ", robot)
target = self.estimator.get_pos_from_grid_point(robot.x, robot.y, robot.orientation)
self.controller.move_to_pos(target, robot)
def main(args):
rc = RemoteController()
rc.run()
if __name__ == '__main__':
main(sys.argv)