test(bpp_common): add tests for the static drivable area (#9324)

Signed-off-by: Maxime CLEMENT <[email protected]>

common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp

@@ -330,6 +330,19 @@ void updateNodeOptions(
  */
 PathWithLaneId loadPathWithLaneIdInYaml();
 
+/**
+ * @brief create a straight lanelet from 2 segments defined by 4 points
+ * @param [in] left0 start of the left segment
+ * @param [in] left1 end of the left segment
+ * @param [in] right0 start of the right segment
+ * @param [in] right1 end of the right segment
+ * @return a ConstLanelet with the given left and right bounds and a unique lanelet id
+ *
+ */
+lanelet::ConstLanelet make_lanelet(
+  const lanelet::BasicPoint2d & left0, const lanelet::BasicPoint2d & left1,
+  const lanelet::BasicPoint2d & right0, const lanelet::BasicPoint2d & right1);
+
 /**
  * @brief Generates a trajectory with specified parameters.
  *

common/autoware_test_utils/src/autoware_test_utils.cpp

@@ -313,6 +313,19 @@ PathWithLaneId loadPathWithLaneIdInYaml()
   return parse<PathWithLaneId>(yaml_path);
 }
 
+lanelet::ConstLanelet make_lanelet(
+  const lanelet::BasicPoint2d & left0, const lanelet::BasicPoint2d & left1,
+  const lanelet::BasicPoint2d & right0, const lanelet::BasicPoint2d & right1)
+{
+  lanelet::LineString3d left_bound;
+  left_bound.push_back(lanelet::Point3d(lanelet::InvalId, left0.x(), left0.y(), 0.0));
+  left_bound.push_back(lanelet::Point3d(lanelet::InvalId, left1.x(), left1.y(), 0.0));
+  lanelet::LineString3d right_bound;
+  right_bound.push_back(lanelet::Point3d(lanelet::InvalId, right0.x(), right0.y(), 0.0));
+  right_bound.push_back(lanelet::Point3d(lanelet::InvalId, right1.x(), right1.y(), 0.0));
+  return {lanelet::utils::getId(), left_bound, right_bound};
+}
+
 std::optional<std::string> resolve_pkg_share_uri(const std::string & uri_path)
 {
   std::smatch match;

planning/behavior_path_planner/autoware_behavior_path_planner_common/CMakeLists.txt

@@ -45,6 +45,7 @@ if(BUILD_TESTING)
   ament_add_ros_isolated_gmock(test_${PROJECT_NAME}_drivable_area_expansion
     test/test_drivable_area_expansion.cpp
     test/test_footprints.cpp
+    test/test_static_drivable_area.cpp
   )
 
   target_link_libraries(test_${PROJECT_NAME}_drivable_area_expansion

planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp

@@ -25,25 +25,67 @@ namespace autoware::behavior_path_planner::utils
 {
 using autoware::behavior_path_planner::drivable_area_expansion::DrivableAreaExpansionParameters;
 
+/**
+ * @brief finds the index of the first lane in the provided vector that overlaps with a preceding
+ * lane (excluding the immediate predecessor in the vector)
+ * @param [ink] lanes vector of DrivableLanes
+ * @return the index of the first overlapping lane (if any)
+ */
 std::optional<size_t> getOverlappedLaneletId(const std::vector<DrivableLanes> & lanes);
 
+/**
+ * @brief modify a path to only keep points inside the given lanes (before any lane overlap)
+ * @details the path point lanelet_ids are used to determine if they are inside a lanelet
+ * @param [inout] path path to be cut
+ * @param [in] lanes lanes used to cut the path
+ * @return the shortened lanes without overlaps
+ */
 std::vector<DrivableLanes> cutOverlappedLanes(
   PathWithLaneId & path, const std::vector<DrivableLanes> & lanes);
 
-std::vector<DrivableLanes> generateDrivableLanes(const lanelet::ConstLanelets & current_lanes);
+/**
+ * @brief generate DrivableLanes objects from a sequence of lanelets
+ * @param [in] lanelets sequence of lanelets
+ * @return a vector of DrivableLanes constructed from the given lanelets
+ */
+std::vector<DrivableLanes> generateDrivableLanes(const lanelet::ConstLanelets & lanelets);
 
 std::vector<DrivableLanes> generateDrivableLanesWithShoulderLanes(
   const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & shoulder_lanes);
 
 std::vector<DrivableLanes> getNonOverlappingExpandedLanes(
   PathWithLaneId & path, const std::vector<DrivableLanes> & lanes,
   const DrivableAreaExpansionParameters & parameters);
+
+/**
+ * @brief generate the drivable area of the given path
+ * @param [inout] path path whose center line is used to calculate the drivable area and whose
+ * left/right bound are generated
+ * @param [in] lanes lanes driven by the ego vehicle
+ * @param [in] enable_expanding_hatched_road_markings if true, expand the drivable area into hatched
+ * road markings
+ * @param [in] enable_expanding_intersection_areas if true, expand the drivable area into
+ * intersection areas
+ * @param [in] enable_expanding_freespace_areas if true, expand the drivable area into freespace
+ * areas
+ * @param [in] planner_data planner data with the route handler (and map), the parameters, the ego
+ * pose, etc
+ * @param [in] is_driving_forward true if the ego vehicle drives in the forward direction
+ */
 void generateDrivableArea(
   PathWithLaneId & path, const std::vector<DrivableLanes> & lanes,
   const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas,
   const bool enable_expanding_freespace_areas,
   const std::shared_ptr<const PlannerData> planner_data, const bool is_driving_forward = true);
 
+/**
+ * @brief generate the drivable area of the given path by applying the given offsets to its points
+ * @param [inout] path path whose center line is used to calculate the drivable area and whose
+ * left/right bound are generated
+ * @param [in] vehicle_length [m] length of the ego vehicle
+ * @param [in] offset [m] lateral offset between the path points and the drivable area
+ * @param [in] is_driving_forward true if the ego vehicle drives in the forward direction
+ */
 void generateDrivableArea(
   PathWithLaneId & path, const double vehicle_length, const double offset,
   const bool is_driving_forward = true);
@@ -72,6 +114,14 @@ std::vector<lanelet::ConstPoint3d> getBoundWithHatchedRoadMarkings(
   const std::vector<lanelet::ConstPoint3d> & original_bound,
   const std::shared_ptr<RouteHandler> & route_handler);
 
+/**
+ * @brief Expand the given bound to include intersection areas from the map
+ * @param [in] original_bound original bound to expand
+ * @param [in] drivable_lanes lanelets to consider
+ * @param [in] route_handler route handler with the map information
+ * @param [in] is_left whether the bound to calculate is on the left or not
+ * @return the bound including intersection areas
+ */
 std::vector<lanelet::ConstPoint3d> getBoundWithIntersectionAreas(
   const std::vector<lanelet::ConstPoint3d> & original_bound,
   const std::shared_ptr<RouteHandler> & route_handler,
@@ -90,6 +140,12 @@ std::vector<geometry_msgs::msg::Point> postProcess(
   const std::vector<DrivableLanes> & drivable_lanes, const bool is_left,
   const bool is_driving_forward = true);
 
+/**
+ * @brief combine two drivable area info objects
+ * @param [in] drivable_area_Info1 first drivable area info
+ * @param [in] drivable_area_Info2 second drivable area info
+ * @return the combined drivable area info
+ */
 DrivableAreaInfo combineDrivableAreaInfo(
   const DrivableAreaInfo & drivable_area_info1, const DrivableAreaInfo & drivable_area_info2);
 

planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp

@@ -626,8 +626,6 @@ std::vector<Point> updateBoundary(
 
 namespace autoware::behavior_path_planner::utils
 {
-using autoware::universe_utils::Point2d;
-
 std::optional<size_t> getOverlappedLaneletId(const std::vector<DrivableLanes> & lanes)
 {
   auto overlaps = [](const DrivableLanes & lanes, const DrivableLanes & target_lanes) {
@@ -745,12 +743,12 @@ std::vector<DrivableLanes> cutOverlappedLanes(
   return shorten_lanes;
 }
 
-std::vector<DrivableLanes> generateDrivableLanes(const lanelet::ConstLanelets & lanes)
+std::vector<DrivableLanes> generateDrivableLanes(const lanelet::ConstLanelets & lanelets)
 {
-  std::vector<DrivableLanes> drivable_lanes(lanes.size());
-  for (size_t i = 0; i < lanes.size(); ++i) {
-    drivable_lanes.at(i).left_lane = lanes.at(i);
-    drivable_lanes.at(i).right_lane = lanes.at(i);
+  std::vector<DrivableLanes> drivable_lanes(lanelets.size());
+  for (size_t i = 0; i < lanelets.size(); ++i) {
+    drivable_lanes.at(i).left_lane = lanelets.at(i);
+    drivable_lanes.at(i).right_lane = lanelets.at(i);
   }
   return drivable_lanes;
 }
@@ -856,6 +854,9 @@ void generateDrivableArea(
       }
     }
   }
+  if (resampled_path.points.empty()) {
+    return;
+  }
   // add last point of path if enough far from the one of resampled path
   constexpr double th_last_point_distance = 0.3;
   if (