From f1d2b9ec7aab0b27d4e2b66e1cc6cf1d97b9a2d6 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 17 May 2022 22:54:46 +0900 Subject: [PATCH] feat(behavior_path_planner): weakened noise filtering of drivable area (#838) * feat(behavior_path_planner): Weakened noise filtering of drivable area Signed-off-by: Takayuki Murooka * fix lanelet's longitudinal disconnection Signed-off-by: Takayuki Murooka * add comments of erode/dilate process Signed-off-by: Takayuki Murooka --- map/lanelet2_extension/lib/utilities.cpp | 34 +++++++++++++++++-- .../behavior_path_planner/src/utilities.cpp | 5 ++- 2 files changed, 36 insertions(+), 3 deletions(-) diff --git a/map/lanelet2_extension/lib/utilities.cpp b/map/lanelet2_extension/lib/utilities.cpp index 072f079028db6..8983d9d4f3678 100644 --- a/map/lanelet2_extension/lib/utilities.cpp +++ b/map/lanelet2_extension/lib/utilities.cpp @@ -315,8 +315,38 @@ lanelet::ConstLanelet getExpandedLanelet( // Note: The lanelet::geometry::offset throws exception when the undesired inversion is found. // Use offsetNoThrow until the logic is updated to handle the inversion. // TODO(Horibe) update - const auto & expanded_left_bound_2d = offsetNoThrow(orig_left_bound_2d, left_offset); - const auto & expanded_right_bound_2d = offsetNoThrow(orig_right_bound_2d, right_offset); + auto expanded_left_bound_2d = offsetNoThrow(orig_left_bound_2d, left_offset); + auto expanded_right_bound_2d = offsetNoThrow(orig_right_bound_2d, right_offset); + + // Note: modify front and back points so that the successive lanelets will not have any + // longitudinal space between them. + { // front + const double diff_x = orig_right_bound_2d.front().x() - orig_left_bound_2d.front().x(); + const double diff_y = orig_right_bound_2d.front().y() - orig_left_bound_2d.front().y(); + const double theta = std::atan2(diff_y, diff_x); + expanded_right_bound_2d.front().x() = + orig_right_bound_2d.front().x() - right_offset * std::cos(theta); + expanded_right_bound_2d.front().y() = + orig_right_bound_2d.front().y() - right_offset * std::sin(theta); + expanded_left_bound_2d.front().x() = + orig_left_bound_2d.front().x() - left_offset * std::cos(theta); + expanded_left_bound_2d.front().y() = + orig_left_bound_2d.front().y() - left_offset * std::sin(theta); + } + { // back + const double diff_x = orig_right_bound_2d.back().x() - orig_left_bound_2d.back().x(); + const double diff_y = orig_right_bound_2d.back().y() - orig_left_bound_2d.back().y(); + const double theta = std::atan2(diff_y, diff_x); + expanded_right_bound_2d.back().x() = + orig_right_bound_2d.back().x() - right_offset * std::cos(theta); + expanded_right_bound_2d.back().y() = + orig_right_bound_2d.back().y() - right_offset * std::sin(theta); + expanded_left_bound_2d.back().x() = + orig_left_bound_2d.back().x() - left_offset * std::cos(theta); + expanded_left_bound_2d.back().y() = + orig_left_bound_2d.back().y() - left_offset * std::sin(theta); + } + rclcpp::Clock clock{RCL_ROS_TIME}; try { checkForInversion(orig_left_bound_2d, expanded_left_bound_2d, left_offset); diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index a2c3545d65c7c..ca29304a65b22 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -1227,7 +1227,10 @@ OccupancyGrid generateDrivableArea( } // Closing - constexpr int num_iter = 10; // TODO(Horibe) Think later. + // NOTE: Because of the discretization error, there may be some discontinuity between two + // successive lanelets in the drivable area. This issue is dealt with by the erode/dilate + // process. + constexpr int num_iter = 1; cv::Mat cv_erode, cv_dilate; cv::erode(cv_image, cv_erode, cv::Mat(), cv::Point(-1, -1), num_iter); cv::dilate(cv_erode, cv_dilate, cv::Mat(), cv::Point(-1, -1), num_iter);