implementing curvature calculation

ShisatoYano · Aug 29, 2024 · f9845b8 · f9845b8
1 parent 821c4ed
commit f9845b8
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 0 deletions.
diff --git a/src/components/course/cubic_spline_course/cubic_spline.py b/src/components/course/cubic_spline_course/cubic_spline.py
@@ -56,6 +56,16 @@ def calculate_first_derivative(self, x):
 
         return dy
 
+    def calculate_second_derivative(self, x):
+        if x < self.x_points[0]: return None
+        elif x > self.x_points[-1]: return None
+
+        i_x = self._search_segment_index(x)
+        dx = x - self.x_points[i_x]
+        ddy = 2.0 * self.c[i_x] + 6.0 * self.d[i_x] * dx
+
+        return ddy
+
     def _search_segment_index(self, x):
         return bisect.bisect(self.x_points, x) - 1
 

diff --git a/src/components/course/cubic_spline_course/cubic_spline_2d.py b/src/components/course/cubic_spline_course/cubic_spline_2d.py
@@ -33,6 +33,13 @@ def calc_yaw_angle(self, s):
         dy = self.sy.calculate_first_derivative(s)
         yaw_angle = math.atan2(dy, dx)
         return yaw_angle
+
+    def calc_curvature(self, s):
+        dx = self.sx.calculate_first_derivative(s)
+        ddx = self.sx.calculate_second_derivative(s)
+
+        dy = self.sy.calculate_first_derivative(s)
+        ddy = self.sy.calculate_second_derivative(s)
 
     def _calc_base_points(self, x_points, y_points):
         dx = np.diff(x_points)