diff --git a/remote_control/casadi_opt.py b/remote_control/casadi_opt.py index cd335cb..13f69d5 100644 --- a/remote_control/casadi_opt.py +++ b/remote_control/casadi_opt.py @@ -138,7 +138,7 @@ class OpenLoopSolver: #return - def solve(self, x0, target, obstacles): + def solve(self, x0, target, obstacles, track): tstart = time.time() # alternative solution using multiple shooting (way faster!) self.opti = Opti() # Optimization problem @@ -250,8 +250,11 @@ class OpenLoopSolver: tstart = time.time() if self.use_warmstart and self.opti_x0 is not None: - self.opti.set_initial(self.opti.lam_g, self.opti_lam_g0) - self.opti.set_initial(self.opti.x, self.opti_x0) + try: + self.opti.set_initial(self.opti.lam_g, self.opti_lam_g0) + self.opti.set_initial(self.opti.x, self.opti_x0) + except RuntimeError: + print("could not set warmstart") sol = self.opti.solve() # actual solve tend = time.time() print("solving the problem took {} seconds".format(tend - tstart)) diff --git a/remote_control/position_controller.py b/remote_control/position_controller.py index 805fdd8..d866866 100644 --- a/remote_control/position_controller.py +++ b/remote_control/position_controller.py @@ -20,12 +20,16 @@ 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 + class Robot: def __init__(self, id, ip=None): self.pos = None @@ -44,6 +48,57 @@ class Obstacle: self.pos = None self.radius = radius +class Track: + def __init__(self): + # ids in clockwise direction + self.inner = OrderedDict() + self.inner[5] = None + self.inner[8] = None + self.inner[7] = None + self.inner[1] = None + + self.outer = OrderedDict() + self.outer[11] = None + self.outer[2] = None + self.outer[4] = None + self.outer[0] = 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 @@ -69,18 +124,20 @@ def f_ode(t, x, u): class RemoteController: def __init__(self): - self.robots = [Robot(3, '192.168.1.103')] + self.robots = [Robot(15, '192.168.1.103')] self.robot_ids = {} for r in self.robots: self.robot_ids[r.id] = r - obst = [Obstacle(6, 0.175), Obstacle(5, 0.175), Obstacle(8, 0.175)] + obst = [Obstacle(1, 0.175), Obstacle(5, 0.175), Obstacle(7, 0.175)] self.obstacles = {} for r in obst: self.obstacles[r.id] = r + self.track = Track() + # connect to robot self.rc_socket = socket.socket() #self.rc_socket = None @@ -131,6 +188,9 @@ class RemoteController: self.dirm, = self.ax.plot([], []) self.dirs, = self.ax.plot([], []) + self.track_line_inner, = self.ax.plot([], []) + self.track_line_outer, = self.ax.plot([], []) + self.circles = [] for o in self.obstacles: self.circles.append(patch.Circle((0.0, 0.0), radius=0.1, fill=False, color='red', linestyle='--')) @@ -154,6 +214,7 @@ class RemoteController: 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) @@ -163,7 +224,11 @@ class RemoteController: self.ax.set_ylim(-2, 2) self.ax.set_aspect('equal', adjustable='box') - return self.line, self.line_sim, self.dirm, self.dirs, self.line_ol, self.circles[0], self.circles[1],self.circles[2], + 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.circles[0], self.circles[1],self.circles[2], def ani_update(self, frame): #print("plotting") @@ -224,9 +289,11 @@ class RemoteController: finally: self.mutex.release() - return self.line, self.line_sim, self.dirm, self.dirs, self.line_ol, self.circles[0], self.circles[1],self.circles[2], + return self.line, self.line_sim, self.dirm, self.dirs, self.line_ol, self.track_line_inner, self.track_line_outer, self.circles[0], self.circles[1],self.circles[2], def measurement_callback(self, data): + #print(data) + # detect robots if data.id in self.robot_ids: r = self.robot_ids[data.id] @@ -259,6 +326,10 @@ class RemoteController: obst = (data.x, data.y) self.obstacles[data.id].pos = obst + # 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") while True: @@ -291,7 +362,7 @@ class RemoteController: tmpc_start = time.time() # solve mpc open loop problem - res = self.ols.solve(x_pred, self.target, self.obstacles) + res = self.ols.solve(x_pred, self.target, self.obstacles, self.track) us1 = res[0] us2 = res[1] @@ -349,6 +420,10 @@ def main(args): 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() threading.Thread(target=rc.controller).start()