feat(motion_velocity_smoother): use utility functions in autoware (au…

…towarefoundation#1718)

* feat(motion_velocity_smoother): use utility functions in autoware

* fix stop distance publisher

Signed-off-by: yutaka <purewater0901@gmail.com>

Signed-off-by: yutaka <purewater0901@gmail.com>

planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp

@@ -44,8 +44,6 @@ TrajectoryPoints extractPathAroundIndex(
   const TrajectoryPoints & trajectory, const size_t index, const double & ahead_length,
   const double & behind_length);
 
-double calcArcLength(const TrajectoryPoints & trajectory, const int idx1, const int idx2);
-
 std::vector<double> calcArclengthArray(const TrajectoryPoints & trajectory);
 
 std::vector<double> calcTrajectoryIntervalDistance(const TrajectoryPoints & trajectory);

planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp

@@ -616,14 +616,13 @@ void MotionVelocitySmootherNode::publishStopDistance(const TrajectoryPoints & tr
   double stop_dist{stop_dist_lim};
   const auto stop_idx{motion_utils::searchZeroVelocityIndex(trajectory)};
   if (stop_idx) {
-    stop_dist = trajectory_utils::calcArcLength(trajectory, closest, *stop_idx);
-    stop_dist = closest > *stop_idx ? stop_dist : -stop_dist;
+    stop_dist = motion_utils::calcSignedArcLength(trajectory, closest, *stop_idx);
   } else {
     stop_dist = closest > 0 ? stop_dist : -stop_dist;
   }
   Float32Stamped dist_to_stopline{};
   dist_to_stopline.stamp = this->now();
-  dist_to_stopline.data = std::max(-stop_dist_lim, std::min(stop_dist_lim, stop_dist));
+  dist_to_stopline.data = std::clamp(stop_dist, -stop_dist_lim, stop_dist_lim);
   pub_dist_to_stopline_->publish(dist_to_stopline);
 }
 

planning/motion_velocity_smoother/src/trajectory_utils.cpp

@@ -140,41 +140,20 @@ TrajectoryPoints extractPathAroundIndex(
   return extracted_traj;
 }
 
-double calcArcLength(const TrajectoryPoints & path, const int idx1, const int idx2)
+std::vector<double> calcArclengthArray(const TrajectoryPoints & trajectory)
 {
-  if (idx1 == idx2) {  // zero distance
-    return 0.0;
-  }
-
-  if (
-    idx1 < 0 || idx2 < 0 || static_cast<int>(path.size()) - 1 < idx1 ||
-    static_cast<int>(path.size()) - 1 < idx2) {
-    RCLCPP_ERROR(
-      rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"),
-      "invalid index");
-    return 0.0;
-  }
-
-  const int idx_from = std::min(idx1, idx2);
-  const int idx_to = std::max(idx1, idx2);
-  double dist_sum = 0.0;
-  for (int i = idx_from; i < idx_to; ++i) {
-    dist_sum += tier4_autoware_utils::calcDistance2d(path.at(i), path.at(i + 1));
+  if (trajectory.empty()) {
+    return {};
   }
-  return dist_sum;
-}
 
-std::vector<double> calcArclengthArray(const TrajectoryPoints & trajectory)
-{
-  std::vector<double> arclength;
+  std::vector<double> arclength(trajectory.size());
   double dist = 0.0;
-  arclength.clear();
-  arclength.push_back(dist);
+  arclength.front() = dist;
   for (unsigned int i = 1; i < trajectory.size(); ++i) {
     const TrajectoryPoint tp = trajectory.at(i);
     const TrajectoryPoint tp_prev = trajectory.at(i - 1);
     dist += tier4_autoware_utils::calcDistance2d(tp.pose, tp_prev.pose);
-    arclength.push_back(dist);
+    arclength.at(i) = dist;
   }
   return arclength;
 }