diff --git a/scripts/launch_onroad_desktop.sh b/scripts/launch_onroad_desktop.sh index ab011dda9..98443248c 100755 --- a/scripts/launch_onroad_desktop.sh +++ b/scripts/launch_onroad_desktop.sh @@ -27,13 +27,14 @@ env_var_truthy() { usage() { cat <<'EOF' Usage: - ./onroad [jobs] (--c3 | --c4 | --raybig | --all | --replay-only) [-nav] [--mici-widget-demo] [--prefix name] + ./onroad [jobs] (--c3 | --c4 | --raybig | --all | --replay-only) [-nav] [--cem] [--prefix name] Examples: ./onroad --c3 ./onroad --c4 --start 30 ./onroad --c4 -nav - ./onroad --c4 --mici-widget-demo --demo + ./onroad --c4 --cem --demo + ./onroad --raybig --cem --demo ./onroad --all ./onroad --replay-only --demo --no-vipc --no-loop @@ -42,7 +43,7 @@ Notes: - A private comma connect route still requires tools/lib/auth.py before replay can download it. - Use multiple UI flags together if you want more than one desktop UI at once. - -nav injects a fake navigation demo stream and blocks replay from publishing navInstruction/navRoute. - - --mici-widget-demo makes the small C4 sidebar cycle through widget states for visual review. + - --cem publishes fake CEM statuses for desktop visual review in the raylib UIs. EOF } @@ -58,9 +59,10 @@ UI_TARGETS=() LEGACY_UI_SELECTION="" REPLAY_ONLY=0 NAV_DEMO=0 -MICI_WIDGET_DEMO=0 +CEM_DEMO=0 REPLAY_PID="" NAV_PID="" +CEM_PID="" UI_PIDS=() parse_args() { @@ -94,8 +96,8 @@ parse_args() { NAV_DEMO=1 shift ;; - --mici-widget-demo|--widget-demo) - MICI_WIDGET_DEMO=1 + --cem|--mici-widget-demo|--widget-demo) + CEM_DEMO=1 shift ;; --ui) @@ -212,6 +214,9 @@ cleanup() { if [[ -n "${NAV_PID}" ]]; then kill "${NAV_PID}" >/dev/null 2>&1 || true fi + if [[ -n "${CEM_PID}" ]]; then + kill "${CEM_PID}" >/dev/null 2>&1 || true + fi for pid in "${UI_PIDS[@]-}"; do if [[ -n "${pid}" ]]; then @@ -224,6 +229,9 @@ cleanup() { if [[ -n "${NAV_PID}" ]]; then wait "${NAV_PID}" >/dev/null 2>&1 || true fi + if [[ -n "${CEM_PID}" ]]; then + wait "${CEM_PID}" >/dev/null 2>&1 || true + fi if [[ -n "${OPENPILOT_PREFIX:-}" && "${OPENPILOT_PREFIX}" == desktop-onroad-* ]]; then echo "Cleaning up temporary prefix environment (${OPENPILOT_PREFIX})..." @@ -322,7 +330,7 @@ prepare_env() { export SP_RAYBIG_FAKE_WIFI=0 export SP_ALLOW_DESKTOP_FAKE_WIFI=0 export SP_ONROAD_NAV_DEMO="${NAV_DEMO}" - export SP_MICI_WIDGET_DEMO="${MICI_WIDGET_DEMO}" + export SP_CEM_DEMO="${CEM_DEMO}" if [[ "$(uname -s)" == "Darwin" ]] || env_var_truthy "${ZMQ:-0}"; then export OPENPILOT_ZMQ_NAMESPACE="${PREFIX_ARG:-${OPENPILOT_ZMQ_NAMESPACE:-desktop-onroad-$$}}" @@ -403,6 +411,18 @@ launch_nav_demo() { fi } +launch_cem_demo() { + echo "Starting fake CEM demo publisher..." + "${ROOT_DIR}/.venv/bin/python3" "${ROOT_DIR}/tools/replay/fake_cem_demo.py" & + CEM_PID=$! + + sleep 0.5 + if ! kill -0 "${CEM_PID}" >/dev/null 2>&1; then + wait "${CEM_PID}" + return 1 + fi +} + launch_c3_ui() { local os_ext="linux" if [[ "$(uname -s)" == "Darwin" ]]; then @@ -496,6 +516,10 @@ if [[ "${NAV_DEMO}" == "1" ]]; then launch_nav_demo fi +if [[ "${CEM_DEMO}" == "1" ]]; then + launch_cem_demo +fi + if [[ ${#UI_TARGETS[@]} -eq 0 ]]; then echo "Replay is running without UI windows. Press Ctrl-C to stop." wait "${REPLAY_PID}" diff --git a/selfdrive/ui/mici/onroad/sidebar_widgets.py b/selfdrive/ui/mici/onroad/sidebar_widgets.py index e114c6f99..07cb37b0c 100644 --- a/selfdrive/ui/mici/onroad/sidebar_widgets.py +++ b/selfdrive/ui/mici/onroad/sidebar_widgets.py @@ -84,7 +84,7 @@ class MiciSidebarWidgets(Widget): self._confidence_ball = confidence_ball self._font_bold = gui_app.font(FontWeight.BOLD) self._font_semi_bold = gui_app.font(FontWeight.SEMI_BOLD) - self._demo = _env_truthy("SP_MICI_WIDGET_DEMO") + self._demo = _env_truthy("SP_CEM_DEMO") or _env_truthy("SP_MICI_WIDGET_DEMO") @property def demo_active(self) -> bool: @@ -195,12 +195,11 @@ class MiciSidebarWidgets(Widget): def _cem_reason(self) -> tuple[str, rl.Color]: if self._demo: - reason = DEMO_REASONS[int(rl.get_time() / DEMO_HOLD_SECONDS) % len(DEMO_REASONS)] - if reason == "chill": - return reason, WHITE - if reason == "stop": - return reason, TRAFFIC_RED - return reason, CEM_BLUE + status = ui_state.params_memory.get_int("CEStatus", default=CEStatus["OFF"]) + status_reason = self._ce_status_reason(status) + if status_reason is not None: + return status_reason + return self._fallback_demo_reason() conditional_experimental = ui_state.params.get_bool("ConditionalExperimental") conditional_chill = ui_state.params.get_bool("ConditionalChill") and not conditional_experimental @@ -217,6 +216,22 @@ class MiciSidebarWidgets(Widget): return "chill", WHITE status = ui_state.params_memory.get_int("CEStatus", default=CEStatus["OFF"]) if conditional_experimental else CEStatus["OFF"] + status_reason = self._ce_status_reason(status) + if status_reason is not None: + return status_reason + if self._model_stop_active(): + return "stop", TRAFFIC_RED + return "chill", WHITE + + def _fallback_demo_reason(self) -> tuple[str, rl.Color]: + reason = DEMO_REASONS[int(rl.get_time() / DEMO_HOLD_SECONDS) % len(DEMO_REASONS)] + if reason == "chill": + return reason, WHITE + if reason == "stop": + return reason, TRAFFIC_RED + return reason, CEM_BLUE + + def _ce_status_reason(self, status: int) -> tuple[str, rl.Color] | None: if status == CEStatus["CURVATURE"]: return "curve", CEM_BLUE if status == CEStatus["LEAD"]: @@ -231,9 +246,7 @@ class MiciSidebarWidgets(Widget): return "chill", WHITE if status == CEStatus["USER_OVERRIDDEN"]: return "chill", WHITE - if self._model_stop_active(): - return "stop", TRAFFIC_RED - return "chill", WHITE + return None def _draw_chill_icon(self, rect: rl.Rectangle) -> None: cx = rect.x + rect.width / 2 diff --git a/selfdrive/ui/onroad/hud_renderer.py b/selfdrive/ui/onroad/hud_renderer.py index e2f77df9c..23518c45a 100644 --- a/selfdrive/ui/onroad/hud_renderer.py +++ b/selfdrive/ui/onroad/hud_renderer.py @@ -87,7 +87,13 @@ class HudRenderer(Widget): car_state = sm['carState'] v_cruise_cluster = car_state.vCruiseCluster - v_cruise = controls_state.deprecated.vCruise if v_cruise_cluster == 0.0 else v_cruise_cluster + if v_cruise_cluster == 0.0: + try: + v_cruise = controls_state.vCruiseDEPRECATED + except AttributeError: + v_cruise = controls_state.deprecated.vCruise + else: + v_cruise = v_cruise_cluster offset = ui_state.starpilot_toggles.get("set_speed_offset", 0.0) self.set_speed = v_cruise + offset if (0 < v_cruise < SET_SPEED_NA) else v_cruise self.is_cruise_set = 0 < v_cruise < SET_SPEED_NA diff --git a/selfdrive/ui/onroad/starpilot/aethergauge.py b/selfdrive/ui/onroad/starpilot/aethergauge.py index c7d8f79c5..ab0eb70ee 100644 --- a/selfdrive/ui/onroad/starpilot/aethergauge.py +++ b/selfdrive/ui/onroad/starpilot/aethergauge.py @@ -1,9 +1,11 @@ from dataclasses import dataclass from enum import Enum import math +import os import pyray as rl from openpilot.common.constants import CV from openpilot.selfdrive.ui.ui_state import ui_state +from openpilot.starpilot.common.experimental_state import CEStatus from openpilot.system.ui.lib.text_measure import measure_text_cached from openpilot.selfdrive.ui.onroad.starpilot.starpilot_border import _csc_state, _intensity, _glow_color @@ -57,6 +59,7 @@ COLOR_STOP_SIGN = rl.Color(196, 30, 58, 255) COLOR_STOP_SIGN_OUTLINE = rl.Color(255, 255, 255, 255) COLOR_STOP_LINE_GLOW = rl.Color(255, 30, 60, 255) COLOR_STOP_LINE_CORE = rl.Color(255, 200, 200, 255) +COLOR_CEM_SPEED = rl.Color(112, 192, 216, 255) # Set to True to force test-cycle mode (flip back to False before pushing) TEST_CYCLE = False @@ -70,6 +73,9 @@ def _speed_conversion() -> float: def _speed_unit() -> str: return "km/h" if ui_state.is_metric else "mph" +def _env_truthy(name: str) -> bool: + return os.getenv(name, "").lower() in {"1", "true", "yes", "on"} + def _to_display_speed(val: float) -> tuple[int, str]: return int(round(val * _speed_conversion())), _speed_unit() @@ -267,6 +273,7 @@ _TEST_STATES = [ (IndicatorType.FORCE_STOP, "", COLOR_FORCE_STOP, 12.0, "", ""), ] _TEST_CYCLE_SEC = 6.0 +CEM_DEMO = _env_truthy("SP_CEM_DEMO") or _env_truthy("SP_MICI_WIDGET_DEMO") def _test_cycle_active() -> bool: return rl.get_time() > 3.0 @@ -299,6 +306,63 @@ def _test_cycle_data() -> AetherGaugeData: indicator_extra=extra, reduction_text=reduction, is_numeric=True) +def _cem_demo_active() -> bool: + return CEM_DEMO and ui_state.conditional_status in { + CEStatus["CURVATURE"], + CEStatus["LEAD"], + CEStatus["STOP_LIGHT"], + CEStatus["SPEED"], + CEStatus["SPEED_LIMIT"], + } + + +def _cem_demo_data() -> AetherGaugeData: + status = ui_state.conditional_status + + if status == CEStatus["CURVATURE"]: + v_ego = _get_val("carState", "vEgo", 18.0) + target_speed = max(8.0, v_ego * 0.72) + v_cruise = max(target_speed + 4.0, _get_val("starpilotPlan", "vCruise", target_speed + 4.0)) + return _build_curve_gauge_data(0.005, target_speed, v_cruise) + + if status == CEStatus["LEAD"]: + if _sm_valid("radarState") and ui_state.sm["radarState"].leadOne.status: + return _lead_data() + is_stopped = int(rl.get_time() / 1.0) % 2 == 0 + return AetherGaugeData( + text="STOPPED" if is_stopped else "SLOW", + color=COLOR_LEAD_STOPPED if is_stopped else COLOR_LEAD_SLOWER, + indicator_type=IndicatorType.LEAD, + indicator_value=12.0 if is_stopped else 24.0, + indicator_extra="stopped" if is_stopped else "slower", + ) + + if status == CEStatus["STOP_LIGHT"]: + phase = rl.get_time() % 2.0 + distance = max(6.0, 35.0 - phase * 12.0) + v_ego = max(0.1, _get_val("carState", "vEgo", 10.0)) + return AetherGaugeData( + text=_time_to_stop(distance, v_ego), + unit="s", + color=COLOR_FORCE_STOP, + indicator_type=IndicatorType.STOP_LIGHT, + indicator_value=distance, + indicator_extra="red", + is_numeric=True, + ) + + v_ego = _get_val("carState", "vEgo", 20.0) + display_speed, unit = _to_display_speed(max(8.0, v_ego * 0.85)) + return AetherGaugeData( + text=str(display_speed), + unit=unit, + color=COLOR_CEM_SPEED, + indicator_type=IndicatorType.ROAD_CURVE, + indicator_value=0.0, + is_numeric=True, + ) + + # --- Main widget --- class AetherGauge: @@ -312,6 +376,8 @@ class AetherGauge: ] if TEST_CYCLE: self._sources.insert(0, (_test_cycle_active, _test_cycle_data)) + if CEM_DEMO: + self._sources.insert(0, (_cem_demo_active, _cem_demo_data)) self._chevron_accum = 0.0 self._lead_chevron_accum = 0.0 diff --git a/tools/replay/fake_cem_demo.py b/tools/replay/fake_cem_demo.py new file mode 100644 index 000000000..c8bd7af9f --- /dev/null +++ b/tools/replay/fake_cem_demo.py @@ -0,0 +1,57 @@ +#!/usr/bin/env python3 + +import argparse +import signal +import time +from itertools import cycle + +from openpilot.common.params import Params +from openpilot.starpilot.common.experimental_state import CEStatus, CE_STATUS_PARAM + + +SCENARIOS = [ + ("curve", CEStatus["CURVATURE"]), + ("lead", CEStatus["LEAD"]), + ("stop light", CEStatus["STOP_LIGHT"]), + ("speed", CEStatus["SPEED"]), +] + +running = True + + +def _handle_signal(_signum, _frame) -> None: + global running + running = False + + +def main() -> None: + parser = argparse.ArgumentParser(description="Publish fake CEM statuses for desktop onroad replay.") + parser.add_argument("--hold-seconds", type=float, default=2.0, help="How long each CEM status stays active.") + parser.add_argument("--publish-interval", type=float, default=0.25, help="Seconds between CEStatus writes.") + args = parser.parse_args() + + signal.signal(signal.SIGINT, _handle_signal) + signal.signal(signal.SIGTERM, _handle_signal) + + params_memory = Params(memory=True) + scenario_cycle = cycle(SCENARIOS) + label, status = next(scenario_cycle) + next_switch = time.monotonic() + args.hold_seconds + print(f"showing CEM: {label}", flush=True) + + try: + while running: + now = time.monotonic() + if now >= next_switch: + label, status = next(scenario_cycle) + next_switch = now + args.hold_seconds + print(f"showing CEM: {label}", flush=True) + + params_memory.put_int(CE_STATUS_PARAM, status) + time.sleep(args.publish_interval) + finally: + params_memory.put_int(CE_STATUS_PARAM, CEStatus["OFF"]) + + +if __name__ == "__main__": + main()