reach target speed smoothly

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;
   }
 }

common/params.cc

@@ -158,6 +158,7 @@ std::unordered_map<std::string, uint32_t> 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},

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,17 +145,29 @@ def report(args, logs, fp):
     f.write(f"<p style='display: none'>{json.dumps(logs)}</p>")
   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()
 
+    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)

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),