Handle cubic spline endpoints and invalid knot sequences

PathPlanning/CubicSpline/cubic_spline_planner.py

@@ -45,9 +45,13 @@ class CubicSpline1D:
 
     def __init__(self, x, y):
 
+        if len(x) != len(y):
+            raise ValueError("x and y must have the same number of coordinates")
+        if len(x) < 2:
+            raise ValueError("at least two coordinates are required")
         h = np.diff(x)
-        if np.any(h < 0):
-            raise ValueError("x coordinates must be sorted in ascending order")
+        if np.any(h <= 0):
+            raise ValueError("x coordinates must be strictly increasing")
 
         self.a, self.b, self.c, self.d = [], [], [], []
         self.x = x
@@ -181,7 +185,7 @@ def __search_index(self, x):
         """
         search data segment index
         """
-        return bisect.bisect(self.x, x) - 1
+        return min(bisect.bisect(self.x, x) - 1, self.nx - 2)
 
     def __calc_A(self, h):
         """
@@ -320,6 +324,8 @@ def calc_curvature(self, s):
         k : float
             curvature for given s.
         """
+        if s < self.s[0] or s > self.s[-1]:
+            return None
         dx = self.sx.calc_first_derivative(s)
         ddx = self.sx.calc_second_derivative(s)
         dy = self.sy.calc_first_derivative(s)
@@ -342,6 +348,8 @@ def calc_curvature_rate(self, s):
         k : float
             curvature rate for given s.
         """
+        if s < self.s[0] or s > self.s[-1]:
+            return None
         dx = self.sx.calc_first_derivative(s)
         dy = self.sy.calc_first_derivative(s)
         ddx = self.sx.calc_second_derivative(s)
@@ -369,6 +377,8 @@ def calc_yaw(self, s):
         yaw : float
             yaw angle (tangent vector) for given s.
         """
+        if s < self.s[0] or s > self.s[-1]:
+            return None
         dx = self.sx.calc_first_derivative(s)
         dy = self.sy.calc_first_derivative(s)
         yaw = math.atan2(dy, dx)

tests/test_cubic_spline_boundaries.py

@@ -0,0 +1,35 @@
+import math
+import unittest
+
+from PathPlanning.CubicSpline.cubic_spline_planner import CubicSpline1D, CubicSpline2D
+
+
+class TestSplineBoundaries(unittest.TestCase):
+    def test_final_knot_uses_last_polynomial(self):
+        spline = CubicSpline1D([0., 1.], [1., 3.])
+        assert [method(1.) for method in (spline.calc_position, spline.calc_first_derivative,
+                                          spline.calc_second_derivative, spline.calc_third_derivative)] == [3., 2., 0., 0.]
+
+
+    def test_repeated_knots(self):
+        with self.assertRaises(ValueError):
+            CubicSpline1D([0, 1, 1], [0, 1, 2])
+
+
+    def test_minimum_knots(self):
+        with self.assertRaises(ValueError):
+            CubicSpline1D([0], [1])
+
+
+    def test_unpaired_samples(self):
+        with self.assertRaises(ValueError):
+            CubicSpline1D([0, 1], [1, 2, 3])
+
+
+    def test_queries_beyond_path(self):
+        spline = CubicSpline2D([0, 1], [0, 1])
+        for s in (-1, 3):
+            assert spline.calc_yaw(s) is None
+            assert spline.calc_curvature(s) is None
+            assert spline.calc_curvature_rate(s) is None
+        assert math.isclose(spline.calc_yaw(0), math.pi / 4)