changes

selfdrive/controls/lib/lateral_planner.py

@@ -1,10 +1,4 @@
-import time
 import numpy as np
-from openpilot.common.realtime import DT_MDL
-from openpilot.common.numpy_fast import interp
-from openpilot.system.swaglog import cloudlog
-from openpilot.selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import LateralMpc
-from openpilot.selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import N as LAT_MPC_N
 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
@@ -13,18 +7,6 @@ from cereal import log
 TRAJECTORY_SIZE = 33
 CAMERA_OFFSET = 0.04
 
-
-PATH_COST = 1.0
-LATERAL_MOTION_COST = 0.11
-LATERAL_ACCEL_COST = 0.0
-LATERAL_JERK_COST = 0.04
-# Extreme steering rate is unpleasant, even
-# when it does not cause bad jerk.
-# TODO this cost should be lowered when low
-# speed lateral control is stable on all cars
-STEERING_RATE_COST = 700.0
-
-
 class LateralPlanner:
   def __init__(self, CP, debug=False):
     self.DH = DesireHelper()
@@ -37,43 +19,27 @@ class LateralPlanner:
 
     self.path_xyz = np.zeros((TRAJECTORY_SIZE, 3))
     self.velocity_xyz = np.zeros((TRAJECTORY_SIZE, 3))
-    self.plan_yaw = np.zeros((TRAJECTORY_SIZE,))
-    self.plan_yaw_rate = np.zeros((TRAJECTORY_SIZE,))
-    self.t_idxs = np.arange(TRAJECTORY_SIZE)
-    self.y_pts = np.zeros((TRAJECTORY_SIZE,))
     self.v_plan = np.zeros((TRAJECTORY_SIZE,))
-    self.v_ego = 0.0
+    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.d_path_w_lines_xyz = np.zeros((TRAJECTORY_SIZE, 3))
 
     self.debug_mode = debug
 
-    self.lat_mpc = LateralMpc()
-    self.reset_mpc(np.zeros(4))
-
-  def reset_mpc(self, x0=None):
-    if x0 is None:
-      x0 = np.zeros(4)
-    self.x0 = x0
-    self.lat_mpc.reset(x0=self.x0)
-
   def update(self, sm):
-    # clip speed , lateral planning is not possible at 0 speed
-    measured_curvature = sm['controlsState'].curvature
     v_ego_car = sm['carState'].vEgo
 
     # Parse model predictions
     md = sm['modelV2']
-    if len(md.position.x) == TRAJECTORY_SIZE and len(md.orientation.x) == TRAJECTORY_SIZE:
+    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.t_idxs = np.array(md.position.t)
-      self.plan_yaw = np.array(md.orientation.z)
-      self.plan_yaw_rate = np.array(md.orientationRate.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
@@ -83,66 +49,23 @@ class LateralPlanner:
     lane_change_prob = self.l_lane_change_prob + self.r_lane_change_prob
     self.DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
 
-    self.lat_mpc.set_weights(PATH_COST, LATERAL_MOTION_COST,
-                             LATERAL_ACCEL_COST, LATERAL_JERK_COST,
-                             STEERING_RATE_COST)
-
-    y_pts = self.path_xyz[:LAT_MPC_N+1, 1]
-    heading_pts = self.plan_yaw[:LAT_MPC_N+1]
-    yaw_rate_pts = self.plan_yaw_rate[:LAT_MPC_N+1]
-    self.y_pts = y_pts
-
-    assert len(y_pts) == LAT_MPC_N + 1
-    assert len(heading_pts) == LAT_MPC_N + 1
-    assert len(yaw_rate_pts) == LAT_MPC_N + 1
-    lateral_factor = np.clip(self.factor1 - (self.factor2 * self.v_plan**2), 0.0, np.inf)
-    p = np.column_stack([self.v_plan, lateral_factor])
-    self.lat_mpc.run(self.x0,
-                     p,
-                     y_pts,
-                     heading_pts,
-                     yaw_rate_pts)
-    # init state for next iteration
-    # mpc.u_sol is the desired second derivative of psi given x0 curv state.
-    # with x0[3] = measured_yaw_rate, this would be the actual desired yaw rate.
-    # instead, interpolate x_sol so that x0[3] is the desired yaw rate for lat_control.
-    self.x0[3] = interp(DT_MDL, self.t_idxs[:LAT_MPC_N + 1], self.lat_mpc.x_sol[:, 3])
-
-    #  Check for infeasible MPC solution
-    mpc_nans = np.isnan(self.lat_mpc.x_sol[:, 3]).any()
-    t = time.monotonic()
-    if mpc_nans or self.lat_mpc.solution_status != 0:
-      self.reset_mpc()
-      self.x0[3] = measured_curvature * self.v_ego
-      if t > self.last_cloudlog_t + 5.0:
-        self.last_cloudlog_t = t
-        cloudlog.warning("Lateral mpc - nan: True")
-
-    if self.lat_mpc.cost > 1e6 or mpc_nans:
-      self.solution_invalid_cnt += 1
-    else:
-      self.solution_invalid_cnt = 0
-
   def publish(self, sm, pm):
-    plan_solution_valid = self.solution_invalid_cnt < 2
     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.y_pts.tolist()
-    lateralPlan.psis = self.lat_mpc.x_sol[0:CONTROL_N, 2].tolist()
+    lateralPlan.dPathPoints = self.path_xyz[:,1].tolist()
+    lateralPlan.psis = self.x_sol[0:CONTROL_N, 2].tolist()
 
-    lateralPlan.curvatures = (self.lat_mpc.x_sol[0:CONTROL_N, 3]/self.v_ego).tolist()
-    lateralPlan.curvatureRates = [float(x.item() / self.v_ego) for x in self.lat_mpc.u_sol[0:CONTROL_N - 1]] + [0.0]
+    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(plan_solution_valid)
-    lateralPlan.solverExecutionTime = self.lat_mpc.solve_time
+    lateralPlan.mpcSolutionValid = bool(1)
+    lateralPlan.solverExecutionTime = 0.0
     if self.debug_mode:
-      lateralPlan.solverCost = self.lat_mpc.cost
       lateralPlan.solverState = log.LateralPlan.SolverState.new_message()
-      lateralPlan.solverState.x = self.lat_mpc.x_sol.tolist()
-      lateralPlan.solverState.u = self.lat_mpc.u_sol.flatten().tolist()
+      lateralPlan.solverState.x = self.x_sol.tolist()
 
     lateralPlan.desire = self.DH.desire
     lateralPlan.useLaneLines = False

selfdrive/controls/plannerd.py

@@ -21,8 +21,8 @@ def publish_ui_plan(sm, pm, lateral_planner, longitudinal_planner):
   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.lat_mpc.x_sol[:,0]).tolist()
-  uiPlan.position.y = np.interp(plan_odo, model_odo, lateral_planner.lat_mpc.x_sol[:,1]).tolist()
+  uiPlan.position.x = np.interp(plan_odo, model_odo, lateral_planner.x_sol[:,0]).tolist()
+  uiPlan.position.y = np.interp(plan_odo, model_odo, lateral_planner.x_sol[:,1]).tolist()
   uiPlan.position.z = np.interp(plan_odo, model_odo, lateral_planner.path_xyz[:,2]).tolist()
   uiPlan.accel = longitudinal_planner.a_desired_trajectory_full.tolist()
   pm.send('uiPlan', ui_send)

selfdrive/modeld/constants.py

@@ -21,6 +21,7 @@ class ModelConstants:
   NAV_FEATURE_LEN = 256
   NAV_INSTRUCTION_LEN = 150
   DRIVING_STYLE_LEN = 12
+  LAT_PLANNER_STATE_LEN = 4
 
   # model outputs constants
   FCW_THRESHOLDS_5MS2 = np.array([.05, .05, .15, .15, .15], dtype=np.float32)
@@ -37,6 +38,7 @@ class ModelConstants:
   ROAD_EDGES_WIDTH = 2
   PLAN_WIDTH = 15
   DESIRE_PRED_WIDTH = 8
+  LAT_PLANNER_SOLUTION_WIDTH = 4
 
   NUM_LANE_LINES = 4
   NUM_ROAD_EDGES = 2

selfdrive/modeld/fill_model_msg.py

@@ -71,6 +71,11 @@ def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: Dict[str,
   orientation_rate = modelV2.orientationRate
   fill_xyzt(orientation_rate, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ORIENTATION_RATE].T)
 
+  # lateral planning
+  solution = modelV2.lateralPlannerSolution
+  solution.x, solution.y, solution.yaw, solution.yawRate = [net_output_data['lat_planner_solution'][0,:,i].tolist() for i in range(4)]
+  solution.xStd, solution.yStd, solution.yawStd, solution.yawRateStd = [net_output_data['lat_planner_solution_stds'][0,:,i].tolist() for i in range(4)]
+
   # times at X_IDXS according to model plan
   PLAN_T_IDXS = [np.nan] * ModelConstants.IDX_N
   PLAN_T_IDXS[0] = 0.0

selfdrive/modeld/modeld.py

@@ -12,6 +12,8 @@ from cereal.messaging import PubMaster, SubMaster
 from cereal.visionipc import VisionIpcClient, VisionStreamType, VisionBuf
 from openpilot.system.swaglog import cloudlog
 from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.common.numpy_fast import interp
 from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.common.realtime import config_realtime_process
 from openpilot.common.transformations.model import get_warp_matrix
@@ -53,6 +55,7 @@ class ModelState:
     self.inputs = {
       'desire': np.zeros(ModelConstants.DESIRE_LEN * (ModelConstants.HISTORY_BUFFER_LEN+1), dtype=np.float32),
       'traffic_convention': np.zeros(ModelConstants.TRAFFIC_CONVENTION_LEN, dtype=np.float32),
+      'lat_planner_state': np.zeros(ModelConstants.LAT_PLANNER_STATE_LEN, 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),
       'features_buffer': np.zeros(ModelConstants.HISTORY_BUFFER_LEN * ModelConstants.FEATURE_LEN, dtype=np.float32),
@@ -104,6 +107,8 @@ class ModelState:
 
     self.inputs['features_buffer'][:-ModelConstants.FEATURE_LEN] = self.inputs['features_buffer'][ModelConstants.FEATURE_LEN:]
     self.inputs['features_buffer'][-ModelConstants.FEATURE_LEN:] = outputs['hidden_state'][0, :]
+    self.inputs['lat_planner_state'][2] = interp(DT_MDL, ModelConstants.T_IDXS, outputs['lat_planner_solution'][0, :, 2])
+    self.inputs['lat_planner_state'][3] = interp(DT_MDL, ModelConstants.T_IDXS, outputs['lat_planner_solution'][0, :, 3])
     return outputs
 
 

selfdrive/modeld/parse_model_outputs.py

@@ -93,6 +93,7 @@ class Parser:
     self.parse_mdn('wide_from_device_euler', outs, in_N=0, out_N=0, out_shape=(ModelConstants.WIDE_FROM_DEVICE_WIDTH,))
     self.parse_mdn('lead', outs, in_N=ModelConstants.LEAD_MHP_N, out_N=ModelConstants.LEAD_MHP_SELECTION,
                    out_shape=(ModelConstants.LEAD_TRAJ_LEN,ModelConstants.LEAD_WIDTH))
+    self.parse_mdn('lat_planner_solution', outs, in_N=0, out_N=0, out_shape=(ModelConstants.IDX_N,ModelConstants.LAT_PLANNER_SOLUTION_WIDTH))
     for k in ['lead_prob', 'lane_lines_prob', 'meta']:
       self.parse_binary_crossentropy(k, outs)
     self.parse_categorical_crossentropy('desire_state', outs, out_shape=(ModelConstants.DESIRE_PRED_WIDTH,))