calculate yaw angle by first derivative

src/components/course/cubic_spline_course/cubic_spline.py

@@ -46,6 +46,16 @@ def calculate_position(self, x):
 
         return y
 
+    def calculate_first_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]
+        dy = self.b[i_x] + 2.0 * self.c[i_x] * dx + 3.0 * self.d[i_x] * dx ** 2.0
+
+        return dy
+
     def _search_segment_index(self, x):
         return bisect.bisect(self.x_points, x) - 1
 

src/components/course/cubic_spline_course/cubic_spline_2d.py

@@ -5,6 +5,7 @@
 """
 
 import numpy as np
+import math
 
 from cubic_spline import CubicSpline
 
@@ -26,6 +27,12 @@ def calc_interpolated_xy(self, s):
         interpolated_y = self.sy.calculate_position(s)
 
         return interpolated_x, interpolated_y
+
+    def calc_yaw_angle(self, s):
+        dx = self.sx.calculate_first_derivative(s)
+        dy = self.sy.calculate_first_derivative(s)
+        yaw_angle = math.atan2(dy, dx)
+        return yaw_angle
 
     def _calc_base_points(self, x_points, y_points):
         dx = np.diff(x_points)

src/simulations/course/cubic_spline/cubic_spline_2d_plot.py

@@ -42,6 +42,7 @@ def main():
         i_x, i_y = cs.calc_interpolated_xy(i_s)
         xs.append(i_x)
         ys.append(i_y)
+        yaws.append(cs.calc_yaw_angle(i_s))
 
     plt.subplots(1)
     plt.plot(x_points, y_points, "xb", label="Input points")
@@ -53,6 +54,14 @@ def main():
     plt.legend()
     if show_plot: plt.savefig("cubic_spline_2d.png")
 
+    plt.subplots(1)
+    plt.plot(s, [np.rad2deg(yaw) for yaw in yaws], "-r", label="Yaw angle")
+    plt.grid(True)
+    plt.xlabel("Line length[m]")
+    plt.ylabel("Yaw angle[deg]")
+    plt.legend()
+    if show_plot: plt.savefig("cubic_spline_yaw_angle.png")
+
 
 if __name__ == "__main__":
     main()