RoboRally/remote_control/position_controller.py

501 lines
16 KiB
Python
Raw Normal View History

2019-05-06 20:23:31 +00:00
# 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
2019-05-06 20:23:31 +00:00
import sys
import rospy
import pygame
import numpy as np
import socket
2019-05-24 14:21:54 +00:00
import scipy.integrate
2019-06-13 11:18:46 +00:00
import copy
2019-05-24 14:21:54 +00:00
import threading
from copy import deepcopy
import matplotlib.pyplot as plt
import matplotlib.animation as anim
2019-06-27 10:12:42 +00:00
import matplotlib.patches as patch
2019-05-24 14:21:54 +00:00
2019-07-04 08:54:50 +00:00
from shapely.geometry import Polygon
2019-05-24 14:21:54 +00:00
import time
2019-05-06 20:23:31 +00:00
from casadi_opt import OpenLoopSolver
from marker_pos_angle.msg import id_pos_angle
2019-05-24 14:21:54 +00:00
2019-07-04 08:54:50 +00:00
from collections import OrderedDict
2019-05-06 20:23:31 +00:00
class Robot:
def __init__(self, id, ip=None):
self.pos = None
2019-05-24 14:21:54 +00:00
self.orient = None
2019-05-06 20:23:31 +00:00
self.id = id
2019-05-24 14:21:54 +00:00
self.pos = None
self.euler = None
self.ip = ip
2019-06-27 10:12:42 +00:00
class Obstacle:
def __init__(self, id, radius):
self.id = id
self.pos = None
self.radius = radius
2019-07-04 08:54:50 +00:00
class Track:
def __init__(self):
# ids in clockwise direction
self.inner = OrderedDict()
self.inner[1] = None
2019-07-09 17:42:19 +00:00
self.inner[2] = None
self.inner[3] = None
self.inner[4] = None
2019-07-04 08:54:50 +00:00
self.outer = OrderedDict()
2019-07-09 17:42:19 +00:00
self.outer[5] = None
self.outer[6] = None
self.outer[7] = None
self.outer[8] = None
2019-07-04 08:54:50 +00:00
self.track_complete = False
self.inner_poly = None
self.outer_poly = None
def set_id(self, d):
if not self.track_complete:
if d.id in self.inner:
print("Detected marker {} at pos {}".format(d.id, (d.x,d.y)))
self.inner[d.id] = (d.x, d.y)
elif d.id in self.outer:
print("Detected marker {} at pos {}".format(d.id, (d.x, d.y)))
self.outer[d.id] = (d.x, d.y)
else:
print("Unknown marker!")
else:
return
if not None in self.inner.values() and not None in self.outer.values():
print("Track marker positions detected!")
self.track_complete = True
self.inner_poly = Polygon(self.inner.values())
self.outer_poly = Polygon(self.outer.values())
def plot_track(self):
if self.track_complete:
plt.figure(2)
x_in, y_in = self.inner_poly.exterior.xy
x_out, y_out = self.outer_poly.exterior.xy
plt.plot(x_in, y_in)
plt.plot(x_out, y_out)
else:
print("plot is not complete yet!")
2019-05-24 14:21:54 +00:00
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
2019-05-06 20:23:31 +00:00
class RemoteController:
def __init__(self):
2019-05-24 14:21:54 +00:00
self.robots = [Robot(15, '192.168.1.103')]
#self.robots = [Robot(14, '192.168.1.102')]
2019-05-06 20:23:31 +00:00
self.robot_ids = {}
for r in self.robots:
self.robot_ids[r.id] = r
2019-05-06 20:23:31 +00:00
obst = [Obstacle(12, 0.275), Obstacle(10, 0.275), Obstacle(13, 0.275), Obstacle(14, 0.275)]
2019-06-27 10:12:42 +00:00
self.obstacles = {}
for r in obst:
self.obstacles[r.id] = r
2019-07-04 08:54:50 +00:00
self.track = Track()
# connect to robot
2019-05-06 20:23:31 +00:00
self.rc_socket = socket.socket()
2019-06-27 10:12:42 +00:00
#self.rc_socket = None
2019-05-06 20:23:31 +00:00
try:
2019-06-27 10:12:42 +00:00
for r in self.robots:
self.rc_socket.connect((r.ip, 1234)) # connect to robot
2019-05-06 20:23:31 +00:00
except socket.error:
print("could not connect to socket")
2019-07-04 11:48:32 +00:00
sys.exit(1)
2019-06-27 10:12:42 +00:00
2019-05-06 20:23:31 +00:00
self.t = time.time()
# variables for simulated state
self.x0 = None
self.ts = np.array([])
self.xs = []
# variables for measurements
self.tms_0 = None
self.xm_0 = None
self.tms = None
self.xms = None
2019-06-27 10:12:42 +00:00
# variable for mpc open loop
self.ol_x = None
self.ol_y = None
2019-05-24 14:21:54 +00:00
self.mutex = threading.Lock()
# ROS subscriber for detected markers
marker_sub = rospy.Subscriber("/marker_id_pos_angle", id_pos_angle, self.measurement_callback)
# pid parameters
2019-05-24 14:21:54 +00:00
self.controlling = False
# currently active control
2019-05-24 14:21:54 +00:00
self.u1 = 0.0
self.u2 = 0.0
# animation
2019-05-24 14:21:54 +00:00
self.fig = plt.figure()
2019-07-04 11:48:32 +00:00
self.ax = self.fig.add_subplot(2,2,1)
self.ax2 = self.fig.add_subplot(2, 2, 2)
self.ax3 = self.fig.add_subplot(2, 2, 4)
2019-05-24 14:21:54 +00:00
self.xdata, self.ydata = [], []
2019-06-27 10:12:42 +00:00
self.line, = self.ax.plot([],[], color='grey', linestyle=':')
2019-05-24 14:21:54 +00:00
self.line_sim, = self.ax.plot([], [])
2019-06-27 10:12:42 +00:00
self.line_ol, = self.ax.plot([],[], color='green', linestyle='--')
self.dirm, = self.ax.plot([], [])
self.dirs, = self.ax.plot([], [])
2019-06-27 10:12:42 +00:00
2019-07-04 11:48:32 +00:00
self.line_x, = self.ax2.plot([],[])
self.line_y, = self.ax3.plot([], [])
2019-07-04 08:54:50 +00:00
self.track_line_inner, = self.ax.plot([], [])
self.track_line_outer, = self.ax.plot([], [])
2019-06-27 10:12:42 +00:00
self.circles = []
for o in self.obstacles:
self.circles.append(patch.Circle((0.0, 0.0), radius=0.1, fill=False, color='red', linestyle='--'))
for s in self.circles:
self.ax.add_artist(s)
2019-07-04 11:48:32 +00:00
self.ax.set_xlabel('x-position')
self.ax.set_ylabel('y-position')
self.ax.grid()
self.ax2.set_xlabel('Zeit t')
self.ax2.set_ylabel('x-position')
self.ax2.grid()
self.ax3.set_xlabel('Zeit t')
self.ax3.set_ylabel('y-position')
self.ax3.grid()
2019-05-24 14:21:54 +00:00
self.mstep = 2
self.ols = OpenLoopSolver(N=20, T=4.0)
2019-06-13 11:18:46 +00:00
self.ols.setup()
self.dt = self.ols.T / self.ols.N
2019-06-13 11:18:46 +00:00
self.target = (0.0, 0.0, 0.0)
# integrator
self.r = scipy.integrate.ode(f_ode)
self.omega_max = 5.0
#self.omega_max = 13.32
def ani(self):
2019-07-04 08:54:50 +00:00
print("starting animation")
self.ani = anim.FuncAnimation(self.fig, init_func=self.ani_init, func=self.ani_update, interval=10, blit=True)
plt.ion()
plt.show(block=True)
2019-05-24 14:21:54 +00:00
def ani_init(self):
self.ax.set_xlim(-2, 2)
self.ax.set_ylim(-2, 2)
self.ax.set_aspect('equal', adjustable='box')
2019-07-04 11:48:32 +00:00
self.ax2.set_ylim(-2, 2)
self.ax2.set_xlim(0, 10)
self.ax3.set_ylim(-2, 2)
self.ax3.set_xlim(0, 10)
2019-07-04 08:54:50 +00:00
self.track_line_inner.set_data(self.track.inner_poly.exterior.xy)
self.track_line_outer.set_data(self.track.outer_poly.exterior.xy)
return self.line, self.line_sim, self.dirm, self.dirs, self.line_ol,\
self.track_line_inner, self.track_line_outer, self.line_x,self.line_y, self.circles[0], self.circles[1],\
self.circles[2], self.circles[3],
2019-05-24 14:21:54 +00:00
def ani_update(self, frame):
#print("plotting")
2019-05-24 14:21:54 +00:00
self.mutex.acquire()
2019-05-06 20:23:31 +00:00
try:
2019-05-24 14:21:54 +00:00
# copy data for plot from global arrays
if self.tms is not None:
tm_local = deepcopy(self.tms)
xm_local = deepcopy(self.xms)
if len(tm_local) > 0:
# plot path of the robot
2019-05-24 14:21:54 +00:00
self.line.set_data(xm_local[:,0], xm_local[:,1])
# compute and plot direction the robot is facing
2019-05-24 14:21:54 +00:00
a = xm_local[-1, 0]
b = xm_local[-1, 1]
2019-05-24 14:21:54 +00:00
2019-06-27 10:12:42 +00:00
a2 = a + np.cos(xm_local[-1, 2]) * 0.2
b2 = b + np.sin(xm_local[-1, 2]) * 0.2
2019-05-24 14:21:54 +00:00
self.dirm.set_data(np.array([a, a2]), np.array([b, b2]))
2019-05-24 14:21:54 +00:00
2019-07-04 11:48:32 +00:00
n_plot = 300
if len(tm_local) > n_plot:
# plot x and y coordinate
self.line_x.set_data(tm_local[-n_plot:] - (tm_local[-1] - 10), xm_local[-n_plot:,0])
self.line_y.set_data(tm_local[-n_plot:] - (tm_local[-1] - 10), xm_local[-n_plot:, 1])
2019-05-24 14:21:54 +00:00
ts_local = deepcopy(self.ts)
xs_local = deepcopy(self.xs)
if len(ts_local) > 0:
# plot simulated path of the robot
2019-05-24 14:21:54 +00:00
self.line_sim.set_data(xs_local[:,0], xs_local[:,1])
# compute and plot direction the robot is facing
a = xs_local[-1, 0]
b = xs_local[-1, 1]
2019-06-27 10:12:42 +00:00
a2 = a + np.cos(xs_local[-1, 2]) * 0.2
b2 = b + np.sin(xs_local[-1, 2]) * 0.2
self.dirs.set_data(np.array([a, a2]), np.array([b, b2]))
2019-06-27 10:12:42 +00:00
ol_x_local = deepcopy(self.ol_x)
ol_y_local = deepcopy(self.ol_y)
if ol_x_local is not None:
self.line_ol.set_data(ol_x_local, ol_y_local)
else:
self.line_ol.set_data([],[])
i = 0
obst_keys = self.obstacles.keys()
for s in self.circles:
o = self.obstacles[obst_keys[i]]
i = i + 1
if o.pos is not None:
s.center = o.pos
s.radius = o.radius
2019-05-24 14:21:54 +00:00
finally:
self.mutex.release()
2019-07-04 11:48:32 +00:00
return self.line, self.line_sim, self.dirm, self.dirs, self.line_ol, self.track_line_inner, self.track_line_outer,\
self.line_x, self.line_y, self.circles[0], self.circles[1],self.circles[2],self.circles[3],
2019-05-24 14:21:54 +00:00
def measurement_callback(self, data):
2019-07-04 08:54:50 +00:00
#print(data)
# detect robots
if data.id in self.robot_ids:
r = self.robot_ids[data.id]
2019-05-06 20:23:31 +00:00
r.pos = (data.x, data.y) # only x and y component are important for us
r.euler = data.angle
2019-05-06 20:23:31 +00:00
# save measured position and angle for plotting
measurement = np.array([r.pos[0], r.pos[1], r.euler])
if self.tms_0 is None:
self.tms_0 = time.time()
self.xm_0 = measurement
2019-05-06 20:23:31 +00:00
self.mutex.acquire()
try:
self.tms = np.array([0.0])
self.xms = measurement
finally:
self.mutex.release()
2019-05-24 14:21:54 +00:00
else:
self.mutex.acquire()
try:
self.tms = np.vstack((self.tms, time.time() - self.tms_0))
self.xms = np.vstack((self.xms, measurement))
finally:
self.mutex.release()
2019-05-24 14:21:54 +00:00
# detect obstacles
2019-06-27 10:12:42 +00:00
if data.id in self.obstacles.keys():
obst = (data.x, data.y)
self.obstacles[data.id].pos = obst
2019-07-04 08:54:50 +00:00
# detect track
if data.id in self.track.inner.keys() or data.id in self.track.outer.keys():
self.track.set_id(data)
def controller(self):
print("starting control")
2019-07-04 11:48:32 +00:00
targets = {}
markers_in = self.track.inner.values()
markers_out = self.track.outer.values()
# find targets:
2019-07-09 17:42:19 +00:00
lamb = 0.3
2019-07-04 11:48:32 +00:00
for i in range(0,4):
2019-07-09 17:42:19 +00:00
p = np.array(markers_in[i]) + lamb * (np.array(markers_out[i]) - np.array(markers_in[i]))
2019-07-04 11:48:32 +00:00
targets[i] = (p[0],p[1], 0.0)
auto_control = False
current_target = 0
2019-05-06 20:23:31 +00:00
while True:
# 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.rc_socket:
self.rc_socket.send('(0.0,0.0)\n')
elif event.key == pygame.K_0:
self.target = (0.0, 0.0, 0.0)
elif event.key == pygame.K_1:
2019-07-04 11:48:32 +00:00
#self.target = (0.5,0.5, -np.pi/2.0)
self.target = targets[0]
elif event.key == pygame.K_2:
2019-07-04 11:48:32 +00:00
#self.target = (0.5, -0.5, 0.0)
self.target = targets[1]
elif event.key == pygame.K_3:
2019-07-04 11:48:32 +00:00
#self.target = (-0.5,-0.5, np.pi/2.0)
self.target = targets[2]
elif event.key == pygame.K_4:
2019-07-04 11:48:32 +00:00
#self.target = (-0.5,0.5, 0.0)
self.target = targets[3]
elif event.key == pygame.K_5:
auto_control = not auto_control
if auto_control:
self.target = targets[current_target]
if self.controlling:
2019-07-04 11:48:32 +00:00
x_pred = self.get_measurement_prediction()
2019-07-04 11:48:32 +00:00
if auto_control:
if np.linalg.norm(x_pred[0:2]-np.array(self.target[0:2])) < 0.3:
2019-07-04 11:48:32 +00:00
print("close to target!")
current_target = (current_target + 1) % 4
self.target = targets[current_target]
print("new target = {}".format(current_target))
tmpc_start = time.time()
# solve mpc open loop problem
2019-07-04 08:54:50 +00:00
res = self.ols.solve(x_pred, self.target, self.obstacles, self.track)
us1 = res[0]
us2 = res[1]
# save open loop trajectories for plotting
2019-05-24 14:21:54 +00:00
self.mutex.acquire()
try:
self.ol_x = res[2]
self.ol_y = res[3]
2019-05-24 14:21:54 +00:00
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)
# 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.rc_socket:
self.rc_socket.send('({},{})\n'.format(u1, u2))
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):
# get measurement
self.mutex.acquire()
try:
window = 3
last_measurement = copy.deepcopy(self.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.tms[-1])
finally:
self.mutex.release()
2019-05-24 14:21:54 +00:00
# prediction of state at time the mpc will terminate
self.r.set_f_params(np.array([self.u1 * self.omega_max, self.u2 * self.omega_max]))
self.r.set_initial_value(last_measurement, last_time)
x_pred = self.r.integrate(self.r.t + self.dt)
2019-06-13 11:18:46 +00:00
return x_pred
2019-06-13 11:18:46 +00:00
2019-05-06 20:23:31 +00:00
def main(args):
rospy.init_node('controller_node', anonymous=True)
rc = RemoteController()
pygame.init()
screenheight = 480
screenwidth = 640
2019-06-27 10:12:42 +00:00
pygame.display.set_mode([screenwidth, screenheight])
2019-07-04 08:54:50 +00:00
print("waiting until track is completely detected")
while not rc.track.track_complete:
pass
#threading.Thread(target=rc.input_handling).start()
threading.Thread(target=rc.controller).start()
rc.ani()
2019-05-06 20:23:31 +00:00
if __name__ == '__main__':
main(sys.argv)