diff --git a/planning/behavior_velocity_planner/src/utilization/path_utilization.cpp b/planning/behavior_velocity_planner/src/utilization/path_utilization.cpp index 7513346ed93d2..9e481197828e7 100644 --- a/planning/behavior_velocity_planner/src/utilization/path_utilization.cpp +++ b/planning/behavior_velocity_planner/src/utilization/path_utilization.cpp @@ -109,6 +109,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) { @@ -120,7 +128,7 @@ autoware_auto_planning_msgs::msg::Path interpolatePath( std::vector y; std::vector z; std::vector v; - std::vector s_in, s_out; + std::vector s_in; if (2000 < path.points.size()) { RCLCPP_WARN( logger, "because path size is too large, calculation cost is high. size is %d.", @@ -162,20 +170,24 @@ autoware_auto_planning_msgs::msg::Path interpolatePath( } // Calculate query points - // Remove query point if query point is near input path point - std::vector s_tmp = s_in; - for (double s = 0.0; s < path_len; s += interval) { - s_tmp.push_back(s); - } - std::sort(s_tmp.begin(), s_tmp.end()); + // 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 s_out = s_in; - for (const double s : s_tmp) { - if (!s_out.empty() && std::fabs(s_out.back() - s) < epsilon) { - continue; + 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) { + 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; @@ -186,11 +198,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 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; } diff --git a/planning/behavior_velocity_planner/test/src/test_utilization.cpp b/planning/behavior_velocity_planner/test/src/test_utilization.cpp index f2c557454b273..f362ee2b30f80 100644 --- a/planning/behavior_velocity_planner/test/src/test_utilization.cpp +++ b/planning/behavior_velocity_planner/test/src/test_utilization.cpp @@ -12,6 +12,8 @@ // 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 @@ -19,6 +21,18 @@ #include +#include + +#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 px{0.0, 1.0, 2.0, 3.0}; + const std::vector 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 px{0.0, 1.0, 2.1, 3.0}; + const std::vector 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 px{0.0, 1.0, 2.001, 3.0}; + const std::vector 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 px{0.0, 1.0, 1.999, 2.0, 2.001, 3.0}; + const std::vector 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 px{0.0, 1.0, 1.999, 2.0, 2.001, 3.0}; + const std::vector 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 px{0.0, 1.0, 1.999, 3.0}; + const std::vector 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 px{0.0, 0.1, 0.2, 0.3, 0.4}; + const std::vector 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 px{0.0, 0.1, 0.2, 0.3, 0.4}; + const std::vector vx{5.5, 5.5, 5.5, 5.5, 0.0}; + EXPECT_DOUBLE_EQ(calcInterpolatedStopDist(px, vx), expected); + } +}