init

openpilot/common/params_keys.h

@@ -249,6 +249,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"LagdValueCache", {PERSISTENT, FLOAT, "0.2"}},
     {"LaneTurnDesire", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"LaneTurnValue", {PERSISTENT | BACKUP, FLOAT, "19.0"}},
+    {"LanefulStrength", {PERSISTENT | BACKUP, FLOAT, "0.0"}},
     {"PlanplusControl", {PERSISTENT | BACKUP, FLOAT, "1.0"}},
 
     // mapd

openpilot/selfdrive/ui/mici/onroad/model_renderer.py

@@ -153,6 +153,7 @@ class ModelRenderer(Widget, ModelRendererSP):
     if ui_state.status != UIStatus.DISENGAGED:
       self._draw_lane_lines()
       self._draw_path(sm)
+      self.laneful.draw(self._rect)
 
     # if render_lead_indicator and radar_state:
     #   self._draw_lead_indicator()
@@ -204,6 +205,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.laneful.update(self, max_idx)
 
     # Update path using raw points
     if lead and lead.present:

openpilot/selfdrive/ui/sunnypilot/mici/onroad/laneful.py

@@ -0,0 +1,107 @@
+"""
+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.
+
+Laneful visuals: cyan lane-center target, thin white "after" path (model path bent by laneful's nudge),
+and a top-center readout of the pull. Mirrors the math in sunnypilot/modeld_v2/laneful.py.
+"""
+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.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
+
+CENTER_COLOR = (0, 200, 255)
+AFTER_COLOR = (255, 255, 255)
+# keep in sync with sunnypilot/modeld_v2/laneful.py
+LOOKAHEAD_TIME = 1.5
+MIN_LOOKAHEAD = 10.0
+MAX_ERROR = 0.5
+
+
+class LanefulRenderer:
+  def __init__(self):
+    self._center_points = np.empty((0, 2), dtype=np.float32)
+    self._after_points = np.empty((0, 2), dtype=np.float32)
+    self._trust = 0.0
+    self._strength = 0.0
+    self._frame = 0
+    self._font = gui_app.font(FontWeight.BOLD)
+    # smoothed copies for the readout so the numbers don't flicker
+    self._pull_display = FirstOrderFilter(0.0, 0.5, DT_MDL)
+    self._trust_display = FirstOrderFilter(0.0, 0.5, DT_MDL)
+
+  def update(self, renderer, max_idx: int) -> None:
+    """Called from the model renderer's update with its lane lines and path already in raw points."""
+    if self._frame % 60 == 0:
+      self._strength = ui_state.params.get("LanefulStrength", return_default=True)
+    self._frame += 1
+
+    left, right = renderer._lane_lines[1].raw_points, renderer._lane_lines[2].raw_points
+    stds = list(ui_state.sm['modelV2'].laneLineStds)
+    if self._strength <= 0.0 or len(left) == 0 or len(right) != len(left) or len(stds) < 3:
+      self._center_points = np.empty((0, 2), dtype=np.float32)
+      self._after_points = np.empty((0, 2), dtype=np.float32)
+      return
+
+    probs = renderer._lane_line_probs
+    l_prob = float(probs[1] * np.interp(stds[1], [0.15, 0.3], [1.0, 0.0]))
+    r_prob = float(probs[2] * np.interp(stds[2], [0.15, 0.3], [1.0, 0.0]))
+    width = np.clip(right[:, 1] - left[:, 1], 2.8, 4.5) if l_prob * r_prob > 0.5 else 3.7
+    center = left.copy()
+    center[:, 1] = (l_prob * (left[:, 1] + width / 2) + r_prob * (right[:, 1] - width / 2)) / (l_prob + r_prob + 1e-4)
+    self._trust = l_prob + r_prob - l_prob * r_prob
+    self._center_points = renderer._map_line_to_polygon(center, 0.1, 0.0, max_idx)
+
+    # "after" path: model path bent toward center the way laneful's pure-pursuit term steers, ending at the lookahead
+    path = renderer._path.raw_points
+    lookahead = max(ui_state.sm['carState'].vEgo * LOOKAHEAD_TIME, MIN_LOOKAHEAD)
+    error = float(np.clip(np.interp(lookahead, center[:, 0], center[:, 1]) - np.interp(lookahead, path[:, 0], path[:, 1]),
+                          -MAX_ERROR, MAX_ERROR))
+    self._pull_display.update(self._trust * error)  # what laneful actually pulls, 0 when lines are untrusted
+    self._trust_display.update(self._trust)
+    after = path.copy()
+    after[:, 1] += self._strength * self._trust * error * (path[:, 0] / lookahead) ** 2
+    self._after_points = renderer._map_line_to_polygon(after, 0.15, renderer._path_offset_z,
+                                                       renderer._get_path_length_idx(path[:, 0], lookahead))
+
+  def draw(self, rect: rl.Rectangle) -> None:
+    if self._center_points.size and self._trust > 0.05:
+      draw_polygon(rect, self._center_points, rl.Color(*CENTER_COLOR, int(np.clip(self._trust, 0.2, 1.0) * 255)))
+    if self._after_points.size:
+      draw_polygon(rect, self._after_points, rl.Color(*AFTER_COLOR, 200))
+    if self._strength > 0.0:
+      self._draw_readout(rect)
+
+  def _draw_readout(self, rect: rl.Rectangle) -> None:
+    """Top-center pill: pull toward lane center in cm (capped at 50), arrow = side,
+    bar grows from center toward the pull (full at the cap), brighter with more lane line trust."""
+    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, rl.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, rl.WHITE)
+
+    active = rl.Color(*CENTER_COLOR, int(np.clip(abs(pull) / 0.1, 0.0, 1.0) * 255))
+    idle = rl.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, rl.Color(255, 255, 255, 40))
+    rl.draw_rectangle(int(cx - 1), int(bar.y - 2), 2, int(bar.height + 4), rl.Color(255, 255, 255, 120))
+    fill = bar.width / 2 * float(np.clip(abs(pull) / MAX_ERROR, 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,
+                                rl.Color(*CENTER_COLOR, int(np.clip(0.3 + 0.7 * trust, 0.0, 1.0) * 255)))

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.mici.onroad.laneful import LanefulRenderer
 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.laneful = LanefulRenderer()

openpilot/sunnypilot/modeld_v2/laneful.py

@@ -0,0 +1,79 @@
+"""
+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.
+
+Laneful: nudge the model's curvature toward the lane center seen by the lane lines.
+The model stays in charge; this only adds a pure-pursuit correction weighted by lane line confidence,
+and fades to zero (plain laneless) when the lines are weak.
+"""
+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, Plan
+
+LANE_WIDTH_DEFAULT = 3.7  # m
+LANE_WIDTH_CLIP = (2.8, 4.5)  # m
+LOOKAHEAD_TIME = 1.5  # s
+MIN_LOOKAHEAD = 10.0  # m
+MAX_ERROR = 0.5  # m, cap on how far laneful can pull
+MIN_SPEED = 9.0  # m/s
+
+
+class Laneful:
+  def __init__(self):
+    self.lane_width = FirstOrderFilter(LANE_WIDTH_DEFAULT, 10.0, DT_MDL)
+
+  def update(self, model_output: dict[str, np.ndarray], v_ego: float, strength: float, desire_active: bool) -> float:
+    """Returns a curvature offset to add to the model's raw desired curvature."""
+    if strength <= 0.0 or desire_active or v_ego < MIN_SPEED or 'lane_lines' not in model_output:
+      return 0.0
+
+    lookahead = max(v_ego * LOOKAHEAD_TIME, MIN_LOOKAHEAD)
+    lines = model_output['lane_lines'][0, :, :, 0]
+    left_y = np.interp(lookahead, ModelConstants.X_IDXS, lines[1])
+    right_y = np.interp(lookahead, ModelConstants.X_IDXS, lines[2])
+
+    # line confidence, discounted when the model is unsure where the line is
+    probs = model_output['lane_lines_prob'][0, 1::2]
+    stds = model_output['lane_lines_stds'][0, :, 0, 0]
+    l_prob = float(probs[1] * np.interp(stds[1], [0.15, 0.3], [1.0, 0.0]))
+    r_prob = float(probs[2] * np.interp(stds[2], [0.15, 0.3], [1.0, 0.0]))
+
+    if l_prob * r_prob > 0.5:
+      self.lane_width.update(float(np.clip(right_y - left_y, *LANE_WIDTH_CLIP)))
+    half_width = self.lane_width.x / 2
+
+    # one weak line -> hug the strong one at half a lane width
+    center_y = (l_prob * (left_y + half_width) + r_prob * (right_y - half_width)) / (l_prob + r_prob + 1e-4)
+    lane_trust = l_prob + r_prob - l_prob * r_prob
+
+    plan = model_output['plan'][0, :, Plan.POSITION]
+    plan_y = np.interp(lookahead, plan[:, 0], plan[:, 1])
+
+    error = np.clip(center_y - plan_y, -MAX_ERROR, MAX_ERROR)
+    return float(strength * lane_trust * 2.0 * error / lookahead ** 2)
+
+
+if __name__ == "__main__":
+  def fake_output(left_y, right_y, prob=0.9, std=0.1, plan_y=0.0):
+    x = np.array(ModelConstants.X_IDXS)
+    lines = np.zeros((1, 4, len(x), 2), dtype=np.float32)
+    lines[0, 1, :, 0], lines[0, 2, :, 0] = left_y, right_y
+    plan = np.zeros((1, len(x), 15), dtype=np.float32)
+    plan[0, :, 0], plan[0, :, 1] = x, plan_y
+    return {'lane_lines': lines, 'plan': plan,
+            'lane_lines_prob': np.full((1, 8), prob, dtype=np.float32),
+            'lane_lines_stds': np.full((1, 4, len(x), 2), std, dtype=np.float32)}
+
+  lf = Laneful()
+  assert abs(lf.update(fake_output(-1.85, 1.85), 25.0, 1.0, False)) < 1e-6, "centered -> no nudge"
+  assert lf.update(fake_output(-1.0, 2.7), 25.0, 1.0, False) > 0, "lane center to the +y side -> nudge toward +y"
+  assert lf.update(fake_output(-2.7, 1.0), 25.0, 1.0, False) < 0, "lane center to the -y side -> nudge toward -y"
+  assert lf.update(fake_output(-1.0, 2.7), 25.0, 1.0, True) == 0.0, "off during lane change/turn"
+  assert lf.update(fake_output(-1.0, 2.7, std=0.5), 25.0, 1.0, False) == 0.0, "blurry lines -> laneless"
+  assert lf.update(fake_output(-1.0, 2.7), 25.0, 0.0, False) == 0.0, "strength 0 -> off"
+  assert lf.update(fake_output(-1.0, 2.7), 25.0, 0.5, False) < lf.update(fake_output(-1.0, 2.7), 25.0, 1.0, False)
+  print("laneful self-check ok")

openpilot/sunnypilot/modeld_v2/modeld.py

@@ -37,6 +37,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.laneful import Laneful
 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
@@ -224,7 +225,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,
+                            laneful_curvature: float = 0.0) -> 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,
@@ -236,6 +238,7 @@ class ModelState(ModelStateBase):
     else:
       desired_accel = model_output['action'][0, 1]
       desired_curvature = model_output['action'][0, 0] / (max(1.0, v_ego))**2
+    desired_curvature += laneful_curvature
 
     stop = v_ego < 0.3 and desired_accel < 0.1
     desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, self.LONG_SMOOTH_SECONDS)
@@ -350,6 +353,8 @@ def main(demo=False):
   DH = DesireHelper()
   meta_constants = load_meta_constants()
   RELC = RoadEdgeLaneChangeController()
+  laneful = Laneful()
+  laneful_strength = 0.0
 
   while True:
     # Keep receiving frames until we are at least 1 frame ahead of previous extra frame
@@ -393,6 +398,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))
+      laneful_strength = params.get("LanefulStrength", return_default=True)
     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)
@@ -464,7 +470,9 @@ def main(demo=False):
       posenet_send = messaging.new_message('cameraOdometry')
       mdv2sp_send = messaging.new_message('modelDataV2SP')
 
-      action = model.get_action_from_model(model_output, prev_action, lat_action_t, long_action_t, v_ego)
+      laneful_curvature = laneful.update(model_output, v_ego, laneful_strength,
+                                         desire != log.Desire.none or 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, laneful_curvature)
       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,