Skip to content

Commit

Permalink
fix(utils): remove sharp angle bound (#5384)
Browse files Browse the repository at this point in the history
* fix(utils): remove sharp angle bound

Signed-off-by: satoshi-ota <[email protected]>

* fix(utils): guard invalid access

Signed-off-by: satoshi-ota <[email protected]>

* chore(utils): add comment

Signed-off-by: satoshi-ota <[email protected]>

---------

Signed-off-by: satoshi-ota <[email protected]>
  • Loading branch information
satoshi-ota authored Oct 24, 2023
1 parent db27686 commit f400b91
Showing 1 changed file with 17 additions and 3 deletions.
20 changes: 17 additions & 3 deletions planning/behavior_path_planner/src/utils/utils.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -2011,6 +2011,11 @@ void makeBoundLongitudinallyMonotonic(
std::vector<Point> ret = bound;
auto itr = std::next(ret.begin());
while (std::next(itr) != ret.end()) {
if (itr == ret.begin()) {
itr++;
continue;
}

const auto p1 = *std::prev(itr);
const auto p2 = *itr;
const auto p3 = *std::next(itr);
Expand All @@ -2024,11 +2029,20 @@ void makeBoundLongitudinallyMonotonic(
const auto dist_3to2 = tier4_autoware_utils::calcDistance3d(p3, p2);

constexpr double epsilon = 1e-3;

// Remove overlapped point.
if (dist_1to2 < epsilon || dist_3to2 < epsilon) {
itr = std::prev(ret.erase(itr));
continue;
}

// If the angle between the points is sharper than 45 degrees, remove the middle point.
if (std::cos(M_PI_4) < product / dist_1to2 / dist_3to2 + epsilon) {
itr = ret.erase(itr);
} else {
itr++;
itr = std::prev(ret.erase(itr));
continue;
}

itr++;
}

return ret;
Expand Down

0 comments on commit f400b91

Please sign in to comment.