diff --git a/cereal b/cereal index d3a943ef66..6323950101 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit d3a943ef660dd29f73700806ee0baf1d5aff6834 +Subproject commit 632395010102aabdd0ed87aba50d25042cdcb70e diff --git a/release/files_common b/release/files_common index 10c66a8960..ad99af752f 100644 --- a/release/files_common +++ b/release/files_common @@ -173,7 +173,6 @@ selfdrive/controls/lib/alerts_offroad.json selfdrive/controls/lib/desire_helper.py selfdrive/controls/lib/drive_helpers.py selfdrive/controls/lib/events.py -selfdrive/controls/lib/lane_planner.py selfdrive/controls/lib/latcontrol_angle.py selfdrive/controls/lib/latcontrol_indi.py selfdrive/controls/lib/latcontrol_torque.py diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index b8b66a5677..f2b2ddc98e 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -15,7 +15,7 @@ from system.swaglog import cloudlog from system.version import get_short_branch from selfdrive.boardd.boardd import can_list_to_can_capnp from selfdrive.car.car_helpers import get_car, get_startup_event, get_one_can -from selfdrive.controls.lib.lane_planner import CAMERA_OFFSET +from selfdrive.controls.lib.lateral_planner import CAMERA_OFFSET from selfdrive.controls.lib.drive_helpers import V_CRUISE_INITIAL, update_v_cruise, initialize_v_cruise from selfdrive.controls.lib.drive_helpers import get_lag_adjusted_curvature from selfdrive.controls.lib.latcontrol import LatControl diff --git a/selfdrive/controls/lib/lane_planner.py b/selfdrive/controls/lib/lane_planner.py deleted file mode 100644 index 1facb66d63..0000000000 --- a/selfdrive/controls/lib/lane_planner.py +++ /dev/null @@ -1,97 +0,0 @@ -import numpy as np -from cereal import log -from common.filter_simple import FirstOrderFilter -from common.numpy_fast import interp -from common.realtime import DT_MDL -from system.swaglog import cloudlog - - -TRAJECTORY_SIZE = 33 -# camera offset is meters from center car to camera -# model path is in the frame of the camera -PATH_OFFSET = 0.00 -CAMERA_OFFSET = 0.04 - - -class LanePlanner: - def __init__(self, wide_camera=False): - self.ll_t = np.zeros((TRAJECTORY_SIZE,)) - self.ll_x = np.zeros((TRAJECTORY_SIZE,)) - self.lll_y = np.zeros((TRAJECTORY_SIZE,)) - self.rll_y = np.zeros((TRAJECTORY_SIZE,)) - self.lane_width_estimate = FirstOrderFilter(3.7, 9.95, DT_MDL) - self.lane_width_certainty = FirstOrderFilter(1.0, 0.95, DT_MDL) - self.lane_width = 3.7 - - self.lll_prob = 0. - self.rll_prob = 0. - self.d_prob = 0. - - self.lll_std = 0. - self.rll_std = 0. - - self.l_lane_change_prob = 0. - self.r_lane_change_prob = 0. - - self.camera_offset = -CAMERA_OFFSET if wide_camera else CAMERA_OFFSET - self.path_offset = -PATH_OFFSET if wide_camera else PATH_OFFSET - - def parse_model(self, md): - lane_lines = md.laneLines - if len(lane_lines) == 4 and len(lane_lines[0].t) == TRAJECTORY_SIZE: - self.ll_t = (np.array(lane_lines[1].t) + np.array(lane_lines[2].t))/2 - # left and right ll x is the same - self.ll_x = lane_lines[1].x - self.lll_y = np.array(lane_lines[1].y) + self.camera_offset - self.rll_y = np.array(lane_lines[2].y) + self.camera_offset - self.lll_prob = md.laneLineProbs[1] - self.rll_prob = md.laneLineProbs[2] - self.lll_std = md.laneLineStds[1] - self.rll_std = md.laneLineStds[2] - - 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] - - def get_d_path(self, v_ego, path_t, path_xyz): - # Reduce reliance on lanelines that are too far apart or - # will be in a few seconds - path_xyz[:, 1] += self.path_offset - l_prob, r_prob = self.lll_prob, self.rll_prob - width_pts = self.rll_y - self.lll_y - prob_mods = [] - for t_check in (0.0, 1.5, 3.0): - width_at_t = interp(t_check * (v_ego + 7), self.ll_x, width_pts) - prob_mods.append(interp(width_at_t, [4.0, 5.0], [1.0, 0.0])) - mod = min(prob_mods) - l_prob *= mod - r_prob *= mod - - # Reduce reliance on uncertain lanelines - l_std_mod = interp(self.lll_std, [.15, .3], [1.0, 0.0]) - r_std_mod = interp(self.rll_std, [.15, .3], [1.0, 0.0]) - l_prob *= l_std_mod - r_prob *= r_std_mod - - # Find current lanewidth - self.lane_width_certainty.update(l_prob * r_prob) - current_lane_width = abs(self.rll_y[0] - self.lll_y[0]) - self.lane_width_estimate.update(current_lane_width) - speed_lane_width = interp(v_ego, [0., 31.], [2.8, 3.5]) - self.lane_width = self.lane_width_certainty.x * self.lane_width_estimate.x + \ - (1 - self.lane_width_certainty.x) * speed_lane_width - - clipped_lane_width = min(4.0, self.lane_width) - path_from_left_lane = self.lll_y + clipped_lane_width / 2.0 - path_from_right_lane = self.rll_y - clipped_lane_width / 2.0 - - self.d_prob = l_prob + r_prob - l_prob * r_prob - lane_path_y = (l_prob * path_from_left_lane + r_prob * path_from_right_lane) / (l_prob + r_prob + 0.0001) - safe_idxs = np.isfinite(self.ll_t) - if safe_idxs[0]: - lane_path_y_interp = np.interp(path_t, self.ll_t[safe_idxs], lane_path_y[safe_idxs]) - path_xyz[:,1] = self.d_prob * lane_path_y_interp + (1.0 - self.d_prob) * path_xyz[:,1] - else: - cloudlog.warning("Lateral mpc - NaNs in laneline times, ignoring") - return path_xyz diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 29dfc77111..3470754bc6 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -4,16 +4,15 @@ from common.numpy_fast import interp from system.swaglog import cloudlog from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import LateralMpc from selfdrive.controls.lib.drive_helpers import CONTROL_N, MPC_COST_LAT, LAT_MPC_N -from selfdrive.controls.lib.lane_planner import LanePlanner, TRAJECTORY_SIZE from 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, use_lanelines=True, wide_camera=False): - self.use_lanelines = use_lanelines - self.LP = LanePlanner(wide_camera) + def __init__(self, CP): self.DH = DesireHelper() # Vehicle model parameters used to calculate lateral movement of car @@ -42,7 +41,6 @@ class LateralPlanner: # Parse model predictions md = sm['modelV2'] - self.LP.parse_model(md) if len(md.position.x) == TRAJECTORY_SIZE and len(md.orientation.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) @@ -51,23 +49,17 @@ class LateralPlanner: self.path_xyz_stds = np.column_stack([md.position.xStd, md.position.yStd, md.position.zStd]) # Lane change logic - lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob + 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) - # Turn off lanes during lane change - if self.DH.desire == log.LateralPlan.Desire.laneChangeRight or self.DH.desire == log.LateralPlan.Desire.laneChangeLeft: - self.LP.lll_prob *= self.DH.lane_change_ll_prob - self.LP.rll_prob *= self.DH.lane_change_ll_prob - - # Calculate final driving path and set MPC costs - if self.use_lanelines: - d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz) - self.lat_mpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, MPC_COST_LAT.STEER_RATE) - else: - d_path_xyz = self.path_xyz - # Heading cost is useful at low speed, otherwise end of plan can be off-heading - heading_cost = interp(v_ego, [5.0, 10.0], [MPC_COST_LAT.HEADING, 0.15]) - self.lat_mpc.set_weights(MPC_COST_LAT.PATH, heading_cost, MPC_COST_LAT.STEER_RATE) + d_path_xyz = self.path_xyz + # Heading cost is useful at low speed, otherwise end of plan can be off-heading + heading_cost = interp(v_ego, [5.0, 10.0], [MPC_COST_LAT.HEADING, 0.15]) + self.lat_mpc.set_weights(MPC_COST_LAT.PATH, heading_cost, MPC_COST_LAT.STEER_RATE) y_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:, 1]) heading_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw) @@ -112,20 +104,16 @@ class LateralPlanner: lateralPlan = plan_send.lateralPlan lateralPlan.modelMonoTime = sm.logMonoTime['modelV2'] - lateralPlan.laneWidth = float(self.LP.lane_width) lateralPlan.dPathPoints = self.y_pts.tolist() lateralPlan.psis = self.lat_mpc.x_sol[0:CONTROL_N, 2].tolist() lateralPlan.curvatures = self.lat_mpc.x_sol[0:CONTROL_N, 3].tolist() lateralPlan.curvatureRates = [float(x) for x in self.lat_mpc.u_sol[0:CONTROL_N - 1]] + [0.0] - lateralPlan.lProb = float(self.LP.lll_prob) - lateralPlan.rProb = float(self.LP.rll_prob) - lateralPlan.dProb = float(self.LP.d_prob) lateralPlan.mpcSolutionValid = bool(plan_solution_valid) lateralPlan.solverExecutionTime = self.lat_mpc.solve_time lateralPlan.desire = self.DH.desire - lateralPlan.useLaneLines = self.use_lanelines + lateralPlan.useLaneLines = False lateralPlan.laneChangeState = self.DH.lane_change_state lateralPlan.laneChangeDirection = self.DH.lane_change_direction diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index ca7523f2ee..513131a33b 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -16,13 +16,8 @@ def plannerd_thread(sm=None, pm=None): CP = car.CarParams.from_bytes(params.get("CarParams", block=True)) cloudlog.info("plannerd got CarParams: %s", CP.carName) - use_lanelines = False - wide_camera = params.get_bool('WideCameraOnly') - - cloudlog.event("e2e mode", on=use_lanelines) - longitudinal_planner = Planner(CP) - lateral_planner = LateralPlanner(CP, use_lanelines=use_lanelines, wide_camera=wide_camera) + lateral_planner = LateralPlanner(CP) if sm is None: sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2'], diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 672763461a..cf3eb89f94 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -5cb8e7ea92f333bdb49682b0593ab2ae5a5f3824 +e1c189b002a179763fa34f24e5d96f2b2d0c4c49