long maneuvers https://github.com/commaai/openpilot/issues/26778

info.txt

@@ -0,0 +1,32 @@
+## programmable longitudinal maneuvers
+
+probably use with chill mode for reproducible planning
+
+### Maneuver object
+jump instantaneously to velocity at next point,
+or interpolate smoothly
+
+{
+  duration: 14, // seconds
+  should_interpolate: false,
+  points: [
+    // time, velocitymph
+    {0, 0},
+    {5, 10},
+    {8, 10},
+    {12, 2},
+    {14, 0},
+  ],
+}
+
+### maneuver execution
+once maneuver is selected and commanded
+override setspeed from cruisestate with points from maneuver
+
+### web app
+c3 hosted web app to choose maneuver, start/stop, view progress
+start with a few maneuvers to choose from
+
+maybe later add a graph to display progress on phone
+
+

selfdrive/controls/controlsd.py

@@ -596,6 +596,13 @@ def state_control(self, CS):
       # accel PID loop
       pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, self.v_cruise_helper.v_cruise_kph * CV.KPH_TO_MS)
       t_since_plan = (self.sm.frame - self.sm.rcv_frame['longitudinalPlan']) * DT_CTRL
+
+      ## TODO
+      # when starting a maneuver
+      self.LoC.reset(v_pid=CS.vEgo) #vpid = vego or first point in plan
+
+      # fill planfrom maneuver
+
       actuators.accel = self.LoC.update(CC.longActive, CS, long_plan, pid_accel_limits, t_since_plan)
 
       # Steering PID loop and lateral MPC

selfdrive/controls/lib/longitudinal_planner.py

@@ -3,6 +3,7 @@
 import numpy as np
 from common.numpy_fast import clip, interp
 
+from tools.joystick.maneuver import ManeuverController
 import cereal.messaging as messaging
 from common.conversions import Conversions as CV
 from common.filter_simple import FirstOrderFilter
@@ -46,6 +47,7 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
 
 class LongitudinalPlanner:
   def __init__(self, CP, init_v=0.0, init_a=0.0):
+    self.mc = ManeuverController()
     self.CP = CP
     self.mpc = LongitudinalMpc()
     self.fcw = False
@@ -81,6 +83,8 @@ def update(self, sm):
     v_ego = sm['carState'].vEgo
     v_cruise_kph = sm['controlsState'].vCruise
     v_cruise_kph = min(v_cruise_kph, V_CRUISE_MAX)
+    v_override = self.mc.update(vcruise_kph)
+    v_cruise_kph = v_override
     v_cruise = v_cruise_kph * CV.KPH_TO_MS
 
     long_control_off = sm['controlsState'].longControlState == LongCtrlState.off

tools/joystick/maneuver/Makefile

@@ -0,0 +1,5 @@
+
+
+
+run:
+	./maneuver 0

tools/joystick/maneuver/__init__.py

@@ -0,0 +1,127 @@
+#!/usr/bin/env python3
+import datetime
+import json
+import atexit
+import os
+from common.numpy_fast import interp
+from common.conversions import Conversions as CV
+
+
+# TODO: use maneuvercontroller in longitudinal planner
+
+# TODO: make script that uses mem to start a maneuver or something
+
+whereami = str(os.path.dirname(os.path.abspath(__file__)))
+
+maneuvers_directory = whereami + "/maneuvers"
+
+def available_maneuver_files():
+  all_files = os.listdir(maneuvers_directory)
+  files = [f for f in all_files if f[f.rfind(".")+1:] == "json"]
+  return files
+
+class ManeuverController:
+  maneuvering = False
+  maneuver = None
+  t = 0.
+  start_time = None
+  def update(self, vcruise):
+    should_start_maneuver = not self.maneuvering and self.mem.maneuver_requested()
+    if not self.maneuvering and not should_start_maneuver:
+      return vcruise
+
+    # TODO this is wrong. this function decides when to stop
+    #should_stop_maneuver = self.maneuvering and not self.mem.maneuver_requested()
+
+    should_stop_maneuver = self.maneuvering && self.t > self.maneuver.duration
+
+    if should_start_maneuver:
+      self.t = 0.
+      self.start_time = datetime.datetime.now()
+      file_name = str(self.mem.maneuver_num()) + ".json"
+      self.maneuver = Maneuver(file_name)
+      self.maneuvering = True
+    elif should_stop_maneuver:
+      self.maneuvering = False
+      self.maneuver = None
+      self.t = 0.
+      self.start_time = None
+      self.mem.maneuver_finish()
+      return vcruise
+      # TODO
+      # do/finish this and see what else i missed and put it into the car
+      # start over ssh for now
+
+    dt = datetime.datetime.now() - self.start_time
+    self.t = float(dt.seconds) + float(dt.microseconds) / 1.0e6
+    return self.maneuver.get_velocity_kph(self.t)
+  def set_finished(self):
+    self.maneuvering = False
+    self.maneuver = None
+  def request_maneuver_start(self, maneuver):
+    self.maneuver = maneuver
+    self.maneuvering = True
+  def __init__(self):
+    self.mem = Mem(autounlink=True)
+
+class Maneuver:
+  def get_wolfram_alpha_paste(self):
+    out = "plot points["
+    for point in self.object["points"]:
+      out += ("(" + str(point[0]) + "," + str(point[1]) + "),")
+    out = out[:-1] + "]"
+    return out
+  def get_velocity_kph(self, t):
+    if self.should_interpolate:
+      return interp(t, self.times, self.speeds)
+    else:
+      return "ffff"
+  def __points_to_interpables(self):
+    times = []
+    speeds_kph = []
+    conversions = {
+      "mph": CV.MPH_TO_KPH,
+      "kph": 1.,
+      "mps": CV.MS_TO_KPH,
+      "m/s": CV.MS_TO_KPH,
+      "si": CV.MS_TO_KPH,
+    }
+    unit = self.object["velocity_unit"]
+    conversion = conversions[unit]
+    for point in self.object["points"]:
+      times.append(float(point[0]))
+      # TODO calculate accels as slopes of speedss
+      speeds_kph.append(float(point[1]) * conversion)
+    self.times = times
+    self.speeds = speeds_kph
+  def __init__(self, file_name):
+    self.file_name = file_name
+    self.object = json.load(open(maneuvers_directory + "/" + file_name))
+    self.duration = float(self.object["duration"])
+    self.should_interpolate = bool(self.object["should_interpolate"])
+    self.__points_to_interpables()
+
+class Mem:
+  __mem = None
+  name = "maneuvermem"
+  size = 2
+  def maneuver_requested(self):
+    return bool(buf[0])
+  def maneuver_num(self):
+    return buf[1]
+  def maneuver_request(self, maneuver_num):
+    self.buf[1] = maneuver_num
+    self.buf[0] = 1
+  def maneuver_finish(self):
+    self.__mem.buf[0:size] = bytearray([0 for _ in range(size)])
+  def __create_or_connect(self):
+    self.__mem = shared_memory.SharedMemory(name=self.name, create=True, size=self.size)
+  def __cleanup(self):
+    self.__mem.close()
+    if self.__shouldunlink:
+      self.__mem.unlink()
+  def __init__(self, autounlink=False):
+    self.__shouldunlink = autounlink
+    self.__create_or_connect()
+    self.__mem.buf[0:size] = bytearray([0 for _ in range(size)])
+    atexit.register(self.__cleanup)

tools/joystick/maneuver/maneuver

@@ -0,0 +1,12 @@
+#!/bin/bash
+set -euo pipefail
+
+num="$1"
+
+python3 -c "from . import Mem
+mem = Mem()
+if mem.maneuver_requested():
+  print(\"already maneuvering\")
+  exit()
+mem.request(${num})
+"

tools/joystick/maneuver/maneuvers/0.json

@@ -0,0 +1,14 @@
+{
+  "name": "first test maneuver",
+  "duration": 14,
+  "should_interpolate": false,
+  "velocity_unit": "mph",
+  "points": [
+    [0, 0],
+    [5, 10],
+    [8, 10],
+    [12, 2],
+    [14, 0]
+  ]
+}
+