From 666448fce191e196aac68d06e29a0745e6620db9 Mon Sep 17 00:00:00 2001 From: Yassine Date: Wed, 25 Sep 2024 22:55:26 -0700 Subject: [PATCH] use temporal pose instead of flag --- selfdrive/controls/lib/drive_helpers.py | 4 ++-- selfdrive/controls/lib/longitudinal_planner.py | 5 ++--- selfdrive/modeld/fill_model_msg.py | 7 +++++++ 3 files changed, 11 insertions(+), 5 deletions(-) diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 369aeaf984dd08..64cbf473d63d29 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -22,7 +22,7 @@ def clip_curvature(v_ego, prev_curvature, new_curvature): def get_speed_error(modelV2: log.ModelDataV2, v_ego: float) -> float: # ToDo: Try relative error, and absolute speed - if len(modelV2.velocity.x): - vel_err = clip(modelV2.velocity.x[0] - v_ego, -MAX_VEL_ERR, MAX_VEL_ERR) + if len(modelV2.temporalPose.trans): + vel_err = clip(modelV2.temporalPose.trans[0] - v_ego, -MAX_VEL_ERR, MAX_VEL_ERR) return float(vel_err) return 0.0 diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 5d36ee1be2c515..0b65119383d2da 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -64,12 +64,11 @@ def get_accel_from_plan(CP, speeds, accels): class LongitudinalPlanner: - def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL, calibrate_v_model_error=True): + def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL): self.CP = CP self.mpc = LongitudinalMpc(dt=dt) self.fcw = False self.dt = dt - self.calibrate_v_model_error = calibrate_v_model_error self.a_desired = init_a self.v_desired_filter = FirstOrderFilter(init_v, 2.0, self.dt) @@ -130,7 +129,7 @@ def update(self, sm): # Prevent divergence, smooth in current v_ego self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego)) # Compute model v_ego error - self.v_model_error = get_speed_error(sm['modelV2'], v_ego) if self.calibrate_v_model_error else 0.0 + self.v_model_error = get_speed_error(sm['modelV2'], v_ego) if force_slow_decel: v_cruise = 0.0 diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py index 18dd9c7bac90e3..10a1860b581582 100644 --- a/selfdrive/modeld/fill_model_msg.py +++ b/selfdrive/modeld/fill_model_msg.py @@ -87,6 +87,13 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D orientation_rate = modelV2.orientationRate fill_xyzt(orientation_rate, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ORIENTATION_RATE].T) + # temporal pose + temporal_pose = modelV2.temporalPose + temporal_pose.trans = net_output_data['plan'][0,0,Plan.VELOCITY].tolist() + temporal_pose.transStd = net_output_data['plan_stds'][0,0,Plan.VELOCITY].tolist() + temporal_pose.rot = net_output_data['plan'][0,0,Plan.ORIENTATION_RATE].tolist() + temporal_pose.rotStd = net_output_data['plan_stds'][0,0,Plan.ORIENTATION_RATE].tolist() + # poly path poly_path = driving_model_data.path fill_xyz_poly(poly_path, ModelConstants.POLY_PATH_DEGREE, *net_output_data['plan'][0,:,Plan.POSITION].T)