diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 030de797ff..f197e6b38a 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/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 diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index a0eecb3530..fc65f4ed5a 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/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) diff --git a/selfdrive/controls/lib/vision_turn_controller.py b/selfdrive/controls/lib/vision_turn_controller.py index 76dc7cb52c..21bf52d4d9 100644 --- a/selfdrive/controls/lib/vision_turn_controller.py +++ b/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.