implemented control of multiple robots through GUI using measurement server
This commit is contained in:
parent
d0c17c0b91
commit
c5f3f3babb
|
@ -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)
|
||||||
|
|
|
@ -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]
|
||||||
|
|
|
@ -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
|
|
|
@ -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)
|
||||||
|
|
Loading…
Reference in New Issue
Block a user