mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-07-24 01:12:06 +08:00
Lateral Planner: Reimplement for legacy models
This commit is contained in:
Jason Wen
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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')
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
+66
-32
@@ -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
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user