# 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 time from casadi_opt import OpenLoopSolver from marker_pos_angle.msg import id_pos_angle class Robot: def __init__(self, id, ip=None): self.pos = None self.orient = None self.id = id self.pos = None self.euler = None self.ip = ip 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): self.robots = [Robot(5)] self.robot_ids = {} for r in self.robots: self.robot_ids[r.id] = r # connect to robot self.rc_socket = socket.socket() try: pass self.rc_socket.connect(('192.168.1.103', 1234)) # connect to robot except socket.error: print("could not connect to socket") 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 self.mutex = threading.Lock() marker_sub = rospy.Subscriber("/marker_id_pos_angle", id_pos_angle, self.measurement_callback) # pid parameters self.k = 0 self.ii = 0.1 self.pp = 0.4 self.inc = 0.0 self.alphas = [] self.speed = 1.0 self.controlling = False self.u1 = 0.0 self.u2 = 0.0 # animation self.fig = plt.figure() self.ax = self.fig.add_subplot(1,1,1) self.xdata, self.ydata = [], [] self.line, = self.ax.plot([],[]) self.line_sim, = self.ax.plot([], []) self.dirm, = self.ax.plot([], []) self.dirs, = self.ax.plot([], []) plt.xlabel('x-position') plt.ylabel('y-position') plt.grid() self.ols = OpenLoopSolver() self.ols.setup() self.target = (0.0, 0.0) def ani(self): 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') return self.line, self.line_sim, self.dirm, self.dirs, def ani_update(self, frame): #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]) * 1.0 b2 = b + np.sin(xm_local[-1, 2]) * 1.0 self.dirm.set_data(np.array([a, a2]), np.array([b, b2])) 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]) * 1.0 b2 = b + np.sin(xs_local[-1, 2]) * 1.0 self.dirs.set_data(np.array([a, a2]), np.array([b, b2])) finally: self.mutex.release() return self.line, self.line_sim, self.dirm, self.dirs, def measurement_callback(self, data): #print("data = {}".format(data)) 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 #print("r.pos = {}".format(r.pos)) #print("r.angle = {}".format(r.euler)) # 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() def controller(self): tgrid = None us1 = None us2 = None print("starting control") while True: keyboard_control = False keyboard_control_speed_test = False pid = False open_loop_solve = True if keyboard_control: # keyboard controller events = pygame.event.get() speed = 0.5 for event in events: if event.type == pygame.KEYDOWN: if event.key == pygame.K_LEFT: self.u1 = -speed self.u2 = speed #print("turn left: ({},{})".format(u1, u2)) elif event.key == pygame.K_RIGHT: self.u1 = speed self.u2 = -speed #print("turn right: ({},{})".format(u1, u2)) elif event.key == pygame.K_UP: self.u1 = speed self.u2 = speed #print("forward: ({},{})".format(self.u1, self.u2)) elif event.key == pygame.K_DOWN: self.u1 = -speed self.u2 = -speed #print("forward: ({},{})".format(u1, u2)) self.rc_socket.send('({},{})\n'.format(self.u1, self.u2)) elif event.type == pygame.KEYUP: self.u1 = 0 self.u2 = 0 #print("key released, resetting: ({},{})".format(u1, u2)) self.rc_socket.send('({},{})\n'.format(self.u1, self.u2)) tnew = time.time() dt = tnew - self.t r = scipy.integrate.ode(f_ode) r.set_f_params(np.array([self.u1 * 13.32, self.u2 * 13.32])) #print(self.x0) if self.x0 is None: if self.xm_0 is not None: self.x0 = self.xm_0 self.xs = self.x0 else: print("error: no measurement available to initialize simulation") x = self.x0 r.set_initial_value(x, self.t) xnew = r.integrate(r.t + dt) self.t = tnew self.x0 = xnew self.mutex.acquire() try: self.ts = np.append(self.ts, tnew) self.xs = np.vstack((self.xs, xnew)) finally: self.mutex.release() elif keyboard_control_speed_test: events = pygame.event.get() for event in events: if event.type == pygame.KEYDOWN: if event.key == pygame.K_LEFT: self.speed = self.speed / np.sqrt(np.sqrt(np.sqrt(10.0))) elif event.key == pygame.K_RIGHT: self.speed = self.speed * np.sqrt(np.sqrt(np.sqrt(10.0))) elif event.key == pygame.K_UP: u1 = self.speed u2 = -self.speed elif event.key == pygame.K_DOWN: u1 = 0.0 u2 = 0.0 print("speed = {}".format(self.speed)) self.rc_socket.send('({},{})\n'.format(u1, u2)) elif pid: # pid controller events = pygame.event.get() for event in events: if event.type == pygame.KEYDOWN: if event.key == pygame.K_LEFT: self.ii = self.ii / np.sqrt(np.sqrt(np.sqrt(10.0))) print("ii = {}".format(self.pp)) elif event.key == pygame.K_RIGHT: self.ii = self.ii * np.sqrt(np.sqrt(np.sqrt(10.0))) print("ii = {}".format(self.pp)) elif event.key == pygame.K_UP: self.controlling = True elif event.key == pygame.K_DOWN: self.controlling = False self.rc_socket.send('({},{})\n'.format(0, 0)) dt = 0.05 if self.controlling: # test: turn robot such that angle is zero for r in self.robots: if r.euler is not None: self.k = self.k + 1 alpha = r.euler self.alphas.append(alpha) # compute error e = alpha - 0 # compute integral of error (approximately) self.inc += e * dt # PID p = self.pp * e i = self.ii * self.inc d = 0.0 # compute controls for robot from PID u1 = p + i + d u2 = - p - i - d print("alpha = {}, u = ({}, {})".format(alpha, u1, u2)) self.rc_socket.send('({},{})\n'.format(u1, u2)) time.sleep(dt) elif open_loop_solve: # 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 elif event.key == pygame.K_DOWN: self.controlling = False self.rc_socket.send('(0.1, 0.0,0.0)\n') elif event.key == pygame.K_0: self.target = (0.0, 0.0) elif event.key == pygame.K_1: self.target = (0.5,0.5) elif event.key == pygame.K_2: self.target = (0.5, -0.5) elif event.key == pygame.K_3: self.target = (-0.5,-0.5) elif event.key == pygame.K_4: self.target = (-0.5,0.5) if self.controlling: tmpc_start = time.time() # get measurement last_measurement = copy.deepcopy(self.xms[-1]) res = self.ols.solve(last_measurement, self.target) #tgrid = res[0] us1 = res[0] us2 = res[1] # tt = 0 # x = last_measurement # t_ol = np.array([tt]) # x_ol = np.array([x]) # # compute open loop prediction # for i in range(len(us1)): # r = scipy.integrate.ode(f_ode) # r.set_f_params(np.array([us1[i] * 13.32, us2[i] * 13.32])) # r.set_initial_value(x, tt) # # tt = tt + 0.1 # x = r.integrate(tt) # # t_ol = np.vstack((t_ol, tt)) # x_ol = np.vstack((x_ol, x)) #plt.figure(4) #plt.plot(x_ol[:,0], x_ol[:,1]) #if event.key == pygame.K_DOWN: # if tgrid is not None: tmpc_end = time.time() print("---------------- mpc solution took {} seconds".format(tmpc_end-tmpc_start)) for i in range(0, 5): u1 = us1[i] u2 = us2[i] dt_mpc = time.time() - self.t #if dt_mpc < 0.1: # time.sleep(0.1 - dt_mpc) self.rc_socket.send('({},{},{})\n'.format(0.1,u1, u2)) self.t = time.time() time.sleep(0.1) # pass def main(args): rospy.init_node('controller_node', anonymous=True) rc = RemoteController() pygame.init() screenheight = 480 screenwidth = 640 screen = pygame.display.set_mode([screenwidth, screenheight]) threading.Thread(target=rc.controller).start() rc.ani() if __name__ == '__main__': main(sys.argv)