From 97135965effb3630ed55993903449edb73468d50 Mon Sep 17 00:00:00 2001 From: James Vecellio-Grant <159560811+Discountchubbs@users.noreply.github.com> Date: Wed, 28 Oct 2026 17:02:55 -0700 Subject: [PATCH] controls: Blendful lateral policy (alpha) (#2073) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * init * settings * simple * modeld_v2: blended laneful using weighted stds * red diff that * default to 1.0 while i test variable weighting * blended correction over .3 s * decay trust when model intends to move plant off lane. think going around cones and stuff * sharp curvies have some issues :O * quicker entry * blend 30% plan with action projeciton * blending 50% on sharp curvaruerqw * jerk limiting correctin strength so that hecker doesnt spin spin yo wheel * handle one line flicker by trust÷2 * less is more * winner takes all trust system in lane lines * Ui fix * blendful * sm logging + qlog logging * Update modeld.py * Update modeld.py * relearn ideal sim pose * interpolate smoothness on straights to reduce mr kumar and his mdv3 jiggle shennanigans * let unwind be handled by laneless * ffs * blendful * no tap * use constants * fade on trust * use blendful trust * dont guess * white is model path * match width * fade out blue * dead * blue only * sqrt bar * ill take it from here thanks * tizi * move to models * cleanup * gate like rainbow * tick on new data * so I can sleep at night * keep pill current * fine * smoother release authority * satisfy royjr * decay one line and give full authority in curvy curves; kappa=curv * he does not like 1ms * smoother * force tg * oop * hold blue * rm * early return * add custom model gate & Alpha tag * more accurate * more * reduce * better * more accurate * ci * small regression test * ui: tighten Blendful gating and settings copy * ui: clarify Blendful toggle description * ui: gate Blendful on the launched model runner --------- Co-authored-by: royjr Co-authored-by: nayan Co-authored-by: Jason Wen --- openpilot/cereal/custom.capnp | 7 + openpilot/cereal/services.py | 2 +- openpilot/common/params_keys.h | 1 + .../ui/mici/onroad/model_renderer.py | 2 + .../selfdrive/ui/onroad/model_renderer.py | 2 + .../ui/sunnypilot/layouts/settings/models.py | 17 +- .../ui/sunnypilot/mici/layouts/models.py | 21 ++- .../sunnypilot/mici/onroad/model_renderer.py | 2 + .../ui/sunnypilot/onroad/blendful.py | 95 ++++++++++++ .../ui/sunnypilot/onroad/model_renderer.py | 13 +- openpilot/selfdrive/ui/sunnypilot/ui_state.py | 13 +- openpilot/sunnypilot/modeld_v2/blendful.py | 145 ++++++++++++++++++ openpilot/sunnypilot/modeld_v2/modeld.py | 21 ++- .../modeld_v2/tests/test_blendful.py | 112 ++++++++++++++ .../sunnypilot/sunnylink/capabilities.py | 5 + .../sunnypilot/sunnylink/settings_ui.json | 15 +- .../sunnylink/settings_ui_src/_macros.yaml | 4 + .../settings_ui_src/pages/models.yaml | 8 +- 18 files changed, 468 insertions(+), 17 deletions(-) create mode 100644 openpilot/selfdrive/ui/sunnypilot/onroad/blendful.py create mode 100644 openpilot/sunnypilot/modeld_v2/blendful.py create mode 100644 openpilot/sunnypilot/modeld_v2/tests/test_blendful.py diff --git a/openpilot/cereal/custom.capnp b/openpilot/cereal/custom.capnp index d0e9dd463c..430aa140d9 100644 --- a/openpilot/cereal/custom.capnp +++ b/openpilot/cereal/custom.capnp @@ -468,6 +468,13 @@ struct ModelDataV2SP @0xa1680744031fdb2d { laneTurnDirection @0 :TurnDirection; leftLaneChangeEdgeBlock @1 :Bool; rightLaneChangeEdgeBlock @2 :Bool; + blendfulCurvature @3 :Float32; + blendfulTrust @4 :Float32; + modelCurvature @5 :Float32; + blendfulGoingAround @6 :Bool; + blendfulLeftProb @7 :Float32; + blendfulRightProb @8 :Float32; + blendfulLaneWidth @9 :Float32; enum TurnDirection { none @0; diff --git a/openpilot/cereal/services.py b/openpilot/cereal/services.py index 541a06298a..449778ba2a 100755 --- a/openpilot/cereal/services.py +++ b/openpilot/cereal/services.py @@ -94,7 +94,7 @@ _services: dict[str, tuple] = { "carControlSP": (True, 100., 10), "carStateSP": (True, 100., 10), "liveMapDataSP": (True, 1., 1), - "modelDataV2SP": (True, 20., None, QueueSize.BIG), + "modelDataV2SP": (True, 20., 10, QueueSize.BIG), "liveLocationKalman": (True, 20.), # debug diff --git a/openpilot/common/params_keys.h b/openpilot/common/params_keys.h index 9d28f04082..1ccd8110a6 100644 --- a/openpilot/common/params_keys.h +++ b/openpilot/common/params_keys.h @@ -257,6 +257,7 @@ inline static std::unordered_map keys = { {"LagdValueCache", {PERSISTENT, FLOAT, "0.2"}}, {"LaneTurnDesire", {PERSISTENT | BACKUP, BOOL, "0"}}, {"LaneTurnValue", {PERSISTENT | BACKUP, FLOAT, "19.0"}}, + {"Blendful", {PERSISTENT | BACKUP, BOOL, "0"}}, {"PlanplusControl", {PERSISTENT | BACKUP, FLOAT, "1.0"}}, // mapd diff --git a/openpilot/selfdrive/ui/mici/onroad/model_renderer.py b/openpilot/selfdrive/ui/mici/onroad/model_renderer.py index 68d7488471..d86b31aff5 100644 --- a/openpilot/selfdrive/ui/mici/onroad/model_renderer.py +++ b/openpilot/selfdrive/ui/mici/onroad/model_renderer.py @@ -141,6 +141,7 @@ class ModelRenderer(Widget, ModelRendererSP): if ui_state.status != UIStatus.DISENGAGED: self._draw_lane_lines() self._draw_path() + self.blendful.draw(self._rect) if render_lead_indicator: self._update_leads(sm) @@ -219,6 +220,7 @@ class ModelRenderer(Widget, ModelRendererSP): # Update road edges using raw points for road_edge in self._road_edges: road_edge.projected_points = self._map_line_to_polygon(road_edge.raw_points, line_width_factor, 0.0, max_idx) + self.blendful.update(self, max_idx) # Update path using raw points if lead and lead.present: diff --git a/openpilot/selfdrive/ui/onroad/model_renderer.py b/openpilot/selfdrive/ui/onroad/model_renderer.py index 6db7d348a2..84573b7732 100644 --- a/openpilot/selfdrive/ui/onroad/model_renderer.py +++ b/openpilot/selfdrive/ui/onroad/model_renderer.py @@ -123,6 +123,7 @@ class ModelRenderer(Widget, ChevronMetrics, ModelRendererSP): # Draw elements self._draw_lane_lines() self._draw_path() + self.blendful.draw(self._rect) if render_lead_indicator and radar_state: self._draw_lead_indicator() @@ -172,6 +173,7 @@ class ModelRenderer(Widget, ChevronMetrics, ModelRendererSP): # Update road edges using raw points for road_edge in self._road_edges: road_edge.projected_points = self._map_line_to_polygon(road_edge.raw_points, 0.025, 0.0, max_idx, max_distance) + self.blendful.update(self, max_idx, max_distance) # Update path using raw points if lead and lead.present: diff --git a/openpilot/selfdrive/ui/sunnypilot/layouts/settings/models.py b/openpilot/selfdrive/ui/sunnypilot/layouts/settings/models.py index 22cc818ff4..4cdca1253b 100644 --- a/openpilot/selfdrive/ui/sunnypilot/layouts/settings/models.py +++ b/openpilot/selfdrive/ui/sunnypilot/layouts/settings/models.py @@ -102,13 +102,19 @@ class ModelsLayout(Widget): self.lagd_toggle = toggle_item_sp(tr("Live Learning Steer Delay"), "", param="LagdToggle") + self.blendful_toggle = toggle_item_sp(tr("Blendful (Alpha)"), + tr("Adds a gentle nudge toward lane center on top of the model's own steering, stronger when lane lines are clear." + + " Yields around obstacles. Not available with the Default model."), + param="Blendful") + self.camera_offset = option_item_sp(tr("Adjust Camera Offset"), "CameraOffset", -35, 35, tr("Virtually shift camera's perspective to move model's center to Left(+ values) or Right (- values)"), 1, None, True, "", style.BUTTON_ACTION_WIDTH, None, True, lambda v: f"{v / 100:.2f} m") self.items = [self.small_model_item, self.big_model_item, self.cancel_download_item, self.download_item, self.refresh_item, self.clear_cache_item, - self.lane_turn_desire_toggle, self.lane_turn_value_control, self.lagd_toggle, self.delay_control, self.camera_offset] + self.blendful_toggle, self.lane_turn_desire_toggle, self.lane_turn_value_control, self.lagd_toggle, self.delay_control, + self.camera_offset] def _update_lagd_description(self, lagd_toggle: bool): desc = tr("Enable this for the car to learn and adapt its steering response time. Disable to use a fixed steering response time. " + @@ -322,6 +328,15 @@ class ModelsLayout(Widget): self.lane_turn_desire_toggle.action_item.set_state(turn_desire) self.lane_turn_value_control.set_visible(turn_desire and advanced_controls) self.lagd_toggle.action_item.set_state(live_delay) + + blendful_allowed = ui_state.active_runner_tinygrad + if blendful_allowed: + self.blendful_toggle.action_item.set_state(ui_state.params.get_bool("Blendful")) + self.blendful_toggle.action_item.set_enabled(True) + else: + self.blendful_toggle.action_item.set_state(False) + self.blendful_toggle.action_item.set_enabled(False) + self.delay_control.set_visible(not live_delay and advanced_controls) new_step = int(round(100 / CV.MPH_TO_KPH)) if ui_state.is_metric else 100 if self.lane_turn_value_control.action_item is not None and self.lane_turn_value_control.action_item.value_change_step != new_step: diff --git a/openpilot/selfdrive/ui/sunnypilot/mici/layouts/models.py b/openpilot/selfdrive/ui/sunnypilot/mici/layouts/models.py index c1f1f09155..af53932c9b 100644 --- a/openpilot/selfdrive/ui/sunnypilot/mici/layouts/models.py +++ b/openpilot/selfdrive/ui/sunnypilot/mici/layouts/models.py @@ -11,7 +11,7 @@ import pyray as rl from openpilot.cereal import custom from openpilot.selfdrive.ui.mici.widgets.dialog import BigConfirmationDialog, BigDialog from openpilot.sunnypilot.models.helpers import ACTIVE_BUNDLE_KEYS, get_selected_bundle -from openpilot.selfdrive.ui.mici.widgets.button import BigButton +from openpilot.selfdrive.ui.mici.widgets.button import BigButton, BigParamControl from openpilot.selfdrive.ui.ui_state import ui_state, device from openpilot.selfdrive.ui.sunnypilot.model_info import (active_source, big_model_state, bundles_for_source, carrying_model, default_model_name, model_cache_size_mb, model_info, queued_name, @@ -95,7 +95,12 @@ class ModelsLayoutMici(NavScroller): self.clear_cache_btn.set_click_callback(self._confirm_clear_cache) self._cache_size_time = 0.0 - self.main_items = [self.current_model_info, self.select_model_btn, self.cancel_download_btn, self.refresh_btn, self.clear_cache_btn] + self.blendful_toggle = BigParamControl(tr("blendful (alpha)"), "Blendful", + description=tr("Adds a gentle nudge toward lane center on top of the model's own steering, stronger when " + + "lane lines are clear. Yields around obstacles. Not available with the Default model.")) + + self.main_items = [self.current_model_info, self.select_model_btn, self.cancel_download_btn, self.refresh_btn, self.clear_cache_btn, + self.blendful_toggle] self._scroller.add_widgets(self.main_items) @property @@ -205,6 +210,10 @@ class ModelsLayoutMici(NavScroller): device.set_override_interactive_timeout(None) self._was_downloading = False + def show_event(self): + super().show_event() + self.blendful_toggle.refresh() + def _update_state(self): super()._update_state() @@ -213,6 +222,14 @@ class ModelsLayoutMici(NavScroller): self.current_model_info.current_model_header._shimmer = False self.current_model_info.info_header._shimmer = False + blendful_allowed = ui_state.active_runner_tinygrad + if blendful_allowed: + self.blendful_toggle.set_checked(ui_state.params.get_bool("Blendful")) + self.blendful_toggle.set_enabled(True) + else: + self.blendful_toggle.set_checked(False) + self.blendful_toggle.set_enabled(False) + manager = self.model_manager self._download_frame += 1 should_update = self._download_frame % (gui_app.target_fps / 2) == 0 diff --git a/openpilot/selfdrive/ui/sunnypilot/mici/onroad/model_renderer.py b/openpilot/selfdrive/ui/sunnypilot/mici/onroad/model_renderer.py index e3de9401ce..c5a8aca127 100644 --- a/openpilot/selfdrive/ui/sunnypilot/mici/onroad/model_renderer.py +++ b/openpilot/selfdrive/ui/sunnypilot/mici/onroad/model_renderer.py @@ -6,6 +6,7 @@ See the LICENSE.md file in the root directory for more details. """ import pyray as rl from openpilot.selfdrive.ui.ui_state import UIStatus +from openpilot.selfdrive.ui.sunnypilot.onroad.blendful import BlendfulRenderer from openpilot.selfdrive.ui.sunnypilot.onroad.rainbow_path import RainbowPath LANE_LINE_COLORS_SP = { @@ -17,3 +18,4 @@ LANE_LINE_COLORS_SP = { class ModelRendererSP: def __init__(self): self.rainbow_path = RainbowPath() + self.blendful = BlendfulRenderer(rect_relative=True) diff --git a/openpilot/selfdrive/ui/sunnypilot/onroad/blendful.py b/openpilot/selfdrive/ui/sunnypilot/onroad/blendful.py new file mode 100644 index 0000000000..f6f3d4c849 --- /dev/null +++ b/openpilot/selfdrive/ui/sunnypilot/onroad/blendful.py @@ -0,0 +1,95 @@ +""" +Copyright (c) 2021-, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" +import numpy as np +import pyray as rl +from openpilot.common.filter_simple import FirstOrderFilter +from openpilot.common.realtime import DT_MDL +from openpilot.selfdrive.ui.ui_state import ui_state +from openpilot.selfdrive.ui.sunnypilot.ui_state import UIStateSP +from openpilot.sunnypilot.modeld_v2.blendful import LANE_WIDTH_CLIP, LOOKAHEAD_TIME, MIN_LOOKAHEAD +from openpilot.system.ui.lib.application import gui_app, FontWeight +from openpilot.system.ui.lib.shader_polygon import draw_polygon +from openpilot.system.ui.lib.text_measure import measure_text_cached + +BLENDFUL_COLOR = (0, 200, 255) +FULL_BAR = 1.0 + + +class BlendfulRenderer: + def __init__(self, readout: bool = True, rect_relative: bool = False): + self._readout = readout + self._rect_relative = rect_relative + self._center_points = np.empty((0, 2), dtype=np.float32) + self._trust = 0.0 + self._font = gui_app.font(FontWeight.BOLD) + self._pull_display = FirstOrderFilter(0.0, 0.5, DT_MDL) + self._trust_display = FirstOrderFilter(0.0, 0.5, DT_MDL) + self._alpha = FirstOrderFilter(0.0, 0.3, DT_MDL) + + def update(self, renderer, max_idx: int, *map_args) -> None: + blendful = ui_state.sm['modelDataV2SP'] + if ui_state.sm.updated['modelDataV2SP']: + self._alpha.update(float(blendful.blendfulTrust > 0.0)) + if blendful.blendfulTrust > 0.0: + self._trust = blendful.blendfulTrust + if self._readout: + lookahead = max(ui_state.sm['carState'].vEgo * LOOKAHEAD_TIME, MIN_LOOKAHEAD) + self._pull_display.update(blendful.blendfulCurvature * lookahead ** 2 / 2) + self._trust_display.update(self._trust) + + left, right = renderer._lane_lines[1].raw_points, renderer._lane_lines[2].raw_points + stds = list(ui_state.sm['modelV2'].laneLineStds) + if self._alpha.x < 0.01 or len(left) == 0 or len(right) != len(left) or len(stds) < 3: + self._center_points = np.empty((0, 2), dtype=np.float32) + return + + l_prob, r_prob = blendful.blendfulLeftProb, blendful.blendfulRightProb + if blendful.blendfulTrust > 0.0 and l_prob + r_prob > 0.0: + half_width = np.clip(right[:, 1] - left[:, 1], *LANE_WIDTH_CLIP) / 2 if l_prob * r_prob > 0.5 else blendful.blendfulLaneWidth / 2 + l_var = stds[1] ** 2 / max(l_prob, 1e-4) + r_var = stds[2] ** 2 / max(r_prob, 1e-4) + center = left.copy() + center[:, 1] = (r_var * (left[:, 1] + half_width) + l_var * (right[:, 1] - half_width)) / (l_var + r_var) + self._center_points = renderer._map_line_to_polygon(center, 0.15, 0.0, max_idx, *map_args) + + def _color(self, rgb, alpha: float) -> rl.Color: + return rl.Color(*rgb, int(alpha * self._alpha.x)) + + def draw(self, rect: rl.Rectangle) -> None: + if not UIStateSP.lateral_active(ui_state): + return + if self._center_points.size and self._trust > 0.05: + points = self._center_points + (rect.x, rect.y) if self._rect_relative else self._center_points + draw_polygon(rect, points, self._color(BLENDFUL_COLOR, np.clip(self._trust, 0.2, 1.0) * 255)) + if self._center_points.size and self._readout: + self._draw_readout(rect) + + def _draw_readout(self, rect: rl.Rectangle) -> None: + pull, trust = self._pull_display.x, self._trust_display.x + w, h = 190, 76 + x, y = rect.x + (rect.width - w) / 2, rect.y + 10 + cx, cy = x + w / 2, y + 30 + rl.draw_rectangle_rounded(rl.Rectangle(x, y, w, h), 0.5, 12, self._color((0, 0, 0), 170)) + + text = f"{abs(pull) * 100:.0f}" + text_w = measure_text_cached(self._font, text, 44, 0).x + rl.draw_text_ex(self._font, text, rl.Vector2(cx - text_w / 2, cy - 24), 44, 0, self._color((255, 255, 255), 255)) + + active = self._color(BLENDFUL_COLOR, np.clip(abs(pull) / 0.1, 0.0, 1.0) * 255) + idle = self._color((255, 255, 255), 40) + lx, rx = x + 18, x + w - 18 + rl.draw_triangle(rl.Vector2(lx, cy), rl.Vector2(lx + 16, cy + 12), rl.Vector2(lx + 16, cy - 12), active if pull < 0 else idle) + rl.draw_triangle(rl.Vector2(rx - 16, cy - 12), rl.Vector2(rx - 16, cy + 12), rl.Vector2(rx, cy), active if pull > 0 else idle) + + bar = rl.Rectangle(x + 24, y + h - 16, w - 48, 6) + rl.draw_rectangle_rounded(bar, 1.0, 6, self._color((255, 255, 255), 40)) + rl.draw_rectangle(int(cx - 1), int(bar.y - 2), 2, int(bar.height + 4), self._color((255, 255, 255), 120)) + fill = bar.width / 2 * float(np.sqrt(np.clip(abs(pull) / FULL_BAR, 0.0, 1.0))) + if fill > 1: + fill_x = cx if pull > 0 else cx - fill + rl.draw_rectangle_rounded(rl.Rectangle(fill_x, bar.y, fill, bar.height), 1.0, 6, + self._color(BLENDFUL_COLOR, np.clip(0.3 + 0.7 * trust, 0.0, 1.0) * 255)) diff --git a/openpilot/selfdrive/ui/sunnypilot/onroad/model_renderer.py b/openpilot/selfdrive/ui/sunnypilot/onroad/model_renderer.py index 4be82bef4a..92b2c8c8d2 100644 --- a/openpilot/selfdrive/ui/sunnypilot/onroad/model_renderer.py +++ b/openpilot/selfdrive/ui/sunnypilot/onroad/model_renderer.py @@ -5,10 +5,11 @@ This file is part of sunnypilot and is licensed under the MIT License. See the LICENSE.md file in the root directory for more details. """ from openpilot.common.filter_simple import FirstOrderFilter -from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus +from openpilot.selfdrive.ui.ui_state import ui_state +from openpilot.selfdrive.ui.sunnypilot.onroad.blendful import BlendfulRenderer from openpilot.selfdrive.ui.sunnypilot.onroad.chevron_metrics import ChevronMetrics from openpilot.selfdrive.ui.sunnypilot.onroad.rainbow_path import RainbowPath -from openpilot.selfdrive.ui.sunnypilot.ui_state import MADSState +from openpilot.selfdrive.ui.sunnypilot.ui_state import UIStateSP from openpilot.system.ui.lib.application import gui_app @@ -16,16 +17,12 @@ class ModelRendererSP: def __init__(self): self.rainbow_path = RainbowPath() self.chevron_metrics = ChevronMetrics() + self.blendful = BlendfulRenderer(readout=False) self._width_filter = FirstOrderFilter(0.9, 0.1, 1 / gui_app.target_fps) @property def _lateral_active(self) -> bool: - sm = ui_state.sm - if sm.valid["selfdriveStateSP"]: - mads = sm["selfdriveStateSP"].mads - if mads.available: - return mads.enabled and mads.state != MADSState.paused - return ui_state.status in (UIStatus.ENGAGED, UIStatus.LAT_ONLY) + return UIStateSP.lateral_active(ui_state) def _get_path_half_width(self) -> float: target = 0.9 if self._lateral_active else 0.40 diff --git a/openpilot/selfdrive/ui/sunnypilot/ui_state.py b/openpilot/selfdrive/ui/sunnypilot/ui_state.py index e5ddd70400..521d1cee34 100644 --- a/openpilot/selfdrive/ui/sunnypilot/ui_state.py +++ b/openpilot/selfdrive/ui/sunnypilot/ui_state.py @@ -36,7 +36,7 @@ class UIStateSP: self.is_sp_release: bool = self.params.get_bool("IsReleaseSpBranch") self.sm_services_ext = [ "modelManagerSP", "selfdriveStateSP", "longitudinalPlanSP", "backupManagerSP", - "lateralTorqueParameters", "carStateSP", "liveMapDataSP", "carParamsSP", "lateralDelay" + "lateralTorqueParameters", "carStateSP", "liveMapDataSP", "carParamsSP", "lateralDelay", "modelDataV2SP" ] self.sunnylink_state = SunnylinkState() @@ -46,6 +46,7 @@ class UIStateSP: self.active_bundle = None self.model_runner_tinygrad: bool = False + self.active_runner_tinygrad: bool = False self.blindspot: bool = False self.chevron_metrics = None self.custom_interactive_timeout: int = 0 @@ -109,6 +110,14 @@ class UIStateSP: def auto_onroad_brightness(self) -> bool: return self.onroad_brightness in (OnroadBrightness.AUTO, OnroadBrightness.AUTO_DARK) + @staticmethod + def lateral_active(_ui_state) -> bool: + if _ui_state.sm.valid["selfdriveStateSP"]: + mads = _ui_state.sm["selfdriveStateSP"].mads + if mads.available: + return mads.enabled and mads.state != MADSState.paused + return _ui_state.status.value in ("engaged", "lat_only") + @staticmethod def update_status(ss, ss_sp, onroad_evt) -> str: state = ss.state @@ -157,6 +166,8 @@ class UIStateSP: chestnut_loading=self.chestnut_loading, offroad=self.is_offroad()) self.active_bundle = self.params.get(ACTIVE_BUNDLE_KEYS[source]) self.model_runner_tinygrad = self.active_bundle is not None and self.active_bundle.get("runner") == "tinygrad" + # runner the manager launches (process_config.is_tinygrad_model); with a chestnut the big slot decides + self.active_runner_tinygrad = self.params.get("ModelRunnerTypeCache") == custom.ModelManagerSP.Runner.tinygrad # stock only counts the default big model's compiled pkl. a downloaded big bundle runs on the # chestnut just the same, so ChestnutState has to see it as available too. self.chestnut_compiled = self.chestnut_compiled or self.model_runner_tinygrad diff --git a/openpilot/sunnypilot/modeld_v2/blendful.py b/openpilot/sunnypilot/modeld_v2/blendful.py new file mode 100644 index 0000000000..f787fcaf07 --- /dev/null +++ b/openpilot/sunnypilot/modeld_v2/blendful.py @@ -0,0 +1,145 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" +import numpy as np + +from openpilot.common.filter_simple import FirstOrderFilter +from openpilot.common.realtime import DT_MDL +from openpilot.sunnypilot.modeld_v2.constants import ModelConstants + +LANE_WIDTH_DEFAULT = 3.6 # m +LANE_WIDTH_CLIP = (2.8, 4.5) # m +LOOKAHEAD_TIME = 1.5 # s +MIN_LOOKAHEAD = 10.0 # m +MIN_SPEED = 5.0 # m/s +LINE_STD = (0.2, 0.6) +POINT_STD = 0.1 +YIELD = (0.2, 0.45) +X_IDXS = np.array(ModelConstants.X_IDXS) + + +class Blendful: + def __init__(self): + self.lane_width = FirstOrderFilter(LANE_WIDTH_DEFAULT, 10.0, DT_MDL, initialized=False) + self.line_gone = FirstOrderFilter(np.zeros(2), 2.0, DT_MDL) + self.line_probs_slow = FirstOrderFilter(np.zeros(2), 3.0, DT_MDL) + self.line_probs = FirstOrderFilter(np.zeros(2), 0.5, DT_MDL) + self.usual_offset = FirstOrderFilter(0.0, 10.0, DT_MDL, initialized=False) + self.going_around = False + self.since_turning = 0.0 + self.trust = 0.0 + self.line_trust = 0.0 + self.dk = 0.0 # delta kappa; difference between lane center and model plan + self.l_prob = 0.0 + self.r_prob = 0.0 + self.model_curvature = 0.0 + self.smooth = 0.0 # smooths the correction target below .53 lat accel + self.curvature = 0.0 + + def update(self, model_output: dict[str, np.ndarray], v_ego: float, active: bool, model_curvature: float, enabled: bool = True) -> float: + self.l_prob = self.r_prob = 0.0 + self.model_curvature = model_curvature + if not enabled: + if self.curvature or self.trust: + self.__init__() + return 0.0 + fading = not active or v_ego < MIN_SPEED + if fading: + self.trust = self.line_trust = 0.0 + self.usual_offset.initialized = False + self.lane_width.initialized = False + self.going_around = False + target = 0.0 if fading else self._target(model_output, v_ego, model_curvature) + target = self.smooth = self.smooth + (target - self.smooth) * DT_MDL / (DT_MDL + np.interp(abs(model_curvature) * v_ego ** 2, [0.18, 0.53], [0.5, 0.0])) + build = 0.5 / max(v_ego, 1.0) ** 2 * DT_MDL # jerk over 1 second + release = (0.5 if fading else 2.0) / max(v_ego, 1.0) ** 2 * DT_MDL # lets go fast, but past zero it builds like any other pull + k = self.curvature + self.curvature = float(np.clip(target, max(k - release, -build) if k > 0 else k - build, min(k + release, build) if k < 0 else k + build)) + return self.curvature + + def _target(self, model_output: dict[str, np.ndarray], v_ego: float, model_curvature: float) -> float: + lookahead = max(v_ego * LOOKAHEAD_TIME, MIN_LOOKAHEAD) + lines = model_output['lane_lines'][0, :, :, 0] + stds = model_output['lane_lines_stds'][0, :, :, 0] + + lone_weight = np.interp(abs(model_curvature) * v_ego ** 2, [0.18, 0.53], [0.5, 1.0]) # 0.5 on straights, 1.0 in curves + probs = self.line_probs.update(model_output['lane_lines_prob'][0, 1::2][1:3]) + absent = (probs < 0.3) & (lone_weight > 0.5) + gone = self.line_gone.update((model_output['lane_lines_prob'][0, 1::2][1:3] < 0.3).astype(float)) + missing = np.where(absent, 1.0, gone) + dropping = bool(np.any((probs < 0.6 * self.line_probs_slow.update(model_output['lane_lines_prob'][0, 1::2][1:3])) & ~absent)) + l_prob = float(probs[0] * np.interp(np.interp(0.5 * lookahead, X_IDXS, stds[1]), LINE_STD, [1.0, 0.0])) + r_prob = float(probs[1] * np.interp(np.interp(0.5 * lookahead, X_IDXS, stds[2]), LINE_STD, [1.0, 0.0])) + self.l_prob, self.r_prob = l_prob, r_prob + # one line steadily on its own still centers at lone_weight. a line that is disappearing gives control back to laneless + if missing[1] > 0.8 and l_prob > 0.5: + lane_trust = l_prob * lone_weight + elif missing[0] > 0.8 and r_prob > 0.5: + lane_trust = r_prob * lone_weight + elif dropping: + lane_trust = min(l_prob, r_prob) + else: + lane_trust = l_prob + r_prob - l_prob * r_prob + if lone_weight > 0.5 and lane_trust < self.line_trust: + lane_trust = self.line_trust + (lane_trust - self.line_trust) * DT_MDL / (lone_weight - 0.5 + DT_MDL) + self.line_trust = lane_trust + if lane_trust <= 0.0: + self.trust = 0.0 + return 0.0 + if l_prob + r_prob <= 0.0: # keep the last fit while trust lets go + self.trust = min(self.trust, lane_trust) + return float(self.trust * self.dk) + + win = (X_IDXS >= 0.5 * lookahead) & (X_IDXS <= 1.5 * lookahead) + x = X_IDXS[win] + half_width = self.lane_width.x / 2 + if l_prob * r_prob > 0.5: + width = np.interp(lookahead, X_IDXS, lines[2]) - np.interp(lookahead, X_IDXS, lines[1]) + self.lane_width.update(float(np.clip(width, *LANE_WIDTH_CLIP))) + half_width = np.clip(lines[2, win] - lines[1, win], *LANE_WIDTH_CLIP) / 2 + l_var = stds[1, win] ** 2 / max(l_prob, 1e-4) + r_var = stds[2, win] ** 2 / max(r_prob, 1e-4) + center = (r_var * (lines[1, win] + half_width) + l_var * (lines[2, win] - half_width)) / (l_var + r_var) + weight = 1.0 / (l_var * r_var / (l_var + r_var) + POINT_STD ** 2) + + # Fit curvature to place the plant in center: e ~ dk x^2 / 2 + plan_x = np.interp(x, model_output['plan'][0, :, 0], model_output['plan'][0, :, 1]) + plan_share = np.interp(abs(model_curvature) * lookahead ** 2 / 2, [0.5, 2.0], [0.3, 0.5]) # up to 50% plan y vs curvature parabola in sharp curves + error = np.clip(center - ((1 - plan_share) * model_curvature * x ** 2 / 2 + plan_share * plan_x), -1.0, 1.0) + dk = 2.0 * np.sum(weight * error * x ** 2) / np.sum(weight * x ** 4) + + # A model planning to end up further from its usual spot in the lane than the plant is now is likely going around something + hw_now = np.clip(lines[2, 0] - lines[1, 0], *LANE_WIDTH_CLIP) / 2 if l_prob * r_prob > 0.5 else self.lane_width.x / 2 + center_now = (l_prob * (lines[1, 0] + hw_now) + r_prob * (lines[2, 0] - hw_now)) / (l_prob + r_prob) + plan_y = np.interp(lookahead, model_output['plan'][0, :, 0], model_output['plan'][0, :, 1]) + offset = plan_y - np.interp(lookahead, x, center) + if min(gone) > 0.5: # both lines gone: turns, intersections, relearn ideal model position + self.usual_offset.initialized = False + usual = self.usual_offset.x if self.usual_offset.initialized else offset + swerve = abs(offset - usual) - 0.25 * abs(model_curvature) * lookahead ** 2 / 2 + away = swerve - abs(-center_now - usual) + lane_curvature = 2.0 * np.polyfit(x, center, 2)[0] + inside_curve = abs(lane_curvature) * lookahead ** 2 / 2 > 0.1 and (offset - usual) * lane_curvature > 0 + self.since_turning = 0.0 if abs(model_curvature) * lookahead ** 2 / 2 > 0.5 else self.since_turning + DT_MDL + if inside_curve: + self.going_around = False + elif away > YIELD[0] and self.since_turning > 2.0: + self.going_around = True + elif swerve < YIELD[0] / 2: + self.going_around = False + self.usual_offset.update_alpha(20.0 if self.going_around else 10.0) + self.usual_offset.update(offset) + + if inside_curve: + lane_trust *= np.interp(abs(offset) - abs(center_now) - 0.25 * abs(model_curvature) * lookahead ** 2 / 2, (0.5, 1.0), [1.0, 0.0]) + else: + lane_trust *= np.interp(swerve, (YIELD[0] / 2, YIELD[0]), [1.0, 0.0]) if self.going_around else np.interp(away, YIELD, [1.0, 0.0]) + + if lane_trust < self.trust: + dk = float(np.clip(dk, min(self.dk, 0.0), max(self.dk, 0.0))) + self.trust = lane_trust if lane_trust < self.trust else self.trust + (lane_trust - self.trust) * DT_MDL / (1.5 + DT_MDL) + self.dk = dk + return float(self.trust * dk) diff --git a/openpilot/sunnypilot/modeld_v2/modeld.py b/openpilot/sunnypilot/modeld_v2/modeld.py index dda0f834a4..ee8e538e76 100755 --- a/openpilot/sunnypilot/modeld_v2/modeld.py +++ b/openpilot/sunnypilot/modeld_v2/modeld.py @@ -7,6 +7,7 @@ See the LICENSE.md file in the root directory for more details. """ from collections.abc import Callable +from functools import partial import gc import os os.environ['GMMU'] = '0' @@ -39,6 +40,7 @@ from openpilot.selfdrive.modeld.modeld import ChestnutGpuState from openpilot.sunnypilot.modeld_v2.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState, get_curvature_from_output from openpilot.sunnypilot.modeld_v2.parse_model_outputs import Parser +from openpilot.sunnypilot.modeld_v2.blendful import Blendful from openpilot.sunnypilot.modeld_v2.constants import ModelConstants, Plan from openpilot.sunnypilot.modeld_v2.meta_helper import load_meta_constants from openpilot.sunnypilot.modeld_v2.camera_offset_helper import CameraOffsetHelper @@ -228,7 +230,8 @@ class ModelState(ModelStateBase): return outputs def get_action_from_model(self, model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action, - lat_action_t: float, long_action_t: float, v_ego: float) -> log.ModelDataV2.Action: + lat_action_t: float, long_action_t: float, v_ego: float, + blendful: Callable[[float], float] | None = None) -> log.ModelDataV2.Action: if 'action' not in model_output: plan = model_output['plan'][0] desired_accel = get_accel_from_plan(plan[:, Plan.VELOCITY][:, 0], plan[:, Plan.ACCELERATION][:, 0], self.constants.T_IDXS, @@ -240,6 +243,8 @@ class ModelState(ModelStateBase): else: desired_accel = model_output['action'][0, 1] desired_curvature = model_output['action'][0, 0] / (max(1.0, v_ego))**2 + if blendful is not None: + desired_curvature += blendful(desired_curvature) stop = v_ego < 0.3 and desired_accel < 0.1 desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, self.LONG_SMOOTH_SECONDS) @@ -366,6 +371,8 @@ def main(demo=False): DH = DesireHelper() meta_constants = load_meta_constants() RELC = RoadEdgeLaneChangeController() + blendful = Blendful() + blendful_enabled = False while True: # Keep receiving frames until we are at least 1 frame ahead of previous extra frame @@ -409,6 +416,7 @@ def main(demo=False): model.lat_delay = get_lat_delay(params, sm["lateralDelay"].lateralDelay) model.PLANPLUS_CONTROL = params.get("PlanplusControl", return_default=True) camera_offset_helper.set_offset(params.get("CameraOffset", return_default=True)) + blendful_enabled = params.get_bool("Blendful") lat_delay = model.lat_delay + model.LAT_SMOOTH_SECONDS if sm.updated["extrinsicsCalibration"] and sm.seen['narrowRoadCameraState'] and sm.seen['deviceState']: device_from_calib_euler = np.array(sm["extrinsicsCalibration"].rpyCalib, dtype=np.float32) @@ -482,7 +490,9 @@ def main(demo=False): if not sm['carControl'].latActive: prev_action.desiredCurvature = sm['controlsState'].curvature - action = model.get_action_from_model(model_output, prev_action, lat_action_t, long_action_t, v_ego) + blendful_active = desire == log.Desire.none and DH.lane_change_state == log.LaneChangeState.off + action = model.get_action_from_model(model_output, prev_action, lat_action_t, long_action_t, v_ego, + partial(blendful.update, model_output, v_ego, blendful_active, enabled=blendful_enabled)) prev_action = action fill_model_msg(drivingdata_send, modelv2_send, model_output, action, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id, @@ -499,6 +509,13 @@ def main(demo=False): modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction mdv2sp_send.valid = modelv2_send.valid mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction + mdv2sp_send.modelDataV2SP.blendfulCurvature = blendful.curvature + mdv2sp_send.modelDataV2SP.blendfulTrust = float(blendful.trust) + mdv2sp_send.modelDataV2SP.modelCurvature = float(blendful.model_curvature) + mdv2sp_send.modelDataV2SP.blendfulGoingAround = bool(blendful.going_around) + mdv2sp_send.modelDataV2SP.blendfulLeftProb = blendful.l_prob + mdv2sp_send.modelDataV2SP.blendfulRightProb = blendful.r_prob + mdv2sp_send.modelDataV2SP.blendfulLaneWidth = float(blendful.lane_width.x) drivingdata_send.drivingModelData.meta.laneChangeState = DH.lane_change_state drivingdata_send.drivingModelData.meta.laneChangeDirection = DH.lane_change_direction diff --git a/openpilot/sunnypilot/modeld_v2/tests/test_blendful.py b/openpilot/sunnypilot/modeld_v2/tests/test_blendful.py new file mode 100644 index 0000000000..28a6458320 --- /dev/null +++ b/openpilot/sunnypilot/modeld_v2/tests/test_blendful.py @@ -0,0 +1,112 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" +import numpy as np + +from openpilot.common.realtime import DT_MDL +from openpilot.common.test import OpenpilotTestCase +from openpilot.sunnypilot.modeld_v2.blendful import Blendful +from openpilot.sunnypilot.modeld_v2.constants import ModelConstants + +X = np.array(ModelConstants.X_IDXS) +BUILD, RELEASE = 0.5, 2.0 + + +def frame(center=0.0, plan=0.0, k=0.0, probs=(0.9, 0.9), stds=(0.1, 0.1), width=3.5): + road = k * X ** 2 / 2 + lines = np.zeros((1, 4, 33, 2)) + lane_stds = np.ones((1, 4, 33, 2)) + for i, off in enumerate((-1.5 * width, -width / 2, width / 2, 1.5 * width)): + lines[0, i, :, 0] = road + center + off + lane_stds[0, 1, :, 0], lane_stds[0, 2, :, 0] = stds + lane_probs = np.zeros((1, 8)) + lane_probs[0, 3], lane_probs[0, 5] = probs + path = np.zeros((1, 33, 3)) + path[0, :, 0] = X + path[0, :, 1] = road + plan * np.clip(X / 10, 0, 1) + return {'lane_lines': lines, 'lane_lines_stds': lane_stds, 'lane_lines_prob': lane_probs, 'plan': path} + + +def drive(bf, seconds, v=20.0, k=0.0, **kw): + out = [bf.update(frame(k=k, **kw), v, True, k) for _ in range(int(seconds / DT_MDL))] + return np.array(out) * v ** 2 # m/s^2 + + +class TestBlendful(OpenpilotTestCase): + def setup_method(self): + self.bf = Blendful() + + def test_centered_adds_nothing(self): + push = drive(self.bf, 5.0) + assert self.bf.trust > 0.9 + assert np.abs(push).max() < 1e-3 + + def test_pulls_toward_lane_center(self): + push = drive(self.bf, 5.0, center=0.5) + assert push[-1] > 0.02 # + y side, where the lane center is + assert self.bf.trust > 0.9 + assert np.all(np.diff(push) <= BUILD * DT_MDL + 1e-9) + + def test_flipping_sides_builds_slowly(self): + k = 0.002 # 0.8 m/s^2 at 20 m/s + drive(self.bf, 5.0, k=k, center=0.5) + push = drive(self.bf, 5.0, k=k, center=-0.5) + rate = np.diff(push) / DT_MDL + assert push[-1] < 0 + assert np.all(rate >= -RELEASE - 1e-6) + assert np.all(rate[push[:-1] <= 0] >= -BUILD - 1e-6) # once past zero it builds + crossing = push[1:][(push[:-1] > 0) & (push[1:] <= 0)] + assert len(crossing) and np.all(crossing >= -BUILD * DT_MDL - 1e-9) + + def test_weak_lines_drive_laneless(self): + clear = drive(Blendful(), 5.0, center=0.5)[-1] + faint = Blendful() + push = drive(faint, 5.0, center=0.5, probs=(0.1, 0.1)) + assert faint.trust < 0.25 and abs(push[-1]) < 0.25 * clear # faint lines = trust and push scale down + blurry = Blendful() + push = drive(blurry, 5.0, center=0.5, stds=(0.8, 0.8)) + assert blurry.trust == 0.0 and np.abs(push).max() < 1e-6 # lines too uncertain = laneless + + def test_lone_line_holds_through_a_blink_in_a_curve(self): + k, v = 1 / 20, 6.0 + drive(self.bf, 6.0, v=v, k=k, plan=0.4, probs=(0.9, 0.05)) + before = self.bf.trust + drive(self.bf, 0.25, v=v, k=k, plan=0.4, probs=(0.1, 0.05), stds=(1.0, 0.1)) + bottom = self.bf.trust + drive(self.bf, 2.0, v=v, k=k, plan=0.4, probs=(0.9, 0.05)) + assert before > 0.5 + assert bottom > 0.5 * before # keeps latched through flicker + assert self.bf.trust > 0.85 * before + + def test_yields_when_the_model_goes_around(self): + drive(self.bf, 6.0) + push = drive(self.bf, 3.0, plan=0.8) + assert self.bf.going_around + assert self.bf.trust < 0.05 + assert abs(push[-1]) < 0.01 + + def test_fit_does_not_grow_while_trust_falls(self): + drive(self.bf, 5.0, center=0.3) + for i in range(40): + dk, trust = self.bf.dk, self.bf.trust + p = 0.9 - 0.02 * i + self.bf.update(frame(center=0.3, plan=-0.02 * i, probs=(p, p)), 20.0, True, 0.0) + if self.bf.trust < trust: + assert abs(self.bf.dk) <= abs(dk) + 1e-12 + + def test_toggle_off_returns_zero_and_resets(self): + drive(self.bf, 5.0, center=0.5) + assert self.bf.curvature != 0.0 + assert self.bf.update(frame(center=0.5), 20.0, True, 0.0, enabled=False) == 0.0 + assert self.bf.curvature == 0.0 and self.bf.trust == 0.0 + + def test_fades_below_min_speed(self): + drive(self.bf, 5.0, v=10.0, center=0.5) + start = self.bf.curvature + out = np.array([self.bf.update(frame(center=0.5), 4.0, True, 0.0) for _ in range(100)]) + assert self.bf.trust == 0.0 + assert abs(out[0]) < abs(start) + assert abs(out[-1]) < 1e-6 diff --git a/openpilot/sunnypilot/sunnylink/capabilities.py b/openpilot/sunnypilot/sunnylink/capabilities.py index eba72f162e..a1dd7057b5 100644 --- a/openpilot/sunnypilot/sunnylink/capabilities.py +++ b/openpilot/sunnypilot/sunnylink/capabilities.py @@ -45,6 +45,7 @@ CAPABILITY_FIELDS = ( "subaru_has_sng", "hyundai_alpha_long_available", "toyota_tss2", + "custom_model_active", ) CAPABILITY_LABELS: dict[str, str] = { @@ -68,6 +69,7 @@ CAPABILITY_LABELS: dict[str, str] = { "subaru_has_sng": "Subaru Stop-and-Go available", "hyundai_alpha_long_available": "Hyundai Alpha Longitudinal available", "toyota_tss2": "Toyota TSS2 platform", + "custom_model_active": "Non-Default model selected", } # Explicit defaults for non-boolean capability fields @@ -145,6 +147,9 @@ def generate_capabilities(params: Params | None = None) -> dict: caps["is_development"] = params.get_bool("IsDevelopmentBranch") caps["stock_longitudinal"] = params.get_bool("ToyotaEnforceStockLongitudinal") + # runner the manager launches (process_config.is_tinygrad_model); with a chestnut the big slot decides + caps["custom_model_active"] = params.get("ModelRunnerTypeCache") == custom.ModelManagerSP.Runner.tinygrad + bundle = params.get("CarPlatformBundle") bundle_brand = _bundle_field(bundle, "brand") bundle_platform = _bundle_field(bundle, "platform") diff --git a/openpilot/sunnypilot/sunnylink/settings_ui.json b/openpilot/sunnypilot/sunnylink/settings_ui.json index c9d83bdbcb..ab1661bb05 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui.json +++ b/openpilot/sunnypilot/sunnylink/settings_ui.json @@ -2021,8 +2021,21 @@ { "id": "model_behavior", "title": "Model Behavior", - "description": "Lane desire and lead-vehicle awareness tuning", + "description": "Lane desire, Blendful lane centering, and lead-vehicle awareness tuning", "items": [ + { + "key": "Blendful", + "widget": "toggle", + "title": "Blendful (Alpha)", + "description": "Adds a gentle nudge toward lane center on top of the model's own steering, stronger when lane lines are clear. Yields around obstacles. Not available with the Default model.", + "enablement": [ + { + "type": "capability", + "field": "custom_model_active", + "equals": true + } + ] + }, { "key": "LaneTurnDesire", "widget": "toggle", diff --git a/openpilot/sunnypilot/sunnylink/settings_ui_src/_macros.yaml b/openpilot/sunnypilot/sunnylink/settings_ui_src/_macros.yaml index a771720831..aee8ac54c0 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui_src/_macros.yaml +++ b/openpilot/sunnypilot/sunnylink/settings_ui_src/_macros.yaml @@ -63,3 +63,7 @@ macros: release_branches_hide: - type: not condition: {type: capability, field: is_sp_release, equals: true} + + # Gate for custom model features based on active source. + custom_model: + - { type: capability, field: custom_model_active, equals: true } diff --git a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/models.yaml b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/models.yaml index bcb8b895b9..7d0ce4ebee 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/models.yaml +++ b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/models.yaml @@ -9,8 +9,14 @@ description: Driving model behavior and camera calibration sections: - id: model_behavior title: Model Behavior - description: Lane desire and lead-vehicle awareness tuning + description: Lane desire, Blendful lane centering, and lead-vehicle awareness tuning items: + - key: Blendful + widget: toggle + title: Blendful (Alpha) + description: Adds a gentle nudge toward lane center on top of the model's own steering, stronger when lane lines are clear. Yields around obstacles. Not available with the Default model. + enablement: + - $ref: '#/macros/custom_model' - key: LaneTurnDesire widget: toggle title: Use Lane Turn Desires