From d0eaa84eae0d2f806d7cfb28b2859a360752da98 Mon Sep 17 00:00:00 2001 From: Bruce Wayne Date: Wed, 27 Nov 2024 19:05:13 -0800 Subject: [PATCH 1/5] Use raw plan --- selfdrive/controls/lib/longitudinal_planner.py | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index eba8019117166b..9598a367b2c801 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -83,6 +83,7 @@ def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL): self.a_desired_trajectory = np.zeros(CONTROL_N) self.j_desired_trajectory = np.zeros(CONTROL_N) self.solverExecutionTime = 0.0 + self.mode = 'acc' @staticmethod def parse_model(model_msg, model_error): @@ -105,7 +106,7 @@ def parse_model(model_msg, model_error): return x, v, a, j, throttle_prob def update(self, sm): - self.mpc.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc' + self.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc' if len(sm['carControl'].orientationNED) == 3: accel_coast = get_coast_accel(sm['carControl'].orientationNED[1]) @@ -128,7 +129,7 @@ def update(self, sm): # No change cost when user is controlling the speed, or when standstill prev_accel_constraint = not (reset_state or sm['carState'].standstill) - if self.mpc.mode == 'acc': + if self.mode == 'acc': accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)] 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) @@ -200,6 +201,11 @@ def publish(self, sm, pm): action_t = self.CP.longitudinalActuatorDelay + DT_MDL a_target, should_stop = get_accel_from_plan(longitudinalPlan.speeds, longitudinalPlan.accels, action_t=action_t, vEgoStopping=self.CP.vEgoStopping) + if self.mode == 'blended': + a_target_e2e, should_stop_e2e = get_accel_from_plan(longitudinalPlan.speeds, longitudinalPlan.accels, + action_t=action_t, vEgoStopping=self.CP.vEgoStopping) + a_target = min(a_target, a_target_e2e) + should_stop = should_stop or should_stop_e2e longitudinalPlan.aTarget = a_target longitudinalPlan.shouldStop = should_stop longitudinalPlan.allowBrake = True From 9a1c08f2f0cecbeb35dcc5f02d1a3297be80199f Mon Sep 17 00:00:00 2001 From: Bruce Wayne Date: Wed, 27 Nov 2024 20:16:46 -0800 Subject: [PATCH 2/5] typo --- selfdrive/controls/lib/longitudinal_planner.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 9598a367b2c801..181916aa0a670d 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -202,8 +202,9 @@ def publish(self, sm, pm): a_target, should_stop = get_accel_from_plan(longitudinalPlan.speeds, longitudinalPlan.accels, action_t=action_t, vEgoStopping=self.CP.vEgoStopping) if self.mode == 'blended': - a_target_e2e, should_stop_e2e = get_accel_from_plan(longitudinalPlan.speeds, longitudinalPlan.accels, - action_t=action_t, vEgoStopping=self.CP.vEgoStopping) + a_target_e2e, should_stop_e2e = get_accel_from_plan(list(sm['modelV2'].velocity.x)[:CONTROL_N], + list(sm['modelV2'].acceleration.x)[:CONTROL_N], + action_t=action_t, vEgoStopping=self.CP.vEgoStopping) a_target = min(a_target, a_target_e2e) should_stop = should_stop or should_stop_e2e longitudinalPlan.aTarget = a_target From f1edbdf6dfe78ddf4d592e336008a550e9241a5d Mon Sep 17 00:00:00 2001 From: Bruce Wayne Date: Thu, 19 Dec 2024 10:43:53 -0800 Subject: [PATCH 3/5] 8de150f3-908e-4e1c-ad0a-30c42b040b3f/400 --- selfdrive/modeld/models/supercombo.onnx | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/modeld/models/supercombo.onnx b/selfdrive/modeld/models/supercombo.onnx index 777aa8aa7e4d35..3d64b113bfe557 100644 --- a/selfdrive/modeld/models/supercombo.onnx +++ b/selfdrive/modeld/models/supercombo.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:72d3d6f8d3c98f5431ec86be77b6350d7d4f43c25075c0106f1d1e7ec7c77668 +oid sha256:96e12bcc1a7bfeef5fb565f562558be627b88bf1fda3863e432d2f28cb2a0b96 size 49096168 From 0b048c9dbf745046dfcfc66ac58450b579e15fbe Mon Sep 17 00:00:00 2001 From: Bruce Wayne Date: Thu, 19 Dec 2024 20:22:40 -0800 Subject: [PATCH 4/5] 18ba1aaa-2758-4cd3-b294-948c1c61c5e2/385 --- selfdrive/modeld/models/supercombo.onnx | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/modeld/models/supercombo.onnx b/selfdrive/modeld/models/supercombo.onnx index 3d64b113bfe557..a5e779577c2dc6 100644 --- a/selfdrive/modeld/models/supercombo.onnx +++ b/selfdrive/modeld/models/supercombo.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:96e12bcc1a7bfeef5fb565f562558be627b88bf1fda3863e432d2f28cb2a0b96 +oid sha256:cf568f7b7449481d65b8276d63e746c7eae02fe81a86a28c8ccd8f254ee1f4b6 size 49096168 From cc6f438e5c228c8a56631fbf8896f274702b912f Mon Sep 17 00:00:00 2001 From: Bruce Wayne Date: Fri, 20 Dec 2024 12:41:52 -0800 Subject: [PATCH 5/5] 135603ce-b24c-4dcc-9879-28b438242456/400 --- selfdrive/modeld/models/supercombo.onnx | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/modeld/models/supercombo.onnx b/selfdrive/modeld/models/supercombo.onnx index a5e779577c2dc6..ce864db9134219 100644 --- a/selfdrive/modeld/models/supercombo.onnx +++ b/selfdrive/modeld/models/supercombo.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:cf568f7b7449481d65b8276d63e746c7eae02fe81a86a28c8ccd8f254ee1f4b6 +oid sha256:d9615aa78a4cab296dfe3a994597caf909c814246dee1e3ac57653f7eb7acbe4 size 49096168