diff --git a/selfdrive/modeld/constants.py b/selfdrive/modeld/constants.py index ade958cd62..bf9d4a95cf 100644 --- a/selfdrive/modeld/constants.py +++ b/selfdrive/modeld/constants.py @@ -1,3 +1,5 @@ +import numpy as np + IDX_N = 33 def index_function(idx, max_val=192, max_idx=32): @@ -10,6 +12,9 @@ LEAD_T_IDXS = [0., 2., 4., 6., 8., 10.] LEAD_T_OFFSETS = [0., 2., 4.] META_T_IDXS = [2., 4., 6., 8., 10.] +FCW_THRESHOLDS_5MS2 = np.array([.05, .05, .15, .15, .15], dtype=np.float32) +FCW_THRESHOLDS_3MS2 = np.array([.7, .7], dtype=np.float32) + class Plan: POSITION = slice(0, 3) VELOCITY = slice(3, 6) diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py index 068d95e47d..b54e860155 100644 --- a/selfdrive/modeld/fill_model_msg.py +++ b/selfdrive/modeld/fill_model_msg.py @@ -1,10 +1,13 @@ import capnp import numpy as np from typing import List, Dict -from openpilot.selfdrive.modeld.models.driving_pyx import PublishState -from openpilot.selfdrive.modeld.constants import T_IDXS, X_IDXS, LEAD_T_IDXS, META_T_IDXS, LEAD_T_OFFSETS, Meta, Plan +from openpilot.selfdrive.modeld.constants import T_IDXS, X_IDXS, LEAD_T_IDXS, META_T_IDXS, LEAD_T_OFFSETS, Meta, Plan, FCW_THRESHOLDS_5MS2, FCW_THRESHOLDS_3MS2 -# TODO: use Enum Slices when possible instead of hardcoded indices +class PublishState: + def __init__(self): + self.disengage_buffer = np.zeros(5*5, dtype=np.float32) + self.prev_brake_5ms2_probs = np.zeros(5, dtype=np.float32) + self.prev_brake_3ms2_probs = np.zeros(3, dtype=np.float32) def fill_xyzt(builder, t, x, y, z, x_std=None, y_std=None, z_std=None): builder.t = t @@ -26,7 +29,7 @@ def fill_xyvat(builder, t, x, y, v, a, x_std=None, y_std=None, v_std=None, a_std if v_std is not None:builder.vStd = v_std.tolist() if a_std is not None:builder.aStd = a_std.tolist() -def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: Dict[str, np.ndarray], ps: PublishState, +def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: Dict[str, np.ndarray], publish_state: PublishState, 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, valid: bool) -> None: @@ -96,9 +99,14 @@ def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: Dict[str, disengage_predictions.steerOverrideProbs = net_output_data['meta'][0,Meta.STEER_OVERRIDE].tolist() disengage_predictions.brake3MetersPerSecondSquaredProbs = net_output_data['meta'][0,Meta.HARD_BRAKE_3].tolist() disengage_predictions.brake4MetersPerSecondSquaredProbs = net_output_data['meta'][0,Meta.HARD_BRAKE_4].tolist() - disengage_predictions.brake5MetersPerSecondSquaredProbs =net_output_data['meta'][0,Meta.HARD_BRAKE_5].tolist() - - meta.hardBrakePredicted = False + disengage_predictions.brake5MetersPerSecondSquaredProbs = net_output_data['meta'][0,Meta.HARD_BRAKE_5].tolist() + + publish_state.prev_brake_5ms2_probs[:-1] = publish_state.prev_brake_5ms2_probs[1:] + publish_state.prev_brake_5ms2_probs[-1] = net_output_data['meta'][0,Meta.HARD_BRAKE_5][0] + publish_state.prev_brake_3ms2_probs[:-1] = publish_state.prev_brake_3ms2_probs[1:] + publish_state.prev_brake_3ms2_probs[-1] = net_output_data['meta'][0,Meta.HARD_BRAKE_3][0] + hard_brake_predicted = (publish_state.prev_brake_5ms2_probs > FCW_THRESHOLDS_5MS2).all() and (publish_state.prev_brake_3ms2_probs > FCW_THRESHOLDS_3MS2).all() + meta.hardBrakePredicted = hard_brake_predicted.item() # temporal pose temporal_pose = modelV2.temporalPose diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index d006087bc6..4b765db641 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -16,10 +16,10 @@ from openpilot.common.realtime import config_realtime_process from openpilot.common.transformations.model import get_warp_matrix from openpilot.selfdrive.modeld.runners import ModelRunner, Runtime from openpilot.selfdrive.modeld.parse_model_outputs import parse_outputs -from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_pose_msg +from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState from openpilot.selfdrive.modeld.models.commonmodel_pyx import ModelFrame, CLContext from openpilot.selfdrive.modeld.models.driving_pyx import ( - PublishState, FEATURE_LEN, HISTORY_BUFFER_LEN, DESIRE_LEN, TRAFFIC_CONVENTION_LEN, NAV_FEATURE_LEN, NAV_INSTRUCTION_LEN, + FEATURE_LEN, HISTORY_BUFFER_LEN, DESIRE_LEN, TRAFFIC_CONVENTION_LEN, NAV_FEATURE_LEN, NAV_INSTRUCTION_LEN, NET_OUTPUT_SIZE, MODEL_FREQ) MODEL_PATHS = { @@ -133,7 +133,7 @@ def main(): pm = PubMaster(["modelV2", "cameraOdometry"]) sm = SubMaster(["lateralPlan", "roadCameraState", "liveCalibration", "driverMonitoringState", "navModel", "navInstruction"]) - state = PublishState() + publish_state = PublishState() params = Params() # setup filter to track dropped frames @@ -256,7 +256,7 @@ def main(): if model_output is not None: modelv2_send = messaging.new_message('modelV2') posenet_send = messaging.new_message('cameraOdometry') - fill_model_msg(modelv2_send, model_output, state, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio, + 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, live_calib_seen) fill_pose_msg(posenet_send, model_output, meta_main.frame_id, vipc_dropped_frames, meta_main.timestamp_eof, live_calib_seen)