refactor(motion_velocity_smoother): change curvature calculation (#1638)

* refactor(motion_velocity_smoother): change curvature calculation

Signed-off-by: Berkay Karaman <berkay@leodrive.ai>

* ci(pre-commit): autofix

* add error handler for too close points

Signed-off-by: Berkay Karaman <berkay@leodrive.ai>

* ci(pre-commit): autofix

* take copy for first and last points

Signed-off-by: Berkay Karaman <berkay@leodrive.ai>

* ci(pre-commit): autofix

Signed-off-by: Berkay Karaman <berkay@leodrive.ai>
Co-authored-by: Berkay Karaman <berkay@leodrive.ai>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

planning/motion_velocity_smoother/src/trajectory_utils.cpp

@@ -186,23 +186,32 @@ std::vector<double> calcTrajectoryCurvatureFrom3Points(
   }
 
   // calculate curvature by circle fitting from three points
-  std::vector<double> k_arr;
-  for (size_t i = idx_dist; i < trajectory.size() - idx_dist; ++i) {
+  std::vector<double> k_arr(trajectory.size(), 0.0);
+
+  for (size_t i = 1; i + 1 < trajectory.size(); i++) {
+    double curvature = 0.0;
+    const auto p0 = getPoint(trajectory.at(i - std::min(idx_dist, i)));
+    const auto p1 = getPoint(trajectory.at(i));
+    const auto p2 = getPoint(trajectory.at(i + std::min(idx_dist, trajectory.size() - 1 - i)));
     try {
-      const auto p0 = getPoint(trajectory.at(i - idx_dist));
-      const auto p1 = getPoint(trajectory.at(i));
-      const auto p2 = getPoint(trajectory.at(i + idx_dist));
-      k_arr.push_back(calcCurvature(p0, p1, p2));
-    } catch (...) {
-      k_arr.push_back(0.0);  // points are too close. No curvature.
+      curvature = calcCurvature(p0, p1, p2);
+    } catch (std::exception const & e) {
+      // ...code that handles the error...
+      RCLCPP_WARN(
+        rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"), "%s",
+        e.what());
+      if (i > 1) {
+        curvature = k_arr.at(i - 1);  // previous curvature
+      } else {
+        curvature = 0.0;
+      }
     }
+    k_arr.at(i) = curvature;
   }
+  // copy curvatures for the last and first points;
+  k_arr.at(0) = k_arr.at(1);
+  k_arr.back() = k_arr.at((trajectory.size() - 2));
 
-  // first and last curvature is copied from next value
-  for (size_t i = 0; i < idx_dist; ++i) {
-    k_arr.insert(k_arr.begin(), k_arr.front());
-    k_arr.push_back(k_arr.back());
-  }
   return k_arr;
 }