From 058a1ccfbce41c569857bb4f10398aba53fe3346 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Mon, 23 Oct 2023 12:18:17 +0900 Subject: [PATCH] fix(utils): fix sharp drivable area Signed-off-by: satoshi-ota --- .../behavior_path_planner/src/utils/utils.cpp | 32 ++++++++++++------- 1 file changed, 20 insertions(+), 12 deletions(-) diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index ed2979bd1d5a1..0d3517579e633 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -1899,19 +1899,27 @@ void makeBoundLongitudinallyMonotonic( return bound; } - std::vector ret; - - for (size_t i = 0; i < bound.size(); i++) { - const auto & p_new = bound.at(i); - const auto duplicated_points_itr = std::find_if( - ret.begin(), ret.end(), - [&p_new](const auto & p) { return calcDistance2d(p, p_new) < 0.1; }); - - if (duplicated_points_itr != ret.end() && std::next(duplicated_points_itr, 2) == ret.end()) { - ret.erase(duplicated_points_itr, ret.end()); + std::vector ret = bound; + auto itr = std::next(ret.begin()); + while (std::next(itr) != ret.end()) { + const auto p1 = *std::prev(itr); + const auto p2 = *itr; + const auto p3 = *std::next(itr); + + const std::vector vec_1to2 = {p2.x - p1.x, p2.y - p1.y, p2.z - p1.z}; + const std::vector vec_3to2 = {p2.x - p3.x, p2.y - p3.y, p2.z - p3.z}; + const auto product = + std::inner_product(vec_1to2.begin(), vec_1to2.end(), vec_3to2.begin(), 0.0); + + const auto dist_1to2 = tier4_autoware_utils::calcDistance3d(p1, p2); + const auto dist_3to2 = tier4_autoware_utils::calcDistance3d(p3, p2); + + constexpr double epsilon = 1e-3; + if (std::cos(M_PI_4) < product / dist_1to2 / dist_3to2 + epsilon) { + itr = ret.erase(itr); + } else { + itr++; } - - ret.push_back(p_new); } return ret;