diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index eebb443..8a79cfc 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -293,9 +293,15 @@ class Controls: self.events.add(EventName.laneChangeBlocked) else: if direction == LaneChangeDirection.left: - self.events.add(EventName.preLaneChangeLeft) + if self.sm['frogpilotPlan'].laneWidthLeft >= self.frogpilot_toggles.lane_detection_width: + self.events.add(EventName.preLaneChangeLeft) + else: + self.events.add(EventName.noLaneAvailable) else: - self.events.add(EventName.preLaneChangeRight) + if self.sm['frogpilotPlan'].laneWidthRight >= self.frogpilot_toggles.lane_detection_width: + self.events.add(EventName.preLaneChangeRight) + else: + self.events.add(EventName.noLaneAvailable) elif self.sm['modelV2'].meta.laneChangeState in (LaneChangeState.laneChangeStarting, LaneChangeState.laneChangeFinishing): self.events.add(EventName.laneChange) diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index 5ac1731..db91dbd 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -47,6 +47,12 @@ class DesireHelper: one_blinker = carstate.leftBlinker != carstate.rightBlinker below_lane_change_speed = v_ego < frogpilot_toggles.minimum_lane_change_speed + if not (frogpilot_toggles.lane_detection and one_blinker) or below_lane_change_speed: + lane_available = True + else: + desired_lane = frogpilotPlan.laneWidthLeft if carstate.leftBlinker else frogpilotPlan.laneWidthRight + lane_available = desired_lane >= frogpilot_toggles.lane_detection_width + if not lateral_active or self.lane_change_timer > LANE_CHANGE_TIME_MAX: self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none @@ -78,7 +84,7 @@ class DesireHelper: if not one_blinker or below_lane_change_speed: self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none - elif torque_applied and not blindspot_detected and self.lane_change_wait_timer >= frogpilot_toggles.lane_change_delay: + elif torque_applied and not blindspot_detected and lane_available and self.lane_change_wait_timer >= frogpilot_toggles.lane_change_delay: self.lane_change_state = LaneChangeState.laneChangeStarting self.lane_change_wait_timer = 0 diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index 747d799..64a9697 100755 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -336,6 +336,16 @@ def joystick_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, return NormalPermanentAlert("Joystick Mode", vals) +# FrogPilot Alerts +def no_lane_available_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: + lane_width = sm['frogpilotPlan'].laneWidthLeft if CS.leftBlinker else sm['frogpilotPlan'].laneWidthRight + lane_width_msg = f"{lane_width:.1f} meters" if metric else f"{lane_width * CV.METER_TO_FOOT:.1f} feet" + + return Alert( + "No lane available", + f"Detected lane width is only {lane_width_msg}", + AlertStatus.normal, AlertSize.mid, + Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2) EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { # ********** events with no alerts ********** @@ -965,6 +975,10 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { ET.NO_ENTRY: NoEntryAlert("Please don't use the 'Development' branch!"), }, + EventName.noLaneAvailable : { + ET.PERMANENT: no_lane_available_alert, + }, + EventName.openpilotCrashed: { ET.PERMANENT: Alert( "openpilot crashed", diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index f4f6bc8..c3c5373 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -44,6 +44,14 @@ class FrogPilotPlanner: lead_distance = self.lead_one.dRel + check_lane_width = frogpilot_toggles.lane_detection + if check_lane_width and v_ego >= frogpilot_toggles.minimum_lane_change_speed: + self.lane_width_left = float(calculate_lane_width(modelData.laneLines[0], modelData.laneLines[1], modelData.roadEdges[0])) + self.lane_width_right = float(calculate_lane_width(modelData.laneLines[3], modelData.laneLines[2], modelData.roadEdges[1])) + else: + self.lane_width_left = 0 + self.lane_width_right = 0 + self.base_acceleration_jerk, self.base_speed_jerk = get_jerk_factor(frogpilot_toggles.custom_personalities, frogpilot_toggles.aggressive_jerk_acceleration, frogpilot_toggles.aggressive_jerk_speed, frogpilot_toggles.standard_jerk_acceleration, frogpilot_toggles.standard_jerk_speed,