From b5e7f4505b0f8f3f5cd4bc909f355aa8b0fc1b6d Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 10 Sep 2024 21:21:42 -0700 Subject: [PATCH] reach target speed smoothly --- cereal/log.capnp | 3 +- common/params.cc | 1 + ...l-profiles.py => longitudinal_profiles.py} | 67 +++++++++++++------ system/manager/process_config.py | 12 +++- 4 files changed, 59 insertions(+), 24 deletions(-) rename examples/{longitudinal-profiles.py => longitudinal_profiles.py} (82%) diff --git a/cereal/log.capnp b/cereal/log.capnp index 258c1f7ab4..421ae7c041 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -2407,11 +2407,11 @@ struct Event { uiDebug @102 :UIDebug; # *********** debug *********** - testJoystick @52 :Joystick; roadEncodeData @86 :EncodeData; driverEncodeData @87 :EncodeData; wideRoadEncodeData @88 :EncodeData; qRoadEncodeData @89 :EncodeData; + # debugAlert @104 :DebugAlert; livestreamRoadEncodeData @120 :EncodeData; livestreamWideRoadEncodeData @121 :EncodeData; @@ -2476,5 +2476,6 @@ struct Event { uiPlanDEPRECATED @106 :UiPlan; liveLocationKalmanDEPRECATED @72 :LiveLocationKalman; liveTracksDEPRECATED @16 :List(LiveTracksDEPRECATED); + testJoystickDEPRECATED @52 :Joystick; } } 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/examples/longitudinal-profiles.py b/examples/longitudinal_profiles.py similarity index 82% rename from examples/longitudinal-profiles.py rename to examples/longitudinal_profiles.py index c901eb2dfa..f9f4c2f1a9 100755 --- a/examples/longitudinal-profiles.py +++ b/examples/longitudinal_profiles.py @@ -10,10 +10,14 @@ from collections import defaultdict from dataclasses import dataclass, asdict from pathlib import Path -from cereal import messaging +from cereal import messaging, car from opendbc.car.structs import CarControl from opendbc.car.common.conversions import Conversions from openpilot.common.realtime import DT_CTRL, Ratekeeper +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.controls.lib.longitudinal_planner import get_accel_from_plan # TODOs # - support lateral maneuvers @@ -54,16 +58,16 @@ class Maneuver: t0 += lt MANEUVERS = [ - Maneuver( - "creep: alternate between +1m/ss and -1m/ss", - [ - Action(1, 2), Action(-1, 2), - Action(1, 2), Action(-1, 2), - Action(1, 2), Action(-1, 2), - ], - repeat=2, - initial_speed=0., - ), + # Maneuver( + # "creep: alternate between +1m/ss and -1m/ss", + # [ + # Action(1, 2), Action(-1, 2), + # Action(1, 2), Action(-1, 2), + # Action(1, 2), Action(-1, 2), + # ], + # repeat=2, + # initial_speed=0., + # ), Maneuver( "brake step response: -1m/ss from 20mph", [Action(0, 2), Action(-1, 3)], @@ -90,6 +94,7 @@ MANEUVERS = [ ), ] + def report(args, logs, fp): output_path = Path(__file__).resolve().parent / "longitudinal_reports" output_fn = args.output or output_path / f"{fp}_{time.strftime('%Y%m%d-%H_%M_%S')}.html" @@ -140,16 +145,28 @@ def report(args, logs, fp): f.write(f"

{json.dumps(logs)}

") print(f"\nReport written to {output_fn}\n") -def main(args): + +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']) maneuvers = iter(MANEUVERS) + maneuver = None + repeat = 0 while True: sm.update() - maneuver = next(maneuvers, None) + if maneuver is None: + maneuver = next(maneuvers, None) + + if maneuver is None: + print('We are done!') + print(maneuver) if maneuver is not None: pass @@ -159,12 +176,20 @@ def main(args): longitudinalPlan = plan_send.longitudinalPlan - longitudinalPlan.speeds = [] - longitudinalPlan.accels = [] - longitudinalPlan.jerks = [] - - longitudinalPlan.aTarget = 0. - longitudinalPlan.shouldStop = False + if maneuver is not None: + v_ego = sm['carState'].vEgo + diff = maneuver.initial_speed - v_ego + longitudinalPlan.speeds = [(v_ego + min(i / (CONTROL_N - 1), 1) * diff) for i in range(CONTROL_N)] + longitudinalPlan.accels = [(maneuver.initial_speed - sm['carState'].vEgo) * 0.8] * CONTROL_N + longitudinalPlan.jerks = [0] * CONTROL_N + print('v_ego', sm['carState'].vEgo) + print('speeds:', longitudinalPlan.speeds) + print('accels:', longitudinalPlan.accels) + + a_target, should_stop = get_accel_from_plan(CP, longitudinalPlan.speeds, longitudinalPlan.accels) + longitudinalPlan.aTarget = a_target + longitudinalPlan.shouldStop = should_stop + print('aTarget:', longitudinalPlan.aTarget) longitudinalPlan.allowBrake = True longitudinalPlan.allowThrottle = True @@ -172,7 +197,6 @@ def main(args): pm.send('longitudinalPlan', plan_send) - assistance_send = messaging.new_message('driverAssistance') assistance_send.valid = True pm.send('driverAssistance', assistance_send) @@ -240,6 +264,9 @@ def main(args): if __name__ == "__main__": + main() + exit() + maneuver_help = "\n".join([f"{i+1}. {m.description}" for i, m in enumerate(MANEUVERS)]) parser = argparse.ArgumentParser(description="A tool for longitudinal control testing.", formatter_class=argparse.RawTextHelpFormatter) diff --git a/system/manager/process_config.py b/system/manager/process_config.py index 2731ffe522..fff9ec7a02 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() @@ -70,8 +76,8 @@ procs = [ PythonProcess("calibrationd", "selfdrive.locationd.calibrationd", only_onroad), PythonProcess("torqued", "selfdrive.locationd.torqued", only_onroad), PythonProcess("controlsd", "selfdrive.controls.controlsd", not_joystick), - # PythonProcess("joystickd", "tools.joystick.joystickd", joystick), - PythonProcess("longitudinal_profilesd", "examples.longitudinal_profiles", joystick), + PythonProcess("joystickd", "tools.joystick.joystickd", joystick), + PythonProcess("longitudinal_profilesd", "examples.longitudinal_profiles", long_maneuver), PythonProcess("selfdrived", "selfdrive.selfdrived.selfdrived", only_onroad), PythonProcess("card", "selfdrive.car.card", only_onroad), PythonProcess("deleter", "system.loggerd.deleter", always_run), @@ -82,7 +88,7 @@ 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("radard", "selfdrive.controls.radard", only_onroad), PythonProcess("hardwared", "system.hardware.hardwared", always_run), PythonProcess("tombstoned", "system.tombstoned", always_run, enabled=not PC),