feat(avoidance): make it possible to use freespace areas in avoidance module (#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

planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp

@@ -173,11 +173,9 @@ AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData(
   const auto shorten_lanes =
     utils::cutOverlappedLanes(data.reference_path_rough, data.drivable_lanes);
   data.left_bound = toLineString3d(utils::calcBound(
-    planner_data_->route_handler, shorten_lanes, avoidance_parameters_->use_hatched_road_markings,
-    avoidance_parameters_->use_intersection_areas, true));
+    data.reference_path_rough, planner_data_, shorten_lanes, false, false, false, true));
   data.right_bound = toLineString3d(utils::calcBound(
-    planner_data_->route_handler, shorten_lanes, avoidance_parameters_->use_hatched_road_markings,
-    avoidance_parameters_->use_intersection_areas, false));
+    data.reference_path_rough, planner_data_, shorten_lanes, false, false, false, false));
 
   // get related objects from dynamic_objects, and then separates them as target objects and non
   // target objects

planning/behavior_path_avoidance_module/config/avoidance.param.yaml

@@ -19,6 +19,7 @@
       use_opposite_lane: true
       use_intersection_areas: true
       use_hatched_road_markings: true
+      use_freespace_areas: true
 
       # for debug
       publish_debug_marker: false

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp

@@ -115,6 +115,9 @@ struct AvoidanceParameters
   // use intersection area for avoidance
   bool use_intersection_areas{false};
 
+  // use freespace area for avoidance
+  bool use_freespace_areas{false};
+
   // consider avoidance return dead line
   bool enable_dead_line_for_goal{false};
   bool enable_dead_line_for_traffic_light{false};

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp

@@ -57,6 +57,7 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
     p.use_intersection_areas = getOrDeclareParameter<bool>(*node, ns + "use_intersection_areas");
     p.use_hatched_road_markings =
       getOrDeclareParameter<bool>(*node, ns + "use_hatched_road_markings");
+    p.use_freespace_areas = getOrDeclareParameter<bool>(*node, ns + "use_freespace_areas");
   }
 
   // target object

planning/behavior_path_avoidance_module/src/scene.cpp

@@ -235,11 +235,13 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat
   auto tmp_path = getPreviousModuleOutput().path;
   const auto shorten_lanes = utils::cutOverlappedLanes(tmp_path, data.drivable_lanes);
   data.left_bound = toLineString3d(utils::calcBound(
-    planner_data_->route_handler, shorten_lanes, parameters_->use_hatched_road_markings,
-    parameters_->use_intersection_areas, true));
+    getPreviousModuleOutput().path, planner_data_, shorten_lanes,
+    parameters_->use_hatched_road_markings, parameters_->use_intersection_areas,
+    parameters_->use_freespace_areas, true));
   data.right_bound = toLineString3d(utils::calcBound(
-    planner_data_->route_handler, shorten_lanes, parameters_->use_hatched_road_markings,
-    parameters_->use_intersection_areas, false));
+    getPreviousModuleOutput().path, planner_data_, shorten_lanes,
+    parameters_->use_hatched_road_markings, parameters_->use_intersection_areas,
+    parameters_->use_freespace_areas, false));
 
   // reference path
   if (isDrivingSameLane(helper_->getPreviousDrivingLanes(), data.current_lanelets)) {
@@ -938,6 +940,8 @@ BehaviorModuleOutput AvoidanceModule::plan()
     // expand intersection areas
     current_drivable_area_info.enable_expanding_intersection_areas =
       parameters_->use_intersection_areas;
+    // expand freespace areas
+    current_drivable_area_info.enable_expanding_freespace_areas = parameters_->use_freespace_areas;
 
     output.drivable_area_info = utils::combineDrivableAreaInfo(
       current_drivable_area_info, getPreviousModuleOutput().drivable_area_info);

planning/behavior_path_planner/src/planner_manager.cpp

@@ -226,8 +226,7 @@ void PlannerManager::generateCombinedDrivableArea(
   } else if (di.is_already_expanded) {
     // for single side shift
     utils::generateDrivableArea(
-      output.path, di.drivable_lanes, false, false, data->parameters.vehicle_length, data,
-      is_driving_forward);
+      output.path, di.drivable_lanes, false, false, false, data, is_driving_forward);
   } else {
     const auto shorten_lanes = utils::cutOverlappedLanes(output.path, di.drivable_lanes);
 
@@ -239,7 +238,7 @@ void PlannerManager::generateCombinedDrivableArea(
     // for other modules where multiple modules may be launched
     utils::generateDrivableArea(
       output.path, expanded_lanes, di.enable_expanding_hatched_road_markings,
-      di.enable_expanding_intersection_areas, data->parameters.vehicle_length, data,
+      di.enable_expanding_intersection_areas, di.enable_expanding_freespace_areas, data,
       is_driving_forward);
   }
 

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

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