# 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 class Robot: def __init__(self, id, ip): self.pos = None self.orient = None self.id = id self.pos = None self.euler = None self.ip = ip class Obstacle: def __init__(self, id, radius): self.id = id self.pos = None self.radius = radius class Track: def __init__(self): # ids in clockwise direction self.inner = OrderedDict() self.inner[0] = None self.inner[1] = None self.inner[2] = None self.inner[3] = None self.outer = OrderedDict() self.outer[4] = None self.outer[5] = None self.outer[6] = None self.outer[7] = None 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!") 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(14, '192.168.1.103')] #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.track = Track() # connect to robot self.rc_socket = socket.socket() #self.rc_socket = None try: for r in self.robots: self.rc_socket.connect((r.ip, 1234)) # connect to robot except socket.error: print("could not connect to socket") sys.exit(1) 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 # variable for mpc open loop self.ol_x = None self.ol_y = None 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 self.controlling = False # currently active control self.u1 = 0.0 self.u2 = 0.0 # animation self.fig = plt.figure() 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) self.xdata, self.ydata = [], [] self.line, = self.ax.plot([],[], color='grey', linestyle=':') self.line_sim, = self.ax.plot([], []) self.line_ol, = self.ax.plot([],[], color='green', linestyle='--') self.dirm, = self.ax.plot([], []) self.dirs, = self.ax.plot([], []) self.line_x, = self.ax2.plot([],[]) self.line_y, = self.ax3.plot([], []) self.track_line_inner, = self.ax.plot([], []) self.track_line_outer, = self.ax.plot([], []) 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() self.mstep = 2 self.ols = OpenLoopSolver(N=20, T=4.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.omega_max = 13.32 def ani(self): 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) def ani_init(self): self.ax.set_xlim(-2, 2) self.ax.set_ylim(-2, 2) self.ax.set_aspect('equal', adjustable='box') self.ax2.set_ylim(-2, 2) self.ax2.set_xlim(0, 10) self.ax3.set_ylim(-2, 2) self.ax3.set_xlim(0, 10) 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, def ani_update(self, frame): if self.anim_stopped: self.ani.event_source.stop() sys.exit(0) #print("plotting") self.mutex.acquire() try: # 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 self.line.set_data(xm_local[:,0], xm_local[:,1]) # compute and plot direction the robot is facing a = xm_local[-1, 0] b = xm_local[-1, 1] a2 = a + np.cos(xm_local[-1, 2]) * 0.2 b2 = b + np.sin(xm_local[-1, 2]) * 0.2 self.dirm.set_data(np.array([a, a2]), np.array([b, b2])) 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]) ts_local = deepcopy(self.ts) xs_local = deepcopy(self.xs) if len(ts_local) > 0: # plot simulated path of the robot 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] 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])) 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([],[]) finally: self.mutex.release() 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, 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 self.tms_0 is None: self.tms_0 = time.time() self.xm_0 = measurement self.mutex.acquire() try: self.tms = np.array([0.0]) self.xms = measurement finally: self.mutex.release() 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() # 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") targets = {} markers_in = self.track.inner.values() markers_out = self.track.outer.values() # find targets: # generate waypoints lamb = 0.5 j = 0 for i in range(0,4): p = np.array(markers_in[i]) + lamb * (np.array(markers_out[i]) - np.array(markers_in[i])) targets[j] = (p[0],p[1], 0.0) j += 1 if i < 3: mean_in = (np.array(markers_in[i]) + np.array(markers_in[i+1])) * 0.5 mean_out = (np.array(markers_out[i]) + np.array(markers_out[i+1])) * 0.5 mean = mean_in + (1.0 - lamb) * (mean_out - mean_in) targets[j] = (mean[0], mean[1], 0.0) j += 1 # final connection between first and last marker mean_in = (np.array(markers_in[3]) + np.array(markers_in[0])) * 0.5 mean_out = (np.array(markers_out[3]) + np.array(markers_out[0])) * 0.5 mean = mean_in + (1.0 - lamb) * (mean_out - mean_in) targets[j] = (mean[0], mean[1], 0.0) print("targets = {}".format(targets.keys())) auto_control = False current_target = 0 control_scaling = 0.3 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: #self.target = (0.5,0.5, -np.pi/2.0) self.target = targets[0] elif event.key == pygame.K_2: #self.target = (0.5, -0.5, 0.0) self.target = targets[1] elif event.key == pygame.K_3: #self.target = (-0.5,-0.5, np.pi/2.0) self.target = targets[2] elif event.key == pygame.K_4: #self.target = (-0.5,0.5, 0.0) self.target = targets[3] elif event.key == pygame.K_5: self.target = targets[4] elif event.key == pygame.K_6: self.target = targets[5] elif event.key == pygame.K_7: self.target = targets[6] elif event.key == pygame.K_8: self.target = targets[7] elif event.key == pygame.K_SPACE: auto_control = not auto_control if auto_control: self.target = targets[current_target] elif event.key == pygame.K_PLUS: control_scaling += 0.1 control_scaling = min(control_scaling, 1.0) elif event.key == pygame.K_MINUS: control_scaling -= 0.1 control_scaling = max(control_scaling, 0.3) elif event.key == pygame.K_ESCAPE: print("quit!") self.controlling = False if self.rc_socket: self.rc_socket.send('(0.0,0.0)\n') self.anim_stopped = True return elif event.key == pygame.QUIT: print("quit!") self.controlling = False if self.rc_socket: self.rc_socket.send('(0.0,0.0)\n') self.anim_stopped = True return if self.controlling: x_pred = self.get_measurement_prediction() if auto_control: if np.linalg.norm(x_pred[0:2]-np.array(self.target[0:2])) < 0.3: print("close to target!") current_target = (current_target + 1) % 8 self.target = targets[current_target] print("new target = {}".format(current_target)) tmpc_start = time.time() # solve mpc open loop problem res = self.ols.solve(x_pred, self.target, [], self.track) #us1 = res[0] #us2 = res[1] us1 = res[0] * control_scaling us2 = res[1] * 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) # 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() # 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) return 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 #threading.Thread(target=rc.input_handling).start() controller_thread = threading.Thread(target=rc.controller) controller_thread.start() rc.ani() if __name__ == '__main__': main(sys.argv)