update config file with safe_lap parameter

config/speed_profiler.yaml

@@ -12,6 +12,7 @@ mission_1:
   lateral_acceleration_limit: 14.0
 
   path_similar_mse: 0.1
+  safe_lap: true
 
 # Skidpad
 mission_2:
@@ -27,6 +28,7 @@ mission_2:
   lateral_acceleration_limit: 14.0
 
   path_similar_mse: 0.1
+  safe_lap: true
 
 # Trackdrive
 mission_3:
@@ -42,6 +44,7 @@ mission_3:
   lateral_acceleration_limit: 14.0
 
   path_similar_mse: 0.1
+  safe_lap: true
 
 # Uses Emergency Brakes System (EBS) speed limit
 mission_4:
@@ -56,6 +59,7 @@ mission_4:
   lateral_acceleration_limit: 4
   accel_min: -2
   accel_max: 2
+  safe_lap: true
 
   # If the MSE of positions and previous path positions is below,
   # speed profile computation will be skipped and previous solution is used.

src/speed_profiler/selector.py

@@ -42,11 +42,9 @@ def __init__(self, parameters, speed_profiler, mission):
         self.time = 0  
 
         ## Lap 1 complete flag
-        self._in_first_lap = False
+        self._in_first_lap = parameter['safe_lap']
         self.lap = 1
 
-        rospy.loginfo("Finished intializing SpeedProfileSelector")
-
     def update_lap(self, lap):
         """ Check if car is in first lap """
         if lap > 1:
@@ -107,9 +105,11 @@ def select_speed_profile(self, path, pose):
             return self._use_safe_speed(path)
         else:
             # Use v_final from the previous profile (if it exists) as v_init for the current computation
-            # v_init = self._previous_path.speed_profile[-1] if self._previous_path and self._previous_path.speed_profile else self._safe_speed
-            path = self._use_optimal_speed_profile(path, speed_limit=self.real_speed_limit, v_init=self._safe_speed)
-            # path = self._use_optimal_speed_profile(path, speed_limit=self.real_speed_limit, v_init=self._safe_speed)
+            if self._previous_path:
+                last_known_velocity = self._previous_path.speed_profile[-1]
+                path = self._use_optimal_speed_profile(path, speed_limit=self.real_speed_limit, v_init=last_known_velocity)
+            else:
+                path = self._use_optimal_speed_profile(path, speed_limit=self.real_speed_limit, v_init=self._safe_speed)
 
         # If path is too similar to path for which speed profile was computed,
         # skip new computation