RoboRally/remote_control/mpc_test.py

114 lines
4.3 KiB
Python
Raw Normal View History

import sys
import socket
import threading
import time
import json
2021-08-25 20:53:09 +00:00
from robot import Robot, ControlledRobot
from mpc_controller import MPCController
class RemoteController:
def __init__(self):
2020-10-24 17:59:56 +00:00
self.robots = []
2021-08-25 20:53:09 +00:00
#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):
2021-08-25 20:53:09 +00:00
#print("waiting until all markers are detected...")
#while not (self.estimator.all_corners_detected() and self.estimator.all_robots_detected()):
# pass
print("starting control")
2021-08-25 20:53:09 +00:00
#self.controller.interactive_control(robots=self.robots)
for c in self.controllers:
c.start()
2021-08-25 20:53:09 +00:00
pass
2020-10-24 19:12:03 +00:00
def main(args):
rc = RemoteController()
rc.run()
if __name__ == '__main__':
main(sys.argv)
2021-08-25 20:53:09 +00:00
# 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
2021-08-25 20:53:09 +00:00
# TODO document and improve program structure