adjusted to rename of aruco estimator and changed order for server commands
parent
ab6cc79eea
commit
a22a3fdea3
|
|
@ -5,8 +5,7 @@ import time
|
|||
|
||||
from mpc_controller import MPCController
|
||||
from robot import Robot
|
||||
|
||||
import opencv_viewer_example
|
||||
from aruco_estimator import ArucoEstimator
|
||||
|
||||
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
|
||||
# position)
|
||||
else:
|
||||
print("error: invalid move")
|
||||
print(f"error: invalid move: {move_type}")
|
||||
sys.exit(1)
|
||||
|
||||
def __str__(self):
|
||||
|
|
@ -91,7 +90,7 @@ class RemoteController:
|
|||
|
||||
def __init__(self):
|
||||
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 = {}
|
||||
for r in self.robots:
|
||||
|
|
@ -110,7 +109,7 @@ class RemoteController:
|
|||
self.t = time.time()
|
||||
|
||||
# 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.start()
|
||||
|
||||
|
|
@ -171,7 +170,57 @@ class RemoteController:
|
|||
print("could not read robot id!")
|
||||
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':
|
||||
clientsocket.sendall(bytes('{}\n'.format(self.robot_ids[robot_id].grid_pos)))
|
||||
elif cmd == b'set position':
|
||||
|
|
@ -194,46 +243,7 @@ class RemoteController:
|
|||
else:
|
||||
self.grid_control(robot_id, cmd)
|
||||
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:
|
||||
print("invalid robot id!")
|
||||
clientsocket.sendall(b'Invalid robot id!\n')
|
||||
|
|
|
|||
Loading…
Reference in New Issue