Minor changes

- Add comments
- Fix path to map files

README.md

@@ -31,7 +31,7 @@ The rest of this readme is structured as follows. In [Section 2](#implementation
 The multi-purpose control framework presented in this repository consists of four main components. These components are intended to make the implementation as modular as possible, facilitating extensions to certain components without having to alter the overall structure of the framework. An illustration of the components and their interaction is displayed below.
 
 <p align="center">
-<img src="Images/MPC_Framework.png">
+  <img src="Images/MPC_Framework.svg" width="100%">
 </p>
 
 ### Map 

src/map.py

@@ -156,7 +156,7 @@ class Map:
 
 
 if __name__ == '__main__':
-    map = Map('real_map.png')
+    map = Map('maps/real_map.png')
-    # map = Map('sim_map.png')
+    # map = Map('maps/sim_map.png')
     plt.imshow(np.flipud(map.data), cmap='gray')
     plt.show()

src/reference_path.py

@@ -118,7 +118,7 @@ class ReferencePath:
         # Number of waypoints
         n_wp = [int(np.sqrt((wp_x[i + 1] - wp_x[i]) ** 2 +
                             (wp_y[i + 1] - wp_y[i]) ** 2) /
-            self.resolution) for i in range(len(wp_x) - 1)]
+                self.resolution) for i in range(len(wp_x) - 1)]
 
         # Construct waypoints with specified resolution
         gp_x, gp_y = wp_x[-1], wp_y[-1]
@@ -656,7 +656,7 @@ if __name__ == '__main__':
     if path == 'Sim_Track':
 
         # Load map file
-        map = Map(file_path='sim_map.png', origin=[-1, -2], resolution=0.005)
+        map = Map(file_path='maps/sim_map.png', origin=[-1, -2], resolution=0.005)
 
         # Specify waypoints
         wp_x = [-0.75, -0.25, -0.25, 0.25, 0.25, 1.25, 1.25, 0.75, 0.75, 1.25,
@@ -687,7 +687,7 @@ if __name__ == '__main__':
     elif path == 'Real_Track':
 
         # Load map file
-        map = Map(file_path='real_map.png', origin=(-30.0, -24.0),
+        map = Map(file_path='maps/real_map.png', origin=(-30.0, -24.0),
                   resolution=0.06)
 
         # Specify waypoints
@@ -727,8 +727,9 @@ if __name__ == '__main__':
         print('Invalid path!')
         exit(1)
 
-    ub, lb, border_cells = reference_path.update_path_constraints(0,
+    ub, lb, border_cells = \
-                                    reference_path.n_waypoints, 0.1, 0.01)
+        reference_path.update_path_constraints(0, reference_path.n_waypoints,
+                                               0.1, 0.01)
     SpeedProfileConstraints = {'a_min': -0.1, 'a_max': 0.5,
                                'v_min': 0, 'v_max': 1.0, 'ay_max': 4.0}
     reference_path.compute_speed_profile(SpeedProfileConstraints)

src/simulation.py

@@ -17,7 +17,8 @@ if __name__ == '__main__':
     if sim_mode == 'Sim_Track':
 
         # Load map file
-        map = Map(file_path='sim_map.png', origin=[-1, -2], resolution=0.005)
+        map = Map(file_path='maps/sim_map.png', origin=[-1, -2],
+                  resolution=0.005)
 
         # Specify waypoints
         wp_x = [-0.75, -0.25, -0.25, 0.25, 0.25, 1.25, 1.25, 0.75, 0.75, 1.25,
@@ -34,7 +35,7 @@ if __name__ == '__main__':
                                        circular=True)
 
         # Add obstacles
-        use_obstacles = False
+        use_obstacles = True
         if use_obstacles:
             obs1 = Obstacle(cx=0.0, cy=0.0, radius=0.05)
             obs2 = Obstacle(cx=-0.8, cy=-0.5, radius=0.08)
@@ -50,14 +51,14 @@ if __name__ == '__main__':
 
         # Instantiate motion model
         car = BicycleModel(length=0.12, width=0.06,
-                            reference_path=reference_path, Ts=0.05)
+                           reference_path=reference_path, Ts=0.05)
 
     # Real-World Environment. Track used for testing the algorithm on a 1:12
     # RC car.
     elif sim_mode == 'Real_Track':
 
         # Load map file
-        map = Map(file_path='real_map.png', origin=(-30.0, -24.0),
+        map = Map(file_path='maps/real_map.png', origin=(-30.0, -24.0),
                   resolution=0.06)
 
         # Specify waypoints
@@ -132,18 +133,18 @@ if __name__ == '__main__':
     # Until arrival at end of path
     while car.s < reference_path.length:
 
-        # get control signals
+        # Get control signals
         u = mpc.get_control()
 
-        # drive car
+        # Simulate car
         car.drive(u)
 
-        # log
+        # Log car state
         x_log.append(car.temporal_state.x)
         y_log.append(car.temporal_state.y)
         v_log.append(u[0])
 
-        # Increase simulation time
+        # Increment simulation time
         t += car.Ts
 
         # Plot path and drivable area

src/spatial_bicycle_models.py

@@ -1,13 +1,11 @@
 import numpy as np
 from abc import abstractmethod
-
 try:
     from abc import ABC
 except:
     # for Python 2.7
     from abc import ABCMeta
 
-
     class ABC(object):
         __metaclass__ = ABCMeta
         pass