117 lines
3.8 KiB
Python
117 lines
3.8 KiB
Python
|
from flask import Flask, render_template, request
|
||
|
|
|
||
|
|
import socket
|
||
|
|
import threading
|
||
|
|
import time
|
||
|
|
import numpy as np
|
||
|
|
|
||
|
|
from robot import Robot
|
||
|
|
|
||
|
|
from mpc_controller import MPCController
|
||
|
|
from aruco_estimator import ArucoEstimator
|
||
|
|
|
||
|
|
from shapely.geometry import LineString
|
||
|
|
|
||
|
|
width = 600
|
||
|
|
height = 400
|
||
|
|
|
||
|
|
robot_ids = [11, 12, 13, 14]
|
||
|
|
robot_ids_control_history = [11, 12, 13, 14]
|
||
|
|
|
||
|
|
app = Flask(__name__)
|
||
|
|
|
||
|
|
def calc_target_pos(px, py):
|
||
|
|
if rc.estimator.all_corners_detected():
|
||
|
|
# compute column line
|
||
|
|
a = np.array([rc.estimator.corner_estimates['a']['x'], rc.estimator.corner_estimates['a']['y']])
|
||
|
|
b = np.array([rc.estimator.corner_estimates['b']['x'], rc.estimator.corner_estimates['b']['y']])
|
||
|
|
c = np.array([rc.estimator.corner_estimates['c']['x'], rc.estimator.corner_estimates['c']['y']])
|
||
|
|
d = np.array([rc.estimator.corner_estimates['d']['x'], rc.estimator.corner_estimates['d']['y']])
|
||
|
|
x_frac = (px + 0.5) / width
|
||
|
|
y_frac = (py + 0.5) / height
|
||
|
|
|
||
|
|
vab = b - a
|
||
|
|
vdc = c - d
|
||
|
|
column_line_top = a + x_frac * vab
|
||
|
|
column_line_bottom = d + x_frac * vdc
|
||
|
|
|
||
|
|
vad = d - a
|
||
|
|
vbc = c - b
|
||
|
|
row_line_top = a + y_frac * vad
|
||
|
|
row_line_bottom = b + y_frac * vbc
|
||
|
|
|
||
|
|
column_line = LineString([column_line_top, column_line_bottom])
|
||
|
|
row_line = LineString([row_line_top, row_line_bottom])
|
||
|
|
|
||
|
|
int_pt = column_line.intersection(row_line)
|
||
|
|
point_of_intersection = np.array([int_pt.x, int_pt.y, 0.0])
|
||
|
|
|
||
|
|
return point_of_intersection
|
||
|
|
|
||
|
|
|
||
|
|
@app.route('/', methods=('GET', 'POST'))
|
||
|
|
def presence():
|
||
|
|
global robot_ids_control_history
|
||
|
|
if request.method == 'GET':
|
||
|
|
return render_template('telepresence.html', oldest_control=robot_ids.index(robot_ids_control_history[0]))
|
||
|
|
elif request.method == 'POST':
|
||
|
|
x = int(float(request.form.get('x')))
|
||
|
|
y = int(float(request.form.get('y')))
|
||
|
|
print(x, y)
|
||
|
|
margin = 0.1
|
||
|
|
x = min(max(width * margin, x), width * (1 - margin))
|
||
|
|
y = min(max(height * margin, y), height * (1 - margin))
|
||
|
|
|
||
|
|
id = int(request.form.get('robot'))
|
||
|
|
robot_ids_control_history.append(robot_ids_control_history.pop(robot_ids_control_history.index(id)))
|
||
|
|
|
||
|
|
target_pos = calc_target_pos(x, y)
|
||
|
|
rc.estimator.event_queue.put(('robot', id, target_pos))
|
||
|
|
|
||
|
|
return 'OK'
|
||
|
|
|
||
|
|
|
||
|
|
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(11, '10.10.11.88'), Robot(12, '10.10.11.91'),
|
||
|
|
Robot(13, '10.10.11.90'), Robot(14, '10.10.11.89')]
|
||
|
|
|
||
|
|
self.robot_ids = {}
|
||
|
|
for r in self.robots:
|
||
|
|
self.robot_ids[r.id] = r
|
||
|
|
|
||
|
|
# 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:
|
||
|
|
robot.connect()
|
||
|
|
|
||
|
|
self.comm_socket.bind(('', 1337))
|
||
|
|
self.comm_socket.listen(5)
|
||
|
|
|
||
|
|
self.t = time.time()
|
||
|
|
|
||
|
|
# start thread for marker position detection
|
||
|
|
self.estimator = ArucoEstimator(self.robot_ids.keys())
|
||
|
|
self.estimator_thread = threading.Thread(target=self.estimator.run_tracking, kwargs={'draw_markers':False})
|
||
|
|
self.estimator_thread.start()
|
||
|
|
|
||
|
|
self.controller = MPCController(self.estimator)
|
||
|
|
|
||
|
|
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)
|
||
|
|
|
||
|
|
|
||
|
|
if __name__ == "__main__":
|
||
|
|
rc = RemoteController()
|
||
|
|
rc_thread = threading.Thread(target=rc.run)
|
||
|
|
rc_thread.start()
|
||
|
|
|
||
|
|
app.run('0.0.0.0')
|