fix(motion_utils): add guard to calcCurvature (#6070)

* fix(motion_utils): add guard to calcCurvature Signed-off-by: kosuke55 <[email protected]> * style(pre-commit): autofix --------- Signed-off-by: kosuke55 <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
autowarefoundation · Jan 14, 2024 · 0aa3927 · 0aa3927
1 parent 39642d2
commit 0aa3927
Showing 1 changed file with 4 additions and 1 deletion.
diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp
@@ -952,7 +952,10 @@ calcArcLength<std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>>(
 template <class T>
 std::vector<double> calcCurvature(const T & points)
 {
-  std::vector<double> curvature_vec(points.size());
+  std::vector<double> curvature_vec(points.size(), 0.0);
+  if (points.size() < 3) {
+    return curvature_vec;
+  }
 
   for (size_t i = 1; i < points.size() - 1; ++i) {
     const auto p1 = tier4_autoware_utils::getPoint(points.at(i - 1));