feat(avoidance): make it possible to use freespace areas in avoidance module (autowarefoundation#6001)

* fix(static_drivable_area_expansion): check right/left bound id

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* feat(static_drivable_area): use freespace area

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* feat(avoidance): use freespace

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(AbLC): fix flag

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(planner_manager): fix flag

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* refactor(static_drivable_area_expansion): remove unused arg

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* refactor(static_drivable_area_expansion): use lambda

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(static_drivable_area_expansion): fix invalid access

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* refactor(static_drivable_area_expansion): improve readability

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(avoidance): add param

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

---------

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

Author: satoshi-ota

Diff for: planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp

@@ -106,6 +106,7 @@ struct DrivableAreaInfo
   std::vector<Obstacle> obstacles{};  // obstacles to extract from the drivable area
   bool enable_expanding_hatched_road_markings{false};
   bool enable_expanding_intersection_areas{false};
+  bool enable_expanding_freespace_areas{false};
 
   // temporary only for pull over's freespace planning
   double drivable_margin{0.0};

Diff for: planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp

@@ -19,6 +19,7 @@
 
 #include <memory>
 #include <string>
+#include <utility>
 #include <vector>
 namespace behavior_path_planner::utils
 {
@@ -40,8 +41,8 @@ std::vector<DrivableLanes> getNonOverlappingExpandedLanes(
 void generateDrivableArea(
   PathWithLaneId & path, const std::vector<DrivableLanes> & lanes,
   const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas,
-  const double vehicle_length, const std::shared_ptr<const PlannerData> planner_data,
-  const bool is_driving_forward = true);
+  const bool enable_expanding_freespace_areas,
+  const std::shared_ptr<const PlannerData> planner_data, const bool is_driving_forward = true);
 
 void generateDrivableArea(
   PathWithLaneId & path, const double vehicle_length, const double offset,
@@ -62,6 +63,11 @@ std::vector<DrivableLanes> expandLanelets(
 void extractObstaclesFromDrivableArea(
   PathWithLaneId & path, const std::vector<DrivableAreaInfo::Obstacle> & obstacles);
 
+std::pair<std::vector<lanelet::ConstPoint3d>, bool> getBoundWithFreeSpaceAreas(
+  const std::vector<lanelet::ConstPoint3d> & original_bound,
+  const std::vector<lanelet::ConstPoint3d> & other_side_bound,
+  const std::shared_ptr<const PlannerData> planner_data, const bool is_left);
+
 std::vector<lanelet::ConstPoint3d> getBoundWithHatchedRoadMarkings(
   const std::vector<lanelet::ConstPoint3d> & original_bound,
   const std::shared_ptr<RouteHandler> & route_handler);
@@ -72,14 +78,22 @@ std::vector<lanelet::ConstPoint3d> getBoundWithIntersectionAreas(
   const std::vector<DrivableLanes> & drivable_lanes, const bool is_left);
 
 std::vector<geometry_msgs::msg::Point> calcBound(
-  const std::shared_ptr<RouteHandler> route_handler,
+  const PathWithLaneId & path, const std::shared_ptr<const PlannerData> planner_data,
+  const std::vector<DrivableLanes> & drivable_lanes,
+  const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas,
+  const bool enable_expanding_freespace_areas, const bool is_left,
+  const bool is_driving_forward = true);
+
+std::vector<geometry_msgs::msg::Point> postProcess(
+  const std::vector<geometry_msgs::msg::Point> & original_bound, const PathWithLaneId & path,
+  const std::shared_ptr<const PlannerData> planner_data,
   const std::vector<DrivableLanes> & drivable_lanes,
   const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas,
-  const bool is_left);
+  const bool is_left, const bool is_driving_forward = true);
 
-void makeBoundLongitudinallyMonotonic(
-  PathWithLaneId & path, const std::shared_ptr<const PlannerData> & planner_data,
-  const bool is_bound_left);
+std::vector<geometry_msgs::msg::Point> makeBoundLongitudinallyMonotonic(
+  const std::vector<geometry_msgs::msg::Point> & original_bound, const PathWithLaneId & path,
+  const std::shared_ptr<const PlannerData> & planner_data, const bool is_left);
 
 DrivableAreaInfo combineDrivableAreaInfo(
   const DrivableAreaInfo & drivable_area_info1, const DrivableAreaInfo & drivable_area_info2);