From 16220d1e17626ba2623529697c95984c267c0546 Mon Sep 17 00:00:00 2001 From: huxiaoyi-ovo Date: Wed, 28 Oct 2026 03:29:21 -0400 Subject: [PATCH 1/3] fix(planning): include exact quintic terminal time without overshooting --- .../QuinticPolynomialsPlanner/quintic_polynomials_planner.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py b/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py index 86f9f662da..27a6ee61da 100644 --- a/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py +++ b/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py @@ -111,7 +111,10 @@ def quintic_polynomials_planner(sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_ time, rx, ry, ryaw, rv, ra, rj = [], [], [], [], [], [], [] - for t in np.arange(0.0, T + dt, dt): + # Sample up to T without extrapolating beyond the terminal boundary. + # Always include T, even when dt does not divide the duration. + sample_times = np.append(np.arange(0.0, T, dt), T) + for t in sample_times: time.append(t) rx.append(xqp.calc_point(t)) ry.append(yqp.calc_point(t)) From 3c0b8b04e25ab4919ceaf507a1e18f2e61062446 Mon Sep 17 00:00:00 2001 From: huxiaoyi-ovo Date: Wed, 28 Oct 2026 03:29:34 -0400 Subject: [PATCH 2/3] test(planning): cover quintic terminal sampling for nondivisible dt --- tests/test_quintic_polynomials_planner.py | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/tests/test_quintic_polynomials_planner.py b/tests/test_quintic_polynomials_planner.py index 43f3c6bada..7f4967438c 100644 --- a/tests/test_quintic_polynomials_planner.py +++ b/tests/test_quintic_polynomials_planner.py @@ -1,4 +1,6 @@ import conftest # Add root path to sys.path +import numpy as np +import pytest from PathPlanning.QuinticPolynomialsPlanner import quintic_polynomials_planner as m @@ -7,5 +9,22 @@ def test1(): m.main() + +@pytest.mark.parametrize("dt", [0.5, 0.7, 6.0]) +def test_quintic_trajectory_includes_exact_terminal_time(dt): + m.show_animation = False + time, rx, ry, ryaw, rv, ra, rj = m.quintic_polynomials_planner( + sx=0.0, sy=0.0, syaw=0.0, sv=0.0, sa=0.0, + gx=10.0, gy=5.0, gyaw=0.0, gv=0.0, ga=0.0, + max_accel=1000.0, max_jerk=1000.0, dt=dt + ) + + assert time[-1] == m.MIN_T + assert np.all(np.diff(time) > 0) + assert np.all(np.asarray(time) <= m.MIN_T) + assert len({len(values) for values in (time, rx, ry, ryaw, rv, ra, rj)}) == 1 + np.testing.assert_allclose([rx[-1], ry[-1]], [10.0, 5.0], atol=1e-9) + + if __name__ == '__main__': conftest.run_this_test(__file__) From 474ddb25cde886dacf32a42b5331687bf3b84313 Mon Sep 17 00:00:00 2001 From: huxiaoyi-ovo Date: Wed, 28 Oct 2026 03:31:05 -0400 Subject: [PATCH 3/3] style(tests): use standard spacing between test functions --- tests/test_quintic_polynomials_planner.py | 1 - 1 file changed, 1 deletion(-) diff --git a/tests/test_quintic_polynomials_planner.py b/tests/test_quintic_polynomials_planner.py index 7f4967438c..bfda9564db 100644 --- a/tests/test_quintic_polynomials_planner.py +++ b/tests/test_quintic_polynomials_planner.py @@ -9,7 +9,6 @@ def test1(): m.main() - @pytest.mark.parametrize("dt", [0.5, 0.7, 6.0]) def test_quintic_trajectory_includes_exact_terminal_time(dt): m.show_animation = False