added plot for race track

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)
+            try:
-            self.opti.set_initial(self.opti.x, self.opti_x0)
+                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))

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()