Delete lat planner (#31089)

* Initial commit

* Fixup

* typo

* ignore lateral plan

* Update cereal

* Remove lateralPlan

* Fix release build

* Fix release build

* give car params

* Add carParams to include_all_types

* Write car param in powerdraw test

* add demo mode

* Update model regf

* proc replay ref commit

* Try

* Move enum definition

* Update cereal

* typo

* Write car param for modeld test

* Update ref

* Update model ref again

---------

Co-authored-by: Kacper Rączy <gfw.kra@gmail.com>

cereal

@@ -1 +1 @@
-Subproject commit 20b65eeb1f6c580cdd7d63e53639f4fc48bc2f56
+Subproject commit c2adb4f7cf30e53735ee43cc3a8a3698c5410819

release/files_common

@@ -142,7 +142,6 @@ selfdrive/controls/lib/latcontrol_angle.py
 selfdrive/controls/lib/latcontrol_torque.py
 selfdrive/controls/lib/latcontrol_pid.py
 selfdrive/controls/lib/latcontrol.py
-selfdrive/controls/lib/lateral_planner.py
 selfdrive/controls/lib/longcontrol.py
 selfdrive/controls/lib/longitudinal_planner.py
 selfdrive/controls/lib/pid.py

selfdrive/car/car_helpers.py

@@ -207,3 +207,15 @@ def get_car(logcan, sendcan, experimental_long_allowed, num_pandas=1):
   CP.fuzzyFingerprint = not exact_match
 
   return CarInterface(CP, CarController, CarState), CP
+
+def write_car_param(fingerprint="mock"):
+  params = Params()
+  CarInterface, _, _ = interfaces[fingerprint]
+  CP = CarInterface.get_non_essential_params(fingerprint)
+  params.put("CarParams", CP.to_bytes())
+
+def get_demo_car_params():
+  fingerprint="mock"
+  CarInterface, _, _ = interfaces[fingerprint]
+  CP = CarInterface.get_non_essential_params(fingerprint)
+  return CP

selfdrive/car/interfaces.py

@@ -125,7 +125,6 @@ class CarInterfaceBase(ABC):
   @staticmethod
   def get_steer_feedforward_default(desired_angle, v_ego):
     # Proportional to realigning tire momentum: lateral acceleration.
-    # TODO: something with lateralPlan.curvatureRates
     return desired_angle * (v_ego**2)
 
   def get_steer_feedforward_function(self):

selfdrive/controls/controlsd.py

@@ -17,8 +17,7 @@ from openpilot.common.swaglog import cloudlog
 from openpilot.system.version import get_short_branch
 from openpilot.selfdrive.boardd.boardd import can_list_to_can_capnp
 from openpilot.selfdrive.car.car_helpers import get_car, get_startup_event, get_one_can
-from openpilot.selfdrive.controls.lib.lateral_planner import CAMERA_OFFSET
+from openpilot.selfdrive.controls.lib.drive_helpers import VCruiseHelper, clip_curvature
-from openpilot.selfdrive.controls.lib.drive_helpers import VCruiseHelper, get_lag_adjusted_curvature
 from openpilot.selfdrive.controls.lib.latcontrol import LatControl, MIN_LATERAL_CONTROL_SPEED
 from openpilot.selfdrive.controls.lib.longcontrol import LongControl
 from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
@@ -32,6 +31,7 @@ from openpilot.system.hardware import HARDWARE
 SOFT_DISABLE_TIME = 3  # seconds
 LDW_MIN_SPEED = 31 * CV.MPH_TO_MS
 LANE_DEPARTURE_THRESHOLD = 0.1
+CAMERA_OFFSET = 0.04
 
 REPLAY = "REPLAY" in os.environ
 SIMULATION = "SIMULATION" in os.environ
@@ -41,9 +41,9 @@ IGNORE_PROCESSES = {"loggerd", "encoderd", "statsd"}
 ThermalStatus = log.DeviceState.ThermalStatus
 State = log.ControlsState.OpenpilotState
 PandaType = log.PandaState.PandaType
-Desire = log.LateralPlan.Desire
+Desire = log.Desire
-LaneChangeState = log.LateralPlan.LaneChangeState
+LaneChangeState = log.LaneChangeState
-LaneChangeDirection = log.LateralPlan.LaneChangeDirection
+LaneChangeDirection = log.LaneChangeDirection
 EventName = car.CarEvent.EventName
 ButtonType = car.CarState.ButtonEvent.Type
 SafetyModel = car.CarParams.SafetyModel
@@ -77,7 +77,7 @@ class Controls:
     if SIMULATION:
       ignore += ['driverCameraState', 'managerState']
     self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration',
-                                   'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman',
+                                   'driverMonitoringState', 'longitudinalPlan', 'liveLocationKalman',
                                    'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
                                    'testJoystick'] + self.camera_packets + self.sensor_packets,
                                   ignore_alive=ignore, ignore_avg_freq=['radarState', 'testJoystick'], ignore_valid=['testJoystick', ])
@@ -288,8 +288,8 @@ class Controls:
         self.events.add(EventName.calibrationInvalid)
 
     # Handle lane change
-    if self.sm['lateralPlan'].laneChangeState == LaneChangeState.preLaneChange:
+    if self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange:
-      direction = self.sm['lateralPlan'].laneChangeDirection
+      direction = self.sm['modelV2'].meta.laneChangeDirection
       if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \
          (CS.rightBlindspot and direction == LaneChangeDirection.right):
         self.events.add(EventName.laneChangeBlocked)
@@ -298,7 +298,7 @@ class Controls:
           self.events.add(EventName.preLaneChangeLeft)
         else:
           self.events.add(EventName.preLaneChangeRight)
-    elif self.sm['lateralPlan'].laneChangeState in (LaneChangeState.laneChangeStarting,
+    elif self.sm['modelV2'].meta.laneChangeState in (LaneChangeState.laneChangeStarting,
                                                     LaneChangeState.laneChangeFinishing):
       self.events.add(EventName.laneChange)
 
@@ -370,8 +370,6 @@ class Controls:
       self.logged_comm_issue = None
 
     if not (self.CP.notCar and self.joystick_mode):
-      if not self.sm['lateralPlan'].mpcSolutionValid:
-        self.events.add(EventName.plannerError)
       if not self.sm['liveLocationKalman'].posenetOK:
         self.events.add(EventName.posenetInvalid)
       if not self.sm['liveLocationKalman'].deviceStable:
@@ -572,8 +570,8 @@ class Controls:
         self.LaC.update_live_torque_params(torque_params.latAccelFactorFiltered, torque_params.latAccelOffsetFiltered,
                                            torque_params.frictionCoefficientFiltered)
 
-    lat_plan = self.sm['lateralPlan']
     long_plan = self.sm['longitudinalPlan']
+    model_v2 = self.sm['modelV2']
 
     CC = car.CarControl.new_message()
     CC.enabled = self.enabled
@@ -588,9 +586,9 @@ class Controls:
     actuators.longControlState = self.LoC.long_control_state
 
     # Enable blinkers while lane changing
-    if self.sm['lateralPlan'].laneChangeState != LaneChangeState.off:
+    if model_v2.meta.laneChangeState != LaneChangeState.off:
-      CC.leftBlinker = self.sm['lateralPlan'].laneChangeDirection == LaneChangeDirection.left
+      CC.leftBlinker = model_v2.meta.laneChangeDirection == LaneChangeDirection.left
-      CC.rightBlinker = self.sm['lateralPlan'].laneChangeDirection == LaneChangeDirection.right
+      CC.rightBlinker = model_v2.meta.laneChangeDirection == LaneChangeDirection.right
 
     if CS.leftBlinker or CS.rightBlinker:
       self.last_blinker_frame = self.sm.frame
@@ -609,11 +607,11 @@ class Controls:
       actuators.accel = self.LoC.update(CC.longActive, CS, long_plan, pid_accel_limits, t_since_plan)
 
       # Steering PID loop and lateral MPC
-      self.desired_curvature = get_lag_adjusted_curvature(self.CP, CS.vEgo, lat_plan.psis, lat_plan.curvatures)
+      self.desired_curvature = clip_curvature(CS.vEgo, self.desired_curvature, model_v2.action.desiredCurvature)
+      actuators.curvature = self.desired_curvature
       actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
                                                                              self.steer_limited, self.desired_curvature,
                                                                              self.sm['liveLocationKalman'])
-      actuators.curvature = self.desired_curvature
     else:
       lac_log = log.ControlsState.LateralDebugState.new_message()
       if self.sm.rcv_frame['testJoystick'] > 0:
@@ -651,7 +649,8 @@ class Controls:
         if undershooting and turning and good_speed and max_torque:
           lac_log.active and self.events.add(EventName.steerSaturated)
       elif lac_log.saturated:
-        dpath_points = lat_plan.dPathPoints
+        # TODO probably should not use dpath_points but curvature
+        dpath_points = model_v2.position.y
         if len(dpath_points):
           # Check if we deviated from the path
           # TODO use desired vs actual curvature
@@ -777,7 +776,7 @@ class Controls:
       controlsState.alertSound = current_alert.audible_alert
 
     controlsState.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan']
-    controlsState.lateralPlanMonoTime = self.sm.logMonoTime['lateralPlan']
+    controlsState.lateralPlanMonoTime = self.sm.logMonoTime['modelV2']
     controlsState.enabled = self.enabled
     controlsState.active = self.active
     controlsState.curvature = curvature

selfdrive/controls/lib/desire_helper.py

@@ -2,30 +2,30 @@ from cereal import log
 from openpilot.common.conversions import Conversions as CV
 from openpilot.common.realtime import DT_MDL
 
-LaneChangeState = log.LateralPlan.LaneChangeState
+LaneChangeState = log.LaneChangeState
-LaneChangeDirection = log.LateralPlan.LaneChangeDirection
+LaneChangeDirection = log.LaneChangeDirection
 
 LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS
 LANE_CHANGE_TIME_MAX = 10.
 
 DESIRES = {
   LaneChangeDirection.none: {
-    LaneChangeState.off: log.LateralPlan.Desire.none,
+    LaneChangeState.off: log.Desire.none,
-    LaneChangeState.preLaneChange: log.LateralPlan.Desire.none,
+    LaneChangeState.preLaneChange: log.Desire.none,
-    LaneChangeState.laneChangeStarting: log.LateralPlan.Desire.none,
+    LaneChangeState.laneChangeStarting: log.Desire.none,
-    LaneChangeState.laneChangeFinishing: log.LateralPlan.Desire.none,
+    LaneChangeState.laneChangeFinishing: log.Desire.none,
   },
   LaneChangeDirection.left: {
-    LaneChangeState.off: log.LateralPlan.Desire.none,
+    LaneChangeState.off: log.Desire.none,
-    LaneChangeState.preLaneChange: log.LateralPlan.Desire.none,
+    LaneChangeState.preLaneChange: log.Desire.none,
-    LaneChangeState.laneChangeStarting: log.LateralPlan.Desire.laneChangeLeft,
+    LaneChangeState.laneChangeStarting: log.Desire.laneChangeLeft,
-    LaneChangeState.laneChangeFinishing: log.LateralPlan.Desire.laneChangeLeft,
+    LaneChangeState.laneChangeFinishing: log.Desire.laneChangeLeft,
   },
   LaneChangeDirection.right: {
-    LaneChangeState.off: log.LateralPlan.Desire.none,
+    LaneChangeState.off: log.Desire.none,
-    LaneChangeState.preLaneChange: log.LateralPlan.Desire.none,
+    LaneChangeState.preLaneChange: log.Desire.none,
-    LaneChangeState.laneChangeStarting: log.LateralPlan.Desire.laneChangeRight,
+    LaneChangeState.laneChangeStarting: log.Desire.laneChangeRight,
-    LaneChangeState.laneChangeFinishing: log.LateralPlan.Desire.laneChangeRight,
+    LaneChangeState.laneChangeFinishing: log.Desire.laneChangeRight,
   },
 }
 
@@ -38,7 +38,7 @@ class DesireHelper:
     self.lane_change_ll_prob = 1.0
     self.keep_pulse_timer = 0.0
     self.prev_one_blinker = False
-    self.desire = log.LateralPlan.Desire.none
+    self.desire = log.Desire.none
 
   def update(self, carstate, lateral_active, lane_change_prob):
     v_ego = carstate.vEgo
@@ -110,5 +110,5 @@ class DesireHelper:
       self.keep_pulse_timer += DT_MDL
       if self.keep_pulse_timer > 1.0:
         self.keep_pulse_timer = 0.0
-      elif self.desire in (log.LateralPlan.Desire.keepLeft, log.LateralPlan.Desire.keepRight):
+      elif self.desire in (log.Desire.keepLeft, log.Desire.keepRight):
-        self.desire = log.LateralPlan.Desire.none
+        self.desire = log.Desire.none

selfdrive/controls/lib/drive_helpers.py

@@ -3,7 +3,7 @@ import math
 from cereal import car, log
 from openpilot.common.conversions import Conversions as CV
 from openpilot.common.numpy_fast import clip, interp
-from openpilot.common.realtime import DT_MDL
+from openpilot.common.realtime import DT_MDL, DT_CTRL
 from openpilot.selfdrive.modeld.constants import ModelConstants
 
 # WARNING: this value was determined based on the model's training distribution,
@@ -163,21 +163,27 @@ def rate_limit(new_value, last_value, dw_step, up_step):
   return clip(new_value, last_value + dw_step, last_value + up_step)
 
 
-def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures):
+def clip_curvature(v_ego, prev_curvature, new_curvature):
+  v_ego = max(MIN_SPEED, v_ego)
+  max_curvature_rate = MAX_LATERAL_JERK / (v_ego**2) # inexact calculation, check https://github.com/commaai/openpilot/pull/24755
+  safe_desired_curvature = clip(new_curvature,
+                                prev_curvature - max_curvature_rate * DT_CTRL,
+                                prev_curvature + max_curvature_rate * DT_CTRL)
+  return safe_desired_curvature
+
+
+def get_lag_adjusted_curvature(steer_delay, v_ego, psis, curvatures):
   if len(psis) != CONTROL_N:
     psis = [0.0]*CONTROL_N
     curvatures = [0.0]*CONTROL_N
   v_ego = max(MIN_SPEED, v_ego)
 
-  # TODO this needs more thought, use .2s extra for now to estimate other delays
-  delay = CP.steerActuatorDelay + .2
-
   # MPC can plan to turn the wheel and turn back before t_delay. This means
   # in high delay cases some corrections never even get commanded. So just use
   # psi to calculate a simple linearization of desired curvature
   current_curvature_desired = curvatures[0]
-  psi = interp(delay, ModelConstants.T_IDXS[:CONTROL_N], psis)
+  psi = interp(steer_delay, ModelConstants.T_IDXS[:CONTROL_N], psis)
-  average_curvature_desired = psi / (v_ego * delay)
+  average_curvature_desired = psi / (v_ego * steer_delay)
   desired_curvature = 2 * average_curvature_desired - current_curvature_desired
 
   # This is the "desired rate of the setpoint" not an actual desired rate

selfdrive/controls/lib/events.py

@@ -912,14 +912,6 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
     ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Cruise Is Off"),
   },
 
-  # For planning the trajectory Model Predictive Control (MPC) is used. This is
-  # an optimization algorithm that is not guaranteed to find a feasible solution.
-  # If no solution is found or the solution has a very high cost this alert is thrown.
-  EventName.plannerError: {
-    ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Planner Solution Error"),
-    ET.NO_ENTRY: NoEntryAlert("Planner Solution Error"),
-  },
-
   # When the relay in the harness box opens the CAN bus between the LKAS camera
   # and the rest of the car is separated. When messages from the LKAS camera
   # are received on the car side this usually means the relay hasn't opened correctly

selfdrive/controls/lib/lateral_planner.py

@@ -1,74 +0,0 @@
-import numpy as np
-from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, MIN_SPEED, get_speed_error
-from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
-import cereal.messaging as messaging
-from cereal import log
-
-TRAJECTORY_SIZE = 33
-CAMERA_OFFSET = 0.04
-
-class LateralPlanner:
-  def __init__(self, CP, debug=False):
-    self.DH = DesireHelper()
-
-    # Vehicle model parameters used to calculate lateral movement of car
-    self.factor1 = CP.wheelbase - CP.centerToFront
-    self.factor2 = (CP.centerToFront * CP.mass) / (CP.wheelbase * CP.tireStiffnessRear)
-    self.last_cloudlog_t = 0
-    self.solution_invalid_cnt = 0
-
-    self.path_xyz = np.zeros((TRAJECTORY_SIZE, 3))
-    self.velocity_xyz = np.zeros((TRAJECTORY_SIZE, 3))
-    self.v_plan = np.zeros((TRAJECTORY_SIZE,))
-    self.x_sol = np.zeros((TRAJECTORY_SIZE, 4), dtype=np.float32)
-    self.v_ego = MIN_SPEED
-    self.l_lane_change_prob = 0.0
-    self.r_lane_change_prob = 0.0
-
-    self.debug_mode = debug
-
-  def update(self, sm):
-    v_ego_car = sm['carState'].vEgo
-
-    # Parse model predictions
-    md = sm['modelV2']
-    if len(md.position.x) == TRAJECTORY_SIZE and len(md.velocity.x) == TRAJECTORY_SIZE and len(md.lateralPlannerSolution.x) == TRAJECTORY_SIZE:
-      self.path_xyz = np.column_stack([md.position.x, md.position.y, md.position.z])
-      self.velocity_xyz = np.column_stack([md.velocity.x, md.velocity.y, md.velocity.z])
-      car_speed = np.linalg.norm(self.velocity_xyz, axis=1) - get_speed_error(md, v_ego_car)
-      self.v_plan = np.clip(car_speed, MIN_SPEED, np.inf)
-      self.v_ego = self.v_plan[0]
-      self.x_sol = np.column_stack([md.lateralPlannerSolution.x, md.lateralPlannerSolution.y, md.lateralPlannerSolution.yaw, md.lateralPlannerSolution.yawRate])
-
-    # Lane change logic
-    desire_state = md.meta.desireState
-    if len(desire_state):
-      self.l_lane_change_prob = desire_state[log.LateralPlan.Desire.laneChangeLeft]
-      self.r_lane_change_prob = desire_state[log.LateralPlan.Desire.laneChangeRight]
-    lane_change_prob = self.l_lane_change_prob + self.r_lane_change_prob
-    self.DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
-
-  def publish(self, sm, pm):
-    plan_send = messaging.new_message('lateralPlan')
-    plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState', 'modelV2'])
-
-    lateralPlan = plan_send.lateralPlan
-    lateralPlan.modelMonoTime = sm.logMonoTime['modelV2']
-    lateralPlan.dPathPoints = self.path_xyz[:,1].tolist()
-    lateralPlan.psis = self.x_sol[0:CONTROL_N, 2].tolist()
-
-    lateralPlan.curvatures = (self.x_sol[0:CONTROL_N, 3]/self.v_ego).tolist()
-    lateralPlan.curvatureRates = [float(0) for _ in range(CONTROL_N-1)] # TODO: unused
-
-    lateralPlan.mpcSolutionValid = bool(1)
-    lateralPlan.solverExecutionTime = 0.0
-    if self.debug_mode:
-      lateralPlan.solverState = log.LateralPlan.SolverState.new_message()
-      lateralPlan.solverState.x = self.x_sol.tolist()
-
-    lateralPlan.desire = self.DH.desire
-    lateralPlan.useLaneLines = False
-    lateralPlan.laneChangeState = self.DH.lane_change_state
-    lateralPlan.laneChangeDirection = self.DH.lane_change_direction
-
-    pm.send('lateralPlan', plan_send)

selfdrive/controls/plannerd.py

@@ -1,29 +1,19 @@
 #!/usr/bin/env python3
-import os
-import numpy as np
 from cereal import car
 from openpilot.common.params import Params
 from openpilot.common.realtime import Priority, config_realtime_process
 from openpilot.common.swaglog import cloudlog
-from openpilot.selfdrive.modeld.constants import ModelConstants
 from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
-from openpilot.selfdrive.controls.lib.lateral_planner import LateralPlanner
 import cereal.messaging as messaging
 
-def cumtrapz(x, t):
+def publish_ui_plan(sm, pm, longitudinal_planner):
-  return np.concatenate([[0], np.cumsum(((x[0:-1] + x[1:])/2) * np.diff(t))])
-
-def publish_ui_plan(sm, pm, lateral_planner, longitudinal_planner):
-  plan_odo = cumtrapz(longitudinal_planner.v_desired_trajectory_full, ModelConstants.T_IDXS)
-  model_odo = cumtrapz(lateral_planner.v_plan, ModelConstants.T_IDXS)
-
   ui_send = messaging.new_message('uiPlan')
   ui_send.valid = sm.all_checks(service_list=['carState', 'controlsState', 'modelV2'])
   uiPlan = ui_send.uiPlan
   uiPlan.frameId = sm['modelV2'].frameId
-  uiPlan.position.x = np.interp(plan_odo, model_odo, lateral_planner.x_sol[:,0]).tolist()
+  uiPlan.position.x = list(sm['modelV2'].position.x)
-  uiPlan.position.y = np.interp(plan_odo, model_odo, lateral_planner.x_sol[:,1]).tolist()
+  uiPlan.position.y = list(sm['modelV2'].position.y)
-  uiPlan.position.z = np.interp(plan_odo, model_odo, lateral_planner.path_xyz[:,2]).tolist()
+  uiPlan.position.z = list(sm['modelV2'].position.z)
   uiPlan.accel = longitudinal_planner.a_desired_trajectory_full.tolist()
   pm.send('uiPlan', ui_send)
 
@@ -36,12 +26,8 @@ def plannerd_thread():
     CP = msg
   cloudlog.info("plannerd got CarParams: %s", CP.carName)
 
-  debug_mode = bool(int(os.getenv("DEBUG", "0")))
-
   longitudinal_planner = LongitudinalPlanner(CP)
-  lateral_planner = LateralPlanner(CP, debug=debug_mode)
+  pm = messaging.PubMaster(['longitudinalPlan', 'uiPlan'])
-
-  pm = messaging.PubMaster(['longitudinalPlan', 'lateralPlan', 'uiPlan'])
   sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2'],
                            poll=['radarState', 'modelV2'], ignore_avg_freq=['radarState'])
 
@@ -49,11 +35,9 @@ def plannerd_thread():
     sm.update()
 
     if sm.updated['modelV2']:
-      lateral_planner.update(sm)
-      lateral_planner.publish(sm, pm)
       longitudinal_planner.update(sm)
       longitudinal_planner.publish(sm, pm)
-      publish_ui_plan(sm, pm, lateral_planner, longitudinal_planner)
+      publish_ui_plan(sm, pm, longitudinal_planner)
 
 def main():
   plannerd_thread()

selfdrive/debug/cycle_alerts.py

@@ -54,7 +54,7 @@ def cycle_alerts(duration=200, is_metric=False):
   CS = car.CarState.new_message()
   CP = CarInterface.get_non_essential_params("HONDA CIVIC 2016")
   sm = messaging.SubMaster(['deviceState', 'pandaStates', 'roadCameraState', 'modelV2', 'liveCalibration',
-                            'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman',
+                            'driverMonitoringState', 'longitudinalPlan', 'liveLocationKalman',
                             'managerState'] + cameras)
 
   pm = messaging.PubMaster(['controlsState', 'pandaStates', 'deviceState'])

selfdrive/locationd/torqued.py

@@ -214,7 +214,7 @@ class TorqueEstimator(ParameterEstimator):
     return msg
 
 
-def main():
+def main(demo=False):
   config_realtime_process([0, 1, 2, 3], 5)
 
   pm = messaging.PubMaster(['liveTorqueParameters'])
@@ -242,4 +242,8 @@ def main():
       params.put_nonblocking("LiveTorqueParameters", msg.to_bytes())
 
 if __name__ == "__main__":
-  main()
+  import argparse
+  parser = argparse.ArgumentParser(description='Process the --demo argument.')
+  parser.add_argument('--demo', action='store_true', help='A boolean for demo mode.')
+  args = parser.parse_args()
+  main(demo=args.demo)

selfdrive/modeld/fill_model_msg.py

@@ -3,6 +3,7 @@ import capnp
 import numpy as np
 from typing import Dict
 from cereal import log
+from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_lag_adjusted_curvature, MIN_SPEED
 from openpilot.selfdrive.modeld.constants import ModelConstants, Plan, Meta
 
 SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
@@ -45,7 +46,7 @@ def fill_xyvat(builder, t, x, y, v, a, x_std=None, y_std=None, v_std=None, a_std
 def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: Dict[str, np.ndarray], publish_state: PublishState,
                    vipc_frame_id: int, vipc_frame_id_extra: int, frame_id: int, frame_drop: float,
                    timestamp_eof: int, timestamp_llk: int, model_execution_time: float,
-                   nav_enabled: bool, valid: bool) -> None:
+                   nav_enabled: bool, v_ego: float, steer_delay: float, valid: bool) -> None:
   frame_age = frame_id - vipc_frame_id if frame_id > vipc_frame_id else 0
   msg.valid = valid
 
@@ -72,9 +73,14 @@ def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: Dict[str,
   fill_xyzt(orientation_rate, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ORIENTATION_RATE].T)
 
   # lateral planning
-  solution = modelV2.lateralPlannerSolution
+  x, y, yaw, yawRate = [net_output_data['lat_planner_solution'][0,:,i].tolist() for i in range(4)]
-  solution.x, solution.y, solution.yaw, solution.yawRate = [net_output_data['lat_planner_solution'][0,:,i].tolist() for i in range(4)]
+  x_sol = np.column_stack([x, y, yaw, yawRate])
-  solution.xStd, solution.yStd, solution.yawStd, solution.yawRateStd = [net_output_data['lat_planner_solution_stds'][0,:,i].tolist() for i in range(4)]
+  v_ego = max(MIN_SPEED, v_ego)
+  psis = x_sol[0:CONTROL_N, 2].tolist()
+  curvatures = (x_sol[0:CONTROL_N, 3]/v_ego).tolist()
+
+  action = modelV2.action
+  action.desiredCurvature = get_lag_adjusted_curvature(steer_delay, v_ego, psis, curvatures)
 
   # times at X_IDXS according to model plan
   PLAN_T_IDXS = [np.nan] * ModelConstants.IDX_N

selfdrive/modeld/modeld

@@ -7,4 +7,4 @@ if [ -f "$DIR/libthneed.so" ]; then
   export LD_PRELOAD="$DIR/libthneed.so"
 fi
 
-exec "$DIR/modeld.py"
+exec "$DIR/modeld.py" "$@"

selfdrive/modeld/modeld.py

@@ -4,6 +4,7 @@ import time
 import pickle
 import numpy as np
 import cereal.messaging as messaging
+from cereal import car, log
 from pathlib import Path
 from typing import Dict, Optional
 from setproctitle import setproctitle
@@ -17,6 +18,8 @@ from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.common.realtime import config_realtime_process
 from openpilot.common.transformations.model import get_warp_matrix
 from openpilot.selfdrive import sentry
+from openpilot.selfdrive.car.car_helpers import get_demo_car_params
+from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
 from openpilot.selfdrive.modeld.runners import ModelRunner, Runtime
 from openpilot.selfdrive.modeld.parse_model_outputs import Parser
 from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState
@@ -93,7 +96,6 @@ class ModelState:
     self.inputs['traffic_convention'][:] = inputs['traffic_convention']
     self.inputs['nav_features'][:] = inputs['nav_features']
     self.inputs['nav_instructions'][:] = inputs['nav_instructions']
-    # self.inputs['driving_style'][:] = inputs['driving_style']
 
     # if getCLBuffer is not None, frame will be None
     self.model.setInputBuffer("input_imgs", self.frame.prepare(buf, transform.flatten(), self.model.getCLBuffer("input_imgs")))
@@ -113,7 +115,7 @@ class ModelState:
     return outputs
 
 
-def main():
+def main(demo=False):
   sentry.set_tag("daemon", PROCESS_NAME)
   cloudlog.bind(daemon=PROCESS_NAME)
   setproctitle(PROCESS_NAME)
@@ -148,7 +150,7 @@ def main():
 
   # messaging
   pm = PubMaster(["modelV2", "cameraOdometry"])
-  sm = SubMaster(["lateralPlan", "roadCameraState", "liveCalibration", "driverMonitoringState", "navModel", "navInstruction"])
+  sm = SubMaster(["carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "navModel", "navInstruction", "carControl"])
 
   publish_state = PublishState()
   params = Params()
@@ -162,13 +164,23 @@ def main():
   model_transform_main = np.zeros((3, 3), dtype=np.float32)
   model_transform_extra = np.zeros((3, 3), dtype=np.float32)
   live_calib_seen = False
-  driving_style = np.array([1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0], dtype=np.float32)
   nav_features = np.zeros(ModelConstants.NAV_FEATURE_LEN, dtype=np.float32)
   nav_instructions = np.zeros(ModelConstants.NAV_INSTRUCTION_LEN, dtype=np.float32)
   buf_main, buf_extra = None, None
   meta_main = FrameMeta()
   meta_extra = FrameMeta()
 
+
+  if demo:
+    CP = get_demo_car_params()
+  with car.CarParams.from_bytes(params.get("CarParams", block=True)) as msg:
+    CP = msg
+  cloudlog.info("plannerd got CarParams: %s", CP.carName)
+  # TODO this needs more thought, use .2s extra for now to estimate other delays
+  steer_delay = CP.steerActuatorDelay + .2
+  DH = DesireHelper()
+
+
   while True:
     # Keep receiving frames until we are at least 1 frame ahead of previous extra frame
     while meta_main.timestamp_sof < meta_extra.timestamp_sof + 25000000:
@@ -205,7 +217,8 @@ def main():
 
     # TODO: path planner timeout?
     sm.update(0)
-    desire = sm["lateralPlan"].desire.raw
+    desire = DH.desire
+    v_ego = sm["carState"].vEgo
     is_rhd = sm["driverMonitoringState"].isRHD
     frame_id = sm["roadCameraState"].frameId
     if sm.updated["liveCalibration"]:
@@ -261,7 +274,6 @@ def main():
     inputs:Dict[str, np.ndarray] = {
       'desire': vec_desire,
       'traffic_convention': traffic_convention,
-      'driving_style': driving_style,
       'nav_features': nav_features,
       'nav_instructions': nav_instructions}
 
@@ -274,7 +286,15 @@ def main():
       modelv2_send = messaging.new_message('modelV2')
       posenet_send = messaging.new_message('cameraOdometry')
       fill_model_msg(modelv2_send, model_output, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio,
-                      meta_main.timestamp_eof, timestamp_llk, model_execution_time, nav_enabled, live_calib_seen)
+                      meta_main.timestamp_eof, timestamp_llk, model_execution_time, nav_enabled, v_ego, steer_delay, live_calib_seen)
+
+      desire_state = modelv2_send.modelV2.meta.desireState
+      l_lane_change_prob = desire_state[log.Desire.laneChangeLeft]
+      r_lane_change_prob = desire_state[log.Desire.laneChangeRight]
+      lane_change_prob = l_lane_change_prob + r_lane_change_prob
+      DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
+      modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
+      modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
 
       fill_pose_msg(posenet_send, model_output, meta_main.frame_id, vipc_dropped_frames, meta_main.timestamp_eof, live_calib_seen)
       pm.send('modelV2', modelv2_send)
@@ -285,7 +305,11 @@ def main():
 
 if __name__ == "__main__":
   try:
-    main()
+    import argparse
+    parser = argparse.ArgumentParser()
+    parser.add_argument('--demo', action='store_true', help='A boolean for demo mode.')
+    args = parser.parse_args()
+    main(demo=args.demo)
   except KeyboardInterrupt:
     cloudlog.warning(f"child {PROCESS_NAME} got SIGINT")
   except Exception:

selfdrive/modeld/tests/test_modeld.py

@@ -7,6 +7,7 @@ import cereal.messaging as messaging
 from cereal.visionipc import VisionIpcServer, VisionStreamType
 from openpilot.common.transformations.camera import tici_f_frame_size
 from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.car.car_helpers import write_car_param
 from openpilot.selfdrive.manager.process_config import managed_processes
 from openpilot.selfdrive.test.process_replay.vision_meta import meta_from_camera_state
 
@@ -22,9 +23,10 @@ class TestModeld(unittest.TestCase):
     self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_DRIVER, 40, False, *tici_f_frame_size)
     self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_WIDE_ROAD, 40, False, *tici_f_frame_size)
     self.vipc_server.start_listener()
+    write_car_param()
 
     self.sm = messaging.SubMaster(['modelV2', 'cameraOdometry'])
-    self.pm = messaging.PubMaster(['roadCameraState', 'wideRoadCameraState', 'liveCalibration', 'lateralPlan'])
+    self.pm = messaging.PubMaster(['roadCameraState', 'wideRoadCameraState', 'liveCalibration'])
 
     managed_processes['modeld'].start()
     self.pm.wait_for_readers_to_update("roadCameraState", 10)

selfdrive/test/process_replay/model_replay.py

@@ -105,11 +105,11 @@ def nav_model_replay(lr):
 
 def model_replay(lr, frs):
   # modeld is using frame pairs
-  modeld_logs = trim_logs_to_max_frames(lr, MAX_FRAMES, {"roadCameraState", "wideRoadCameraState"}, {"roadEncodeIdx", "wideRoadEncodeIdx"})
+  modeld_logs = trim_logs_to_max_frames(lr, MAX_FRAMES, {"roadCameraState", "wideRoadCameraState"}, {"roadEncodeIdx", "wideRoadEncodeIdx", "carParams"})
-  dmodeld_logs = trim_logs_to_max_frames(lr, MAX_FRAMES, {"driverCameraState"}, {"driverEncodeIdx"})
+  dmodeld_logs = trim_logs_to_max_frames(lr, MAX_FRAMES, {"driverCameraState"}, {"driverEncodeIdx", "carParams"})
   if not SEND_EXTRA_INPUTS:
-    modeld_logs = [msg for msg in modeld_logs if msg.which() not in ["liveCalibration", "lateralPlan"]]
+    modeld_logs = [msg for msg in modeld_logs if msg.which() not in ["liveCalibration",]]
-    dmodeld_logs = [msg for msg in dmodeld_logs if msg.which() not in ["liveCalibration", "lateralPlan"]]
+    dmodeld_logs = [msg for msg in dmodeld_logs if msg.which() not in ["liveCalibration",]]
   # initial calibration
   cal_msg = next(msg for msg in lr if msg.which() == "liveCalibration").as_builder()
   cal_msg.logMonoTime = lr[0].logMonoTime

selfdrive/test/process_replay/model_replay_ref_commit

@@ -1 +1 @@
-ad64b6f38c1362e9d184f3fc95299284eacb56d4
+0513d29764980f512710cc2ebd7c14f91ae0351d

selfdrive/test/process_replay/process_replay.py

@@ -461,7 +461,7 @@ CONFIGS = [
     proc_name="controlsd",
     pubs=[
       "can", "deviceState", "pandaStates", "peripheralState", "liveCalibration", "driverMonitoringState",
-      "longitudinalPlan", "lateralPlan", "liveLocationKalman", "liveParameters", "radarState",
+      "longitudinalPlan", "liveLocationKalman", "liveParameters", "radarState",
       "modelV2", "driverCameraState", "roadCameraState", "wideRoadCameraState", "managerState",
       "testJoystick", "liveTorqueParameters", "accelerometer", "gyroscope"
     ],
@@ -486,8 +486,8 @@ CONFIGS = [
   ProcessConfig(
     proc_name="plannerd",
     pubs=["modelV2", "carControl", "carState", "controlsState", "radarState"],
-    subs=["lateralPlan", "longitudinalPlan", "uiPlan"],
+    subs=["longitudinalPlan", "uiPlan"],
-    ignore=["logMonoTime", "longitudinalPlan.processingDelay", "longitudinalPlan.solverExecutionTime", "lateralPlan.solverExecutionTime"],
+    ignore=["logMonoTime", "longitudinalPlan.processingDelay", "longitudinalPlan.solverExecutionTime"],
     init_callback=get_car_params_callback,
     should_recv_callback=FrequencyBasedRcvCallback("modelV2"),
     tolerance=NUMPY_TOLERANCE,
@@ -545,7 +545,7 @@ CONFIGS = [
   ),
   ProcessConfig(
     proc_name="modeld",
-    pubs=["lateralPlan", "roadCameraState", "wideRoadCameraState", "liveCalibration", "driverMonitoringState"],
+    pubs=["roadCameraState", "wideRoadCameraState", "liveCalibration", "driverMonitoringState"],
     subs=["modelV2", "cameraOdometry"],
     ignore=["logMonoTime", "modelV2.frameDropPerc", "modelV2.modelExecutionTime"],
     should_recv_callback=ModeldCameraSyncRcvCallback(),
@@ -555,6 +555,7 @@ CONFIGS = [
     main_pub_drained=False,
     vision_pubs=["roadCameraState", "wideRoadCameraState"],
     ignore_alive_pubs=["wideRoadCameraState"],
+    init_callback=get_car_params_callback,
   ),
   ProcessConfig(
     proc_name="dmonitoringmodeld",

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-1b981ce7f817974d4a7a28b06f01f727a5a7ea7b
+0dffa4e5634108f41d140c74052c38059038abd0

selfdrive/test/test_onroad.py

@@ -82,7 +82,6 @@ TIMINGS = {
   "carState": [2.5, 0.35],
   "carControl": [2.5, 0.35],
   "controlsState": [2.5, 0.35],
-  "lateralPlan": [2.5, 0.5],
   "longitudinalPlan": [2.5, 0.5],
   "roadCameraState": [2.5, 0.35],
   "driverCameraState": [2.5, 0.35],
@@ -344,7 +343,7 @@ class TestOnroad(unittest.TestCase):
     result += "-----------------  MPC Timing ------------------\n"
     result += "------------------------------------------------\n"
 
-    cfgs = [("lateralPlan", 0.05, 0.05), ("longitudinalPlan", 0.05, 0.05)]
+    cfgs = [("longitudinalPlan", 0.05, 0.05),]
     for (s, instant_max, avg_max) in cfgs:
       ts = [getattr(m, s).solverExecutionTime for m in self.service_msgs[s]]
       self.assertLess(max(ts), instant_max, f"high '{s}' execution time: {max(ts)}")

system/hardware/tici/tests/test_power_draw.py

@@ -10,6 +10,7 @@ from typing import List
 
 import cereal.messaging as messaging
 from cereal.services import SERVICE_LIST
+from openpilot.selfdrive.car.car_helpers import write_car_param
 from openpilot.system.hardware import HARDWARE
 from openpilot.system.hardware.tici.power_monitor import get_power
 from openpilot.selfdrive.manager.process_config import managed_processes
@@ -51,6 +52,7 @@ class TestPowerDraw(unittest.TestCase):
   def setUp(self):
     HARDWARE.initialize_hardware()
     HARDWARE.set_power_save(False)
+    write_car_param()
 
     # wait a bit for power save to disable
     time.sleep(5)

tools/latencylogger/README.md

@@ -53,9 +53,7 @@ Frame ID: 1202
     modelV2.modelExecutionTime                           23.62649142742157
     modelV2.gpuExecutionTime                             0.0
   plannerd
-    lateralPlan published                                66.915049
     longitudinalPlan published                           69.715999
-    lateralPlan.solverExecutionTime                      0.8170719956979156
     longitudinalPlan.solverExecutionTime                 0.5619999719783664
   controlsd
     Data sampled                                         70.217763

tools/latencylogger/latency_logger.py

@@ -13,13 +13,11 @@ from openpilot.tools.lib.logreader import LogReader
 DEMO_ROUTE = "9f583b1d93915c31|2022-05-18--10-49-51--0"
 
 SERVICES = ['camerad', 'modeld', 'plannerd', 'controlsd', 'boardd']
-# Retrieve controlsd frameId from lateralPlan, mismatch with longitudinalPlan will be ignored
 MONOTIME_KEYS = ['modelMonoTime', 'lateralPlanMonoTime']
 MSGQ_TO_SERVICE = {
   'roadCameraState': 'camerad',
   'wideRoadCameraState': 'camerad',
   'modelV2': 'modeld',
-  'lateralPlan': 'plannerd',
   'longitudinalPlan': 'plannerd',
   'sendcan': 'controlsd',
   'controlsState': 'controlsd'

tools/replay/ui.py

@@ -55,7 +55,7 @@ def ui_thread(addr):
   top_down_surface = pygame.surface.Surface((UP.lidar_x, UP.lidar_y), 0, 8)
 
   sm = messaging.SubMaster(['carState', 'longitudinalPlan', 'carControl', 'radarState', 'liveCalibration', 'controlsState',
-                            'liveTracks', 'modelV2', 'liveParameters', 'lateralPlan'], addr=addr)
+                            'liveTracks', 'modelV2', 'liveParameters'], addr=addr)
 
   img = np.zeros((480, 640, 3), dtype='uint8')
   imgff = None

tools/tuning/measure_steering_accuracy.py

@@ -51,8 +51,8 @@ class SteeringAccuracyTool:
     standstill = sm['carState'].standstill
     steer_limited = abs(sm['carControl'].actuators.steer - sm['carControl'].actuatorsOutput.steer) > 1e-2
     overriding = sm['carState'].steeringPressed
-    changing_lanes = sm['lateralPlan'].laneChangeState != 0
+    changing_lanes = sm['modelV2'].meta.laneChangeState != 0
-    d_path_points = sm['lateralPlan'].dPathPoints
+    model_points = sm['modelV2'].position.y
     # must be engaged, not at standstill, not overriding steering, and not changing lanes
     if active and not standstill and not overriding and not changing_lanes:
       self.cnt += 1
@@ -75,8 +75,8 @@ class SteeringAccuracyTool:
             self.speed_group_stats[group][angle_abs]["cnt"] += 1
             self.speed_group_stats[group][angle_abs]["err"] += angle_error
             self.speed_group_stats[group][angle_abs]["steer"] += abs(steer)
-            if len(d_path_points):
+            if len(model_points):
-              self.speed_group_stats[group][angle_abs]["dpp"] += abs(d_path_points[0])
+              self.speed_group_stats[group][angle_abs]["dpp"] += abs(model_points[0])
             if steer_limited:
               self.speed_group_stats[group][angle_abs]["limited"] += 1
             if control_state.saturated:
@@ -138,10 +138,10 @@ if __name__ == "__main__":
         sm['carControl'] = msg.carControl
       elif msg.which() == 'controlsState':
         sm['controlsState'] = msg.controlsState
-      elif msg.which() == 'lateralPlan':
+      elif msg.which() == 'modelV2':
-        sm['lateralPlan'] = msg.lateralPlan
+        sm['modelV2'] = msg.modelV2
 
-      if msg.which() == 'carControl' and 'carState' in sm and 'controlsState' in sm and 'lateralPlan' in sm:
+      if msg.which() == 'carControl' and 'carState' in sm and 'controlsState' in sm and 'modelV2' in sm:
         tool.update(sm)
 
   else:
@@ -150,7 +150,7 @@ if __name__ == "__main__":
       messaging.context = messaging.Context()
 
     carControl = messaging.sub_sock('carControl', addr=args.addr, conflate=True)
-    sm = messaging.SubMaster(['carState', 'carControl', 'controlsState', 'lateralPlan'], addr=args.addr)
+    sm = messaging.SubMaster(['carState', 'carControl', 'controlsState', 'modelV2'], addr=args.addr)
     time.sleep(1)  # Make sure all submaster data is available before going further
 
     print("waiting for messages...")