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>
tier4 · Jan 29, 2024 · 1279359 · 1279359
1 parent 4e0b4ac
commit 1279359
Show file tree

Hide file tree

Showing 3 changed files with 400 additions and 202 deletions.
diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp b/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 --git a/...clude/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp b/...clude/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);