Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(tier4_autoware_utils): add new insert point function #1222

Merged
merged 7 commits into from
Jul 6, 2022
Merged

feat(tier4_autoware_utils): add new insert point function #1222

Show file tree
Hide file tree
Changes from 3 commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
6e085b3
add new length function
purewater0901 Jul 4, 2022
ba042d3
add test
purewater0901 Jul 4, 2022
4a7955b
Merge branch 'main' into feat/add-new-insert-point-function
purewater0901 Jul 4, 2022
531bc2f
fix test
purewater0901 Jul 5, 2022
e5f88b2
Merge branch 'feat/add-new-insert-point-function' of github.com:tier4…
purewater0901 Jul 5, 2022
e390d52
add path with lane id funciton
purewater0901 Jul 6, 2022
f750058
Merge branch 'main' into feat/add-new-insert-point-function
purewater0901 Jul 6, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
35 changes: 35 additions & 0 deletions common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/trajectory.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -667,6 +667,41 @@ inline boost::optional<size_t> insertTargetPoint(

return seg_idx;
}

/**
* @brief calculate the point offset from source point along the trajectory (or path)
* @param insert_point_length length to insert point from the beginning of the points
* @param p_target point to be inserted
* @param points output points of trajectory, path, ...
* @return index of insert point
*/
template <class T>
inline boost::optional<size_t> insertTargetPoint(
const double insert_point_length, const geometry_msgs::msg::Point & p_target, T & points,
const double overlap_threshold = 1e-3)
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Don't you add specialized function for PathWithLaneId ?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

done

{
validateNonEmpty(points);

if (insert_point_length < 0.0) {
return boost::none;
}

// Get Nearest segment index
boost::optional<size_t> segment_idx = boost::none;
for (size_t i = 1; i < points.size(); ++i) {
const double length = calcSignedArcLength(points, 0, i);
if (insert_point_length <= length) {
segment_idx = i - 1;
break;
}
}

if (!segment_idx) {
return boost::none;
}

return insertTargetPoint(*segment_idx, p_target, points, overlap_threshold);
}
} // namespace tier4_autoware_utils

#endif // TIER4_AUTOWARE_UTILS__TRAJECTORY__TRAJECTORY_HPP_
247 changes: 245 additions & 2 deletions common/tier4_autoware_utils/test/src/trajectory/test_trajectory.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1676,17 +1676,20 @@ TEST(trajectory, insertTargetPoint)
{
auto traj_out = traj;

const size_t segment_idx = 9U;
const auto p_target = createPoint(10.0, 0.0, 0.0);
const auto insert_idx = insertTargetPoint(9U, p_target, traj_out.points);
const auto insert_idx = insertTargetPoint(segment_idx, p_target, traj_out.points);

EXPECT_EQ(insert_idx, boost::none);
}

// Empty
{
auto empty_traj = generateTestTrajectory<Trajectory>(0, 1.0);
const size_t segment_idx = 0;
EXPECT_THROW(
insertTargetPoint({}, geometry_msgs::msg::Point{}, empty_traj.points), std::invalid_argument);
insertTargetPoint(segment_idx, geometry_msgs::msg::Point{}, empty_traj.points),
std::invalid_argument);
}
}

Expand Down Expand Up @@ -1782,3 +1785,243 @@ TEST(trajectory, insertTargetPoint_OverlapThreshold)
}
}
}

TEST(trajectory, insertTargetPoint_Length)
{
using tier4_autoware_utils::calcArcLength;
using tier4_autoware_utils::calcDistance2d;
using tier4_autoware_utils::createPoint;
using tier4_autoware_utils::deg2rad;
using tier4_autoware_utils::findNearestSegmentIndex;
using tier4_autoware_utils::getPose;
using tier4_autoware_utils::insertTargetPoint;

const auto traj = generateTestTrajectory<Trajectory>(10, 1.0);
const auto total_length = calcArcLength(traj.points);

// Insert
for (double x_start = 0.5; x_start < total_length; x_start += 1.0) {
auto traj_out = traj;

const auto p_target = createPoint(x_start, 0.0, 0.0);
const size_t base_idx = findNearestSegmentIndex(traj.points, p_target);
const auto insert_idx = insertTargetPoint(x_start, p_target, traj_out.points);

EXPECT_NE(insert_idx, boost::none);
EXPECT_EQ(insert_idx.get(), base_idx + 1);
EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1);

for (size_t i = 0; i < traj_out.points.size() - 1; ++i) {
EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3);
}

{
const auto p_insert = getPose(traj_out.points.at(insert_idx.get()));
const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0));
EXPECT_EQ(p_insert.position.x, p_target.x);
EXPECT_EQ(p_insert.position.y, p_target.y);
EXPECT_EQ(p_insert.position.z, p_target.z);
EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon);
EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon);
EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon);
EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon);
}

{
const auto p_base = getPose(traj_out.points.at(base_idx));
const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0));
EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon);
EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon);
EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon);
EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon);
}
}

// Insert(Boundary condition)
for (double x_start = 0.0; x_start < total_length; x_start += 1.0) {
auto traj_out = traj;

const auto p_target = createPoint(x_start + 1.1e-3, 0.0, 0.0);
const size_t base_idx = findNearestSegmentIndex(traj.points, p_target);
const auto insert_idx = insertTargetPoint(x_start + 1.1e-3, p_target, traj_out.points);

EXPECT_NE(insert_idx, boost::none);
EXPECT_EQ(insert_idx.get(), base_idx + 1);
EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1);

for (size_t i = 0; i < traj_out.points.size() - 1; ++i) {
EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3);
}

{
const auto p_insert = getPose(traj_out.points.at(insert_idx.get()));
const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0));
EXPECT_EQ(p_insert.position.x, p_target.x);
EXPECT_EQ(p_insert.position.y, p_target.y);
EXPECT_EQ(p_insert.position.z, p_target.z);
EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon);
EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon);
EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon);
EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon);
}

{
const auto p_base = getPose(traj_out.points.at(base_idx));
const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0));
EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon);
EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon);
EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon);
EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon);
}
}

// Right on the terminal point
{
auto traj_out = traj;

const auto p_target = createPoint(9.0, 0.0, 0.0);
const size_t base_idx = findNearestSegmentIndex(traj.points, p_target);
const auto insert_idx = insertTargetPoint(9.0, p_target, traj_out.points);

EXPECT_NE(insert_idx, boost::none);
EXPECT_EQ(insert_idx.get(), base_idx + 1);
EXPECT_EQ(traj_out.points.size(), traj.points.size());

for (size_t i = 0; i < traj_out.points.size() - 1; ++i) {
EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3);
}

{
const auto p_insert = getPose(traj_out.points.at(insert_idx.get()));
const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0));
EXPECT_EQ(p_insert.position.x, p_target.x);
EXPECT_EQ(p_insert.position.y, p_target.y);
EXPECT_EQ(p_insert.position.z, p_target.z);
EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon);
EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon);
EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon);
EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon);
}

{
const auto p_base = getPose(traj_out.points.at(base_idx));
const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0));
EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon);
EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon);
EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon);
EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon);
}
}

// Insert(Quaternion interpolation)
for (double x_start = 0.25; x_start < total_length; x_start += 1.0) {
auto traj_out = traj;

const auto p_target = createPoint(x_start, 0.25 * std::tan(deg2rad(60.0)), 0.0);
const size_t base_idx = findNearestSegmentIndex(traj.points, p_target);
const auto insert_idx = insertTargetPoint(x_start, p_target, traj_out.points);

EXPECT_NE(insert_idx, boost::none);
EXPECT_EQ(insert_idx.get(), base_idx + 1);
EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1);

for (size_t i = 0; i < traj_out.points.size() - 1; ++i) {
EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3);
}

{
const auto p_insert = getPose(traj_out.points.at(insert_idx.get()));
const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(-30.0));
EXPECT_EQ(p_insert.position.x, p_target.x);
EXPECT_EQ(p_insert.position.y, p_target.y);
EXPECT_EQ(p_insert.position.z, p_target.z);
EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon);
EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon);
EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon);
EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon);
}

{
const auto p_base = getPose(traj_out.points.at(base_idx));
const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(60.0));
EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon);
EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon);
EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon);
EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon);
}
}

// Not insert(Overlap base_idx point)
for (double x_start = 0.0; x_start < total_length - 1.0 + epsilon; x_start += 1.0) {
auto traj_out = traj;

const auto p_target = createPoint(x_start + 1e-4, 0.0, 0.0);
const size_t base_idx = findNearestSegmentIndex(traj.points, p_target);
const auto insert_idx = insertTargetPoint(x_start + 1e-4, p_target, traj_out.points);

EXPECT_NE(insert_idx, boost::none);
EXPECT_EQ(insert_idx.get(), base_idx);
EXPECT_EQ(traj_out.points.size(), traj.points.size());

for (size_t i = 0; i < traj_out.points.size() - 1; ++i) {
EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3);
}
}

// Not insert(Overlap base_idx + 1 point)
for (double x_start = 1.0; x_start < total_length + epsilon; x_start += 1.0) {
auto traj_out = traj;

const auto p_target = createPoint(x_start - 1e-4, 0.0, 0.0);
const size_t base_idx = findNearestSegmentIndex(traj.points, p_target);
const auto insert_idx = insertTargetPoint(x_start - 1e-4, p_target, traj_out.points);

EXPECT_NE(insert_idx, boost::none);
EXPECT_EQ(insert_idx.get(), base_idx + 1);
EXPECT_EQ(traj_out.points.size(), traj.points.size());

for (size_t i = 0; i < traj_out.points.size() - 1; ++i) {
EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3);
}
}

// Invalid target point(In front of the beginning point)
{
auto traj_out = traj;

const auto p_target = createPoint(-1.0, 0.0, 0.0);
const auto insert_idx = insertTargetPoint(-1.0, p_target, traj_out.points);

EXPECT_EQ(insert_idx, boost::none);
}

// Invalid target point(Behind the end point)
{
auto traj_out = traj;

const auto p_target = createPoint(10.0, 0.0, 0.0);
const auto insert_idx = insertTargetPoint(10.0, p_target, traj_out.points);

EXPECT_EQ(insert_idx, boost::none);
}

// Invalid target point(Huge lateral offset)
{
testing::internal::CaptureStderr();
auto traj_out = traj;

const auto p_target = createPoint(4.0, 10.0, 0.0);
const auto insert_idx = insertTargetPoint(4.0, p_target, traj_out.points);

EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n");
EXPECT_EQ(insert_idx, boost::none);
}

// Empty
{
auto empty_traj = generateTestTrajectory<Trajectory>(0, 1.0);
EXPECT_THROW(
insertTargetPoint(0.0, geometry_msgs::msg::Point{}, empty_traj.points),
std::invalid_argument);
}
}