From 4ccbedf461478f4388150ed7655c2fddd55f55a7 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 22 Oct 2024 12:58:38 -0700 Subject: [PATCH 1/3] Reapply "Remove steering wheel offset for planner slow down for curves" (#33848) This reverts commit 4f8b11257e86b407fad67cff047b91d9dcee21cc. --- selfdrive/controls/lib/longitudinal_planner.py | 3 ++- selfdrive/controls/plannerd.py | 4 ++-- selfdrive/test/process_replay/process_replay.py | 2 +- selfdrive/test/process_replay/ref_commit | 2 +- 4 files changed, 6 insertions(+), 5 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index c5aaafa096433f..0fc8b8ef663a21 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -131,7 +131,8 @@ def update(self, sm): if self.mpc.mode == 'acc': accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)] - accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP) + steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg + accel_limits_turns = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_limits, self.CP) else: accel_limits = [ACCEL_MIN, ACCEL_MAX] accel_limits_turns = [ACCEL_MIN, ACCEL_MAX] diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index ae8301871d2575..bcfc4d0c14809b 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -19,7 +19,7 @@ def main(): ldw = LaneDepartureWarning() longitudinal_planner = LongitudinalPlanner(CP) pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance']) - sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2', 'selfdriveState'], + sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState'], poll='modelV2', ignore_avg_freq=['radarState']) while True: @@ -30,7 +30,7 @@ def main(): ldw.update(sm.frame, sm['modelV2'], sm['carState'], sm['carControl']) msg = messaging.new_message('driverAssistance') - msg.valid = sm.all_checks(['carState', 'carControl', 'modelV2']) + msg.valid = sm.all_checks(['carState', 'carControl', 'modelV2', 'liveParameters']) msg.driverAssistance.leftLaneDeparture = ldw.left msg.driverAssistance.rightLaneDeparture = ldw.right pm.send('driverAssistance', msg) diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index ef17325642da95..448dc6896d17cd 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -508,7 +508,7 @@ def selfdrived_config_callback(params, cfg, lr): ), ProcessConfig( proc_name="plannerd", - pubs=["modelV2", "carControl", "carState", "controlsState", "radarState", "selfdriveState"], + pubs=["modelV2", "carControl", "carState", "controlsState", "liveParameters", "radarState", "selfdriveState"], subs=["longitudinalPlan", "driverAssistance"], ignore=["logMonoTime", "longitudinalPlan.processingDelay", "longitudinalPlan.solverExecutionTime"], init_callback=get_car_params_callback, diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index ea0c416f205c21..7892dd52d88e7c 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -4e595fcc2e8e4ef1564d915f697ddd9334067a7f \ No newline at end of file +a000c117d4082c2688735b6e21073e5df0626e63 \ No newline at end of file From 0c5edc2a15e3072301e40711e2c8f2906a0cbf67 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 22 Oct 2024 13:01:00 -0700 Subject: [PATCH 2/3] careless --- selfdrive/test/longitudinal_maneuvers/plant.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index 3c8c09d6befb7b..c08ac6d3692539 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -64,6 +64,7 @@ def step(self, v_lead=0.0, prob_lead=1.0, v_cruise=50., pitch=0.0, prob_throttle control = messaging.new_message('controlsState') ss = messaging.new_message('selfdriveState') car_state = messaging.new_message('carState') + lp = messaging.new_message('liveParameters') car_control = messaging.new_message('carControl') model = messaging.new_message('modelV2') a_lead = (v_lead - self.v_lead_prev)/self.ts @@ -130,6 +131,7 @@ def step(self, v_lead=0.0, prob_lead=1.0, v_cruise=50., pitch=0.0, prob_throttle 'carControl': car_control.carControl, 'controlsState': control.controlsState, 'selfdriveState': ss.selfdriveState, + 'liveParameters': lp.liveParameters, 'modelV2': model.modelV2} self.planner.update(sm) self.speed = self.planner.v_desired_filter.x From 4234dcc0276e73e770ccd47289377cb5f32952dd Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 24 Oct 2024 19:05:12 -0700 Subject: [PATCH 3/3] Update ref_commit --- selfdrive/test/process_replay/ref_commit | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 1365eb1798f70e..66abbd2803bdab 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -e7db12387eecd077c6eaca6622b7863ee1af7105 +22530fd1bd915d5b37db900e2ac42a9501cd5972