From ad43247ef6bf913375e3f4b34d5832f4c0f19d23 Mon Sep 17 00:00:00 2001 From: Yassine Date: Fri, 15 Sep 2023 13:40:42 -0700 Subject: [PATCH] 6f6e3749-1b7c-42e8-a33b-03929b7fc476/700 --- cereal | 2 +- selfdrive/controls/lib/lateral_planner.py | 92 +++-------------------- selfdrive/controls/plannerd.py | 4 +- selfdrive/modeld/models/driving.cc | 28 +++++++ selfdrive/modeld/models/driving.h | 15 ++++ selfdrive/modeld/models/supercombo.onnx | 4 +- 6 files changed, 60 insertions(+), 85 deletions(-) diff --git a/cereal b/cereal index 4b334f6f10..6bb9458726 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 4b334f6f10877e4a666b23983de2d27934ebf3b1 +Subproject commit 6bb94587261692da139ddc861ceaa4a3baff52aa diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 92786f73d8..7c11559467 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -1,29 +1,10 @@ -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 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): @@ -42,29 +23,21 @@ class LateralPlanner: self.t_idxs = np.arange(TRAJECTORY_SIZE) self.y_pts = np.zeros((TRAJECTORY_SIZE,)) self.v_plan = np.zeros((TRAJECTORY_SIZE,)) + self.x_sol = np.zeros((TRAJECTORY_SIZE, 4), dtype=np.float32) self.v_ego = 0.0 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 + # TODO: do something for 0 speed + # TODO: is a small first order filter needed here? 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.orientation.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) @@ -73,6 +46,8 @@ class LateralPlanner: 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] + # YOLO e2e planning + 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 +57,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.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 2b23a0440e..654ac28184 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/models/driving.cc b/selfdrive/modeld/models/driving.cc index 0a7f0c949d..734054ef96 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -175,6 +175,33 @@ void fill_plan(cereal::ModelDataV2::Builder &framed, const ModelOutputPlanPredic fill_xyzt(framed.initOrientationRate(), T_IDXS_FLOAT, rot_rate_x, rot_rate_y, rot_rate_z); } + +void fill_lateral_planner(cereal::ModelDataV2::Builder &framed, const LateralPlannerOutput &model_lateral_planner_solution) { + std::array lateral_plan_solution_x, lateral_plan_solution_y, lateral_plan_solution_yaw, lateral_plan_solution_yaw_rate; + std::array lateral_plan_solution_x_std, lateral_plan_solution_y_std, lateral_plan_solution_yaw_std, lateral_plan_solution_yaw_rate_std; + + for (int i=0; i &plan_t, const ModelOutputLaneLines &lanes) { std::array left_far_y, left_far_z; @@ -258,6 +285,7 @@ void fill_model(cereal::ModelDataV2::Builder &framed, const ModelOutput &net_out } fill_plan(framed, best_plan); + fill_lateral_planner(framed, net_outputs.lateral_planner_solution); fill_lane_lines(framed, plan_t, net_outputs.lane_lines); fill_road_edges(framed, plan_t, net_outputs.road_edges); diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h index de0250d212..876f79b44a 100644 --- a/selfdrive/modeld/models/driving.h +++ b/selfdrive/modeld/models/driving.h @@ -47,6 +47,20 @@ struct ModelOutputYZ { }; static_assert(sizeof(ModelOutputYZ) == sizeof(float)*2); +struct LateralPlannerOutputElement { + float x; + float y; + float yaw; + float yaw_rate; +}; +static_assert(sizeof(LateralPlannerOutputElement) == sizeof(float)*4); + +struct LateralPlannerOutput { + std::array mean; + std::array std; +}; +static_assert(sizeof(LateralPlannerOutput) == (sizeof(LateralPlannerOutputElement)*TRAJECTORY_SIZE*2)); + struct ModelOutputPlanElement { ModelOutputXYZ position; ModelOutputXYZ velocity; @@ -241,6 +255,7 @@ struct ModelOutput { const ModelOutputWideFromDeviceEuler wide_from_device_euler; const ModelOutputTemporalPose temporal_pose; const ModelOutputRoadTransform road_transform; + const LateralPlannerOutput lateral_planner_solution; }; constexpr int OUTPUT_SIZE = sizeof(ModelOutput) / sizeof(float); diff --git a/selfdrive/modeld/models/supercombo.onnx b/selfdrive/modeld/models/supercombo.onnx index 88f19a7d28..7aebdcf298 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:c63ea3eb6c9b5a20c7420c2dc6d6d0f80a6949a39f6d8b74e574f52734154820 -size 47654714 +oid sha256:5fc9b6544e8f8f44b746134630d49ee1858497bd216c2197e7344c4732369af2 +size 48319232