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)) diff --git a/tests/test_quintic_polynomials_planner.py b/tests/test_quintic_polynomials_planner.py index 43f3c6bada..bfda9564db 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,21 @@ 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__)