diff --git a/cereal/log.capnp b/cereal/log.capnp index c624eb0052..a5828d61cb 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -1174,6 +1174,8 @@ struct ModelDataV2 { struct Action { desiredCurvature @0 :Float32; + desiredAcceleration @1 :Float32; + shouldStop @2 :Bool; } } diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 41384abae8..9958755577 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -1,7 +1,7 @@ import numpy as np from cereal import log from opendbc.car.vehicle_model import ACCELERATION_DUE_TO_GRAVITY -from openpilot.common.realtime import DT_CTRL +from openpilot.common.realtime import DT_CTRL, DT_MDL MIN_SPEED = 1.0 CONTROL_N = 17 @@ -43,3 +43,19 @@ def get_speed_error(modelV2: log.ModelDataV2, v_ego: float) -> float: vel_err = np.clip(modelV2.temporalPose.trans[0] - v_ego, -MAX_VEL_ERR, MAX_VEL_ERR) return float(vel_err) return 0.0 + + +def get_accel_from_plan(speeds, accels, t_idxs, action_t=DT_MDL, vEgoStopping=0.05): + if len(speeds) == len(t_idxs): + v_now = speeds[0] + a_now = accels[0] + v_target = np.interp(action_t, t_idxs, speeds) + a_target = 2 * (v_target - v_now) / (action_t) - a_now + v_target_1sec = np.interp(action_t + 1.0, t_idxs, speeds) + else: + v_target = 0.0 + v_target_1sec = 0.0 + a_target = 0.0 + should_stop = (v_target < vEgoStopping and + v_target_1sec < vEgoStopping) + return a_target, should_stop diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index c7a2295a5f..8cbf0beee9 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -11,7 +11,7 @@ from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC -from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_speed_error +from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_speed_error, get_accel_from_plan from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET from openpilot.common.swaglog import cloudlog @@ -48,23 +48,6 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): return [a_target[0], min(a_target[1], a_x_allowed)] -def get_accel_from_plan(speeds, accels, action_t=DT_MDL, vEgoStopping=0.05): - if len(speeds) == CONTROL_N: - v_now = speeds[0] - a_now = accels[0] - - v_target = np.interp(action_t, CONTROL_N_T_IDX, speeds) - a_target = 2 * (v_target - v_now) / (action_t) - a_now - v_target_1sec = np.interp(action_t + 1.0, CONTROL_N_T_IDX, speeds) - else: - v_target = 0.0 - v_target_1sec = 0.0 - a_target = 0.0 - should_stop = (v_target < vEgoStopping and - v_target_1sec < vEgoStopping) - return a_target, should_stop - - class LongitudinalPlanner: def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL): self.CP = CP @@ -176,7 +159,7 @@ class LongitudinalPlanner: self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.a_desired + a_prev) / 2.0 action_t = self.CP.longitudinalActuatorDelay + DT_MDL - output_a_target, self.output_should_stop = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, + output_a_target, self.output_should_stop = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX, action_t=action_t, vEgoStopping=self.CP.vEgoStopping) for idx in range(2): diff --git a/selfdrive/locationd/lagd.py b/selfdrive/locationd/lagd.py index 0a7c71a7ea..f4f46b7469 100755 --- a/selfdrive/locationd/lagd.py +++ b/selfdrive/locationd/lagd.py @@ -327,8 +327,9 @@ def retrieve_initial_lag(params_reader: Params, CP: car.CarParams): if last_CP.carFingerprint != CP.carFingerprint: raise Exception("Car model mismatch") - lag, valid_blocks = ld.lateralDelayEstimate, ld.validBlocks + lag, valid_blocks, status = ld.lateralDelayEstimate, ld.validBlocks, ld.status assert valid_blocks <= BLOCK_NUM, "Invalid number of valid blocks" + assert status != log.LiveDelayData.Status.invalid, "Lag estimate is invalid" return lag, valid_blocks except Exception as e: cloudlog.error(f"Failed to retrieve initial lag: {e}") diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py index 9dc2641c9b..a91c6395c7 100644 --- a/selfdrive/modeld/fill_model_msg.py +++ b/selfdrive/modeld/fill_model_msg.py @@ -56,7 +56,7 @@ def fill_lane_line_meta(builder, lane_lines, lane_line_probs): builder.rightProb = lane_line_probs[2] def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._DynamicStructBuilder, - net_output_data: dict[str, np.ndarray], v_ego: float, delay: float, + net_output_data: dict[str, np.ndarray], action: log.ModelDataV2.Action, publish_state: PublishState, vipc_frame_id: int, vipc_frame_id_extra: int, frame_id: int, frame_drop: float, timestamp_eof: int, model_execution_time: float, valid: bool) -> None: @@ -71,7 +71,8 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D driving_model_data.frameIdExtra = vipc_frame_id_extra driving_model_data.frameDropPerc = frame_drop_perc driving_model_data.modelExecutionTime = model_execution_time - driving_model_data.action.desiredCurvature = float(net_output_data['desired_curvature'][0,0]) + + driving_model_data.action = action modelV2 = extended_msg.modelV2 modelV2.frameId = vipc_frame_id @@ -98,8 +99,8 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D # poly path fill_xyz_poly(driving_model_data.path, ModelConstants.POLY_PATH_DEGREE, *net_output_data['plan'][0,:,Plan.POSITION].T) - # lateral planning - modelV2.action.desiredCurvature = float(net_output_data['desired_curvature'][0,0]) + # action + modelV2.action = action # times at X_IDXS of edges and lines aren't used LINE_T_IDXS: list[float] = [] diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 8c3c8ba4d6..275afcbfd8 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -21,17 +21,20 @@ from opendbc.car.car_helpers import get_demo_car_params from openpilot.common.swaglog import cloudlog from openpilot.common.params import Params from openpilot.common.filter_simple import FirstOrderFilter -from openpilot.common.realtime import config_realtime_process +from openpilot.common.realtime import config_realtime_process, DT_MDL from openpilot.common.transformations.camera import DEVICE_CAMERAS from openpilot.common.transformations.model import get_warp_matrix from openpilot.system import sentry from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper +from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan 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.constants import ModelConstants, Plan from openpilot.selfdrive.modeld.models.commonmodel_pyx import DrivingModelFrame, CLContext + + PROCESS_NAME = "selfdrive.modeld.modeld" SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') @@ -40,6 +43,30 @@ POLICY_PKL_PATH = Path(__file__).parent / 'models/driving_policy_tinygrad.pkl' VISION_METADATA_PATH = Path(__file__).parent / 'models/driving_vision_metadata.pkl' POLICY_METADATA_PATH = Path(__file__).parent / 'models/driving_policy_metadata.pkl' +LAT_SMOOTH_SECONDS = 0.0 +LONG_SMOOTH_SECONDS = 0.0 + +def smooth_value(val, prev_val, tau): + alpha = 1 - np.exp(-DT_MDL / tau) if tau > 0 else 1 + return alpha * val + (1 - alpha) * prev_val + + +def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action, + lat_action_t: float, long_action_t: float,) -> log.ModelDataV2.Action: + plan = model_output['plan'][0] + desired_accel, should_stop = get_accel_from_plan(plan[:,Plan.VELOCITY][:,0], + plan[:,Plan.ACCELERATION][:,0], + ModelConstants.T_IDXS, + action_t=long_action_t) + desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, LONG_SMOOTH_SECONDS) + + desired_curvature = model_output['desired_curvature'][0, 0] + desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, LAT_SMOOTH_SECONDS) + + return log.ModelDataV2.Action(desiredCurvature=float(desired_curvature), + desiredAcceleration=float(desired_accel), + shouldStop=bool(should_stop)) + class FrameMeta: frame_id: int = 0 timestamp_sof: int = 0 @@ -220,6 +247,11 @@ def main(demo=False): CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) cloudlog.info("modeld got CarParams: %s", CP.brand) + # TODO this needs more thought, use .2s extra for now to estimate other delays + # TODO Move smooth seconds to action function + long_delay = CP.longitudinalActuatorDelay + LONG_SMOOTH_SECONDS + prev_action = log.ModelDataV2.Action() + DH = DesireHelper() while True: @@ -260,8 +292,8 @@ def main(demo=False): is_rhd = sm["driverMonitoringState"].isRHD frame_id = sm["roadCameraState"].frameId v_ego = max(sm["carState"].vEgo, 0.) - steer_delay = sm["liveDelay"].lateralDelay - lateral_control_params = np.array([v_ego, steer_delay], dtype=np.float32) + lat_delay = sm["liveDelay"].lateralDelay + LAT_SMOOTH_SECONDS + lateral_control_params = np.array([v_ego, lat_delay], dtype=np.float32) if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']: device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32) dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['roadCameraState'].sensor))] @@ -304,7 +336,9 @@ def main(demo=False): modelv2_send = messaging.new_message('modelV2') drivingdata_send = messaging.new_message('drivingModelData') posenet_send = messaging.new_message('cameraOdometry') - fill_model_msg(drivingdata_send, modelv2_send, model_output, v_ego, steer_delay, + + action = get_action_from_model(model_output, prev_action, lat_delay + DT_MDL, long_delay + DT_MDL) + fill_model_msg(drivingdata_send, modelv2_send, model_output, action, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio, meta_main.timestamp_eof, model_execution_time, live_calib_seen) diff --git a/selfdrive/test/process_replay/model_replay.py b/selfdrive/test/process_replay/model_replay.py index 36398af8ca..af626e2da9 100755 --- a/selfdrive/test/process_replay/model_replay.py +++ b/selfdrive/test/process_replay/model_replay.py @@ -64,6 +64,7 @@ def generate_report(proposed, master, tmp, commit): ModelV2_Plots = zl([ (lambda x: get_idx_if_non_empty(x.velocity.x, 0), "velocity.x"), (lambda x: get_idx_if_non_empty(x.action.desiredCurvature), "desiredCurvature"), + (lambda x: get_idx_if_non_empty(x.action.desiredAcceleration), "desiredAcceleration"), (lambda x: get_idx_if_non_empty(x.leadsV3[0].x, 0), "leadsV3.x"), (lambda x: get_idx_if_non_empty(x.laneLines[1].y, 0), "laneLines.y"), (lambda x: get_idx_if_non_empty(x.meta.desireState, 3), "desireState.laneChangeLeft"), diff --git a/system/ui/lib/application.py b/system/ui/lib/application.py index fd1ad52068..470396bbc7 100644 --- a/system/ui/lib/application.py +++ b/system/ui/lib/application.py @@ -73,7 +73,7 @@ class GuiApplication: for font in self._fonts.values(): rl.unload_font(font) - self._fonts = [] + self._fonts = {} rl.close_window() @@ -90,8 +90,8 @@ class GuiApplication: rl.end_drawing() self._monitor_fps() - def font(self, font_wight: FontWeight=FontWeight.NORMAL): - return self._fonts[font_wight] + def font(self, font_weight: FontWeight=FontWeight.NORMAL): + return self._fonts[font_weight] @property def width(self):