From cfbb472acd893c59f89674af4b9af8800935aee4 Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Tue, 27 Feb 2024 16:34:47 -0700 Subject: [PATCH] Use turn desires when below the minimum lane change speed Added toggle to use turn desires when below the minimum lane change speed for more precise turns. --- cereal/car.capnp | 2 ++ cereal/log.capnp | 7 ++++++ common/params.cc | 1 + selfdrive/controls/controlsd.py | 7 ++++++ selfdrive/controls/lib/desire_helper.py | 28 +++++++++++++++++++++- selfdrive/controls/lib/events.py | 16 +++++++++++++ selfdrive/frogpilot/ui/control_settings.cc | 2 ++ selfdrive/modeld/modeld.py | 1 + 8 files changed, 63 insertions(+), 1 deletion(-) diff --git a/cereal/car.capnp b/cereal/car.capnp index 92ea0db..246ef6d 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -132,6 +132,8 @@ struct CarEvent @0x9b1657f34caf3ad3 { pedalInterceptorNoBrake @131; speedLimitChanged @132; torqueNNLoad @133; + turningLeft @134; + turningRight @135; vCruise69 @136; yourFrogTriedToKillMe @137; diff --git a/cereal/log.capnp b/cereal/log.capnp index 85968d6..964bb26 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -323,6 +323,12 @@ enum LaneChangeDirection { right @2; } +enum TurnDirection { + none @0; + turnLeft @1; + turnRight @2; +} + struct CanData { address @0 :UInt32; busTime @1 :UInt16; @@ -945,6 +951,7 @@ struct ModelDataV2 { hardBrakePredicted @7 :Bool; laneChangeState @8 :LaneChangeState; laneChangeDirection @9 :LaneChangeDirection; + turnDirection @10 :TurnDirection; # deprecated diff --git a/common/params.cc b/common/params.cc index 6fd6c92..21c0b6a 100644 --- a/common/params.cc +++ b/common/params.cc @@ -417,6 +417,7 @@ std::unordered_map keys = { {"TetheringEnabled", PERSISTENT}, {"TrafficMode", PERSISTENT}, {"TrafficModeActive", PERSISTENT}, + {"TurnDesires", PERSISTENT}, {"UnlimitedLength", PERSISTENT}, {"Updated", PERSISTENT}, {"UpdateSchedule", PERSISTENT}, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 5d76062..f93714b 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -455,6 +455,13 @@ class Controls: LaneChangeState.laneChangeFinishing): self.events.add(EventName.laneChange) + # Handle turning + if not CS.standstill: + if self.sm['modelV2'].meta.turnDirection == Desire.turnLeft: + self.events.add(EventName.turningLeft) + elif self.sm['modelV2'].meta.turnDirection == Desire.turnRight: + self.events.add(EventName.turningRight) + for i, pandaState in enumerate(self.sm['pandaStates']): # All pandas must match the list of safetyConfigs, and if outside this list, must be silent or noOutput if i < len(self.CP.safetyConfigs): diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index 35a1413..52240bb 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -5,6 +5,7 @@ from openpilot.common.realtime import DT_MDL LaneChangeState = log.LaneChangeState LaneChangeDirection = log.LaneChangeDirection +TurnDirection = log.Desire LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS LANE_CHANGE_TIME_MAX = 10. @@ -30,6 +31,12 @@ DESIRES = { }, } +TURN_DESIRES = { + TurnDirection.none: log.Desire.none, + TurnDirection.turnLeft: log.Desire.turnLeft, + TurnDirection.turnRight: log.Desire.turnRight, +} + class DesireHelper: def __init__(self): @@ -45,7 +52,10 @@ class DesireHelper: self.params = Params() self.params_memory = Params("/dev/shm/params") + self.turn_direction = TurnDirection.none + self.lane_change_completed = False + self.turn_completed = False self.lane_change_wait_timer = 0 @@ -66,7 +76,15 @@ class DesireHelper: 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 + self.turn_direction = TurnDirection.none + elif one_blinker and below_lane_change_speed and self.turn_desires: + self.turn_direction = TurnDirection.turnLeft if carstate.leftBlinker else TurnDirection.turnRight + # Set the "turn_completed" flag to prevent lane changes after completing a turn + self.turn_completed = True else: + # TurnDirection.turnLeft / turnRight + self.turn_direction = TurnDirection.none + # LaneChangeState.off if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed: self.lane_change_state = LaneChangeState.preLaneChange @@ -130,8 +148,14 @@ class DesireHelper: # Reset the flags self.lane_change_completed &= one_blinker + self.turn_completed &= one_blinker - self.desire = DESIRES[self.lane_change_direction][self.lane_change_state] + if self.turn_direction != TurnDirection.none: + self.desire = TURN_DESIRES[self.turn_direction] + elif not self.turn_completed: + self.desire = DESIRES[self.lane_change_direction][self.lane_change_state] + else: + self.desire = log.Desire.none # Send keep pulse once per second during LaneChangeStart.preLaneChange if self.lane_change_state in (LaneChangeState.off, LaneChangeState.laneChangeStarting): @@ -154,3 +178,5 @@ class DesireHelper: self.lane_detection = self.nudgeless and self.params.get_bool("LaneDetection") self.lane_detection_width = self.params.get_int("LaneDetectionWidth") * (1 if is_metric else CV.FOOT_TO_METER) / 10 if self.lane_detection else 0 self.one_lane_change = self.nudgeless and self.params.get_bool("OneLaneChange") + + self.turn_desires = self.params.get_bool("TurnDesires") diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index 56b32d6..9d94e28 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -1077,6 +1077,22 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { ET.PERMANENT: torque_nn_load_alert, }, + EventName.turningLeft: { + ET.WARNING: Alert( + "Turning Left", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.none, .1, alert_rate=0.75), + }, + + EventName.turningRight: { + ET.WARNING: Alert( + "Turning Right", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.none, .1, alert_rate=0.75), + }, + # Random Events EventName.accel30: { ET.WARNING: Alert( diff --git a/selfdrive/frogpilot/ui/control_settings.cc b/selfdrive/frogpilot/ui/control_settings.cc index 903884d..a6f8b15 100644 --- a/selfdrive/frogpilot/ui/control_settings.cc +++ b/selfdrive/frogpilot/ui/control_settings.cc @@ -82,6 +82,8 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil {"SLCVisuals", "Visuals Settings", "Manage visual settings.", ""}, {"ShowSLCOffset", "Show Speed Limit Offset", "Show the speed limit offset seperated from the speed limit in the onroad UI when using 'Speed Limit Controller'.", ""}, {"UseVienna", "Use Vienna Speed Limit Signs", "Use the Vienna (EU) speed limit style signs as opposed to MUTCD (US).", ""}, + + {"TurnDesires", "Use Turn Desires", "Use turn desires for enhanced precision in turns below the minimum lane change speed.", "../assets/navigation/direction_continue_right.png"}, }; for (const auto &[param, title, desc, icon] : controlToggles) { diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index ffba1df..0c928e6 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -305,6 +305,7 @@ def main(demo=False): DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, sm['frogpilotPlan']) modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction + modelv2_send.modelV2.meta.turnDirection = DH.turn_direction fill_pose_msg(posenet_send, model_output, meta_main.frame_id, vipc_dropped_frames, meta_main.timestamp_eof, live_calib_seen) pm.send('modelV2', modelv2_send)