diff --git a/.github/workflows/build-single-tinygrad-model.yaml b/.github/workflows/build-single-tinygrad-model.yaml index 077077a714..5b10d8dd9f 100644 --- a/.github/workflows/build-single-tinygrad-model.yaml +++ b/.github/workflows/build-single-tinygrad-model.yaml @@ -117,6 +117,10 @@ on: required: false type: string default: 'sunnypilot/sunnypilot-models' + tinygrad_ref: + description: 'Full tinygrad commit sha to convert a precompiled upstream pkl with, when it differs from the upstream pin' + required: false + type: string env: RECOMPILED_DIR: recompiled${{ inputs.recompiled_dir }} JSON_FILE: docs/docs/driving_models_${{ inputs.target_hardware == 'chestnut' && 'chestnut_v' || 'v' }}${{ inputs.json_version }}.json @@ -130,6 +134,7 @@ jobs: is_20hz: ${{ inputs.is_20hz }} artifact_suffix: ${{ inputs.artifact_suffix }} target_hardware: ${{ inputs.target_hardware }} + tinygrad_ref: ${{ inputs.tinygrad_ref }} secrets: inherit publish_model: diff --git a/.github/workflows/sunnypilot-build-model.yaml b/.github/workflows/sunnypilot-build-model.yaml index 8276c40136..c1ae211e44 100644 --- a/.github/workflows/sunnypilot-build-model.yaml +++ b/.github/workflows/sunnypilot-build-model.yaml @@ -35,6 +35,10 @@ on: required: false type: string default: 'qcom' + tinygrad_ref: + description: 'Full tinygrad commit sha to convert a precompiled upstream pkl with, when it differs from the upstream pin' + required: false + type: string workflow_dispatch: inputs: upstream_branch: @@ -58,6 +62,10 @@ on: options: - qcom - chestnut + tinygrad_ref: + description: 'Full tinygrad commit sha to convert a precompiled upstream pkl with, when it differs from the upstream pin' + required: false + type: string default: 'qcom' @@ -224,11 +232,13 @@ jobs: if [ -f "${{ env.MODELS_DIR }}/upstream_pkl.txt" ]; then read -r TG_URL TG_SHA _ < "${{ env.MODELS_DIR }}/upstream_pkl.txt" + if [ -n "${{ inputs.tinygrad_ref }}" ]; then TG_SHA="${{ inputs.tinygrad_ref }}"; fi echo "Converting commaai's precompiled model with tinygrad $TG_SHA" UPSTREAM_TG="${{ runner.temp }}/upstream_tinygrad" - rm -rf "$UPSTREAM_TG" && git init -q "$UPSTREAM_TG" && git -C "$UPSTREAM_TG" fetch -q --depth 1 "$TG_URL" "$TG_SHA" && git -C "$UPSTREAM_TG" checkout -q FETCH_HEAD + rm -rf "$UPSTREAM_TG" && git init -q "$UPSTREAM_TG" && git -C "$UPSTREAM_TG" fetch -q --depth 1 "$TG_URL" "$TG_SHA" && git -C "$UPSTREAM_TG" checkout -q FETCH_HEAD \ + || { echo "::error::could not fetch tinygrad $TG_SHA from $TG_URL (a full 40-character commit sha is required)"; exit 1; } CONVERTED="${{ runner.temp }}/big_driving_tinygrad_plain.pkl" - env DEV=USB+AMD PYTHONPATH="$UPSTREAM_TG:${{ github.workspace }}" taskset -c 7 python3 openpilot/sunnypilot/modeld_v2/convert_upstream_pkl.py \ + env DEV=USB+AMD PYTHONPATH="$UPSTREAM_TG:${{ github.workspace }}" taskset -c 7 python3 -u -X faulthandler openpilot/sunnypilot/modeld_v2/convert_upstream_pkl.py \ "${{ env.MODELS_DIR }}/big_driving_tinygrad.pkl" "$CONVERTED" mv "$CONVERTED" "$OUTPUT_PKL" elif NEW_ARCH_ONNX=$(find "${{ env.MODELS_DIR }}" -maxdepth 1 -name "${{ inputs.target_hardware == 'chestnut' && 'big_' || '' }}*driving*.onnx" -print -quit) && 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 275af5a2f5..823958d1ea 100644 --- a/openpilot/common/params_keys.h +++ b/openpilot/common/params_keys.h @@ -258,6 +258,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..f9c1dc161e 100644 --- a/openpilot/selfdrive/ui/onroad/model_renderer.py +++ b/openpilot/selfdrive/ui/onroad/model_renderer.py @@ -123,6 +123,8 @@ class ModelRenderer(Widget, ChevronMetrics, ModelRendererSP): # Draw elements self._draw_lane_lines() self._draw_path() + if self._lateral_active: + self.blendful.draw(self._rect) if render_lead_indicator and radar_state: self._draw_lead_indicator() @@ -172,6 +174,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..28a83598b8 100644 --- a/openpilot/selfdrive/ui/sunnypilot/layouts/settings/models.py +++ b/openpilot/selfdrive/ui/sunnypilot/layouts/settings/models.py @@ -102,13 +102,18 @@ class ModelsLayout(Widget): self.lagd_toggle = toggle_item_sp(tr("Live Learning Steer Delay"), "", param="LagdToggle") + self.blendful_toggle = toggle_item_sp(tr("Blendful"), + tr("Nudges the car toward the lane center when the lane lines are clear. Drives laneless when they are not."), + 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 +327,7 @@ 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) + self.blendful_toggle.action_item.set_state(ui_state.params.get_bool("Blendful")) 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..6440accd0c 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("blendful", "Blendful", + description="Nudges the car toward the lane center when the lane lines are clear.\n" + + "Drives laneless when they are not.") + + 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() diff --git a/openpilot/selfdrive/ui/sunnypilot/mici/onroad/model_renderer.py b/openpilot/selfdrive/ui/sunnypilot/mici/onroad/model_renderer.py index e3de9401ce..b185693584 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() diff --git a/openpilot/selfdrive/ui/sunnypilot/onroad/blendful.py b/openpilot/selfdrive/ui/sunnypilot/onroad/blendful.py new file mode 100644 index 0000000000..bd4b8511fa --- /dev/null +++ b/openpilot/selfdrive/ui/sunnypilot/onroad/blendful.py @@ -0,0 +1,90 @@ +""" +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.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): + self._readout = readout + 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 self._center_points.size and self._trust > 0.05: + draw_polygon(rect, self._center_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..db868d777a 100644 --- a/openpilot/selfdrive/ui/sunnypilot/onroad/model_renderer.py +++ b/openpilot/selfdrive/ui/sunnypilot/onroad/model_renderer.py @@ -6,6 +6,7 @@ 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.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 @@ -16,6 +17,7 @@ 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 diff --git a/openpilot/selfdrive/ui/sunnypilot/ui_state.py b/openpilot/selfdrive/ui/sunnypilot/ui_state.py index 04972b3ded..8550e883d5 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() diff --git a/openpilot/sunnypilot/modeld_v2/blendful.py b/openpilot/sunnypilot/modeld_v2/blendful.py new file mode 100644 index 0000000000..32209f45c6 --- /dev/null +++ b/openpilot/sunnypilot/modeld_v2/blendful.py @@ -0,0 +1,140 @@ +""" +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 # smoothens lanelines 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) -> float: + self.l_prob = self.r_prob = 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 + self.model_curvature = model_curvature + 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]) # blending up to 50% plan with action during sharp curvature + 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..5ad1732437 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 = blendful_enabled and 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)) 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/sunnylink/settings_ui.json b/openpilot/sunnypilot/sunnylink/settings_ui.json index e34878767d..1014cba271 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui.json +++ b/openpilot/sunnypilot/sunnylink/settings_ui.json @@ -2034,8 +2034,14 @@ { "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 Mode", + "description": "Nudges the car toward the lane center when the lane lines are clear. Drives laneless when they are not." + }, { "key": "LaneTurnDesire", "widget": "toggle", diff --git a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/models.yaml b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/models.yaml index bcb8b895b9..741b348019 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/models.yaml +++ b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/models.yaml @@ -9,8 +9,12 @@ 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 Mode + description: Nudges the car toward the lane center when the lane lines are clear. Drives laneless when they are not. - key: LaneTurnDesire widget: toggle title: Use Lane Turn Desires