From 82f8db87f464ad826c03f116a35940331304667f Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 12 Sep 2024 14:10:18 -0700 Subject: [PATCH] add longitudinal maneuvers (#33527) * add longitudinal profiles * stash * unfortunately even longitudinalPlan causes circle * add to process config * reach target speed smoothly * stash * works * clean up * debug alert * rename * fix * better text * toggle via exp button * try coming to a stop better, smoother target reaching * closer to target * revert controlsd migration * add description to alert * generate report from local logs * hide bad maneuvers * pdflike * Revert "pdflike" This reverts commit 6d4af1bf9be2e9e0798eaecf026a53d860da7613. * try this * use alert manager * fix that check * wat * Revert "wat" This reverts commit 93d0d27ab838d3f580d06ff212f380e0b912d599. * some clean up * rm * cleanup * move * fix test * more fix * clean up * fix that --- cereal/car.capnp | 1 + cereal/log.capnp | 6 + cereal/services.py | 1 + common/params.cc | 1 + release/release_files.py | 1 + selfdrive/selfdrived/events.py | 16 ++ selfdrive/selfdrived/selfdrived.py | 8 +- system/manager/process_config.py | 9 +- .../longitudinal_maneuvers/generate_report.py | 115 ++++++++++++ tools/longitudinal_maneuvers/maneuversd.py | 165 ++++++++++++++++++ 10 files changed, 320 insertions(+), 3 deletions(-) create mode 100755 tools/longitudinal_maneuvers/generate_report.py create mode 100755 tools/longitudinal_maneuvers/maneuversd.py diff --git a/cereal/car.capnp b/cereal/car.capnp index f5469a54d7..2c18c7c617 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -54,6 +54,7 @@ struct OnroadEvent @0x9b1657f34caf3ad3 { manualRestart @30; lowSpeedLockout @31; joystickDebug @34; + longitudinalManeuver @124; steerTempUnavailableSilent @35; resumeRequired @36; preDriverDistracted @37; diff --git a/cereal/log.capnp b/cereal/log.capnp index d6629206b3..6cd7fe7534 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -2304,6 +2304,11 @@ struct EncodeData { height @5 :UInt32; } +struct DebugAlert { + alertText1 @0 :Text; + alertText2 @1 :Text; +} + struct UserFlag { } @@ -2411,6 +2416,7 @@ struct Event { driverEncodeData @87 :EncodeData; wideRoadEncodeData @88 :EncodeData; qRoadEncodeData @89 :EncodeData; + alertDebug @133 :DebugAlert; livestreamRoadEncodeData @120 :EncodeData; livestreamWideRoadEncodeData @121 :EncodeData; diff --git a/cereal/services.py b/cereal/services.py index e47ddc6c3f..d94c3f11fb 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -75,6 +75,7 @@ _services: dict[str, tuple] = { # debug "uiDebug": (True, 0., 1), + "alertDebug": (True, 0.), "roadEncodeData": (False, 20.), "driverEncodeData": (False, 20.), "wideRoadEncodeData": (False, 20.), diff --git a/common/params.cc b/common/params.cc index 0932acd54d..c75a09e28b 100644 --- a/common/params.cc +++ b/common/params.cc @@ -158,6 +158,7 @@ std::unordered_map keys = { {"LiveParameters", PERSISTENT}, {"LiveTorqueParameters", PERSISTENT | DONT_LOG}, {"LocationFilterInitialState", PERSISTENT}, + {"LongitudinalManeuverMode", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION}, {"LongitudinalPersonality", PERSISTENT}, {"NetworkMetered", PERSISTENT}, {"ObdMultiplexingChanged", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, diff --git a/release/release_files.py b/release/release_files.py index 788d45b88e..afd0d468b6 100755 --- a/release/release_files.py +++ b/release/release_files.py @@ -53,6 +53,7 @@ whitelist = [ "tools/lib/", "tools/bodyteleop/", "tools/joystick/", + "tools/longitudinal_maneuvers/", "tinygrad_repo/openpilot/compile2.py", "tinygrad_repo/extra/onnx.py", diff --git a/selfdrive/selfdrived/events.py b/selfdrive/selfdrived/events.py index 471e46b88f..29ab060d9b 100755 --- a/selfdrive/selfdrived/events.py +++ b/selfdrive/selfdrived/events.py @@ -325,6 +325,17 @@ def joystick_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, vals = f"Gas: {round(gb * 100.)}%, Steer: {round(steer * 100.)}%" return NormalPermanentAlert("Joystick Mode", vals) + +def longitudinal_maneuver_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: + ad = sm['alertDebug'] + audible_alert = AudibleAlert.prompt if 'Active' in ad.alertText1 else AudibleAlert.none + alert_status = AlertStatus.userPrompt if 'Active' in ad.alertText1 else AlertStatus.normal + alert_size = AlertSize.mid if ad.alertText2 else AlertSize.small + return Alert(ad.alertText1, ad.alertText2, + alert_status, alert_size, + Priority.LOW, VisualAlert.none, audible_alert, 0.2) + + def personality_changed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: personality = str(personality).title() return NormalPermanentAlert(f"Driving Personality: {personality}", duration=1.5) @@ -344,6 +355,11 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { ET.PERMANENT: NormalPermanentAlert("Joystick Mode"), }, + EventName.longitudinalManeuver: { + ET.WARNING: longitudinal_maneuver_alert, + ET.PERMANENT: NormalPermanentAlert("Longitudinal Maneuver Mode"), + }, + EventName.selfdriveInitializing: { ET.NO_ENTRY: NoEntryAlert("System Initializing"), }, diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index 255ce080df..4559c82bd6 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -61,7 +61,7 @@ class SelfdriveD: # TODO: de-couple selfdrived with card/conflate on carState without introducing controls mismatches self.car_state_sock = messaging.sub_sock('carState', timeout=20) - ignore = self.sensor_packets + self.gps_packets + ignore = self.sensor_packets + self.gps_packets + ['alertDebug'] if SIMULATION: ignore += ['driverCameraState', 'managerState'] if REPLAY: @@ -70,7 +70,7 @@ class SelfdriveD: self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration', 'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose', 'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters', - 'controlsState', 'carControl', 'driverAssistance'] + \ + 'controlsState', 'carControl', 'driverAssistance', 'alertDebug'] + \ self.camera_packets + self.sensor_packets + self.gps_packets, ignore_alive=ignore, ignore_avg_freq=ignore+['radarState',], ignore_valid=ignore, frequency=int(1/DT_CTRL)) @@ -135,6 +135,10 @@ class SelfdriveD: self.events.add(EventName.joystickDebug) self.startup_event = None + if self.sm.recv_frame['alertDebug'] > 0: + self.events.add(EventName.longitudinalManeuver) + self.startup_event = None + # Add startup event if self.startup_event is not None: self.events.add(self.startup_event) diff --git a/system/manager/process_config.py b/system/manager/process_config.py index a85bfd1f46..538c55813b 100644 --- a/system/manager/process_config.py +++ b/system/manager/process_config.py @@ -35,6 +35,12 @@ def joystick(started: bool, params, CP: car.CarParams) -> bool: def not_joystick(started: bool, params, CP: car.CarParams) -> bool: return started and not params.get_bool("JoystickDebugMode") +def long_maneuver(started: bool, params, CP: car.CarParams) -> bool: + return started and params.get_bool("LongitudinalManeuverMode") + +def not_long_maneuver(started: bool, params, CP: car.CarParams) -> bool: + return started and not params.get_bool("LongitudinalManeuverMode") + def qcomgps(started, params, CP: car.CarParams) -> bool: return started and not ublox_available() @@ -81,7 +87,8 @@ procs = [ PythonProcess("paramsd", "selfdrive.locationd.paramsd", only_onroad), NativeProcess("ubloxd", "system/ubloxd", ["./ubloxd"], ublox, enabled=TICI), PythonProcess("pigeond", "system.ubloxd.pigeond", ublox, enabled=TICI), - PythonProcess("plannerd", "selfdrive.controls.plannerd", only_onroad), + PythonProcess("plannerd", "selfdrive.controls.plannerd", not_long_maneuver), + PythonProcess("maneuversd", "tools.longitudinal_maneuvers.maneuversd", long_maneuver), PythonProcess("radard", "selfdrive.controls.radard", only_onroad), PythonProcess("hardwared", "system.hardware.hardwared", always_run), PythonProcess("tombstoned", "system.tombstoned", always_run, enabled=not PC), diff --git a/tools/longitudinal_maneuvers/generate_report.py b/tools/longitudinal_maneuvers/generate_report.py new file mode 100755 index 0000000000..262ea93ef3 --- /dev/null +++ b/tools/longitudinal_maneuvers/generate_report.py @@ -0,0 +1,115 @@ +#!/usr/bin/env python3 +import argparse +import base64 +import io +import os +import time +from pathlib import Path +import matplotlib.pyplot as plt + +from openpilot.tools.lib.logreader import LogReader +from openpilot.system.hardware.hw import Paths + + +def report(platform, maneuvers): + output_path = Path(__file__).resolve().parent / "longitudinal_reports" + output_fn = output_path / f"{platform}_{time.strftime('%Y%m%d-%H_%M_%S')}.html" + output_path.mkdir(exist_ok=True) + with open(output_fn, "w") as f: + f.write("

Longitudinal maneuver report

\n") + f.write(f"

{platform}

\n") + for description, runs in maneuvers: + print('plotting maneuver:', description, 'runs:', len(runs)) + f.write("
\n") + f.write(f"

{description}

\n") + for run, msgs in enumerate(runs): + t_carControl, carControl = zip(*[(m.logMonoTime, m.carControl) for m in msgs if m.which() == 'carControl'], strict=True) + t_carOutput, carOutput = zip(*[(m.logMonoTime, m.carOutput) for m in msgs if m.which() == 'carOutput'], strict=True) + t_carState, carState = zip(*[(m.logMonoTime, m.carState) for m in msgs if m.which() == 'carState'], strict=True) + t_longitudinalPlan, longitudinalPlan = zip(*[(m.logMonoTime, m.longitudinalPlan) for m in msgs if m.which() == 'longitudinalPlan'], strict=True) + + longActive = [m.longActive for m in carControl] + gasPressed = [m.gasPressed for m in carState] + brakePressed = [m.brakePressed for m in carState] + + maneuver_valid = all(longActive) and not (any(gasPressed) or any(brakePressed)) + + _open = 'open' if maneuver_valid else '' + title = f'Run #{int(run)+1}' + (' (invalid maneuver!)' if not maneuver_valid else '') + + f.write(f"

{title}

\n") + + plt.rcParams['font.size'] = 40 + fig = plt.figure(figsize=(30, 25)) + ax = fig.subplots(4, 1, sharex=True, gridspec_kw={'hspace': 0, 'height_ratios': [5, 3, 1, 1]}) + + ax[0].grid(linewidth=4) + ax[0].plot(t_carControl, [m.actuators.accel for m in carControl], label='carControl.actuators.accel', linewidth=6) + ax[0].plot(t_carOutput, [m.actuatorsOutput.accel for m in carOutput], label='carOutput.actuatorsOutput.accel', linewidth=6) + ax[0].plot(t_longitudinalPlan, [m.aTarget for m in longitudinalPlan], label='longitudinalPlan.aTarget', linewidth=6) + ax[0].plot(t_carState, [m.aEgo for m in carState], label='carState.aEgo', linewidth=6) + # TODO localizer accel + ax[0].set_ylabel('Acceleration (m/s^2)') + #ax[0].set_ylim(-6.5, 6.5) + ax[0].legend() + + ax[1].grid(linewidth=4) + ax[1].plot(t_carState, [m.vEgo for m in carState], 'g', label='vEgo', linewidth=6) + ax[1].set_ylabel('Velocity (m/s)') + ax[1].legend() + + ax[2].plot(t_carControl, longActive, label='longActive', linewidth=6) + ax[3].plot(t_carState, gasPressed, label='gasPressed', linewidth=6) + ax[3].plot(t_carState, brakePressed, label='brakePressed', linewidth=6) + for i in (2, 3): + ax[i].set_yticks([0, 1], minor=False) + ax[i].set_ylim(-1, 2) + ax[i].legend() + + ax[-1].set_xlabel("Time (s)") + fig.tight_layout() + + buffer = io.BytesIO() + fig.savefig(buffer, format='png') + buffer.seek(0) + f.write(f"\n") + f.write("
\n") + + print(f"\nReport written to {output_fn}\n") + + +if __name__ == '__main__': + parser = argparse.ArgumentParser(description='Generate longitudinal maneuver report from route') + parser.add_argument('route', type=str, help='Route name (e.g. 00000000--5f742174be)') + + args = parser.parse_args() + + if '/' in args.route or '|' in args.route: + lr = LogReader(args.route) + else: + segs = [seg for seg in os.listdir(Paths.log_root()) if args.route in seg] + lr = LogReader([os.path.join(Paths.log_root(), seg, 'rlog') for seg in segs]) + + CP = lr.first('carParams') + platform = CP.carFingerprint + print('processing report for', platform) + + maneuvers: list[tuple[str, list[list]]] = [] + active_prev = False + description_prev = None + + for msg in lr: + if msg.which() == 'alertDebug': + active = 'Maneuver Active' in msg.alertDebug.alertText1 + if active and not active_prev: + if msg.alertDebug.alertText2 == description_prev: + maneuvers[-1][1].append([]) + else: + maneuvers.append((msg.alertDebug.alertText2, [[]])) + description_prev = maneuvers[-1][0] + active_prev = active + + if active_prev: + maneuvers[-1][1][-1].append(msg) + + report(platform, maneuvers) diff --git a/tools/longitudinal_maneuvers/maneuversd.py b/tools/longitudinal_maneuvers/maneuversd.py new file mode 100755 index 0000000000..30304a37c2 --- /dev/null +++ b/tools/longitudinal_maneuvers/maneuversd.py @@ -0,0 +1,165 @@ +#!/usr/bin/env python3 +from dataclasses import dataclass + +from cereal import messaging, car +from opendbc.car.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 + + +@dataclass +class Action: + accel: float # m/s^2 + duration: float # seconds + + +@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, enabled: bool, standstill: bool) -> float: + ready = abs(v_ego - self.initial_speed) < 0.3 and enabled and not 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] + + self._action_frames += 1 + + # reached duration of action + if self._action_frames > (action.duration / 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 action.accel + + @property + def finished(self): + return self._finished + + @property + def active(self): + return self._active + + +MANEUVERS = [ + Maneuver( + "creep: alternate between +1m/s^2 and -1m/s^2", + [ + Action(1, 2), Action(-1, 2), + Action(1, 2), Action(-1, 2), + Action(1, 2), Action(-1, 2), + ], + repeat=1, + 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( + "brake step response: -4m/s^2 from 20mph", + [Action(-4, 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, + ), +] + + +def main(): + params = Params() + cloudlog.info("joystickd is waiting for CarParams") + CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) + + sm = messaging.SubMaster(['carState', 'controlsState', 'selfdriveState', 'modelV2'], poll='modelV2') + pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'alertDebug']) + + maneuvers = iter(MANEUVERS) + maneuver = None + + while True: + sm.update() + + if maneuver is None: + maneuver = next(maneuvers, None) + + alert_msg = messaging.new_message('alertDebug') + alert_msg.valid = True + + plan_send = messaging.new_message('longitudinalPlan') + plan_send.valid = sm.all_checks() + + longitudinalPlan = plan_send.longitudinalPlan + accel = 0 + cs = sm['carState'] + v_ego = max(cs.vEgo, 0) + + if maneuver is not None: + accel = maneuver.get_accel(v_ego, cs.cruiseState.enabled, cs.cruiseState.standstill) + + if maneuver.active: + alert_msg.alertDebug.alertText1 = f'Maneuver Active: {accel:0.2f} m/s^2' + else: + alert_msg.alertDebug.alertText1 = f'Reaching Speed: {maneuver.initial_speed * CV.MS_TO_MPH:0.2f} mph' + alert_msg.alertDebug.alertText2 = f'{maneuver.description}' + else: + alert_msg.alertDebug.alertText1 = 'Maneuvers Finished' + + pm.send('alertDebug', alert_msg) + + longitudinalPlan.aTarget = accel + longitudinalPlan.shouldStop = v_ego < CP.vEgoStopping and accel < 1e-2 + + longitudinalPlan.allowBrake = True + longitudinalPlan.allowThrottle = True + longitudinalPlan.hasLead = True + + pm.send('longitudinalPlan', plan_send) + + assistance_send = messaging.new_message('driverAssistance') + assistance_send.valid = True + pm.send('driverAssistance', assistance_send) + + if maneuver is not None and maneuver.finished: + maneuver = None