forked from Telos4/RoboRally
492 lines
18 KiB
Python
492 lines
18 KiB
Python
# startup:
|
|
# roscore -> start ros
|
|
# rosparam set cv_camera/device_id 0 -> set appropriate camera device
|
|
# rosrun cv_camera cv_camera_node -> start the camera
|
|
# roslaunch aruco_detect aruco_detect.launch camera:=cv_camera image:=image_raw dictionary:=16 transport:= fiducial_len:=0.1 # aruco marker detection
|
|
# python fiducial_to_2d_pos_angle.py -> compute position and angle of markers in 2d plane
|
|
|
|
import sys
|
|
import rospy
|
|
import pygame
|
|
import numpy as np
|
|
import socket
|
|
import scipy.integrate
|
|
import copy
|
|
|
|
import threading
|
|
from copy import deepcopy
|
|
|
|
import matplotlib.pyplot as plt
|
|
import matplotlib.animation as anim
|
|
import matplotlib.patches as patch
|
|
|
|
from shapely.geometry import Polygon
|
|
|
|
import time
|
|
|
|
from casadi_opt import OpenLoopSolver
|
|
|
|
#from marker_pos_angle.msg import id_pos_angle
|
|
|
|
from collections import OrderedDict
|
|
|
|
from argparse import ArgumentParser
|
|
|
|
import opencv_viewer_example
|
|
|
|
MSGLEN = 64
|
|
def myreceive(sock):
|
|
chunks = []
|
|
bytes_recd = 0
|
|
while bytes_recd < MSGLEN:
|
|
chunk = sock.recv(1)
|
|
if chunk == b'':
|
|
raise RuntimeError("socket connection broken")
|
|
chunks.append(chunk)
|
|
bytes_recd = bytes_recd + len(chunk)
|
|
|
|
if chunks[-1] == b'\n':
|
|
break
|
|
|
|
return b''.join(chunks)
|
|
|
|
|
|
class Robot:
|
|
def __init__(self, id, ip, grid_pos = (0,0,0)):
|
|
self.pos = None
|
|
self.orient = None
|
|
self.grid_pos = grid_pos
|
|
|
|
self.id = id
|
|
|
|
self.pos = None
|
|
self.euler = None
|
|
|
|
self.ip = ip
|
|
self.socket = socket.socket()
|
|
|
|
# variables for measurements
|
|
self.tms_0 = None
|
|
self.xm_0 = None
|
|
self.tms = None
|
|
self.xms = None
|
|
|
|
# currently active control
|
|
self.u1 = 0.0
|
|
self.u2 = 0.0
|
|
|
|
|
|
def connect(self):
|
|
# connect to robot
|
|
try:
|
|
print("connecting to robot {} with ip {} ...".format(self.id, self.ip))
|
|
self.socket.connect((self.ip, 1234)) # connect to robot
|
|
print("connected!")
|
|
except socket.error:
|
|
print("could not connect to robot {} with ip {}".format(self.id, self.ip))
|
|
sys.exit(1)
|
|
|
|
def f_ode(t, x, u):
|
|
# dynamical model of the two-wheeled robot
|
|
# TODO: find exact values for these parameters
|
|
r = 0.03
|
|
R = 0.05
|
|
d = 0.02
|
|
|
|
theta = x[2]
|
|
|
|
omega_r = u[0]
|
|
omega_l = u[1]
|
|
|
|
dx = np.zeros(3)
|
|
|
|
dx[0] = (r/2.0 * np.cos(theta) - r*d/(2.0*R) * np.sin(theta)) * omega_r \
|
|
+ (r/2.0 * np.cos(theta) + r*d/(2.0 * R) * np.sin(theta)) * omega_l
|
|
dx[1] = (r/2.0 * np.sin(theta) + r*d/(2.0*R) * np.cos(theta)) * omega_r \
|
|
+ (r/2 * np.sin(theta) - r*d/(2.0*R) * np.cos(theta)) * omega_l
|
|
dx[2] = -r/(2.0*R) * (omega_r - omega_l)
|
|
|
|
return dx
|
|
|
|
class RemoteController:
|
|
def __init__(self, id, ip):
|
|
|
|
self.anim_stopped = False
|
|
|
|
# self.robots = #[Robot(11, '192.168.1.11', (6, -3, np.pi)), Robot(12, '192.168.1.12', (6, -3, np.pi)),
|
|
# Robot(13, '192.168.1.13', (6, -3, np.pi)), Robot(14, '192.168.1.14', (6, -2, np.pi))]
|
|
#self.robots = [Robot(13, '192.168.1.13', (6, -3, np.pi))]
|
|
#self.robots = []
|
|
#self.robots = [Robot(11, '192.168.1.11', (6,-3,0)), Robot(14, '192.168.1.14', (6,3,0))]
|
|
#self.robots = [Robot(11, '192.168.1.11'), Robot(14, '192.168.1.14')]
|
|
self.robots = [Robot(12, '192.168.1.12')]
|
|
#self.robots = [Robot(15, '192.168.1.102')]
|
|
#self.robots = [Robot(id, ip)]
|
|
|
|
self.robot_ids = {}
|
|
for r in self.robots:
|
|
self.robot_ids[r.id] = r
|
|
|
|
self.valid_cmds = ['forward', 'backward', 'turn left', 'turn right', 'turn around', 'get position', 'set position']
|
|
|
|
# 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((socket.gethostname(), 1337))
|
|
self.comm_socket.bind(('', 1337))
|
|
self.comm_socket.listen(5)
|
|
|
|
self.t = time.time()
|
|
|
|
# variables for simulated state
|
|
self.x0 = None
|
|
self.ts = np.array([])
|
|
self.xs = []
|
|
|
|
|
|
# variable for mpc open loop
|
|
self.ol_x = None
|
|
self.ol_y = None
|
|
|
|
self.mutex = threading.Lock()
|
|
|
|
# ROS subscriber for detected markers
|
|
self.estimator = opencv_viewer_example.ArucoEstimator()
|
|
|
|
self.estimator_thread = threading.Thread(target=self.estimator.run_tracking)
|
|
self.estimator_thread.start()
|
|
|
|
# pid parameters
|
|
self.controlling = False
|
|
|
|
self.mstep = 2
|
|
self.ols = OpenLoopSolver(N=20, T=1.0)
|
|
self.ols.setup()
|
|
self.dt = self.ols.T / self.ols.N
|
|
|
|
self.target = (0.0, 0.0, 0.0)
|
|
|
|
# integrator
|
|
self.r = scipy.integrate.ode(f_ode)
|
|
self.omega_max = 5.0
|
|
self.control_scaling = 0.1
|
|
#self.omega_max = 13.32
|
|
|
|
|
|
def measurement_callback(self, data):
|
|
#print(data)
|
|
|
|
# detect robots
|
|
if data.id in self.robot_ids:
|
|
r = self.robot_ids[data.id]
|
|
|
|
r.pos = (data.x, data.y) # only x and y component are important for us
|
|
r.euler = data.angle
|
|
|
|
# save measured position and angle for plotting
|
|
measurement = np.array([r.pos[0], r.pos[1], r.euler])
|
|
if r.tms_0 is None:
|
|
r.tms_0 = time.time()
|
|
r.xm_0 = measurement
|
|
|
|
self.mutex.acquire()
|
|
try:
|
|
r.tms = np.array([0.0])
|
|
r.xms = measurement
|
|
finally:
|
|
self.mutex.release()
|
|
else:
|
|
self.mutex.acquire()
|
|
try:
|
|
r.tms = np.vstack((r.tms, time.time() - r.tms_0))
|
|
r.xms = np.vstack((r.xms, measurement))
|
|
finally:
|
|
self.mutex.release()
|
|
|
|
def controller(self):
|
|
print("starting control")
|
|
|
|
running = True
|
|
while running:
|
|
(clientsocket, address) = self.comm_socket.accept()
|
|
clientsocket.settimeout(None)
|
|
|
|
connected = True
|
|
while connected:
|
|
try:
|
|
data = myreceive(clientsocket)
|
|
print("data received: ", data)
|
|
|
|
inputs = data.split(b',')
|
|
cmd = inputs[0]
|
|
cmd = cmd.strip().decode()
|
|
|
|
if len(inputs) > 1:
|
|
if cmd in self.valid_cmds:
|
|
try:
|
|
robot_id = int(inputs[1])
|
|
except ValueError:
|
|
print("could not read robot id!")
|
|
clientsocket.sendall(b'Could not read robot id!\n')
|
|
|
|
if robot_id in self.robot_ids:
|
|
if cmd == b'get position':
|
|
clientsocket.sendall(bytes('{}\n'.format(self.robot_ids[robot_id].grid_pos)))
|
|
elif cmd == b'set position':
|
|
try:
|
|
pos_data = ",".join(inputs[2:])
|
|
new_grid_pos = tuple(map(lambda x: int(x[1]) if x[0] < 2 else float(x[1]), enumerate(pos_data.strip().strip('()').split(','))))
|
|
self.robot_ids[robot_id].grid_pos = new_grid_pos
|
|
clientsocket.sendall(b'OK\n')
|
|
except IndexError as e:
|
|
print("could not set grid position!")
|
|
clientsocket.sendall(bytes(
|
|
'could not set grid position! (invalid format)\n'.format(
|
|
self.robot_ids[robot_id].grid_pos)))
|
|
except ValueError as e:
|
|
print("could not set grid position!")
|
|
clientsocket.sendall(bytes('could not set grid position! (invalid format)\n'.format(self.robot_ids[robot_id].grid_pos)))
|
|
else:
|
|
self.mpc_control(robot_id, cmd)
|
|
clientsocket.sendall(b'OK\n')
|
|
else:
|
|
print("invalid robot id!")
|
|
clientsocket.sendall(b'Invalid robot id!\n')
|
|
|
|
else:
|
|
clientsocket.sendall(b'Invalid command!\n')
|
|
else: # len(inputs) <= 1
|
|
if b'quit' in inputs[0]:
|
|
clientsocket.close()
|
|
self.comm_socket.close()
|
|
connected = False
|
|
running = False
|
|
else:
|
|
print("could not process command!")
|
|
clientsocket.sendall(b'Could not process request!\n')
|
|
|
|
except RuntimeError:
|
|
print("disconnected")
|
|
connected = False
|
|
clientsocket.close()
|
|
|
|
def mpc_control(self, robot_id, cmd):
|
|
robot = self.robot_ids[robot_id] # get robot to be controlled
|
|
grid_pos = robot.grid_pos # grid position of the robot
|
|
|
|
print("old grid pos for robot {}: {}".format(robot_id, grid_pos))
|
|
|
|
# compute new grid position and orientation
|
|
if cmd == 'forward':
|
|
new_x = int(round(grid_pos[0] + 1 * np.cos(grid_pos[2])))
|
|
new_y = int(round(grid_pos[1] + 1 * np.sin(grid_pos[2])))
|
|
new_angle = grid_pos[2]
|
|
elif cmd == 'backward':
|
|
new_x = int(round(grid_pos[0] - 1 * np.cos(grid_pos[2])))
|
|
new_y = int(round(grid_pos[1] - 1 * np.sin(grid_pos[2])))
|
|
new_angle = grid_pos[2]
|
|
elif cmd == 'turn left':
|
|
new_x = grid_pos[0]
|
|
new_y = grid_pos[1]
|
|
new_angle = np.unwrap([0, grid_pos[2] + np.pi / 2])[1]
|
|
elif cmd == 'turn right':
|
|
new_x = grid_pos[0]
|
|
new_y = grid_pos[1]
|
|
new_angle = np.unwrap([0, grid_pos[2] - np.pi / 2])[1]
|
|
elif cmd == 'turn around':
|
|
new_x = grid_pos[0]
|
|
new_y = grid_pos[1]
|
|
new_angle = np.unwrap([0, grid_pos[2] + np.pi])[1]
|
|
else:
|
|
print("unknown command!")
|
|
sys.exit(1)
|
|
if new_x != grid_pos[0] and new_y != grid_pos[1]:
|
|
print("problem detected!")
|
|
|
|
grid_pos = (new_x, new_y, new_angle)
|
|
print("new grid pos for robot {}: {}\n".format(robot_id, grid_pos))
|
|
|
|
target_pos = self.estimator.get_pos_from_grid_point(new_x, new_y)
|
|
self.target = np.array((target_pos[0], target_pos[1], grid_pos[2]))
|
|
#np.array((0.25 * grid_pos[0], 0.25 * grid_pos[1], grid_pos[2]))
|
|
|
|
self.pid = False
|
|
self.mpc = True
|
|
|
|
near_target = 0
|
|
|
|
while near_target < 5:
|
|
# open loop controller
|
|
events = pygame.event.get()
|
|
|
|
for event in events:
|
|
if event.type == pygame.KEYDOWN:
|
|
if event.key == pygame.K_UP:
|
|
self.controlling = True
|
|
self.t = time.time()
|
|
elif event.key == pygame.K_DOWN:
|
|
self.controlling = False
|
|
if self.robot_ids[robot_id].socket:
|
|
self.robot_ids[robot_id].socket.send('(0.0,0.0)\n')
|
|
elif event.key == pygame.K_0:
|
|
self.target = np.array([0,0,0])
|
|
elif event.key == pygame.K_PLUS:
|
|
self.control_scaling += 0.1
|
|
self.control_scaling = min(self.control_scaling, 1.0)
|
|
print("control scaling = ", self.control_scaling)
|
|
elif event.key == pygame.K_MINUS:
|
|
self.control_scaling -= 0.1
|
|
self.control_scaling = max(self.control_scaling, 0.1)
|
|
print("control scaling = ", self.control_scaling)
|
|
elif event.key == pygame.K_ESCAPE:
|
|
print("quit!")
|
|
self.controlling = False
|
|
if self.robot_ids[robot_id].socket:
|
|
self.robot_ids[robot_id].socket.send('(0.0,0.0)\n')
|
|
self.anim_stopped = True
|
|
return
|
|
elif event.key == pygame.QUIT:
|
|
print("quit!")
|
|
self.controlling = False
|
|
if self.robot_ids[robot_id].socket:
|
|
self.robot_ids[robot_id].socket.send('(0.0,0.0)\n')
|
|
self.anim_stopped = True
|
|
return
|
|
|
|
if self.mpc:
|
|
#x_pred = self.get_measurement_prediction(robot_id)
|
|
x_pred = self.get_measurement()
|
|
|
|
tmpc_start = time.time()
|
|
|
|
error_pos = np.linalg.norm(x_pred[0:2] - self.target[0:2])
|
|
angles_unwrapped = np.unwrap([x_pred[2], self.target[2]]) # unwrap angle to avoid jump in data
|
|
error_ang = np.abs(angles_unwrapped[0] - angles_unwrapped[1])
|
|
#print("error pos = ", error_pos)
|
|
#print("error_pos = {}, error_ang = {}".format(error_pos, error_ang))
|
|
|
|
#if error_pos > 0.075 or error_ang > 0.35:
|
|
if error_pos > 0.05 or error_ang > 0.2:
|
|
# solve mpc open loop problem
|
|
res = self.ols.solve(x_pred, self.target)
|
|
|
|
#us1 = res[0]
|
|
#us2 = res[1]
|
|
us1 = res[0] * self.control_scaling
|
|
us2 = res[1] * self.control_scaling
|
|
#print("u = {}", (us1, us2))
|
|
|
|
# save open loop trajectories for plotting
|
|
self.mutex.acquire()
|
|
try:
|
|
self.ol_x = res[2]
|
|
self.ol_y = res[3]
|
|
finally:
|
|
self.mutex.release()
|
|
|
|
tmpc_end = time.time()
|
|
#print("---------------- mpc solution took {} seconds".format(tmpc_end - tmpc_start))
|
|
dt_mpc = time.time() - self.t
|
|
if dt_mpc < self.dt: # wait until next control can be applied
|
|
#print("sleeping for {} seconds...".format(self.dt - dt_mpc))
|
|
time.sleep(self.dt - dt_mpc)
|
|
else:
|
|
us1 = [0] * self.mstep
|
|
us2 = [0] * self.mstep
|
|
|
|
near_target += 1
|
|
robot.grid_pos = grid_pos
|
|
|
|
# send controls to the robot
|
|
for i in range(0, self.mstep): # option to use multistep mpc if len(range) > 1
|
|
u1 = us1[i]
|
|
u2 = us2[i]
|
|
if self.robot_ids[robot_id].socket:
|
|
self.robot_ids[robot_id].socket.send('({},{})\n'.format(u1, u2).encode())
|
|
if i < self.mstep:
|
|
time.sleep(self.dt)
|
|
self.t = time.time() # save time the most recent control was applied
|
|
|
|
|
|
def get_measurement_prediction(self, robot_id):
|
|
# get measurement
|
|
self.mutex.acquire()
|
|
try:
|
|
window = 3
|
|
last_measurement = copy.deepcopy(self.robot_ids[robot_id].xms[-window:])
|
|
#print("last_measurements = {}".format(last_measurement))
|
|
#print("mean = {}".format(np.mean(last_measurement, axis=0)))
|
|
last_measurement = np.mean(last_measurement, axis=0)
|
|
last_time = copy.deepcopy(self.robot_ids[robot_id].tms[-1])
|
|
finally:
|
|
self.mutex.release()
|
|
|
|
# prediction of state at time the mpc will terminate
|
|
self.r.set_f_params(np.array([self.robot_ids[robot_id].u1 * self.omega_max, self.robot_ids[robot_id].u2 * self.omega_max]))
|
|
|
|
self.r.set_initial_value(last_measurement, last_time)
|
|
|
|
x_pred = self.r.integrate(self.r.t + self.dt)
|
|
|
|
return x_pred
|
|
|
|
def get_measurement_old(self):
|
|
self.mutex.acquire()
|
|
try:
|
|
last_measurement = copy.deepcopy(self.xms[-1:])
|
|
finally:
|
|
self.mutex.release()
|
|
return last_measurement[0]
|
|
|
|
def get_measurement(self):
|
|
return np.array(self.estimator.get_robot_state_estimate(12))
|
|
|
|
def pos_getter(self):
|
|
while True:
|
|
x_pred = self.get_measurement_prediction()
|
|
|
|
print("pos = ", x_pred)
|
|
|
|
def main(args):
|
|
parser = ArgumentParser()
|
|
parser.add_argument('id', metavar='id', type=str, help='marker id of the controlled robot')
|
|
parser.add_argument('ip', metavar='ip', type=str, help='ip address of the controlled robot')
|
|
args = parser.parse_args()
|
|
|
|
marker_id = int(args.id)
|
|
ip = args.ip
|
|
|
|
|
|
#rospy.init_node('controller_node', anonymous=True)
|
|
|
|
rc = RemoteController(marker_id, ip)
|
|
|
|
pygame.init()
|
|
|
|
screenheight = 480
|
|
screenwidth = 640
|
|
pygame.display.set_mode([screenwidth, screenheight])
|
|
|
|
# print("waiting until track is completely detected")
|
|
# while not rc.track.track_complete:
|
|
# pass
|
|
|
|
|
|
|
|
time.sleep(1)
|
|
|
|
#threading.Thread(target=rc.input_handling).start()
|
|
controller_thread = threading.Thread(target=rc.controller)
|
|
controller_thread.start()
|
|
|
|
#time.sleep(10)
|
|
#rc.ani()
|
|
|
|
|
|
if __name__ == '__main__':
|
|
main(sys.argv)
|