From 958abbdef8d9605f8e92b6fe30a40d91ae83735f Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Sun, 30 Jun 2024 17:40:38 -0700 Subject: [PATCH] Controls - Quality of Life - Force Standstill State - Only For Stop Lights/Stop Signs --- .../frogpilot/controls/frogpilot_planner.py | 18 ++++++++++++++++-- .../lib/conditional_experimental_mode.py | 12 ++++++------ 2 files changed, 22 insertions(+), 8 deletions(-) diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index 1659d2a59..dedc2ca3d 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -52,6 +52,7 @@ class FrogPilotPlanner: self.mtsc_target = 0 self.road_curvature = 0 self.speed_jerk = 0 + self.tracked_model_length = 0 self.v_cruise = 0 def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState, frogpilotNavigation, modelData, radarState, frogpilot_toggles): @@ -74,7 +75,7 @@ class FrogPilotPlanner: stopping_distance = STOP_DISTANCE + distance_offset if frogpilot_toggles.conditional_experimental_mode and controlsState.enabled: - self.cem.update(carState, frogpilotNavigation, self.lead_one, modelData, self.model_length, self.road_curvature, self.slower_lead, self.tracking_lead, v_ego, v_lead, frogpilot_toggles) + self.cem.update(carState, frogpilotNavigation, self.lead_one, modelData, self.model_length, self.road_curvature, self.slower_lead, self.tracking_lead, self.v_cruise, v_ego, v_lead, frogpilot_toggles) check_lane_width = frogpilot_toggles.lane_detection if check_lane_width and v_ego >= frogpilot_toggles.minimum_lane_change_speed: @@ -90,6 +91,9 @@ class FrogPilotPlanner: if v_ego > CRUISING_SPEED: self.override_force_stop = False self.tracking_lead = self.lead_one.status + self.tracked_model_length = 0 + elif carState.standstill and frogpilot_toggles.force_stops: + self.override_force_stop = True else: self.tracking_lead &= self.lead_one.status @@ -192,12 +196,22 @@ class FrogPilotPlanner: else: self.mtsc_target = v_cruise if v_cruise != V_CRUISE_UNSET else 0 - if frogpilot_toggles.force_standstill and v_ego < 1 and not self.override_force_stop: + if (frogpilot_toggles.force_standstill or frogpilot_toggles.force_stops) and v_ego < 1 and not self.override_force_stop: if carState.gasPressed: self.override_force_stop = True else: self.v_cruise = -1 + elif frogpilot_toggles.force_stops and v_ego < CRUISING_SPEED and controlsState.experimentalMode and not self.override_force_stop: + if carState.gasPressed or self.tracking_lead or abs(carState.steeringAngleDeg) > 15: + self.override_force_stop = True + else: + if self.tracked_model_length == 0: + self.tracked_model_length = self.model_length + + self.tracked_model_length -= v_ego * DT_MDL + self.v_cruise = self.tracked_model_length / ModelConstants.T_IDXS[TRAJECTORY_SIZE - 1] + else: targets = [self.mtsc_target] self.v_cruise = min([target if target > CRUISING_SPEED else v_cruise for target in targets]) diff --git a/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py b/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py index 95a38ed96..0ee526ab3 100644 --- a/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py +++ b/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py @@ -15,14 +15,14 @@ class ConditionalExperimentalMode: self.slow_lead_mac = MovingAverageCalculator() self.stop_light_mac = MovingAverageCalculator() - def update(self, carState, frogpilotNavigation, lead, modelData, model_length, road_curvature, slower_lead, tracking_lead, v_ego, v_lead, frogpilot_toggles): + def update(self, carState, frogpilotNavigation, lead, modelData, model_length, road_curvature, slower_lead, tracking_lead, v_cruise, v_ego, v_lead, frogpilot_toggles): if frogpilot_toggles.experimental_mode_via_press: self.status_value = self.params_memory.get_int("CEStatus") else: self.status_value = 0 if self.status_value not in {1, 2, 3, 4, 5, 6} and not carState.standstill: - self.update_conditions(lead.dRel, model_length, road_curvature, slower_lead, tracking_lead, v_ego, v_lead, frogpilot_toggles) + self.update_conditions(lead.dRel, model_length, road_curvature, slower_lead, tracking_lead, v_cruise, v_ego, v_lead, frogpilot_toggles) self.experimental_mode = self.check_conditions(carState, frogpilotNavigation, modelData, tracking_lead, v_ego, v_lead, frogpilot_toggles) self.params_memory.put_int("CEStatus", self.status_value if self.experimental_mode else 0) else: @@ -56,10 +56,10 @@ class ConditionalExperimentalMode: return False - def update_conditions(self, lead_distance, model_length, road_curvature, slower_lead, tracking_lead, v_ego, v_lead, frogpilot_toggles): + def update_conditions(self, lead_distance, model_length, road_curvature, slower_lead, tracking_lead, v_cruise, v_ego, v_lead, frogpilot_toggles): self.road_curvature(road_curvature, v_ego, frogpilot_toggles) self.slow_lead(slower_lead, tracking_lead, v_lead, frogpilot_toggles) - self.stop_sign_and_light(lead_distance, model_length, tracking_lead, v_ego, v_lead, frogpilot_toggles) + self.stop_sign_and_light(lead_distance, model_length, tracking_lead, v_cruise, v_ego, v_lead, frogpilot_toggles) def road_curvature(self, road_curvature, v_ego, frogpilot_toggles): curve_detected = (1 / road_curvature)**0.5 < v_ego @@ -79,7 +79,7 @@ class ConditionalExperimentalMode: self.slow_lead_mac.reset_data() self.slow_lead_detected = False - def stop_sign_and_light(self, lead_distance, model_length, tracking_lead, v_ego, v_lead, frogpilot_toggles): + def stop_sign_and_light(self, lead_distance, model_length, tracking_lead, v_cruise, v_ego, v_lead, frogpilot_toggles): lead_close = lead_distance < CITY_SPEED_LIMIT lead_far = lead_distance > CITY_SPEED_LIMIT and v_ego < CRUISING_SPEED lead_stopped = v_lead < 1 @@ -87,7 +87,7 @@ class ConditionalExperimentalMode: following_lead = tracking_lead and (lead_close or lead_stopped or lead_stopping) and not lead_far model_projection = ModelConstants.T_IDXS[TRAJECTORY_SIZE - (5 if frogpilot_toggles.less_sensitive_lights else 3)] - model_stopped = model_length < TRAJECTORY_SIZE + model_stopped = model_length < TRAJECTORY_SIZE or v_cruise < CRUISING_SPEED model_threshold = v_ego * model_projection model_stopping = model_length < model_threshold and not self.curve_detected