114 lines
4.3 KiB
Python
114 lines
4.3 KiB
Python
import sys
|
|
import socket
|
|
import threading
|
|
import time
|
|
import json
|
|
|
|
from robot import Robot, ControlledRobot
|
|
|
|
from mpc_controller import MPCController
|
|
|
|
|
|
class RemoteController:
|
|
def __init__(self):
|
|
self.robots = []
|
|
#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(14, '10.10.11.89')]
|
|
self.robots = [ControlledRobot(13, '192.168.1.13'), ControlledRobot(12, '192.168.1.12')]
|
|
|
|
self.robot_ids = {}
|
|
for r in self.robots:
|
|
self.robot_ids[r.id] = r
|
|
self.controlled_robot = self.robots[0].id
|
|
|
|
for robot in self.robots:
|
|
robot.connect()
|
|
|
|
# thread for handling events received by measurement server (from GUI)
|
|
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()
|
|
|
|
# start thread for marker position detection
|
|
self.controllers = []
|
|
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):
|
|
|
|
#print("waiting until all markers are detected...")
|
|
#while not (self.estimator.all_corners_detected() and self.estimator.all_robots_detected()):
|
|
# pass
|
|
print("starting control")
|
|
#self.controller.interactive_control(robots=self.robots)
|
|
for c in self.controllers:
|
|
c.start()
|
|
pass
|
|
|
|
|
|
def main(args):
|
|
rc = RemoteController()
|
|
rc.run()
|
|
|
|
|
|
if __name__ == '__main__':
|
|
main(sys.argv)
|
|
|
|
# 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
|
|
# next, launch this program (mpc_test.py)
|
|
# you can now direct the robots via the GUI (assuming four corner markers for position reference have been detected)
|
|
# 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 document and improve program structure |