Skip to content

Commit

Permalink
feat(motion_utils): generalize overlap functions (autowarefoundation#…
Browse files Browse the repository at this point in the history 
…1871)

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

Signed-off-by: yutaka <purewater0901@gmail.com>
  • Loading branch information
@purewater0901 @boyali
purewater0901 authored and boyali committed Oct 19, 2022
1 parent 40b3fd7 commit 37fcd0b
Show file tree
Hide file tree
Showing 3 changed files with 155 additions and 32 deletions.
58 changes: 27 additions & 31 deletions common/motion_utils/include/motion_utils/trajectory/trajectory.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,37 +26,6 @@
#include <stdexcept>
#include <vector>

namespace
{
template <class T>
std::vector<geometry_msgs::msg::Point> removeOverlapPoints(const T & points, const size_t & idx)
{
std::vector<geometry_msgs::msg::Point> dst;

for (const auto & pt : points) {
dst.push_back(tier4_autoware_utils::getPoint(pt));
}

if (points.empty()) {
return dst;
}

constexpr double eps = 1.0E-08;
size_t i = idx;
while (i != dst.size() - 1) {
const auto p = tier4_autoware_utils::getPoint(dst.at(i));
const auto p_next = tier4_autoware_utils::getPoint(dst.at(i + 1));
const Eigen::Vector3d v{p_next.x - p.x, p_next.y - p.y, 0.0};
if (v.norm() < eps) {
dst.erase(dst.begin() + i + 1);
} else {
++i;
}
}
return dst;
}
} // namespace

namespace motion_utils
{
template <class T>
Expand Down Expand Up @@ -122,6 +91,33 @@ boost::optional<bool> isDrivingForwardWithTwist(const T points_with_twist)
return isDrivingForward(points_with_twist);
}

template <class T>
T removeOverlapPoints(const T & points, const size_t & start_idx = 0)
{
if (points.size() < start_idx + 1) {
return points;
}

T dst;

for (size_t i = 0; i <= start_idx; ++i) {
dst.push_back(points.at(i));
}

constexpr double eps = 1.0E-08;
for (size_t i = start_idx + 1; i < points.size(); ++i) {
const auto prev_p = tier4_autoware_utils::getPoint(dst.back());
const auto curr_p = tier4_autoware_utils::getPoint(points.at(i));
const double dist = tier4_autoware_utils::calcDistance2d(prev_p, curr_p);
if (dist < eps) {
continue;
}
dst.push_back(points.at(i));
}

return dst;
}

template <class T>
boost::optional<size_t> searchZeroVelocityIndex(
const T & points_with_twist, const size_t src_idx, const size_t dst_idx)
Expand Down
127 changes: 127 additions & 0 deletions common/motion_utils/test/src/trajectory/test_trajectory.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4397,3 +4397,130 @@ TEST(trajectory, calcSignedArcLengthFromPointAndSegmentIndexToPointIndex)
EXPECT_NEAR(calcSignedArcLength(traj.points, 4, p2, 1), -1.7, epsilon);
}
}

TEST(trajectory, removeOverlapPoints)
{
using motion_utils::removeOverlapPoints;

const auto traj = generateTestTrajectory<Trajectory>(10, 1.0, 1.0);
const auto removed_traj = removeOverlapPoints(traj.points, 0);
EXPECT_EQ(traj.points.size(), removed_traj.size());
for (size_t i = 0; i < traj.points.size(); ++i) {
EXPECT_NEAR(traj.points.at(i).pose.position.x, removed_traj.at(i).pose.position.x, epsilon);
EXPECT_NEAR(traj.points.at(i).pose.position.y, removed_traj.at(i).pose.position.y, epsilon);
EXPECT_NEAR(traj.points.at(i).pose.position.z, removed_traj.at(i).pose.position.z, epsilon);
EXPECT_NEAR(
traj.points.at(i).pose.orientation.x, removed_traj.at(i).pose.orientation.x, epsilon);
EXPECT_NEAR(
traj.points.at(i).pose.orientation.y, removed_traj.at(i).pose.orientation.y, epsilon);
EXPECT_NEAR(
traj.points.at(i).pose.orientation.z, removed_traj.at(i).pose.orientation.z, epsilon);
EXPECT_NEAR(
traj.points.at(i).pose.orientation.w, removed_traj.at(i).pose.orientation.w, epsilon);
EXPECT_NEAR(
traj.points.at(i).longitudinal_velocity_mps, removed_traj.at(i).longitudinal_velocity_mps,
epsilon);
}

// No overlap points
{
const auto traj = generateTestTrajectory<Trajectory>(10, 1.0, 1.0);
for (size_t start_idx = 0; start_idx < 10; ++start_idx) {
const auto removed_traj = removeOverlapPoints(traj.points, start_idx);
EXPECT_EQ(traj.points.size(), removed_traj.size());
for (size_t i = 0; i < traj.points.size(); ++i) {
EXPECT_NEAR(traj.points.at(i).pose.position.x, removed_traj.at(i).pose.position.x, epsilon);
EXPECT_NEAR(traj.points.at(i).pose.position.y, removed_traj.at(i).pose.position.y, epsilon);
EXPECT_NEAR(traj.points.at(i).pose.position.z, removed_traj.at(i).pose.position.z, epsilon);
EXPECT_NEAR(
traj.points.at(i).pose.orientation.x, removed_traj.at(i).pose.orientation.x, epsilon);
EXPECT_NEAR(
traj.points.at(i).pose.orientation.y, removed_traj.at(i).pose.orientation.y, epsilon);
EXPECT_NEAR(
traj.points.at(i).pose.orientation.z, removed_traj.at(i).pose.orientation.z, epsilon);
EXPECT_NEAR(
traj.points.at(i).pose.orientation.w, removed_traj.at(i).pose.orientation.w, epsilon);
EXPECT_NEAR(
traj.points.at(i).longitudinal_velocity_mps, removed_traj.at(i).longitudinal_velocity_mps,
epsilon);
}
}
}

// Overlap points from certain point
{
auto traj = generateTestTrajectory<Trajectory>(10, 1.0, 1.0);
traj.points.at(5) = traj.points.at(6);
const auto removed_traj = removeOverlapPoints(traj.points);

EXPECT_EQ(traj.points.size() - 1, removed_traj.size());
for (size_t i = 0; i < 6; ++i) {
EXPECT_NEAR(traj.points.at(i).pose.position.x, removed_traj.at(i).pose.position.x, epsilon);
EXPECT_NEAR(traj.points.at(i).pose.position.y, removed_traj.at(i).pose.position.y, epsilon);
EXPECT_NEAR(traj.points.at(i).pose.position.z, removed_traj.at(i).pose.position.z, epsilon);
EXPECT_NEAR(
traj.points.at(i).pose.orientation.x, removed_traj.at(i).pose.orientation.x, epsilon);
EXPECT_NEAR(
traj.points.at(i).pose.orientation.y, removed_traj.at(i).pose.orientation.y, epsilon);
EXPECT_NEAR(
traj.points.at(i).pose.orientation.z, removed_traj.at(i).pose.orientation.z, epsilon);
EXPECT_NEAR(
traj.points.at(i).pose.orientation.w, removed_traj.at(i).pose.orientation.w, epsilon);
EXPECT_NEAR(
traj.points.at(i).longitudinal_velocity_mps, removed_traj.at(i).longitudinal_velocity_mps,
epsilon);
}

for (size_t i = 6; i < 9; ++i) {
EXPECT_NEAR(
traj.points.at(i + 1).pose.position.x, removed_traj.at(i).pose.position.x, epsilon);
EXPECT_NEAR(
traj.points.at(i + 1).pose.position.y, removed_traj.at(i).pose.position.y, epsilon);
EXPECT_NEAR(
traj.points.at(i + 1).pose.position.z, removed_traj.at(i).pose.position.z, epsilon);
EXPECT_NEAR(
traj.points.at(i + 1).pose.orientation.x, removed_traj.at(i).pose.orientation.x, epsilon);
EXPECT_NEAR(
traj.points.at(i + 1).pose.orientation.y, removed_traj.at(i).pose.orientation.y, epsilon);
EXPECT_NEAR(
traj.points.at(i + 1).pose.orientation.z, removed_traj.at(i).pose.orientation.z, epsilon);
EXPECT_NEAR(
traj.points.at(i + 1).pose.orientation.w, removed_traj.at(i).pose.orientation.w, epsilon);
EXPECT_NEAR(
traj.points.at(i + 1).longitudinal_velocity_mps,
removed_traj.at(i).longitudinal_velocity_mps, epsilon);
}
}

// Overlap points from certain point
{
auto traj = generateTestTrajectory<Trajectory>(10, 1.0, 1.0);
traj.points.at(5) = traj.points.at(6);
const auto removed_traj = removeOverlapPoints(traj.points, 6);

EXPECT_EQ(traj.points.size(), removed_traj.size());
for (size_t i = 0; i < traj.points.size(); ++i) {
EXPECT_NEAR(traj.points.at(i).pose.position.x, removed_traj.at(i).pose.position.x, epsilon);
EXPECT_NEAR(traj.points.at(i).pose.position.y, removed_traj.at(i).pose.position.y, epsilon);
EXPECT_NEAR(traj.points.at(i).pose.position.z, removed_traj.at(i).pose.position.z, epsilon);
EXPECT_NEAR(
traj.points.at(i).pose.orientation.x, removed_traj.at(i).pose.orientation.x, epsilon);
EXPECT_NEAR(
traj.points.at(i).pose.orientation.y, removed_traj.at(i).pose.orientation.y, epsilon);
EXPECT_NEAR(
traj.points.at(i).pose.orientation.z, removed_traj.at(i).pose.orientation.z, epsilon);
EXPECT_NEAR(
traj.points.at(i).pose.orientation.w, removed_traj.at(i).pose.orientation.w, epsilon);
EXPECT_NEAR(
traj.points.at(i).longitudinal_velocity_mps, removed_traj.at(i).longitudinal_velocity_mps,
epsilon);
}
}

// Empty Points
{
const Trajectory traj;
const auto removed_traj = removeOverlapPoints(traj.points);
EXPECT_TRUE(removed_traj.empty());
}
}
2 changes: 1 addition & 1 deletion planning/obstacle_cruise_planner/src/polygon_utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -224,7 +224,7 @@ geometry_msgs::msg::PointStamped calcNearestCollisionPoint(
const autoware_auto_planning_msgs::msg::Trajectory & decimated_traj,
const double vehicle_max_longitudinal_offset, const bool is_driving_forward)
{
std::array<geometry_msgs::msg::Point, 2> segment_points;
std::vector<geometry_msgs::msg::Point> segment_points(2);
if (first_within_idx == 0) {
const auto & traj_front_pose = decimated_traj.points.at(0).pose;
const auto front_pos = tier4_autoware_utils::calcOffsetPose(
Expand Down

0 comments on commit 37fcd0b

Please sign in to comment.