From 6ccde1c4ce7f6744b6951a04643a47fc33f4fa1a Mon Sep 17 00:00:00 2001 From: ShisatoYano Date: Tue, 10 Dec 2024 14:42:58 +0000 Subject: [PATCH] defined array of x, y, yaw, curvature and speed --- .../cubic_spline_course/cubic_spline_course.py | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/src/components/course/cubic_spline_course/cubic_spline_course.py b/src/components/course/cubic_spline_course/cubic_spline_course.py index 6d8a777..4f1f68b 100644 --- a/src/components/course/cubic_spline_course/cubic_spline_course.py +++ b/src/components/course/cubic_spline_course/cubic_spline_course.py @@ -37,3 +37,19 @@ def __init__(self, x_ref_points, y_ref_points, target_speed_kmph, resolution=0.1 cubic_spline = CubicSpline2D(x_ref_points, y_ref_points) base_points = np.arange(0, cubic_spline.s[-1], resolution) + self.x_array, self.y_array = [], [] + self.yaw_array, self.curvature_array = [], [] + for base_point in base_points: + x, y = cubic_spline.calc_interpolated_xy(base_point) + self.x_array.append(x) + self.y_array.append(y) + self.yaw_array.append(cubic_spline.calc_yaw_angle(base_point)) + self.curvature_array.append(cubic_spline.calc_curvature(base_point)) + + self.speed_array = [(target_speed_kmph / 3.6) for _ in self.x_array] + self.speed_array[-1] = 0.0 + + self.color = color + + +csc = CubicSplineCourse([0.0, 10.0, 25, 40, 50], [0.0, 4, -12, 20, -13], 10) \ No newline at end of file