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 <takayuki5168@gmail.com>

* fix lanelet's longitudinal disconnection

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* add comments of erode/dilate process

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

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