diff --git a/cereal b/cereal index 24a522f6ba..416c3d531c 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 24a522f6ba47aba12a9baea53020a8323673c79c +Subproject commit 416c3d531c90ce16498d782bf383625a857ee74c diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 92786f73d8..2417eb3c68 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/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,42 +19,26 @@ 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.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 @@ -82,66 +48,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 diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 1965bf8588..15dbd4c5e2 100755 --- a/selfdrive/controls/plannerd.py +++ b/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) diff --git a/selfdrive/modeld/constants.py b/selfdrive/modeld/constants.py index e03d93d7e8..4d3af51635 100644 --- a/selfdrive/modeld/constants.py +++ b/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 diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py index 55d7abc1aa..2b8a72b9be 100644 --- a/selfdrive/modeld/fill_model_msg.py +++ b/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 diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 4ad4038cbc..2b211b22e6 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -11,6 +11,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 @@ -54,6 +56,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), @@ -105,6 +108,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 diff --git a/selfdrive/modeld/models/supercombo.onnx b/selfdrive/modeld/models/supercombo.onnx index fcbbb2c1f5..8325cc947e 100644 --- a/selfdrive/modeld/models/supercombo.onnx +++ b/selfdrive/modeld/models/supercombo.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:18e53fa5d0f9a185833d50fa0d777725ca715460062f3a2c050847588525d87c -size 52272467 +oid sha256:6330383805f2e64e750f61c47660d2ec8522f55d84217c0520584d2e0d7c3faf +size 52939093 diff --git a/selfdrive/modeld/parse_model_outputs.py b/selfdrive/modeld/parse_model_outputs.py index a7b160fcea..9d37a5fad4 100644 --- a/selfdrive/modeld/parse_model_outputs.py +++ b/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,)) diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit index 2a4113a77f..1d8befbcab 100644 --- a/selfdrive/test/process_replay/model_replay_ref_commit +++ b/selfdrive/test/process_replay/model_replay_ref_commit @@ -1 +1 @@ -0e0f55cf3bb2cf79b44adf190e6387a83deb6646 +eec02217169e6f5d71b8a376368ea96e83b10e4a diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index de54ba6e90..40ac91ee70 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -6e27d5c97fe6554a86e9ee8bb9259e0cc6df5bb1 \ No newline at end of file +dbea36698ba48429b201b138846165eb4c329b92 \ No newline at end of file diff --git a/selfdrive/test/process_replay/test_processes.py b/selfdrive/test/process_replay/test_processes.py index 2baeaa8e52..5429c9b63e 100755 --- a/selfdrive/test/process_replay/test_processes.py +++ b/selfdrive/test/process_replay/test_processes.py @@ -41,23 +41,23 @@ source_segments = [ ] segments = [ - ("BODY", "regen7FE9F3C7CE3|2023-10-25--23-56-32--0"), - ("HYUNDAI", "regen7519EF9EE71|2023-10-25--23-53-59--0"), - ("HYUNDAI2", "regenF68B9F1B286|2023-10-25--23-56-31--0"), - ("TOYOTA", "regen56DC072FA51|2023-10-25--23-53-51--0"), - ("TOYOTA2", "regen78130056536|2023-10-25--23-53-58--0"), - ("TOYOTA3", "regenC554B250909|2023-10-25--23-58-53--0"), - ("HONDA", "regen3ED625586FB|2023-10-25--23-56-29--0"), - ("HONDA2", "regen9F1A8F44FD5|2023-10-25--23-56-34--0"), - ("CHRYSLER", "regen60CE93181EA|2023-10-25--23-59-01--0"), - ("RAM", "regen9E2B62E8E9A|2023-10-26--00-00-41--0"), - ("SUBARU", "regenEEBF379E0ED|2023-10-26--00-01-37--0"), - ("GM", "regen0B0EE5D6E0D|2023-10-25--23-58-57--0"), - ("GM2", "regen043B44E4FBD|2023-10-26--00-03-51--0"), - ("NISSAN", "regen14F35E327BC|2023-10-26--00-01-22--0"), - ("VOLKSWAGEN", "regen63A052AE7D7|2023-10-26--00-01-36--0"), - ("MAZDA", "regenF9047685121|2023-10-26--00-05-02--0"), - ("FORD", "regen5115F2AE4FE|2023-10-26--00-06-17--0"), + ("BODY", "regen997DF2697CB|2023-10-30--23-14-29--0"), + ("HYUNDAI", "regen2A9D2A8E0B4|2023-10-30--23-13-34--0"), + ("HYUNDAI2", "regen6CA24BC3035|2023-10-30--23-14-28--0"), + ("TOYOTA", "regen5C019D76307|2023-10-30--23-13-31--0"), + ("TOYOTA2", "regen5DCADA88A96|2023-10-30--23-14-57--0"), + ("TOYOTA3", "regen7204CA3A498|2023-10-30--23-15-55--0"), + ("HONDA", "regen048F8FA0B24|2023-10-30--23-15-53--0"), + ("HONDA2", "regen7D2D3F82D5B|2023-10-30--23-15-55--0"), + ("CHRYSLER", "regen7125C42780C|2023-10-30--23-16-21--0"), + ("RAM", "regen2731F3213D2|2023-10-30--23-18-11--0"), + ("SUBARU", "regen86E4C1B4DDD|2023-10-30--23-18-14--0"), + ("GM", "regenF6393D64745|2023-10-30--23-17-18--0"), + ("GM2", "regen220F830C05B|2023-10-30--23-18-39--0"), + ("NISSAN", "regen4F671F7C435|2023-10-30--23-18-40--0"), + ("VOLKSWAGEN", "regen8BDFE7307A0|2023-10-30--23-19-36--0"), + ("MAZDA", "regen2E9F1A15FD5|2023-10-30--23-20-36--0"), + ("FORD", "regen6D39E54606E|2023-10-30--23-20-54--0"), ] # dashcamOnly makes don't need to be tested until a full port is done