From eeab29969ade0027978eb1f91044f4e2c0255bef Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Thu, 8 Feb 2024 23:34:05 -0500 Subject: [PATCH] Lateral Planner: Reimplement for legacy models --- selfdrive/controls/controlsd.py | 37 ++++++--- selfdrive/controls/lib/desire_helper.py | 13 ++- selfdrive/controls/lib/drive_helpers.py | 61 +++++++++++++- selfdrive/controls/lib/events.py | 8 ++ selfdrive/controls/lib/lateral_planner.py | 41 ++-------- selfdrive/modeld/constants.py | 1 + selfdrive/modeld/fill_model_msg.py | 13 ++- selfdrive/modeld/modeld.py | 98 +++++++++++++++-------- 8 files changed, 190 insertions(+), 82 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index fed939fbcd..67ef365eeb 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -19,7 +19,7 @@ from openpilot.system.version import get_short_branch from openpilot.selfdrive.athena.registration import is_registered_device 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.drive_helpers import VCruiseHelper, clip_curvature +from openpilot.selfdrive.controls.lib.drive_helpers import VCruiseHelper, clip_curvature, 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 @@ -57,6 +57,8 @@ ACTUATOR_FIELDS = tuple(car.CarControl.Actuators.schema.fields.keys()) ACTIVE_STATES = (State.enabled, State.softDisabling, State.overriding) ENABLED_STATES = (State.preEnabled, *ACTIVE_STATES) +MODEL_USE_LATERAL_PLANNER = False + class Controls: def __init__(self, CI=None): @@ -361,10 +363,12 @@ class Controls: self.events.add(EventName.calibrationInvalid) # Handle lane change - if self.sm['modelV2SP'].laneChangeEdgeBlock: + lane_change_edge_block = self.sm['lateralPlanSPDEPRECATED'].laneChangeEdgeBlockDEPRECATED if MODEL_USE_LATERAL_PLANNER else self.sm['modelV2SP'].laneChangeEdgeBlock + lane_change_svs = self.sm['lateralPlanDEPRECATED'] if MODEL_USE_LATERAL_PLANNER else self.sm['modelV2'].meta + if lane_change_svs.laneChangeState == LaneChangeState.preLaneChange and lane_change_edge_block: self.events.add(EventName.laneChangeRoadEdge) - elif self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange: - direction = self.sm['modelV2'].meta.laneChangeDirection + elif lane_change_svs.laneChangeState == LaneChangeState.preLaneChange: + direction = lane_change_svs.laneChangeDirection if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \ (CS.rightBlindspot and direction == LaneChangeDirection.right): self.events.add(EventName.laneChangeBlocked) @@ -373,7 +377,7 @@ class Controls: self.events.add(EventName.preLaneChangeLeft) else: self.events.add(EventName.preLaneChangeRight) - elif self.sm['modelV2'].meta.laneChangeState in (LaneChangeState.laneChangeStarting, + elif lane_change_svs.laneChangeState in (LaneChangeState.laneChangeStarting, LaneChangeState.laneChangeFinishing): self.events.add(EventName.laneChange) @@ -450,6 +454,9 @@ class Controls: self.logged_comm_issue = None if not (self.CP.notCar and self.joystick_mode): + if MODEL_USE_LATERAL_PLANNER: + if not self.sm['lateralPlan'].mpcSolutionValid: + self.events.add(EventName.plannerErrorDEPRECATED) if not self.sm['liveLocationKalman'].posenetOK: self.events.add(EventName.posenetInvalid) if not self.sm['liveLocationKalman'].deviceStable: @@ -661,8 +668,10 @@ 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'] + blinker_svs = lat_plan if MODEL_USE_LATERAL_PLANNER else model_v2.meta CC = car.CarControl.new_message() CC.enabled = self.enabled @@ -680,9 +689,9 @@ class Controls: actuators.longControlState = self.LoC.long_control_state # Enable blinkers while lane changing - if model_v2.meta.laneChangeState != LaneChangeState.off: - CC.leftBlinker = model_v2.meta.laneChangeDirection == LaneChangeDirection.left - CC.rightBlinker = model_v2.meta.laneChangeDirection == LaneChangeDirection.right + if blinker_svs.laneChangeState != LaneChangeState.off: + CC.leftBlinker = blinker_svs.laneChangeDirection == LaneChangeDirection.left + CC.rightBlinker = blinker_svs.laneChangeDirection == LaneChangeDirection.right if CS.leftBlinker or CS.rightBlinker: self.last_blinker_frame = self.sm.frame @@ -701,12 +710,17 @@ 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 = clip_curvature(CS.vEgo, self.desired_curvature, model_v2.action.desiredCurvature) + if MODEL_USE_LATERAL_PLANNER: + self.desired_curvature = get_lag_adjusted_curvature(self.CP, CS.vEgo, lat_plan.psis, lat_plan.curvatures) + else: + 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'], model_data=model_v2) + if MODEL_USE_LATERAL_PLANNER: + actuators.curvature = self.desired_curvature else: lac_log = log.ControlsState.LateralDebugState.new_message() if self.sm.rcv_frame['testJoystick'] > 0: @@ -745,7 +759,7 @@ class Controls: lac_log.active and self.events.add(EventName.steerSaturated) elif lac_log.saturated: # TODO probably should not use dpath_points but curvature - dpath_points = model_v2.position.y + dpath_points = lat_plan.dPathPoints if MODEL_USE_LATERAL_PLANNER else model_v2.position.y if len(dpath_points): # Check if we deviated from the path # TODO use desired vs actual curvature @@ -854,6 +868,7 @@ class Controls: # Curvature & Steering angle lp = self.sm['liveParameters'] + lp_mono_time_svs = 'lateralPlanDEPRECATED' if MODEL_USE_LATERAL_PLANNER else 'modelV2' steer_angle_without_offset = math.radians(CS.steeringAngleDeg - lp.angleOffsetDeg) curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo, lp.roll) @@ -872,7 +887,7 @@ class Controls: controlsState.alertSound = current_alert.audible_alert controlsState.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan'] - controlsState.lateralPlanMonoTime = self.sm.logMonoTime['modelV2'] + controlsState.lateralPlanMonoTime = self.sm.logMonoTime[lp_mono_time_svs] controlsState.enabled = not (CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)) and (self.enabled or CS.cruiseState.enabled) and CS.gearShifter not in [GearShifter.park, GearShifter.reverse] controlsState.active = not (CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)) and (self.active or CS.cruiseState.enabled) controlsState.curvature = curvature diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index bea5ca578e..d178483f32 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -2,6 +2,7 @@ from cereal import log from openpilot.common.conversions import Conversions as CV from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL +from openpilot.selfdrive.controls.lib.drive_helpers import get_road_edge LaneChangeState = log.LaneChangeState LaneChangeDirection = log.LaneChangeDirection @@ -9,6 +10,8 @@ LaneChangeDirection = log.LaneChangeDirection LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS LANE_CHANGE_TIME_MAX = 10. +MODEL_USE_LATERAL_PLANNER = False + DESIRES = { LaneChangeDirection.none: { LaneChangeState.off: log.Desire.none, @@ -53,16 +56,19 @@ class DesireHelper: self.lane_change_wait_timer = 0 self.prev_lane_change = False self.prev_brake_pressed = False + self.road_edge = False self.param_read_counter = 0 self.read_param() + self.edge_toggle = self.param_s.get_bool("RoadEdge") self.lane_change_set_timer = int(self.param_s.get("AutoLaneChangeTimer", encoding="utf8")) self.lane_change_bsm_delay = self.param_s.get_bool("AutoLaneChangeBsmDelay") def read_param(self): + self.edge_toggle = self.param_s.get_bool("RoadEdge") self.lane_change_set_timer = int(self.param_s.get("AutoLaneChangeTimer", encoding="utf8")) self.lane_change_bsm_delay = self.param_s.get_bool("AutoLaneChangeBsmDelay") - def update(self, carstate, lateral_active, lane_change_prob, lat_plan_sp): + def update(self, carstate, lateral_active, lane_change_prob, model_data=None, lat_plan_sp=None): if self.param_read_counter % 50 == 0: self.read_param() self.param_read_counter += 1 @@ -71,6 +77,9 @@ class DesireHelper: one_blinker = carstate.leftBlinker != carstate.rightBlinker below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN + if MODEL_USE_LATERAL_PLANNER: + self.road_edge = get_road_edge(carstate, model_data, self.edge_toggle) + if not carstate.madsEnabled or self.lane_change_timer > LANE_CHANGE_TIME_MAX: self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none @@ -84,7 +93,7 @@ class DesireHelper: self.lane_change_wait_timer = 0 # LaneChangeState.preLaneChange - elif self.lane_change_state == LaneChangeState.preLaneChange and lat_plan_sp.laneChangeEdgeBlockDEPRECATED: + elif self.lane_change_state == LaneChangeState.preLaneChange and (self.road_edge if MODEL_USE_LATERAL_PLANNER else lat_plan_sp.laneChangeEdgeBlockDEPRECATED): self.lane_change_direction = LaneChangeDirection.none elif self.lane_change_state == LaneChangeState.preLaneChange: # Set lane change direction diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 4d5e82d831..61edf7aade 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -1,9 +1,11 @@ import math +import numpy as np from cereal import car, log, custom from openpilot.common.conversions import Conversions as CV from openpilot.common.numpy_fast import clip, interp -from openpilot.common.realtime import DT_CTRL +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, # model predictions above this speed can be unpredictable @@ -265,6 +267,32 @@ def clip_curvature(v_ego, prev_curvature, new_curvature): return safe_desired_curvature +def get_lag_adjusted_curvature(CP, 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) + average_curvature_desired = psi / (v_ego * delay) + desired_curvature = 2 * average_curvature_desired - current_curvature_desired + + # This is the "desired rate of the setpoint" not an actual desired rate + max_curvature_rate = MAX_LATERAL_JERK / (v_ego**2) # inexact calculation, check https://github.com/commaai/openpilot/pull/24755 + safe_desired_curvature = clip(desired_curvature, + current_curvature_desired - max_curvature_rate * DT_MDL, + current_curvature_desired + max_curvature_rate * DT_MDL) + + return safe_desired_curvature + + def get_friction(lateral_accel_error: float, lateral_accel_deadzone: float, friction_threshold: float, torque_params: car.CarParams.LateralTorqueTuning, friction_compensation: bool) -> float: friction_interp = interp( @@ -282,3 +310,34 @@ def get_speed_error(modelV2: log.ModelDataV2, v_ego: float) -> float: vel_err = clip(modelV2.temporalPose.trans[0] - v_ego, -MAX_VEL_ERR, MAX_VEL_ERR) return float(vel_err) return 0.0 + + +def get_road_edge(carstate, model_v2, toggle): + # Lane detection by FrogAi + one_blinker = carstate.leftBlinker != carstate.rightBlinker + if not toggle: + road_edge = False + elif one_blinker: + # Set the minimum lane threshold to 3.0 meters + min_lane_threshold = 3.0 + # Set the blinker index based on which signal is on + blinker_index = 0 if carstate.leftBlinker else 1 + desired_edge = model_v2.roadEdges[blinker_index] + current_lane = model_v2.laneLines[blinker_index + 1] + # Check if both the desired lane and the current lane have valid x and y values + if all([desired_edge.x, desired_edge.y, current_lane.x, current_lane.y]) and len(desired_edge.x) == len(current_lane.x): + # Interpolate the x and y values to the same length + x = np.linspace(desired_edge.x[0], desired_edge.x[-1], num=len(desired_edge.x)) + lane_y = np.interp(x, current_lane.x, current_lane.y) + desired_y = np.interp(x, desired_edge.x, desired_edge.y) + # Calculate the width of the lane we're wanting to change into + lane_width = np.abs(desired_y - lane_y) + # Set road_edge to False if the lane width is not larger than the threshold + road_edge = not (np.amax(lane_width) > min_lane_threshold) + else: + road_edge = True + else: + # Default to setting "road_edge" to False + road_edge = False + + return road_edge diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index 2b0c4c406d..9b650a4df3 100755 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -1085,6 +1085,14 @@ 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.plannerErrorDEPRECATED: { + 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 diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 8c6ad118e9..1fb08884d6 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -8,7 +8,7 @@ from openpilot.common.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.lane_planner import LanePlanner, TRAJECTORY_SIZE -from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, MIN_SPEED, get_speed_error +from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, MIN_SPEED, get_speed_error, get_road_edge from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper import cereal.messaging as messaging @@ -97,18 +97,18 @@ class LateralPlanner: # Parse model predictions md = sm['modelV2'] - self.LP.parse_model(md) # TODO: SP - Refactor to work with legacy models if MODEL_USE_LATERAL_PLANNER: + self.LP.parse_model(md) if len(md.position.x) == TRAJECTORY_SIZE and (len(md.orientation.x) == TRAJECTORY_SIZE or - (len(md.velocity.x) == TRAJECTORY_SIZE and len(md.lateralPlannerSolution.x) == TRAJECTORY_SIZE)): + (len(md.velocity.x) == TRAJECTORY_SIZE and len(md.lateralPlannerSolutionDEPRECATED.x) == TRAJECTORY_SIZE)): if len(md.orientation.x) == TRAJECTORY_SIZE: 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) if len(md.velocity.x) == TRAJECTORY_SIZE and len(md.lateralPlannerSolution.x) == TRAJECTORY_SIZE: - self.x_sol = np.column_stack([md.lateralPlannerSolution.x, md.lateralPlannerSolution.y, md.lateralPlannerSolution.yaw, md.lateralPlannerSolution.yawRate]) + self.x_sol = np.column_stack([md.lateralPlannerSolutionDEPRECATED.x, md.lateralPlannerSolutionDEPRECATED.y, md.lateralPlannerSolutionDEPRECATED.yaw, md.lateralPlannerSolutionDEPRECATED.yawRate]) 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) @@ -117,7 +117,7 @@ class LateralPlanner: # Lane change logic lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob - self.DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, md) + self.DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, model_data=md) # Turn off lanes during lane change if self.DH.desire == log.LateralPlan.Desire.laneChangeRight or self.DH.desire == log.LateralPlan.Desire.laneChangeLeft: @@ -175,7 +175,8 @@ class LateralPlanner: else: self.solution_invalid_cnt = 0 - self.get_road_edge(sm['carState'], md) + if not MODEL_USE_LATERAL_PLANNER: + self.road_edge = get_road_edge(sm['carState'], md, self.edge_toggle) def get_dynamic_lane_profile(self, longitudinal_plan_sp): if self.dynamic_lane_profile == 1: @@ -201,34 +202,6 @@ class LateralPlanner: return True return False - def get_road_edge(self, carstate, model_v2): - # Lane detection by FrogAi - one_blinker = carstate.leftBlinker != carstate.rightBlinker - if not self.edge_toggle: - self.road_edge = False - elif one_blinker: - # Set the minimum lane threshold to 3.0 meters - min_lane_threshold = 3.0 - # Set the blinker index based on which signal is on - blinker_index = 0 if carstate.leftBlinker else 1 - desired_edge = model_v2.roadEdges[blinker_index] - current_lane = model_v2.laneLines[blinker_index + 1] - # Check if both the desired lane and the current lane have valid x and y values - if all([desired_edge.x, desired_edge.y, current_lane.x, current_lane.y]) and len(desired_edge.x) == len(current_lane.x): - # Interpolate the x and y values to the same length - x = np.linspace(desired_edge.x[0], desired_edge.x[-1], num=len(desired_edge.x)) - lane_y = np.interp(x, current_lane.x, current_lane.y) - desired_y = np.interp(x, desired_edge.x, desired_edge.y) - # Calculate the width of the lane we're wanting to change into - lane_width = np.abs(desired_y - lane_y) - # Set road_edge to False if the lane width is not larger than the threshold - self.road_edge = not (np.amax(lane_width) > min_lane_threshold) - else: - self.road_edge = True - else: - # Default to setting "road_edge" to False - self.road_edge = False - def publish(self, sm, pm): plan_solution_valid = self.solution_invalid_cnt < 2 plan_send = messaging.new_message('lateralPlanDEPRECATED') diff --git a/selfdrive/modeld/constants.py b/selfdrive/modeld/constants.py index dda1ff5e33..8b40383034 100644 --- a/selfdrive/modeld/constants.py +++ b/selfdrive/modeld/constants.py @@ -23,6 +23,7 @@ class ModelConstants: DRIVING_STYLE_LEN = 12 LAT_PLANNER_STATE_LEN = 4 LATERAL_CONTROL_PARAMS_LEN = 2 + PREV_DESIRED_CURVS_LEN = 20 PREV_DESIRED_CURV_LEN = 1 # model outputs constants diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py index 93d1c7e77b..87a9542cd1 100644 --- a/selfdrive/modeld/fill_model_msg.py +++ b/selfdrive/modeld/fill_model_msg.py @@ -9,6 +9,10 @@ SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') ConfidenceClass = log.ModelDataV2.ConfidenceClass +MODEL_USE_LATERAL_PLANNER = False +MODEL_USE_DESIRED_CURVATURES = False + + class PublishState: def __init__(self): self.disengage_buffer = np.zeros(ModelConstants.CONFIDENCE_BUFFER_LEN*ModelConstants.DISENGAGE_WIDTH, dtype=np.float32) @@ -72,8 +76,13 @@ 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 - action = modelV2.action - action.desiredCurvature = float(net_output_data['desired_curvature'][0,0]) + if MODEL_USE_LATERAL_PLANNER: + 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)] + else: + action = modelV2.action + action.desiredCurvature = float(net_output_data['desired_curvature'][0,0]) # times at X_IDXS according to model plan PLAN_T_IDXS = [np.nan] * ModelConstants.IDX_N diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 327e7357e7..e3f4d2c836 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -12,6 +12,8 @@ from cereal.messaging import PubMaster, SubMaster from cereal.visionipc import VisionIpcClient, VisionStreamType, VisionBuf from openpilot.common.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 @@ -37,6 +39,9 @@ CUSTOM_MODEL_PATH = "/data/media/0/models" LaneChangeState = log.LaneChangeState +MODEL_USE_LATERAL_PLANNER = False +MODEL_USE_DESIRED_CURVATURES = False + class FrameMeta: frame_id: int = 0 @@ -62,13 +67,22 @@ 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), - 'lateral_control_params': np.zeros(ModelConstants.LATERAL_CONTROL_PARAMS_LEN, dtype=np.float32), - 'prev_desired_curv': np.zeros(ModelConstants.PREV_DESIRED_CURV_LEN * (ModelConstants.HISTORY_BUFFER_LEN+1), dtype=np.float32), + #'lateral_control_params': np.zeros(ModelConstants.LATERAL_CONTROL_PARAMS_LEN, dtype=np.float32), + #'prev_desired_curv': np.zeros(ModelConstants.PREV_DESIRED_CURV_LEN * (ModelConstants.HISTORY_BUFFER_LEN+1), 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), } + if MODEL_USE_LATERAL_PLANNER: + self.inputs['lat_planner_state'] = np.zeros(ModelConstants.LAT_PLANNER_STATE_LEN, dtype=np.float32) + else: + self.inputs['lateral_control_params'] = np.zeros(ModelConstants.LATERAL_CONTROL_PARAMS_LEN, dtype=np.float32) + if MODEL_USE_DESIRED_CURVATURES: + self.inputs['prev_desired_curvs'] = np.zeros(ModelConstants.PREV_DESIRED_CURVS_LEN, dtype=np.float32) + else: + self.inputs['prev_desired_curv'] = np.zeros(ModelConstants.PREV_DESIRED_CURV_LEN * (ModelConstants.HISTORY_BUFFER_LEN+1), dtype=np.float32) + with open(METADATA_PATH, 'rb') as f: model_metadata = pickle.load(f) @@ -80,13 +94,13 @@ class ModelState: self.param_s = Params() # TODO: SP - Refactor Driving Model Selector to support legacy models - #if self.param_s.get_bool("CustomDrivingModel"): - # _model_name = self.param_s.get("DrivingModelText", encoding="utf8") - # _model_paths = { - # ModelRunner.THNEED: f"{CUSTOM_MODEL_PATH}/supercombo-{_model_name}.thneed"} - #else: - # _model_paths = MODEL_PATHS - self.model = ModelRunner(MODEL_PATHS, self.output, Runtime.GPU, False, context) + if self.param_s.get_bool("CustomDrivingModel"): + _model_name = self.param_s.get("DrivingModelText", encoding="utf8") + _model_paths = { + ModelRunner.THNEED: f"{CUSTOM_MODEL_PATH}/supercombo-{_model_name}.thneed"} + else: + _model_paths = MODEL_PATHS + self.model = ModelRunner(_model_paths, self.output, Runtime.GPU, False, context) self.model.addInput("input_imgs", None) self.model.addInput("big_input_imgs", None) for k,v in self.inputs.items(): @@ -107,7 +121,8 @@ class ModelState: self.prev_desire[:] = inputs['desire'] self.inputs['traffic_convention'][:] = inputs['traffic_convention'] - self.inputs['lateral_control_params'][:] = inputs['lateral_control_params'] + if not MODEL_USE_LATERAL_PLANNER: + self.inputs['lateral_control_params'][:] = inputs['lateral_control_params'] self.inputs['nav_features'][:] = inputs['nav_features'] self.inputs['nav_instructions'][:] = inputs['nav_instructions'] @@ -124,8 +139,15 @@ 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['prev_desired_curv'][:-ModelConstants.PREV_DESIRED_CURV_LEN] = self.inputs['prev_desired_curv'][ModelConstants.PREV_DESIRED_CURV_LEN:] - self.inputs['prev_desired_curv'][-ModelConstants.PREV_DESIRED_CURV_LEN:] = outputs['desired_curvature'][0, :] + if MODEL_USE_LATERAL_PLANNER: + 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]) + elif MODEL_USE_DESIRED_CURVATURES: + self.inputs['prev_desired_curvs'][:-1] = self.inputs['prev_desired_curvs'][1:] + self.inputs['prev_desired_curvs'][-1] = outputs['desired_curvature'][0, 0] + else: + self.inputs['prev_desired_curv'][:-ModelConstants.PREV_DESIRED_CURV_LEN] = self.inputs['prev_desired_curv'][ModelConstants.PREV_DESIRED_CURV_LEN:] + self.inputs['prev_desired_curv'][-ModelConstants.PREV_DESIRED_CURV_LEN:] = outputs['desired_curvature'][0, :] return outputs @@ -164,14 +186,15 @@ def main(demo=False): # messaging pm = PubMaster(["modelV2", "modelV2SP", "cameraOdometry"]) - sm = SubMaster(["carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "navModel", "navInstruction", "carControl", "lateralPlanSPDEPRECATED"]) + sm = SubMaster(["carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "navModel", "navInstruction", "carControl", "lateralPlanDEPRECATED", "lateralPlanSPDEPRECATED"]) publish_state = PublishState() params = Params() - with car.CarParams.from_bytes(params.get("CarParams", block=True)) as msg: - steer_delay = msg.steerActuatorDelay + .2 - #steer_delay = 0.4 + if not MODEL_USE_LATERAL_PLANNER: + with car.CarParams.from_bytes(params.get("CarParams", block=True)) as msg: + steer_delay = msg.steerActuatorDelay + .2 + #steer_delay = 0.4 # setup filter to track dropped frames frame_dropped_filter = FirstOrderFilter(0., 10., 1. / ModelConstants.MODEL_FREQ) @@ -182,6 +205,8 @@ def main(demo=False): model_transform_main = np.zeros((3, 3), dtype=np.float32) model_transform_extra = np.zeros((3, 3), dtype=np.float32) live_calib_seen = False + if MODEL_USE_LATERAL_PLANNER: + 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 @@ -235,12 +260,13 @@ def main(demo=False): # TODO: path planner timeout? sm.update(0) - desire = DH.desire + desire = sm["lateralPlan"].desire.raw if MODEL_USE_LATERAL_PLANNER else DH.desire v_ego = sm["carState"].vEgo is_rhd = sm["driverMonitoringState"].isRHD frame_id = sm["roadCameraState"].frameId - # TODO add lag - lateral_control_params = np.array([sm["carState"].vEgo, steer_delay], dtype=np.float32) + if not MODEL_USE_LATERAL_PLANNER: + # TODO add lag + lateral_control_params = np.array([sm["carState"].vEgo, steer_delay], dtype=np.float32) if sm.updated["liveCalibration"]: device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32) model_transform_main = get_warp_matrix(device_from_calib_euler, main_wide_camera, False).astype(np.float32) @@ -294,10 +320,15 @@ def main(demo=False): inputs:Dict[str, np.ndarray] = { 'desire': vec_desire, 'traffic_convention': traffic_convention, - 'lateral_control_params': lateral_control_params, + #'lateral_control_params': lateral_control_params, 'nav_features': nav_features, 'nav_instructions': nav_instructions} + if MODEL_USE_LATERAL_PLANNER: + inputs['driving_style'] = driving_style + else: + inputs['lateral_control_params'] = lateral_control_params + mt1 = time.perf_counter() model_output = model.run(buf_main, buf_extra, model_transform_main, model_transform_extra, inputs, prepare_only) mt2 = time.perf_counter() @@ -307,27 +338,30 @@ def main(demo=False): if model_output is not None: modelv2_send = messaging.new_message('modelV2') - modelv2_sp_send = messaging.new_message('modelV2SP') 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, 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, lat_plan_sp) - modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state - modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction - modelv2_sp_send.valid = True - modelv2_sp_send.modelV2SP.laneChangePrev = DH.prev_lane_change - modelv2_sp_send.modelV2SP.laneChangeEdgeBlock = DH.lane_change_state == LaneChangeState.preLaneChange and lat_plan_sp.laneChangeEdgeBlockDEPRECATED + if not MODEL_USE_LATERAL_PLANNER: + 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, lat_plan_sp=lat_plan_sp) + 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) - pm.send('modelV2SP', modelv2_sp_send) pm.send('cameraOdometry', posenet_send) + if not MODEL_USE_LATERAL_PLANNER: + modelv2_sp_send = messaging.new_message('modelV2SP') + modelv2_sp_send.valid = True + modelv2_sp_send.modelV2SP.laneChangePrev = DH.prev_lane_change + modelv2_sp_send.modelV2SP.laneChangeEdgeBlock = lat_plan_sp.laneChangeEdgeBlockDEPRECATED + pm.send('modelV2SP', modelv2_sp_send) + last_vipc_frame_id = meta_main.frame_id