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

adjusted to rename of aruco estimator and changed order for server commands

Browse Source
This commit is contained in:
Simon Pirkelmann 2020-10-24 20:11:32 +02:00
parent ab6cc79eea
commit a22a3fdea3
1 changed files with 55 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

100
remote_control/roborally.py
View File

@ -5,8 +5,7 @@ import time
from mpc_controller import MPCController from mpc_controller import MPCController
from robot import Robot from robot import Robot
from aruco_estimator import ArucoEstimator
import opencv_viewer_example
MSGLEN = 64 MSGLEN = 64
@ -75,7 +74,7 @@ class RoboRallyRobot(Robot):
pass # nop command -> robot grid position does not change (used e.g. for driving the robot to initial pass # nop command -> robot grid position does not change (used e.g. for driving the robot to initial
# position) # position)
else: else:
print("error: invalid move") print(f"error: invalid move: {move_type}")
sys.exit(1) sys.exit(1)
def __str__(self): def __str__(self):
@ -91,7 +90,7 @@ class RemoteController:
def __init__(self): def __init__(self):
self.robots = [] self.robots = []
self.robots = [RoboRallyRobot(12, '192.168.1.12', x=1, y=1, orientation='>')] #self.robots = [RoboRallyRobot(12, '192.168.1.12', x=1, y=1, orientation='>')]
self.robot_ids = {} self.robot_ids = {}
for r in self.robots: for r in self.robots:
@ -110,7 +109,7 @@ class RemoteController:
self.t = time.time() self.t = time.time()
# start thread for marker position detection # start thread for marker position detection
self.estimator = opencv_viewer_example.ArucoEstimator(self.robot_ids.keys()) self.estimator = ArucoEstimator(self.robot_ids.keys())
self.estimator_thread = threading.Thread(target=self.estimator.run_tracking) self.estimator_thread = threading.Thread(target=self.estimator.run_tracking)
self.estimator_thread.start() self.estimator_thread.start()
@ -171,7 +170,57 @@ class RemoteController:
print("could not read robot id!") print("could not read robot id!")
clientsocket.sendall(b'Could not read robot id!\n') clientsocket.sendall(b'Could not read robot id!\n')
if robot_id in self.robot_ids: if cmd == 'initialize_robot':
# add a new robot to the game
try:
id = int(inputs[1])
ip = inputs[2].decode().strip()
x = int(inputs[3])
y = int(inputs[4])
orientation = inputs[5].decode().strip()
print(f"initializing new robot with id {id} and ip {ip} at pos ({x},{y}) with "
f"orientation '{orientation}'")
if id not in self.robot_ids:
new_robot = RoboRallyRobot(id=id, ip=ip, x=x, y=y, orientation=orientation)
new_robot.connect()
# store the new robot in the list of robots
self.robots.append(new_robot)
self.robot_ids[new_robot.id] = new_robot # this also means the estimator
# will track the new robot because
# it got a reference to the list of
# robot ids to keep an eye out for
else: # robot is already created -> just re-initialize its position
new_robot = self.robot_ids[id]
new_robot.grid_x = x
new_robot.grid_y = y
new_robot.orientation = orientation
if new_robot.connected:
print("created new robot and successfully connected to it!")
while not self.estimator.all_robots_detected(): # wait until the robot gets detected
pass
# drive the new robot to its starting position
self.grid_control(new_robot.id, 'nop')
clientsocket.sendall(b'OK\n')
else:
clientsocket.sendall(f"error: could not connect to new robot {new_robot}".encode())
except IndexError:
print("could not initialize a new robot")
clientsocket.sendall('could not initialize a new robot: invalid command format\n'
'expected: initialize_robot, <id>, <ip>, <x>, <y>, <orientation>\n'.encode())
except ValueError:
print("could not initialize a new robot")
clientsocket.sendall('could not initialize a new robot: invalid command format\n'
'expected: initialize_robot, <id>, <ip>, <x>, <y>, <orientation>\n'.encode())
elif robot_id in self.robot_ids:
if cmd == b'get position': if cmd == b'get position':
clientsocket.sendall(bytes('{}\n'.format(self.robot_ids[robot_id].grid_pos))) clientsocket.sendall(bytes('{}\n'.format(self.robot_ids[robot_id].grid_pos)))
elif cmd == b'set position': elif cmd == b'set position':
@ -194,46 +243,7 @@ class RemoteController:
else: else:
self.grid_control(robot_id, cmd) self.grid_control(robot_id, cmd)
clientsocket.sendall(b'OK\n') clientsocket.sendall(b'OK\n')
elif cmd == 'initialize_robot':
# add a new robot to the game
try:
id = int(inputs[1])
ip = inputs[2].decode().strip()
x = int(inputs[3])
y = int(inputs[4])
orientation = inputs[5].decode().strip()
print(f"initializing new robot with id {id} and ip {ip} at pos ({x},{y}) with "
f"orientation '{orientation}'")
new_robot = RoboRallyRobot(id=id, ip=ip, x=x, y=y, orientation=orientation)
new_robot.connect()
if new_robot.connected:
print("created new robot and successfully connected to it!")
# store the new robot in the list of robots
self.robots.append(new_robot)
self.robot_ids[new_robot.id] = new_robot # this also means the estimator
# will track the new robot because
# it got a reference to the list of
# robot ids to keep an eye out for
while not self.estimator.all_robots_detected(): # wait until the robot gets detected
pass
# drive the new robot to its starting position
self.grid_control(new_robot.id, 'nop')
clientsocket.sendall(b'OK\n')
else:
clientsocket.sendall(f"error: could not connect to new robot {new_robot}".encode())
except IndexError:
print("could not initialize a new robot")
clientsocket.sendall('could not initialize a new robot: invalid command format\n'
'expected: initialize_robot, <id>, <ip>, <x>, <y>, <orientation>\n'.encode())
except ValueError:
print("could not initialize a new robot")
clientsocket.sendall('could not initialize a new robot: invalid command format\n'
'expected: initialize_robot, <id>, <ip>, <x>, <y>, <orientation>\n'.encode())
else: else:
print("invalid robot id!") print("invalid robot id!")
clientsocket.sendall(b'Invalid robot id!\n') clientsocket.sendall(b'Invalid robot id!\n')