fix(behavior_velocity_planner): fix velocity interpolation changing the stop position inappropriately

planning/behavior_velocity_planner/src/utilization/path_utilization.cpp

@@ -116,6 +116,14 @@ bool splineInterpolate(
   return true;
 }
 
+/*
+ * Interpolate the path with a fixed interval by spline.
+ * In order to correctly inherit the position of the planned velocity points, the position of the
+ * existing points in the input path are kept in the interpolated path.
+ * The velocity is interpolated by zero-order hold, that is, the velocity of the interpolated point
+ * is the velocity of the closest point for the input "sub-path" which consists of the points before
+ * the interpolated point.
+ */
 autoware_auto_planning_msgs::msg::Path interpolatePath(
   const autoware_auto_planning_msgs::msg::Path & path, const double length, const double interval)
 {
@@ -127,7 +135,7 @@ autoware_auto_planning_msgs::msg::Path interpolatePath(
   std::vector<double> y;
   std::vector<double> z;
   std::vector<double> v;
-  std::vector<double> s_in, s_out;
+  std::vector<double> s_in;
   if (2000 < path.points.size()) {
     RCLCPP_WARN(
       logger, "because path size is too large, calculation cost is high. size is %d.",
@@ -169,20 +177,24 @@ autoware_auto_planning_msgs::msg::Path interpolatePath(
   }
 
   // Calculate query points
-  // Remove query point if query point is near input path point
-  std::vector<double> s_tmp = s_in;
+  // Use all values of s_in to inherit the velocity-planned point, and add some points for
+  // interpolation with a constant interval if the point is not closed to the original s_in points.
+  // (because if this interval is very short, the interpolation will be less accurate and may fail.)
+  std::vector<double> s_out = s_in;
+
+  const auto has_almost_same_value = [&](const auto & vec, const auto x) {
+    if (vec.empty()) return false;
+    const auto has_close = [&](const auto v) { return std::abs(v - x) < epsilon; };
+    return std::find_if(vec.begin(), vec.end(), has_close) != vec.end();
+  };
   for (double s = 0.0; s < path_len; s += interval) {
-    s_tmp.push_back(s);
-  }
-  std::sort(s_tmp.begin(), s_tmp.end());
-
-  for (const double s : s_tmp) {
-    if (!s_out.empty() && std::fabs(s_out.back() - s) < epsilon) {
-      continue;
+    if (!has_almost_same_value(s_out, s)) {
+      s_out.push_back(s);
     }
-    s_out.push_back(s);
   }
 
+  std::sort(s_out.begin(), s_out.end());
+
   if (s_out.empty()) {
     RCLCPP_WARN(logger, "Do not interpolate because s_out is empty.");
     return path;
@@ -193,11 +205,14 @@ autoware_auto_planning_msgs::msg::Path interpolatePath(
   const auto y_interp = interpolation::slerp(s_in, y, s_out);
   const auto z_interp = interpolation::slerp(s_in, z, s_out);
 
+  // Find a nearest segment for each point in s_out and use the velocity of the segment's beginning
+  // point. Note that if s_out is almost the same value as s_in within DOUBLE_EPSILON range, the
+  // velocity of s_out should be same as the one of s_in.
   std::vector<double> v_interp;
   size_t closest_segment_idx = 0;
   for (size_t i = 0; i < s_out.size() - 1; ++i) {
     for (size_t j = closest_segment_idx; j < s_in.size() - 1; ++j) {
-      if (s_in.at(j) < s_out.at(i) + DOUBLE_EPSILON && s_out.at(i) < s_in.at(j + 1)) {
+      if (s_in.at(j) - DOUBLE_EPSILON < s_out.at(i) && s_out.at(i) < s_in.at(j + 1)) {
         // find closest segment in s_in
         closest_segment_idx = j;
       }

planning/behavior_velocity_planner/test/src/test_utilization.cpp

@@ -12,13 +12,27 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include "tier4_autoware_utils/trajectory/trajectory.hpp"
+#include "utilization/path_utilization.hpp"
 #include "utils.hpp"
 
 #include <utilization/boost_geometry_helper.hpp>
 #include <utilization/util.hpp>
 
 #include <gtest/gtest.h>
 
+#include <iostream>
+
+#define DEBUG_PRINT_PATH(path)                                                        \
+  {                                                                                   \
+    std::stringstream ss;                                                             \
+    ss << #path << "(px, vx): ";                                                      \
+    for (const auto p : path.points) {                                                \
+      ss << "(" << p.pose.position.x << ", " << p.longitudinal_velocity_mps << "), "; \
+    }                                                                                 \
+    std::cerr << ss.str() << std::endl;                                               \
+  }
+
 TEST(to_footprint_polygon, nominal)
 {
   using behavior_velocity_planner::planning_utils::toFootprintPolygon;
@@ -73,3 +87,98 @@ TEST(smoothDeceleration, calculateMaxSlowDownVelocity)
     }
   }
 }
+
+TEST(specialInterpolation, specialInterpolation)
+{
+  using autoware_auto_planning_msgs::msg::Path;
+  using autoware_auto_planning_msgs::msg::PathPoint;
+  using behavior_velocity_planner::interpolatePath;
+  using tier4_autoware_utils::calcSignedArcLength;
+  using tier4_autoware_utils::searchZeroVelocityIndex;
+
+  const auto genPath = [](const auto p, const auto v) {
+    if (p.size() != v.size()) throw std::invalid_argument("different size is not expected");
+    Path path;
+    for (size_t i = 0; i < p.size(); ++i) {
+      PathPoint pp;
+      pp.pose.position.x = p.at(i);
+      pp.longitudinal_velocity_mps = v.at(i);
+      path.points.push_back(pp);
+    }
+    return path;
+  };
+
+  constexpr auto length = 5.0;
+  constexpr auto interval = 1.0;
+
+  const auto calcInterpolatedStopDist = [&](const auto & px, const auto & vx) {
+    const auto path = genPath(px, vx);
+    const auto res = interpolatePath(path, length, interval);
+    // DEBUG_PRINT_PATH(path);
+    // DEBUG_PRINT_PATH(res);
+    return calcSignedArcLength(res.points, 0, *searchZeroVelocityIndex(res.points));
+  };
+
+  // expected stop position: s=2.0
+  {
+    const std::vector<double> px{0.0, 1.0, 2.0, 3.0};
+    const std::vector<double> vx{5.5, 5.5, 0.0, 0.0};
+    EXPECT_DOUBLE_EQ(calcInterpolatedStopDist(px, vx), 2.0);
+  }
+
+  // expected stop position: s=2.1
+  {
+    constexpr auto expected = 2.1;
+    const std::vector<double> px{0.0, 1.0, 2.1, 3.0};
+    const std::vector<double> vx{5.5, 5.5, 0.0, 0.0};
+    EXPECT_DOUBLE_EQ(calcInterpolatedStopDist(px, vx), expected);
+  }
+
+  // expected stop position: s=2.001
+  {
+    constexpr auto expected = 2.001;
+    const std::vector<double> px{0.0, 1.0, 2.001, 3.0};
+    const std::vector<double> vx{5.5, 5.5, 0.000, 0.0};
+    EXPECT_DOUBLE_EQ(calcInterpolatedStopDist(px, vx), expected);
+  }
+
+  // expected stop position: s=2.001
+  {
+    constexpr auto expected = 2.001;
+    const std::vector<double> px{0.0, 1.0, 1.999, 2.0, 2.001, 3.0};
+    const std::vector<double> vx{5.5, 5.5, 5.555, 5.5, 0.000, 0.0};
+    EXPECT_DOUBLE_EQ(calcInterpolatedStopDist(px, vx), expected);
+  }
+
+  // expected stop position: s=2.0
+  {
+    constexpr auto expected = 2.0;
+    const std::vector<double> px{0.0, 1.0, 1.999, 2.0, 2.001, 3.0};
+    const std::vector<double> vx{5.5, 5.5, 5.555, 0.0, 0.000, 0.0};
+    EXPECT_DOUBLE_EQ(calcInterpolatedStopDist(px, vx), expected);
+  }
+
+  // expected stop position: s=1.999
+  {
+    constexpr auto expected = 1.999;
+    const std::vector<double> px{0.0, 1.0, 1.999, 3.0};
+    const std::vector<double> vx{5.5, 5.5, 0.000, 0.0};
+    EXPECT_DOUBLE_EQ(calcInterpolatedStopDist(px, vx), expected);
+  }
+
+  // expected stop position: s=0.2
+  {
+    constexpr auto expected = 0.2;
+    const std::vector<double> px{0.0, 0.1, 0.2, 0.3, 0.4};
+    const std::vector<double> vx{5.5, 5.5, 0.0, 0.0, 0.0};
+    EXPECT_DOUBLE_EQ(calcInterpolatedStopDist(px, vx), expected);
+  }
+
+  // expected stop position: s=0.4
+  {
+    constexpr auto expected = 0.4;
+    const std::vector<double> px{0.0, 0.1, 0.2, 0.3, 0.4};
+    const std::vector<double> vx{5.5, 5.5, 5.5, 5.5, 0.0};
+    EXPECT_DOUBLE_EQ(calcInterpolatedStopDist(px, vx), expected);
+  }
+}