Skip to content

Commit

Permalink
fix(tier4_autoware_utils): guard invalid segment index (#1211)
Browse files Browse the repository at this point in the history 
Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
  • Loading branch information
@satoshi-ota
satoshi-ota authored Jul 3, 2022
1 parent 0a17fe3 commit 4b3ca87
Show file tree
Hide file tree
Showing 4 changed files with 78 additions and 60 deletions.
8 changes: 7 additions & 1 deletion common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/path_with_lane_id.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -201,20 +201,26 @@ inline boost::optional<geometry_msgs::msg::Pose> calcLongitudinalOffsetPose(
* @return index of insert point
*/
template <>
inline size_t insertTargetPoint(
inline boost::optional<size_t> insertTargetPoint(
const size_t seg_idx, const geometry_msgs::msg::Point & p_target,
std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId> & points,
const double overlap_threshold)
{
validateNonEmpty(points);

// invalid segment index
if (seg_idx + 1 >= points.size()) {
return {};
}

const auto p_front = getPoint(points.at(seg_idx));
const auto p_back = getPoint(points.at(seg_idx + 1));

try {
validateNonSharpAngle(p_front, p_target, p_back);
} catch (const std::exception & e) {
std::cerr << e.what() << std::endl;
return {};
}

const auto overlap_with_front = calcDistance2d(p_target, p_front) < overlap_threshold;
Expand Down
8 changes: 7 additions & 1 deletion common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/trajectory.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -610,19 +610,25 @@ inline boost::optional<geometry_msgs::msg::Pose> calcLongitudinalOffsetPose(
* @return index of insert point
*/
template <class T>
inline size_t insertTargetPoint(
inline boost::optional<size_t> insertTargetPoint(
const size_t seg_idx, const geometry_msgs::msg::Point & p_target, T & points,
const double overlap_threshold = 1e-3)
{
validateNonEmpty(points);

// invalid segment index
if (seg_idx + 1 >= points.size()) {
return {};
}

const auto p_front = getPoint(points.at(seg_idx));
const auto p_back = getPoint(points.at(seg_idx + 1));

try {
validateNonSharpAngle(p_front, p_target, p_back);
} catch (const std::exception & e) {
std::cerr << e.what() << std::endl;
return {};
}

const auto overlap_with_front = calcDistance2d(p_target, p_front) < overlap_threshold;
Expand Down
61 changes: 32 additions & 29 deletions common/tier4_autoware_utils/test/src/trajectory/test_path_with_lane_id.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -410,15 +410,16 @@ TEST(trajectory, insertTargetPoint_PathWithLaneId)
const size_t base_idx = findNearestSegmentIndex(traj.points, p_target);
const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points);

EXPECT_EQ(insert_idx, base_idx + 1);
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));
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);
Expand Down Expand Up @@ -447,15 +448,16 @@ TEST(trajectory, insertTargetPoint_PathWithLaneId)
const size_t base_idx = findNearestSegmentIndex(traj.points, p_target);
const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points);

EXPECT_EQ(insert_idx, base_idx + 1);
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));
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);
Expand Down Expand Up @@ -484,15 +486,16 @@ TEST(trajectory, insertTargetPoint_PathWithLaneId)
const size_t base_idx = findNearestSegmentIndex(traj.points, p_target);
const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points);

EXPECT_EQ(insert_idx, base_idx + 1);
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));
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);
Expand Down Expand Up @@ -521,7 +524,8 @@ TEST(trajectory, insertTargetPoint_PathWithLaneId)
const size_t base_idx = findNearestSegmentIndex(traj.points, p_target);
const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points);

EXPECT_EQ(insert_idx, base_idx);
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) {
Expand All @@ -537,7 +541,8 @@ TEST(trajectory, insertTargetPoint_PathWithLaneId)
const size_t base_idx = findNearestSegmentIndex(traj.points, p_target);
const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points);

EXPECT_EQ(insert_idx, base_idx + 1);
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) {
Expand All @@ -555,12 +560,7 @@ TEST(trajectory, insertTargetPoint_PathWithLaneId)
const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points);

EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n");
EXPECT_EQ(insert_idx, 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);
}
EXPECT_EQ(insert_idx, boost::none);
}

// Invalid target point(Behind of end point)
Expand All @@ -573,12 +573,7 @@ TEST(trajectory, insertTargetPoint_PathWithLaneId)
const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points);

EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n");
EXPECT_EQ(insert_idx, 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);
}
EXPECT_EQ(insert_idx, boost::none);
}

// Invalid target point(Huge lateral offset)
Expand All @@ -591,12 +586,17 @@ TEST(trajectory, insertTargetPoint_PathWithLaneId)
const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points);

EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n");
EXPECT_EQ(insert_idx, base_idx + 1);
EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1);
EXPECT_EQ(insert_idx, boost::none);
}

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 base index
{
auto traj_out = traj;

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

EXPECT_EQ(insert_idx, boost::none);
}

// Empty
Expand Down Expand Up @@ -630,7 +630,8 @@ TEST(trajectory, insertTargetPoint_OverlapThreshold_PathWithLaneId)
const auto insert_idx =
insertTargetPoint(base_idx, p_target, traj_out.points, overlap_threshold);

EXPECT_EQ(insert_idx, base_idx + 1);
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) {
Expand All @@ -639,7 +640,7 @@ TEST(trajectory, insertTargetPoint_OverlapThreshold_PathWithLaneId)
}

{
const auto p_insert = getPose(traj_out.points.at(insert_idx));
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);
Expand Down Expand Up @@ -669,7 +670,8 @@ TEST(trajectory, insertTargetPoint_OverlapThreshold_PathWithLaneId)
const auto insert_idx =
insertTargetPoint(base_idx, p_target, traj_out.points, overlap_threshold);

EXPECT_EQ(insert_idx, base_idx);
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) {
Expand All @@ -687,7 +689,8 @@ TEST(trajectory, insertTargetPoint_OverlapThreshold_PathWithLaneId)
const auto insert_idx =
insertTargetPoint(base_idx, p_target, traj_out.points, overlap_threshold);

EXPECT_EQ(insert_idx, base_idx + 1);
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) {
Expand Down
61 changes: 32 additions & 29 deletions common/tier4_autoware_utils/test/src/trajectory/test_trajectory.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1493,15 +1493,16 @@ TEST(trajectory, insertTargetPoint)
const size_t base_idx = findNearestSegmentIndex(traj.points, p_target);
const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points);

EXPECT_EQ(insert_idx, base_idx + 1);
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));
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);
Expand Down Expand Up @@ -1530,15 +1531,16 @@ TEST(trajectory, insertTargetPoint)
const size_t base_idx = findNearestSegmentIndex(traj.points, p_target);
const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points);

EXPECT_EQ(insert_idx, base_idx + 1);
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));
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);
Expand Down Expand Up @@ -1567,15 +1569,16 @@ TEST(trajectory, insertTargetPoint)
const size_t base_idx = findNearestSegmentIndex(traj.points, p_target);
const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points);

EXPECT_EQ(insert_idx, base_idx + 1);
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));
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);
Expand Down Expand Up @@ -1604,7 +1607,8 @@ TEST(trajectory, insertTargetPoint)
const size_t base_idx = findNearestSegmentIndex(traj.points, p_target);
const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points);

EXPECT_EQ(insert_idx, base_idx);
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) {
Expand All @@ -1620,7 +1624,8 @@ TEST(trajectory, insertTargetPoint)
const size_t base_idx = findNearestSegmentIndex(traj.points, p_target);
const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points);

EXPECT_EQ(insert_idx, base_idx + 1);
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) {
Expand All @@ -1638,12 +1643,7 @@ TEST(trajectory, insertTargetPoint)
const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points);

EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n");
EXPECT_EQ(insert_idx, 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);
}
EXPECT_EQ(insert_idx, boost::none);
}

// Invalid target point(Behind of end point)
Expand All @@ -1656,12 +1656,7 @@ TEST(trajectory, insertTargetPoint)
const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points);

EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n");
EXPECT_EQ(insert_idx, 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);
}
EXPECT_EQ(insert_idx, boost::none);
}

// Invalid target point(Huge lateral offset)
Expand All @@ -1674,12 +1669,17 @@ TEST(trajectory, insertTargetPoint)
const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points);

EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n");
EXPECT_EQ(insert_idx, base_idx + 1);
EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1);
EXPECT_EQ(insert_idx, boost::none);
}

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 base index
{
auto traj_out = traj;

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

EXPECT_EQ(insert_idx, boost::none);
}

// Empty
Expand Down Expand Up @@ -1713,7 +1713,8 @@ TEST(trajectory, insertTargetPoint_OverlapThreshold)
const auto insert_idx =
insertTargetPoint(base_idx, p_target, traj_out.points, overlap_threshold);

EXPECT_EQ(insert_idx, base_idx + 1);
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) {
Expand All @@ -1722,7 +1723,7 @@ TEST(trajectory, insertTargetPoint_OverlapThreshold)
}

{
const auto p_insert = getPose(traj_out.points.at(insert_idx));
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);
Expand Down Expand Up @@ -1752,7 +1753,8 @@ TEST(trajectory, insertTargetPoint_OverlapThreshold)
const auto insert_idx =
insertTargetPoint(base_idx, p_target, traj_out.points, overlap_threshold);

EXPECT_EQ(insert_idx, base_idx);
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) {
Expand All @@ -1770,7 +1772,8 @@ TEST(trajectory, insertTargetPoint_OverlapThreshold)
const auto insert_idx =
insertTargetPoint(base_idx, p_target, traj_out.points, overlap_threshold);

EXPECT_EQ(insert_idx, base_idx + 1);
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) {
Expand Down

0 comments on commit 4b3ca87

Please sign in to comment.