StarPilot/tools/longitudinal_maneuvers/maneuversd.py

#!/usr/bin/env python3
import json
import time
import numpy as np
from dataclasses import dataclass
from collections import defaultdict
from typing import NamedTuple

from cereal import messaging, car
from openpilot.common.conversions import Conversions as CV
from openpilot.common.realtime import DT_MDL
from openpilot.common.params import Params
from openpilot.common.swaglog import cloudlog
from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
from openpilot.selfdrive.modeld.constants import ModelConstants
from openpilot.selfdrive.car.gm.values import GMFlags


@dataclass
class Action:
  accel_bp: list[float]  # m/s^2
  time_bp: list[float]   # seconds

  def __post_init__(self):
    assert len(self.accel_bp) == len(self.time_bp)


@dataclass
class Maneuver:
  description: str
  actions: list[Action]
  repeat: int = 0
  initial_speed: float = 0.  # m/s

  _active: bool = False
  _finished: bool = False
  _action_index: int = 0
  _action_frames: int = 0
  _ready_cnt: int = 0
  _repeated: int = 0

  def get_accel(self, v_ego: float, long_active: bool, standstill: bool, cruise_standstill: bool) -> float:
    ready = abs(v_ego - self.initial_speed) < 0.3 and long_active and not cruise_standstill
    if self.initial_speed < 0.01:
      ready = ready and standstill
    self._ready_cnt = (self._ready_cnt + 1) if ready else 0

    if self._ready_cnt > (3. / DT_MDL):
      self._active = True

    if not self._active:
      return min(max(self.initial_speed - v_ego, -2.), 2.)

    action = self.actions[self._action_index]
    action_accel = np.interp(self._action_frames * DT_MDL, action.time_bp, action.accel_bp)

    self._action_frames += 1

    # reached duration of action
    if self._action_frames > (action.time_bp[-1] / DT_MDL):
      # next action
      if self._action_index < len(self.actions) - 1:
        self._action_index += 1
        self._action_frames = 0
      # repeat maneuver
      elif self._repeated < self.repeat:
        self._repeated += 1
        self._action_index = 0
        self._action_frames = 0
        self._active = False
      # finish maneuver
      else:
        self._finished = True

    return float(action_accel)

  @property
  def finished(self):
    return self._finished

  @property
  def active(self):
    return self._active


BASE_MANEUVERS = [
  Maneuver(
    "come to stop",
    [Action([-0.5], [12])],
    repeat=2,
    initial_speed=5.,
  ),
  Maneuver(
    "start from stop",
    [Action([1.5], [6])],
    repeat=2,
    initial_speed=0.,
  ),
  Maneuver(
    "creep: alternate between +1m/s^2 and -1m/s^2",
    [
      Action([1], [3]), Action([-1], [3]),
      Action([1], [3]), Action([-1], [3]),
      Action([1], [3]), Action([-1], [3]),
    ],
    repeat=2,
    initial_speed=0.,
  ),
  Maneuver(
    "brake step response: -1m/s^2 from 20mph",
    [Action([-1], [3])],
    repeat=2,
    initial_speed=20. * CV.MPH_TO_MS,
  ),
  Maneuver(
    "gas step response: +1m/s^2 from 20mph",
    [Action([1], [3])],
    repeat=2,
    initial_speed=20. * CV.MPH_TO_MS,
  ),
  Maneuver(
    "gas step response: +4m/s^2 from 20mph",
    [Action([4], [3])],
    repeat=2,
    initial_speed=20. * CV.MPH_TO_MS,
  ),
]

STANDARD_BRAKE_STEPS = [
  Maneuver(
    "brake step response: -4m/s^2 from 20mph",
    [Action([-4], [3])],
    repeat=2,
    initial_speed=20. * CV.MPH_TO_MS,
  ),
]

PEDAL_LONG_BRAKE_STEPS = [
  Maneuver(
    "brake step response: -2.0m/s^2 from 20mph",
    [Action([-2.0], [3])],
    repeat=2,
    initial_speed=20. * CV.MPH_TO_MS,
  ),
  Maneuver(
    "brake step response: -2.5m/s^2 from 20mph",
    [Action([-2.5], [3])],
    repeat=2,
    initial_speed=20. * CV.MPH_TO_MS,
  ),
  Maneuver(
    "brake step response: -4m/s^2 from 20mph (informational, pedal-limited)",
    [Action([-4], [3])],
    repeat=2,
    initial_speed=20. * CV.MPH_TO_MS,
  ),
]

PEDAL_LONG_LOW_SPEED_STOP_SWEEP = [
  Maneuver(
    "low-speed stop envelope: -0.8m/s^2 from 8mph",
    [Action([-0.8], [7.0])],
    repeat=0,
    initial_speed=8. * CV.MPH_TO_MS,
  ),
  Maneuver(
    "low-speed stop envelope: -1.2m/s^2 from 12mph",
    [Action([-1.2], [7.0])],
    repeat=0,
    initial_speed=12. * CV.MPH_TO_MS,
  ),
  Maneuver(
    "low-speed stop envelope: -1.6m/s^2 from 16mph",
    [Action([-1.6], [7.0])],
    repeat=0,
    initial_speed=16. * CV.MPH_TO_MS,
  ),
  Maneuver(
    "low-speed stop envelope: -2.0m/s^2 from 20mph",
    [Action([-2.0], [7.0])],
    repeat=0,
    initial_speed=20. * CV.MPH_TO_MS,
  ),
]

PEDAL_LONG_PADDLE_BLEND_PROBES = [
  Maneuver(
    "paddle blend probe: accel/decel toggle around zero from 12mph",
    [
      Action([-0.20], [1.5]),
      Action([0.20], [1.5]),
      Action([-0.20], [1.5]),
      Action([0.20], [1.5]),
      Action([-0.35], [2.5]),
    ],
    repeat=0,
    initial_speed=12. * CV.MPH_TO_MS,
  ),
  Maneuver(
    "paddle blend probe: regen threshold sweep from 18mph",
    [
      Action([-0.45], [2.0]),
      Action([-0.75], [2.0]),
      Action([-0.45], [2.0]),
      Action([-0.90], [2.0]),
      Action([-0.10], [1.5]),
    ],
    repeat=0,
    initial_speed=18. * CV.MPH_TO_MS,
  ),
]

PEDAL_LONG_TERMINAL_STOP_PROBES = [
  Maneuver(
    "terminal stop probe: ramped decel catch from 14mph",
    [
      Action([-0.45], [2.0]),
      Action([-1.20], [2.0]),
      Action([-2.20], [2.5]),
      Action([-0.35], [2.0]),
    ],
    repeat=0,
    initial_speed=14. * CV.MPH_TO_MS,
  ),
  Maneuver(
    "terminal stop probe: late strong regen request from 18mph",
    [
      Action([-0.55], [2.0]),
      Action([-1.45], [2.0]),
      Action([-2.40], [2.2]),
      Action([-0.45], [2.0]),
    ],
    repeat=0,
    initial_speed=18. * CV.MPH_TO_MS,
  ),
]


def clone_maneuver(m: Maneuver, label_suffix: str = "") -> Maneuver:
  actions = [Action(list(a.accel_bp), list(a.time_bp)) for a in m.actions]
  desc = f"{m.description} {label_suffix}".strip()
  return Maneuver(desc, actions, repeat=m.repeat, initial_speed=m.initial_speed)


def build_maneuvers(CP, label_suffix: str = "") -> list[Maneuver]:
  is_gm_pedal_long = (CP.carName == "gm" and CP.enableGasInterceptor and bool(CP.flags & GMFlags.PEDAL_LONG.value))
  maneuvers = [clone_maneuver(m, label_suffix) for m in BASE_MANEUVERS]
  brake_steps = PEDAL_LONG_BRAKE_STEPS if is_gm_pedal_long else STANDARD_BRAKE_STEPS

  # Keep brake step maneuvers grouped with other step responses.
  insert_idx = next((i for i, m in enumerate(maneuvers) if m.description.startswith("gas step response")), len(maneuvers))
  if is_gm_pedal_long:
    for probe in reversed(PEDAL_LONG_PADDLE_BLEND_PROBES):
      maneuvers.insert(insert_idx, clone_maneuver(probe, label_suffix))
    for terminal_probe in reversed(PEDAL_LONG_TERMINAL_STOP_PROBES):
      maneuvers.insert(insert_idx, clone_maneuver(terminal_probe, label_suffix))
    for stop_sweep in reversed(PEDAL_LONG_LOW_SPEED_STOP_SWEEP):
      maneuvers.insert(insert_idx, clone_maneuver(stop_sweep, label_suffix))

  for step in reversed(brake_steps):
    maneuvers.insert(insert_idx, clone_maneuver(step, label_suffix))

  return maneuvers


class ManeuverPhase(NamedTuple):
  name: str
  paddle_mode: str
  maneuvers: list[Maneuver]


def build_maneuver_phases(CP) -> list[ManeuverPhase]:
  is_gm_pedal_long = (CP.carName == "gm" and CP.enableGasInterceptor and bool(CP.flags & GMFlags.PEDAL_LONG.value))
  if not is_gm_pedal_long:
    return [ManeuverPhase("standard", "auto", build_maneuvers(CP))]

  # For pedal-long GM, run paired A/B phases to isolate pedal-only vs pedal+paddle behavior.
  return [
    ManeuverPhase("pedal_only", "off", build_maneuvers(CP, "[pedal-only]")),
    ManeuverPhase("pedal_plus_paddle", "force", build_maneuvers(CP, "[pedal+paddle]")),
  ]


def get_phase_step_count(phase: ManeuverPhase) -> int:
  return sum(m.repeat + 1 for m in phase.maneuvers)


def get_total_step_count(phases: list[ManeuverPhase]) -> int:
  return sum(get_phase_step_count(phase) for phase in phases)


def write_status(params: Params, status: dict, history_line: str | None = None, **fields) -> None:
  if history_line:
    history = list(status.get("history", []))
    history.append(history_line)
    status["history"] = history[-120:]

  status.update(fields)
  status["updatedAtSec"] = time.time()
  params.put("LongitudinalManeuverStatus", json.dumps(status, separators=(",", ":")))


def main():
  params = Params()
  history = []
  raw_status = params.get("LongitudinalManeuverStatus", encoding="utf-8") or ""
  if raw_status:
    try:
      prior_status = json.loads(raw_status)
      if isinstance(prior_status, dict):
        raw_history = prior_status.get("history", [])
        if isinstance(raw_history, list):
          history = [str(line) for line in raw_history if str(line).strip()][-120:]
    except Exception:
      pass

  status = {
    "state": "starting",
    "phase": "",
    "paddleMode": "auto",
    "maneuver": "",
    "runIndex": 0,
    "runTotal": 0,
    "stepIndex": 0,
    "stepTotal": 0,
    "phaseStepIndex": 0,
    "phaseStepTotal": 0,
    "uiShow": True,
    "uiSize": "mid",
    "uiText1": "Long Maneuvers",
    "uiText2": "Waiting for CarParams...",
    "history": history,
  }
  write_status(params, status, history_line="Mode armed, waiting for CarParams.")

  cloudlog.info("maneuversd is waiting for CarParams")
  cp_bytes = params.get("CarParams", block=True)
  try:
    with car.CarParams.from_bytes(cp_bytes) as msg:
      CP = msg
  except Exception:
    # Fallback for branches that decode via Event wrapper only.
    CP = messaging.log_from_bytes(cp_bytes).carParams
  cloudlog.warning("LONG_MANEUVER_MODE|state=STARTED")
  params.put("LongitudinalManeuverPaddleMode", "auto")

  sm = messaging.SubMaster(['carState', 'carControl', 'controlsState', 'modelV2'], poll='modelV2')
  pm = messaging.PubMaster(['longitudinalPlan'])

  phases = build_maneuver_phases(CP)
  total_step_count = max(get_total_step_count(phases), 1)
  phase_idx = 0
  active_phase = phases[phase_idx]
  active_phase_step_total = get_phase_step_count(active_phase)
  params.put("LongitudinalManeuverPaddleMode", active_phase.paddle_mode)
  cloudlog.warning(f"LONG_MANEUVER_PHASE|name={active_phase.name}|paddleMode={active_phase.paddle_mode}")
  write_status(
    params,
    status,
    state="phase",
    phase=active_phase.name,
    paddleMode=active_phase.paddle_mode,
    stepTotal=total_step_count,
    phaseStepTotal=active_phase_step_total,
    uiText1="Long Maneuvers",
    uiText2=f"Phase {phase_idx + 1}/{len(phases)}: {active_phase.name} ({active_phase_step_total} steps)",
    history_line=f"Phase {phase_idx + 1}/{len(phases)} started: {active_phase.name} ({active_phase_step_total} steps).",
  )

  maneuvers = iter(active_phase.maneuvers)
  maneuver = None
  maneuver_run_count: defaultdict[str, int] = defaultdict(int)
  run_active = False
  active_description = ""
  active_run_index = 0
  active_run_total = 0
  finished_logged = False
  active_phase_name = active_phase.name
  active_paddle_mode = active_phase.paddle_mode
  step_index = 0
  phase_step_index = 0
  resume_blocked_active = False
  last_resume_blocked_msg_t = 0.0

  t_idx = ModelConstants.T_IDXS[:CONTROL_N]
  try:
    while True:
      sm.update()

      if maneuver is None:
        maneuver = next(maneuvers, None)
        run_active = False
        active_description = ""
        if maneuver is None:
          if phase_idx + 1 < len(phases):
            phase_idx += 1
            active_phase = phases[phase_idx]
            active_phase_name = active_phase.name
            active_paddle_mode = active_phase.paddle_mode
            active_phase_step_total = get_phase_step_count(active_phase)
            phase_step_index = 0
            params.put("LongitudinalManeuverPaddleMode", active_paddle_mode)
            cloudlog.warning(f"LONG_MANEUVER_PHASE|name={active_phase_name}|paddleMode={active_paddle_mode}")
            write_status(
              params,
              status,
              state="phase",
              phase=active_phase_name,
              paddleMode=active_paddle_mode,
              phaseStepIndex=phase_step_index,
              phaseStepTotal=active_phase_step_total,
              uiText1="Long Maneuvers",
              uiText2=f"Phase {phase_idx + 1}/{len(phases)}: {active_phase_name} ({active_phase_step_total} steps)",
              history_line=f"Phase {phase_idx + 1}/{len(phases)} started: {active_phase_name} ({active_phase_step_total} steps).",
            )
            maneuvers = iter(active_phase.maneuvers)
            maneuver = next(maneuvers, None)
          elif not finished_logged:
            cloudlog.warning("LONG_MANEUVER_MODE|state=FINISHED")
            write_status(
              params,
              status,
              state="finished",
              maneuver="",
              runIndex=0,
              runTotal=0,
              uiShow=True,
              uiSize="mid",
              uiText1="Long Maneuvers Complete",
              uiText2=f"Completed {step_index}/{total_step_count} steps. Returning to normal long control.",
              history_line=f"Complete: {step_index}/{total_step_count} steps finished.",
            )
            params.put_bool("LongitudinalManeuverMode", False)
            finished_logged = True
            break

      plan_send = messaging.new_message('longitudinalPlan')
      plan_send.valid = sm.all_checks()

      longitudinalPlan = plan_send.longitudinalPlan
      accel = 0
      v_ego = max(sm['carState'].vEgo, 0)

      if maneuver is not None:
        accel = maneuver.get_accel(v_ego, sm['carControl'].longActive, sm['carState'].standstill, sm['carState'].cruiseState.standstill)

        resume_blocked = (not CP.autoResumeSng) and (not maneuver.active) and bool(sm['carState'].cruiseState.standstill)
        if resume_blocked:
          now_t = time.monotonic()
          if (not resume_blocked_active) or (now_t - last_resume_blocked_msg_t > 2.0):
            write_status(
              params,
              status,
              state="waiting_resume",
              phase=active_phase_name,
              paddleMode=active_paddle_mode,
              maneuver=maneuver.description,
              runIndex=0,
              runTotal=maneuver.repeat + 1,
              stepIndex=step_index,
              stepTotal=total_step_count,
              phaseStepIndex=phase_step_index,
              phaseStepTotal=active_phase_step_total,
              uiShow=True,
              uiSize="mid",
              uiText1="Long Maneuvers",
              uiText2=f"Step {step_index + 1}/{total_step_count}: press/hold RESUME to continue",
              history_line=(f"Waiting at standstill on '{maneuver.description}'. "
                            f"Press/hold RESUME to continue.")
            )
            last_resume_blocked_msg_t = now_t
          resume_blocked_active = True
        elif resume_blocked_active:
          write_status(
            params,
            status,
            state="phase",
            phase=active_phase_name,
            paddleMode=active_paddle_mode,
            maneuver=maneuver.description,
            runIndex=0,
            runTotal=maneuver.repeat + 1,
            stepIndex=step_index,
            stepTotal=total_step_count,
            phaseStepIndex=phase_step_index,
            phaseStepTotal=active_phase_step_total,
            uiShow=True,
            uiSize="small",
            uiText1="Long Maneuvers",
            uiText2=f"Resume detected: preparing {maneuver.description}",
            history_line=f"Resume detected; continuing '{maneuver.description}'.",
          )
          resume_blocked_active = False

        if maneuver.active and not run_active:
          maneuver_run_count[maneuver.description] += 1
          step_index += 1
          phase_step_index += 1
          run_active = True
          active_description = maneuver.description
          active_run_index = maneuver_run_count[maneuver.description]
          active_run_total = maneuver.repeat + 1
          cloudlog.warning(
            f"LONG_MANEUVER_START|desc={maneuver.description}|run={maneuver_run_count[maneuver.description]}|"
            f"accel={accel:.3f}|phase={active_phase_name}|paddleMode={active_paddle_mode}"
          )
          write_status(
            params,
            status,
            state="running",
            phase=active_phase_name,
            paddleMode=active_paddle_mode,
            maneuver=active_description,
            runIndex=active_run_index,
            runTotal=active_run_total,
            stepIndex=step_index,
            stepTotal=total_step_count,
            phaseStepIndex=phase_step_index,
            phaseStepTotal=active_phase_step_total,
            uiShow=True,
            uiSize="mid",
            uiText1="Long Maneuvers",
            uiText2=f"Step {step_index}/{total_step_count}: {active_description} ({active_run_index}/{active_run_total})",
            history_line=f"Step {step_index}/{total_step_count} started: {active_description} ({active_run_index}/{active_run_total}).",
          )
        elif run_active and not maneuver.active:
          cloudlog.warning(
            f"LONG_MANEUVER_END|desc={active_description}|run={maneuver_run_count[active_description]}|"
            f"phase={active_phase_name}|paddleMode={active_paddle_mode}"
          )
          write_status(
            params,
            status,
            state="running",
            phase=active_phase_name,
            paddleMode=active_paddle_mode,
            maneuver=active_description,
            runIndex=active_run_index,
            runTotal=active_run_total,
            stepIndex=step_index,
            stepTotal=total_step_count,
            phaseStepIndex=phase_step_index,
            phaseStepTotal=active_phase_step_total,
            uiShow=True,
            uiSize="small",
            uiText1="Long Maneuvers",
            uiText2=f"Step {step_index}/{total_step_count} complete",
            history_line=f"Step {step_index}/{total_step_count} complete: {active_description} ({active_run_index}/{active_run_total}).",
          )
          run_active = False
          active_description = ""

      longitudinalPlan.aTarget = accel
      longitudinalPlan.shouldStop = v_ego < CP.vEgoStopping and accel < 1e-2

      longitudinalPlan.allowBrake = True
      longitudinalPlan.allowThrottle = True
      longitudinalPlan.hasLead = True

      # Build a full horizon trajectory so both old and new long APIs can consume the test plan.
      speed_traj = [max(v_ego + accel * t, 0.0) for t in t_idx]
      # controlsd resume logic gates on speeds[-1] > 0.1, not speeds[0].
      # Keep both ends >0.1 so camera-long standstill can resume during maneuver setup.
      speed_traj[0] = max(speed_traj[0], 0.2)
      speed_traj[-1] = max(speed_traj[-1], 0.2)
      longitudinalPlan.speeds = speed_traj
      longitudinalPlan.accels = [accel] * CONTROL_N
      longitudinalPlan.jerks = [0.0] * CONTROL_N

      pm.send('longitudinalPlan', plan_send)

      if maneuver is not None and maneuver.finished:
        if run_active:
          cloudlog.warning(
            f"LONG_MANEUVER_END|desc={active_description}|run={maneuver_run_count[active_description]}|"
            f"phase={active_phase_name}|paddleMode={active_paddle_mode}"
          )
          write_status(
            params,
            status,
            state="running",
            phase=active_phase_name,
            paddleMode=active_paddle_mode,
            maneuver=active_description,
            runIndex=active_run_index,
            runTotal=active_run_total,
            stepIndex=step_index,
            stepTotal=total_step_count,
            phaseStepIndex=phase_step_index,
            phaseStepTotal=active_phase_step_total,
            uiShow=True,
            uiSize="small",
            uiText1="Long Maneuvers",
            uiText2=f"Step {step_index}/{total_step_count} complete",
            history_line=f"Step {step_index}/{total_step_count} complete: {active_description} ({active_run_index}/{active_run_total}).",
          )
        cloudlog.warning(
          f"LONG_MANEUVER_FINISHED|desc={maneuver.description}|runs={maneuver_run_count[maneuver.description]}|"
          f"phase={active_phase_name}|paddleMode={active_paddle_mode}"
        )
        write_status(
          params,
          status,
          state="running",
          phase=active_phase_name,
          paddleMode=active_paddle_mode,
          maneuver="",
          runIndex=0,
          runTotal=0,
          uiShow=True,
          uiSize="small",
          uiText1="Long Maneuvers",
          uiText2=f"Maneuver complete: {maneuver.description}",
          history_line=f"Maneuver complete: {maneuver.description}.",
        )
        maneuver = None
        run_active = False
        active_description = ""
        active_run_index = 0
        active_run_total = 0
  finally:
    if not finished_logged:
      write_status(
        params,
        status,
        state="stopped",
        uiShow=True,
        uiSize="small",
        uiText1="Long Maneuvers Stopped",
        uiText2=f"Stopped at step {step_index}/{total_step_count}.",
        history_line=f"Stopped at step {step_index}/{total_step_count}.",
      )
    params.put("LongitudinalManeuverPaddleMode", "auto")