implemented control of multiple robots through GUI using measurement server

This commit is contained in:
Simon Pirkelmann 2021-08-31 00:05:16 +02:00
parent d0c17c0b91
commit c5f3f3babb
4 changed files with 71 additions and 33 deletions

View File

@ -141,6 +141,7 @@ class ArucoEstimator:
{'name': 'Grid rows', 'type': 'int', 'value': self.grid_rows, 'tip': "Number of rows for the grid"}, {'name': 'Grid rows', 'type': 'int', 'value': self.grid_rows, 'tip': "Number of rows for the grid"},
{'name': 'Display mode', 'type': 'list', 'values': ['color', 'grayscale'], 'value': 'color', 'tip': "Display mode for the video"}, {'name': 'Display mode', 'type': 'list', 'values': ['color', 'grayscale'], 'value': 'color', 'tip': "Display mode for the video"},
{'name': 'Autoexposure', 'type': 'bool', 'value': True}, {'name': 'Autoexposure', 'type': 'bool', 'value': True},
{'name': 'Controlled robot', 'type': 'list', 'values': self.robot_marker_ids, 'tip': 'Robot to control'},
RobotMarkerGroup(name="Robot markers", children=robot_marker_group), RobotMarkerGroup(name="Robot markers", children=robot_marker_group),
CornerMarkerGroup(name="Corner markers", children=corner_marker_group), CornerMarkerGroup(name="Corner markers", children=corner_marker_group),
] ]
@ -156,6 +157,8 @@ class ArucoEstimator:
self.params.param('Robot marker size').sigValueChanged.connect(lambda _, v: self.__setattr__('robot_marker_size', v)) self.params.param('Robot marker size').sigValueChanged.connect(lambda _, v: self.__setattr__('robot_marker_size', v))
self.params.param('Show FPS').sigValueChanged.connect(lambda _, v: self.fps_overlay.show() if v else self.fps_overlay.hide()) self.params.param('Show FPS').sigValueChanged.connect(lambda _, v: self.fps_overlay.show() if v else self.fps_overlay.hide())
self.params.param('Autoexposure').sigValueChanged.connect(lambda _, v: self.set_autoexposure(v)) self.params.param('Autoexposure').sigValueChanged.connect(lambda _, v: self.set_autoexposure(v))
self.params.param('Controlled robot').sigValueChanged.connect(lambda _, v: self.event_queue.put(('controlled_robot',
{'robot_id': v})))
self.paramtree = ParameterTree() self.paramtree = ParameterTree()
self.paramtree.setParameters(self.params, showTop=False) self.paramtree.setParameters(self.params, showTop=False)

View File

@ -143,7 +143,7 @@ class OpenLoopSolver:
def solve(self, x0, target): def solve(self, x0, target):
target = np.asarray(target)
angles_unwrapped = np.unwrap([x0[2], target[2]]) # unwrap angle to avoid jump in data angles_unwrapped = np.unwrap([x0[2], target[2]]) # unwrap angle to avoid jump in data
x0[2] = angles_unwrapped[0] x0[2] = angles_unwrapped[0]
target[2] = angles_unwrapped[1] target[2] = angles_unwrapped[1]

View File

@ -2,25 +2,12 @@ import sys
import socket import socket
import threading import threading
import time import time
import json
from robot import Robot, ControlledRobot from robot import Robot, ControlledRobot
from mpc_controller import MPCController from mpc_controller import MPCController
# HOST, PORT = "localhost", 42424
#
# robot_id = 12
#
# sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
# sock.connect((HOST, PORT))
# sock.sendall(f"{robot_id}\n".encode()) # request data for robot with given id
# #sock.sendall(f"events\n".encode()) # request events
# receiving = True
# while receiving:
# received = str(sock.recv(1024), "utf-8")
# print("Received: {}".format(received))
# receiving = len(received) > 0
class RemoteController: class RemoteController:
def __init__(self): def __init__(self):
@ -28,35 +15,84 @@ class RemoteController:
#self.robots = [Robot(11, '192.168.1.11')] #, Robot(13, '192.168.1.13'), Robot(14, '192.168.1.14')] #self.robots = [Robot(11, '192.168.1.11')] #, Robot(13, '192.168.1.13'), Robot(14, '192.168.1.14')]
#self.robots = [Robot(12, '10.10.11.91'), Robot(13, '10.10.11.90'), Robot(14, '10.10.11.89')] #self.robots = [Robot(12, '10.10.11.91'), Robot(13, '10.10.11.90'), Robot(14, '10.10.11.89')]
#self.robots = [Robot(14, '10.10.11.89')] #self.robots = [Robot(14, '10.10.11.89')]
self.robots = [ControlledRobot(13, '192.168.1.13')] self.robots = [ControlledRobot(13, '192.168.1.13'), ControlledRobot(12, '192.168.1.12')]
self.robot_ids = {} self.robot_ids = {}
for r in self.robots: for r in self.robots:
self.robot_ids[r.id] = r self.robot_ids[r.id] = r
self.controlled_robot = self.robots[0].id
# 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: for robot in self.robots:
robot.connect() robot.connect()
self.comm_socket.bind(('', 1337)) # thread for handling events received by measurement server (from GUI)
self.comm_socket.listen(5) self.event_thread = threading.Thread(target=self.handle_events)
self.event_thread.daemon = True
# connect to socket for events from GUI
try:
HOST, PORT = "localhost", 42424
self.event_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.event_socket.connect((HOST, PORT))
self.event_socket.sendall("events\n".encode())
self.event_socket.settimeout(0.1)
# check if we receive data from the measurement server
response = self.event_socket.recv(1024)
if 'error' not in str(response):
print("... connected! -> start listening for measurements")
self.event_socket.settimeout(None)
# if so we start the measurement thread
self.event_thread.start()
else:
print(f"error: cannot communicate with the measurement server.\n The response was: {response}")
except socket.timeout:
print(f"error: the measurement server did not respond with data.")
#sock.sendall(f"{robot_id}\n".encode()) # request data for robot with given id
# sock.sendall(f"events\n".encode()) # request events
# receiving = True
# while receiving:
# received = str(sock.recv(1024), "utf-8")
# print("Received: {}".format(received))
# receiving = len(received) > 0
self.t = time.time() self.t = time.time()
# start thread for marker position detection # start thread for marker position detection
self.controller = MPCController() self.controllers = []
self.robots[0].attach_controller(self.controller) for r in self.robots:
c = MPCController()
self.controllers.append(c)
r.attach_controller(c)
def handle_events(self):
receiving = True
while receiving:
received = str(self.event_socket.recv(1024), "utf-8")
if len(received) > 0:
last_received = received.split('\n')[-2]
event_type, payload = json.loads(last_received)
print(event_type, payload)
if event_type == 'click':
target_pos = (payload['x'], payload['y'], payload['angle'])
self.robot_ids[self.controlled_robot].move_to_pos(target_pos=target_pos)
elif event_type == 'controlled_robot':
self.controlled_robot = payload['robot_id']
else:
receiving = False
print(f"measurement server stopped sending event data")
def run(self): def run(self):
#print("waiting until all markers are detected...") #print("waiting until all markers are detected...")
#while not (self.estimator.all_corners_detected() and self.estimator.all_robots_detected()): #while not (self.estimator.all_corners_detected() and self.estimator.all_robots_detected()):
# pass # pass
print("starting control") print("starting control")
#self.controller.interactive_control(robots=self.robots) #self.controller.interactive_control(robots=self.robots)
self.controller.start() for c in self.controllers:
c.start()
pass pass
@ -70,10 +106,9 @@ if __name__ == '__main__':
# to use the program, first start measurement_server.py, which launches the Aruco marker detection GUI and streams # to use the program, first start measurement_server.py, which launches the Aruco marker detection GUI and streams
# marker positions and GUI events via socket # marker positions and GUI events via socket
# next, launch this program (mpc_test.py) in debug mode and set a breakpoint after self.controller.start() # next, launch this program (mpc_test.py)
# you can now set the target position for the controlled robot via move_to_pos(), e.g. # you can now direct the robots via the GUI (assuming four corner markers for position reference have been detected)
# self.robots[0].move_to_pos([0.0,0,0], True) # click any position on the screen and the current robot will drive there
# you can also switch which robot should be controlled in the GUI
# TODO # TODO document and improve program structure
# figure out how to get clicked positions from measurement server and drive robot there
# also document and improve program structure

View File

@ -95,10 +95,10 @@ class Robot:
return self.t_last_measurement, self.x, self.y, self.angle return self.t_last_measurement, self.x, self.y, self.angle
def __str__(self): def __str__(self):
connection_state = '' if self.connected else 'not' connection_state = '' if self.connected else ' not'
state = self.get_measurement() state = self.get_measurement()
last_measurement = f'last measurement = {state}' if None not in state else 'no measurements available' last_measurement = f'last measurement = {state}' if None not in state else 'no measurements available'
return f"Robot {self.id}: ip = {self.ip}:{self.port} ({connection_state} connected) " + last_measurement return f"Robot {self.id}: ip = {self.ip}:{self.port} ({connection_state} connected ) " + last_measurement
def __repr__(self): def __repr__(self):
return str(self) return str(self)