mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-09-13 10:54:03 +08:00
Merge remote-tracking branch 'origin/sync-priv-20240201' into sync-priv-20240201
This commit is contained in:
DevTekVE
@@ -236,6 +236,7 @@ std::unordered_map<std::string, uint32_t> keys = {
|
||||
{"DevUIInfo", PERSISTENT},
|
||||
{"DisableOnroadUploads", PERSISTENT},
|
||||
{"DisengageLateralOnBrake", PERSISTENT},
|
||||
{"DrivingModelGeneration", PERSISTENT},
|
||||
{"DrivingModelName", PERSISTENT},
|
||||
{"DrivingModelText", PERSISTENT},
|
||||
{"DrivingModelUrl", PERSISTENT},
|
||||
|
||||
@@ -28,6 +28,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
|
||||
from openpilot.selfdrive.controls.lib.events import Events, ET
|
||||
from openpilot.selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert
|
||||
from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel
|
||||
from openpilot.selfdrive.sunnypilot import get_model_generation
|
||||
from openpilot.system.hardware import HARDWARE
|
||||
|
||||
SOFT_DISABLE_TIME = 3 # seconds
|
||||
@@ -57,8 +58,6 @@ 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):
|
||||
@@ -211,6 +210,9 @@ class Controls:
|
||||
|
||||
self.process_not_running = False
|
||||
|
||||
self.custom_model, self.model_gen = get_model_generation()
|
||||
self.model_use_lateral_planner = self.custom_model and self.model_gen == "1"
|
||||
|
||||
self.can_log_mono_time = 0
|
||||
|
||||
self.startup_event = get_startup_event(car_recognized, controller_available, len(self.CP.carFw) > 0)
|
||||
@@ -363,8 +365,8 @@ class Controls:
|
||||
self.events.add(EventName.calibrationInvalid)
|
||||
|
||||
# Handle lane change
|
||||
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
|
||||
lane_change_edge_block = self.sm['lateralPlanSPDEPRECATED'].laneChangeEdgeBlockDEPRECATED if self.model_use_lateral_planner else self.sm['modelV2SP'].laneChangeEdgeBlock
|
||||
lane_change_svs = self.sm['lateralPlanDEPRECATED'] if self.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 lane_change_svs.laneChangeState == LaneChangeState.preLaneChange:
|
||||
@@ -454,7 +456,7 @@ class Controls:
|
||||
self.logged_comm_issue = None
|
||||
|
||||
if not (self.CP.notCar and self.joystick_mode):
|
||||
if MODEL_USE_LATERAL_PLANNER:
|
||||
if self.model_use_lateral_planner:
|
||||
if not self.sm['lateralPlan'].mpcSolutionValid:
|
||||
self.events.add(EventName.plannerErrorDEPRECATED)
|
||||
if not self.sm['liveLocationKalman'].posenetOK:
|
||||
@@ -671,7 +673,7 @@ class Controls:
|
||||
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
|
||||
blinker_svs = lat_plan if self.model_use_lateral_planner else model_v2.meta
|
||||
|
||||
CC = car.CarControl.new_message()
|
||||
CC.enabled = self.enabled
|
||||
@@ -710,7 +712,7 @@ class Controls:
|
||||
actuators.accel = self.LoC.update(CC.longActive, CS, long_plan, pid_accel_limits, t_since_plan)
|
||||
|
||||
# Steering PID loop and lateral MPC
|
||||
if MODEL_USE_LATERAL_PLANNER:
|
||||
if self.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)
|
||||
@@ -719,7 +721,7 @@ class Controls:
|
||||
self.steer_limited, self.desired_curvature,
|
||||
self.sm['liveLocationKalman'],
|
||||
model_data=model_v2)
|
||||
if MODEL_USE_LATERAL_PLANNER:
|
||||
if self.model_use_lateral_planner:
|
||||
actuators.curvature = self.desired_curvature
|
||||
else:
|
||||
lac_log = log.ControlsState.LateralDebugState.new_message()
|
||||
@@ -759,7 +761,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 = lat_plan.dPathPoints if MODEL_USE_LATERAL_PLANNER else model_v2.position.y
|
||||
dpath_points = lat_plan.dPathPoints if self.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
|
||||
@@ -868,7 +870,7 @@ class Controls:
|
||||
|
||||
# Curvature & Steering angle
|
||||
lp = self.sm['liveParameters']
|
||||
lp_mono_time_svs = 'lateralPlanDEPRECATED' if MODEL_USE_LATERAL_PLANNER else 'modelV2'
|
||||
lp_mono_time_svs = 'lateralPlanDEPRECATED' if self.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)
|
||||
|
||||
@@ -3,6 +3,7 @@ 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
|
||||
from openpilot.selfdrive.sunnypilot import get_model_generation
|
||||
|
||||
LaneChangeState = log.LaneChangeState
|
||||
LaneChangeDirection = log.LaneChangeDirection
|
||||
@@ -10,8 +11,6 @@ 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,
|
||||
@@ -63,6 +62,9 @@ class DesireHelper:
|
||||
self.lane_change_set_timer = int(self.param_s.get("AutoLaneChangeTimer", encoding="utf8"))
|
||||
self.lane_change_bsm_delay = self.param_s.get_bool("AutoLaneChangeBsmDelay")
|
||||
|
||||
self.custom_model, self.model_gen = get_model_generation()
|
||||
self.model_use_lateral_planner = self.custom_model and self.model_gen == "1"
|
||||
|
||||
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"))
|
||||
@@ -77,7 +79,7 @@ class DesireHelper:
|
||||
one_blinker = carstate.leftBlinker != carstate.rightBlinker
|
||||
below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN
|
||||
|
||||
if MODEL_USE_LATERAL_PLANNER:
|
||||
if self.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:
|
||||
@@ -93,7 +95,7 @@ class DesireHelper:
|
||||
self.lane_change_wait_timer = 0
|
||||
|
||||
# LaneChangeState.preLaneChange
|
||||
elif self.lane_change_state == LaneChangeState.preLaneChange and (self.road_edge if MODEL_USE_LATERAL_PLANNER else lat_plan_sp.laneChangeEdgeBlockDEPRECATED):
|
||||
elif self.lane_change_state == LaneChangeState.preLaneChange and (self.road_edge if self.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
|
||||
|
||||
@@ -10,6 +10,7 @@ from openpilot.selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import N as LAT_MP
|
||||
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, get_road_edge
|
||||
from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
|
||||
from openpilot.selfdrive.sunnypilot import get_model_generation
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from cereal import log
|
||||
@@ -27,8 +28,6 @@ LATERAL_JERK_COST = 0.04
|
||||
# speed lateral control is stable on all cars
|
||||
STEERING_RATE_COST = 700.0
|
||||
|
||||
MODEL_USE_LATERAL_PLANNER = False
|
||||
|
||||
|
||||
class LateralPlanner:
|
||||
def __init__(self, CP, debug=False):
|
||||
@@ -75,6 +74,9 @@ class LateralPlanner:
|
||||
self.param_read_counter = 0
|
||||
self.read_param()
|
||||
|
||||
self.custom_model, self.model_gen = get_model_generation()
|
||||
self.model_use_lateral_planner = self.custom_model and self.model_gen == "1"
|
||||
|
||||
def read_param(self):
|
||||
self.dynamic_lane_profile = int(self.param_s.get("DynamicLaneProfile", encoding='utf8'))
|
||||
if self.param_read_counter % 50 == 0:
|
||||
@@ -99,7 +101,7 @@ class LateralPlanner:
|
||||
md = sm['modelV2']
|
||||
|
||||
# TODO: SP - Refactor to work with legacy models
|
||||
if MODEL_USE_LATERAL_PLANNER:
|
||||
if self.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.lateralPlannerSolutionDEPRECATED.x) == TRAJECTORY_SIZE)):
|
||||
@@ -175,7 +177,7 @@ class LateralPlanner:
|
||||
else:
|
||||
self.solution_invalid_cnt = 0
|
||||
|
||||
if not MODEL_USE_LATERAL_PLANNER:
|
||||
if not self.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):
|
||||
|
||||
@@ -8,18 +8,18 @@ from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
|
||||
from openpilot.selfdrive.controls.lib.lateral_planner import LateralPlanner
|
||||
from openpilot.selfdrive.sunnypilot import get_model_generation
|
||||
import cereal.messaging as messaging
|
||||
|
||||
USE_LATERAL_PLANNER = True
|
||||
MODEL_USE_LATERAL_PLANNER = False
|
||||
|
||||
|
||||
def cumtrapz(x, t):
|
||||
return np.concatenate([[0], np.cumsum(((x[0:-1] + x[1:])/2) * np.diff(t))])
|
||||
|
||||
def publish_ui_plan(sm, pm, lateral_planner=None, longitudinal_planner=None):
|
||||
def publish_ui_plan(sm, pm, lateral_planner, longitudinal_planner):
|
||||
# TODO: SP - Reimplement lateral planner with legacy models
|
||||
if MODEL_USE_LATERAL_PLANNER:
|
||||
custom_model, model_gen = get_model_generation()
|
||||
model_use_lateral_planner = custom_model and model_gen == "1"
|
||||
if model_use_lateral_planner:
|
||||
plan_odo = cumtrapz(longitudinal_planner.v_desired_trajectory_full, ModelConstants.T_IDXS)
|
||||
model_odo = cumtrapz(lateral_planner.v_plan, ModelConstants.T_IDXS)
|
||||
|
||||
@@ -28,12 +28,12 @@ def publish_ui_plan(sm, pm, lateral_planner=None, longitudinal_planner=None):
|
||||
uiPlan = ui_send.uiPlan
|
||||
uiPlan.frameId = sm['modelV2'].frameId
|
||||
# TODO: SP - Reimplement lateral planner with legacy models
|
||||
if MODEL_USE_LATERAL_PLANNER:
|
||||
if model_use_lateral_planner:
|
||||
x_fp = (lateral_planner.x_sol if lateral_planner.dynamic_lane_profile_status else lateral_planner.lat_mpc.x_sol)[:,0]
|
||||
y_fp = (lateral_planner.x_sol if lateral_planner.dynamic_lane_profile_status else lateral_planner.lat_mpc.x_sol)[:,1]
|
||||
uiPlan.position.x = np.interp(plan_odo, model_odo, x_fp).tolist() if MODEL_USE_LATERAL_PLANNER else list(sm['modelV2'].position.x)
|
||||
uiPlan.position.y = np.interp(plan_odo, model_odo, y_fp).tolist() if MODEL_USE_LATERAL_PLANNER else list(sm['modelV2'].position.y)
|
||||
uiPlan.position.z = np.interp(plan_odo, model_odo, lateral_planner.path_xyz[:,2]).tolist() if MODEL_USE_LATERAL_PLANNER else list(sm['modelV2'].position.z)
|
||||
uiPlan.position.x = np.interp(plan_odo, model_odo, x_fp).tolist() if model_use_lateral_planner else list(sm['modelV2'].position.x)
|
||||
uiPlan.position.y = np.interp(plan_odo, model_odo, y_fp).tolist() if model_use_lateral_planner else list(sm['modelV2'].position.y)
|
||||
uiPlan.position.z = np.interp(plan_odo, model_odo, lateral_planner.path_xyz[:,2]).tolist() if model_use_lateral_planner else list(sm['modelV2'].position.z)
|
||||
uiPlan.accel = longitudinal_planner.a_desired_trajectory_full.tolist()
|
||||
pm.send('uiPlan', ui_send)
|
||||
|
||||
@@ -50,8 +50,8 @@ def plannerd_thread():
|
||||
|
||||
longitudinal_planner = LongitudinalPlanner(CP)
|
||||
# TODO: SP - Reimplement lateral planner with legacy models
|
||||
lateral_planner = LateralPlanner(CP, debug=debug_mode) if USE_LATERAL_PLANNER else None
|
||||
lateral_planner_svs = ['lateralPlanDEPRECATED', 'lateralPlanSPDEPRECATED'] if USE_LATERAL_PLANNER else []
|
||||
lateral_planner = LateralPlanner(CP, debug=debug_mode)
|
||||
lateral_planner_svs = ['lateralPlanDEPRECATED', 'lateralPlanSPDEPRECATED']
|
||||
|
||||
pm = messaging.PubMaster(['longitudinalPlan', 'uiPlan', 'longitudinalPlanSP'] + lateral_planner_svs)
|
||||
sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2',
|
||||
@@ -63,13 +63,11 @@ def plannerd_thread():
|
||||
sm.update()
|
||||
|
||||
if sm.updated['modelV2']:
|
||||
# TODO: SP - Reimplement lateral planner with legacy models
|
||||
if USE_LATERAL_PLANNER:
|
||||
lateral_planner.update(sm)
|
||||
lateral_planner.publish(sm, pm)
|
||||
lateral_planner.update(sm)
|
||||
lateral_planner.publish(sm, pm)
|
||||
longitudinal_planner.update(sm)
|
||||
longitudinal_planner.publish(sm, pm)
|
||||
publish_ui_plan(sm, pm, lateral_planner=lateral_planner, longitudinal_planner=longitudinal_planner)
|
||||
publish_ui_plan(sm, pm, lateral_planner, longitudinal_planner)
|
||||
|
||||
def main():
|
||||
plannerd_thread()
|
||||
|
||||
@@ -4,14 +4,12 @@ import numpy as np
|
||||
from typing import Dict
|
||||
from cereal import log
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants, Plan, Meta
|
||||
from openpilot.selfdrive.sunnypilot import get_model_generation
|
||||
|
||||
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):
|
||||
@@ -50,6 +48,7 @@ def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: Dict[str,
|
||||
vipc_frame_id: int, vipc_frame_id_extra: int, frame_id: int, frame_drop: float,
|
||||
timestamp_eof: int, timestamp_llk: int, model_execution_time: float,
|
||||
nav_enabled: bool, v_ego: float, steer_delay: float, valid: bool) -> None:
|
||||
custom_model, model_gen = get_model_generation()
|
||||
frame_age = frame_id - vipc_frame_id if frame_id > vipc_frame_id else 0
|
||||
msg.valid = valid
|
||||
|
||||
@@ -76,7 +75,7 @@ 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
|
||||
if MODEL_USE_LATERAL_PLANNER:
|
||||
if custom_model and model_gen == "1":
|
||||
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)]
|
||||
|
||||
+16
-15
@@ -25,6 +25,7 @@ from openpilot.selfdrive.modeld.parse_model_outputs import Parser
|
||||
from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants
|
||||
from openpilot.selfdrive.modeld.models.commonmodel_pyx import ModelFrame, CLContext
|
||||
from openpilot.selfdrive.sunnypilot import get_model_generation
|
||||
|
||||
PROCESS_NAME = "selfdrive.modeld.modeld"
|
||||
SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
|
||||
@@ -39,9 +40,6 @@ 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
|
||||
@@ -61,6 +59,7 @@ class ModelState:
|
||||
model: ModelRunner
|
||||
|
||||
def __init__(self, context: CLContext):
|
||||
self.custom_model, self.model_gen = get_model_generation()
|
||||
self.frame = ModelFrame(context)
|
||||
self.wide_frame = ModelFrame(context)
|
||||
self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32)
|
||||
@@ -74,11 +73,11 @@ class ModelState:
|
||||
'features_buffer': np.zeros(ModelConstants.HISTORY_BUFFER_LEN * ModelConstants.FEATURE_LEN, dtype=np.float32),
|
||||
}
|
||||
|
||||
if MODEL_USE_LATERAL_PLANNER:
|
||||
if self.custom_model and self.model_gen == "1":
|
||||
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:
|
||||
if self.custom_model and self.model_gen == "2":
|
||||
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)
|
||||
@@ -121,7 +120,7 @@ class ModelState:
|
||||
self.prev_desire[:] = inputs['desire']
|
||||
|
||||
self.inputs['traffic_convention'][:] = inputs['traffic_convention']
|
||||
if not MODEL_USE_LATERAL_PLANNER:
|
||||
if not (self.custom_model and self.model_gen == "1"):
|
||||
self.inputs['lateral_control_params'][:] = inputs['lateral_control_params']
|
||||
self.inputs['nav_features'][:] = inputs['nav_features']
|
||||
self.inputs['nav_instructions'][:] = inputs['nav_instructions']
|
||||
@@ -139,10 +138,10 @@ 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, :]
|
||||
if MODEL_USE_LATERAL_PLANNER:
|
||||
if self.custom_model and self.model_gen == "1":
|
||||
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:
|
||||
elif self.custom_model and self.model_gen == "2":
|
||||
self.inputs['prev_desired_curvs'][:-1] = self.inputs['prev_desired_curvs'][1:]
|
||||
self.inputs['prev_desired_curvs'][-1] = outputs['desired_curvature'][0, 0]
|
||||
else:
|
||||
@@ -161,6 +160,8 @@ def main(demo=False):
|
||||
model = ModelState(cl_context)
|
||||
cloudlog.warning("models loaded, modeld starting")
|
||||
|
||||
custom_model, model_gen = get_model_generation()
|
||||
|
||||
# visionipc clients
|
||||
while True:
|
||||
available_streams = VisionIpcClient.available_streams("camerad", block=False)
|
||||
@@ -191,7 +192,7 @@ def main(demo=False):
|
||||
|
||||
publish_state = PublishState()
|
||||
params = Params()
|
||||
if not MODEL_USE_LATERAL_PLANNER:
|
||||
if not (custom_model and model_gen == "1"):
|
||||
with car.CarParams.from_bytes(params.get("CarParams", block=True)) as msg:
|
||||
steer_delay = msg.steerActuatorDelay + .2
|
||||
#steer_delay = 0.4
|
||||
@@ -205,7 +206,7 @@ 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:
|
||||
if custom_model and model_gen == "1":
|
||||
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)
|
||||
@@ -260,11 +261,11 @@ def main(demo=False):
|
||||
|
||||
# TODO: path planner timeout?
|
||||
sm.update(0)
|
||||
desire = sm["lateralPlan"].desire.raw if MODEL_USE_LATERAL_PLANNER else DH.desire
|
||||
desire = sm["lateralPlan"].desire.raw if custom_model and model_gen == "1" else DH.desire
|
||||
v_ego = sm["carState"].vEgo
|
||||
is_rhd = sm["driverMonitoringState"].isRHD
|
||||
frame_id = sm["roadCameraState"].frameId
|
||||
if not MODEL_USE_LATERAL_PLANNER:
|
||||
if not (custom_model and model_gen == "1"):
|
||||
# TODO add lag
|
||||
lateral_control_params = np.array([sm["carState"].vEgo, steer_delay], dtype=np.float32)
|
||||
if sm.updated["liveCalibration"]:
|
||||
@@ -324,7 +325,7 @@ def main(demo=False):
|
||||
'nav_features': nav_features,
|
||||
'nav_instructions': nav_instructions}
|
||||
|
||||
if MODEL_USE_LATERAL_PLANNER:
|
||||
if custom_model and model_gen == "1":
|
||||
inputs['driving_style'] = driving_style
|
||||
else:
|
||||
inputs['lateral_control_params'] = lateral_control_params
|
||||
@@ -342,7 +343,7 @@ def main(demo=False):
|
||||
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)
|
||||
|
||||
if not MODEL_USE_LATERAL_PLANNER:
|
||||
if not (custom_model and model_gen == "1"):
|
||||
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]
|
||||
@@ -355,7 +356,7 @@ def main(demo=False):
|
||||
pm.send('modelV2', modelv2_send)
|
||||
pm.send('cameraOdometry', posenet_send)
|
||||
|
||||
if not MODEL_USE_LATERAL_PLANNER:
|
||||
if not (custom_model and model_gen == "1"):
|
||||
modelv2_sp_send = messaging.new_message('modelV2SP')
|
||||
modelv2_sp_send.valid = True
|
||||
modelv2_sp_send.modelV2SP.laneChangePrev = DH.prev_lane_change
|
||||
|
||||
@@ -0,0 +1,8 @@
|
||||
from openpilot.common.params import Params
|
||||
|
||||
|
||||
def get_model_generation():
|
||||
params = Params()
|
||||
custom_model = params.get_bool("CustomDrivingModel")
|
||||
gen = params.get("DrivingModelGeneration", encoding="utf8")
|
||||
return custom_model, gen
|
||||
|
||||
@@ -28,6 +28,7 @@ public:
|
||||
downloadUri = json["download_uri"].toString();
|
||||
index = json["index"].toString();
|
||||
environment = json["environment"].toString();
|
||||
generation = json["generation"].toString();
|
||||
}
|
||||
|
||||
QJsonObject toJson() const {
|
||||
@@ -38,6 +39,7 @@ public:
|
||||
json["download_uri"] = downloadUri;
|
||||
json["index"] = index;
|
||||
json["environment"] = environment;
|
||||
json["generation"] = generation;
|
||||
return json;
|
||||
}
|
||||
|
||||
@@ -47,6 +49,7 @@ public:
|
||||
QString downloadUri;
|
||||
QString index;
|
||||
QString environment;
|
||||
QString generation;
|
||||
};
|
||||
|
||||
class ModelsFetcher : public QObject {
|
||||
|
||||
@@ -44,6 +44,7 @@ void SoftwarePanelSP::HandleModelDownloadProgressReport() {
|
||||
params.put("DrivingModelText", selectedModelToDownload->fullName.toStdString());
|
||||
params.put("DrivingModelName", selectedModelToDownload->displayName.toStdString());
|
||||
//params.put("DrivingModelUrl", selectedModelToDownload->downloadUri.toStdString()); // TODO: Placeholder for future implementations
|
||||
params.put("DrivingModelGeneration", selectedModelToDownload->generation.toStdString());
|
||||
selectedModelToDownload.reset();
|
||||
params.putBool("CustomDrivingModel", true);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user