diff --git a/openpilot/common/params_keys.h b/openpilot/common/params_keys.h index 82725e807e..019f3090ae 100644 --- a/openpilot/common/params_keys.h +++ b/openpilot/common/params_keys.h @@ -249,6 +249,7 @@ inline static std::unordered_map 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 diff --git a/openpilot/selfdrive/ui/mici/onroad/model_renderer.py b/openpilot/selfdrive/ui/mici/onroad/model_renderer.py index 805bdcc335..eaabd5f630 100644 --- a/openpilot/selfdrive/ui/mici/onroad/model_renderer.py +++ b/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: diff --git a/openpilot/selfdrive/ui/sunnypilot/mici/onroad/laneful.py b/openpilot/selfdrive/ui/sunnypilot/mici/onroad/laneful.py new file mode 100644 index 0000000000..e0c43951c9 --- /dev/null +++ b/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))) diff --git a/openpilot/selfdrive/ui/sunnypilot/mici/onroad/model_renderer.py b/openpilot/selfdrive/ui/sunnypilot/mici/onroad/model_renderer.py index e3de9401ce..b3471042f4 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.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() diff --git a/openpilot/sunnypilot/modeld_v2/laneful.py b/openpilot/sunnypilot/modeld_v2/laneful.py new file mode 100644 index 0000000000..c6ec77a79b --- /dev/null +++ b/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") diff --git a/openpilot/sunnypilot/modeld_v2/modeld.py b/openpilot/sunnypilot/modeld_v2/modeld.py index 2fd1baf999..161b9c9a37 100755 --- a/openpilot/sunnypilot/modeld_v2/modeld.py +++ b/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,