Skip to content

Commit

Permalink
fix(behavior_velocity_planner): fix velocity interpolation changing t…
Browse files Browse the repository at this point in the history 
…he stop position inappropriately (#623)

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

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* fix typo

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* add test for interpolation

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* fix build warning

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>
  • Loading branch information
@TakaHoribe @taikitanaka3
TakaHoribe authored and taikitanaka3 committed Apr 14, 2022
1 parent 7321296 commit 5fa477c
Show file tree
Hide file tree
Showing 2 changed files with 136 additions and 12 deletions.
39 changes: 27 additions & 12 deletions planning/behavior_velocity_planner/src/utilization/path_utilization.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand All @@ -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.",
Expand Down Expand Up @@ -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;
Expand All @@ -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;
}
Expand Down
109 changes: 109 additions & 0 deletions planning/behavior_velocity_planner/test/src/test_utilization.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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);
}
}

0 comments on commit 5fa477c

Please sign in to comment.