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 <[email protected]>

* fix lanelet's longitudinal disconnection

Signed-off-by: Takayuki Murooka <[email protected]>

* add comments of erode/dilate process

Signed-off-by: Takayuki Murooka <[email protected]>

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);

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);