DLP: Auto Lane profile improvements (#43)

* DLP: laneless for curves when using Auto Lane profile

* DLP: force laneless while changing lane in Auto Lane profile

selfdrive/controls/lib/lateral_planner.py

@@ -11,6 +11,8 @@ from selfdrive.controls.lib.desire_helper import DesireHelper
 import cereal.messaging as messaging
 from cereal import log
 
+LaneChangeState = log.LateralPlan.LaneChangeState
+
 
 PATH_COST = 1.0
 LATERAL_MOTION_COST = 0.11
@@ -55,12 +57,15 @@ class LateralPlanner:
     self.standstill_elapsed = 0.0
     self.standstill = False
 
+    self.vision_curve_laneless = self.param_s.get_bool("VisionCurveLaneless")
+
   def reset_mpc(self, x0=np.zeros(4)):
     self.x0 = x0
     self.lat_mpc.reset(x0=self.x0)
 
   def update(self, sm):
     self.dynamic_lane_profile = int(self.param_s.get("DynamicLaneProfile", encoding='utf8'))
+    self.vision_curve_laneless = self.param_s.get_bool("VisionCurveLaneless")
     self.standstill = sm['carState'].standstill
     # clip speed , lateral planning is not possible at 0 speed
     measured_curvature = sm['controlsState'].curvature
@@ -87,7 +92,7 @@ class LateralPlanner:
       self.LP.lll_prob *= self.DH.lane_change_ll_prob
       self.LP.rll_prob *= self.DH.lane_change_ll_prob
 
-    if not self.get_dynamic_lane_profile():
+    if not self.get_dynamic_lane_profile(sm['longitudinalPlan']):
       d_path_xyz = self.LP.get_d_path(self.v_ego, self.t_idxs, self.path_xyz)
       self.dynamic_lane_profile_status = False
     else:
@@ -134,7 +139,7 @@ class LateralPlanner:
     else:
       self.solution_invalid_cnt = 0
 
-  def get_dynamic_lane_profile(self):
+  def get_dynamic_lane_profile(self, longitudinal_plan):
     if not self.dynamic_lane_profile_enabled:
       return True
     elif self.dynamic_lane_profile == 1:
@@ -142,12 +147,19 @@ class LateralPlanner:
     elif self.dynamic_lane_profile == 0:
       return False
     elif self.dynamic_lane_profile == 2:
+      # laneless while lane change in progress
+      if self.DH.lane_change_state in (LaneChangeState.laneChangeStarting, LaneChangeState.laneChangeFinishing):
+        return True
       # only while lane change is off
-      if self.DH.lane_change_state == log.LateralPlan.LaneChangeState.off:
+      elif self.DH.lane_change_state == LaneChangeState.off:
         # laneline probability too low, we switch to laneless mode
-        if (self.LP.lll_prob + self.LP.rll_prob) / 2 < 0.3:
+        if (self.LP.lll_prob + self.LP.rll_prob) / 2 < 0.3 \
+          or ((longitudinal_plan.visionCurrentLatAcc > 1.0 or longitudinal_plan.visionMaxPredLatAcc > 1.4)
+           and self.vision_curve_laneless):
           self.dynamic_lane_profile_status_buffer = True
-        if (self.LP.lll_prob + self.LP.rll_prob) / 2 > 0.5:
+        if (self.LP.lll_prob + self.LP.rll_prob) / 2 > 0.5 \
+          and ((longitudinal_plan.visionCurrentLatAcc < 0.6 and longitudinal_plan.visionMaxPredLatAcc < 0.7)
+           or not self.vision_curve_laneless):
           self.dynamic_lane_profile_status_buffer = False
         if self.dynamic_lane_profile_status_buffer:  # in buffer mode, always laneless
           return True

selfdrive/controls/lib/longitudinal_planner.py

@@ -176,6 +176,8 @@ class LongitudinalPlanner:
 
     longitudinalPlan.visionTurnControllerState = self.vision_turn_controller.state
     longitudinalPlan.visionTurnSpeed = float(self.vision_turn_controller.v_turn)
+    longitudinalPlan.visionCurrentLatAcc = float(self.vision_turn_controller.current_lat_acc)
+    longitudinalPlan.visionMaxPredLatAcc = float(self.vision_turn_controller.max_pred_lat_acc)
 
     longitudinalPlan.speedLimitControlState = self.speed_limit_controller.state
     longitudinalPlan.speedLimit = float(self.speed_limit_controller.speed_limit)

selfdrive/controls/lib/vision_turn_controller.py

@@ -135,6 +135,14 @@ class VisionTurnController():
   def is_active(self):
     return self._state != VisionTurnControllerState.disabled
 
+  @property
+  def current_lat_acc(self):
+    return self._current_lat_acc
+
+  @property
+  def max_pred_lat_acc(self):
+    return self._max_pred_lat_acc
+
   def _reset(self):
     self._current_lat_acc = 0.
     self._max_v_for_current_curvature = 0.