refactor(behavior_path_planner): separate drivable area functions

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>

planning/behavior_path_planner/CMakeLists.txt

@@ -48,6 +48,7 @@ ament_auto_add_library(behavior_path_planner_node SHARED
   src/utils/start_planner/geometric_pull_out.cpp
   src/utils/start_planner/freespace_pull_out.cpp
   src/utils/path_shifter/path_shifter.cpp
+  src/utils/drivable_area_expansion/static_drivable_area.cpp
   src/utils/drivable_area_expansion/drivable_area_expansion.cpp
   src/utils/drivable_area_expansion/map_utils.cpp
   src/utils/drivable_area_expansion/footprints.cpp

planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/static_drivable_area.hpp

@@ -0,0 +1,99 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__STATIC_DRIVABLE_AREA_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__STATIC_DRIVABLE_AREA_HPP_
+
+#include <behavior_path_planner/utils/utils.hpp>
+
+#include <memory>
+#include <string>
+#include <vector>
+namespace behavior_path_planner::utils
+{
+using drivable_area_expansion::DrivableAreaExpansionParameters;
+
+boost::optional<size_t> getOverlappedLaneletId(const std::vector<DrivableLanes> & lanes);
+
+std::vector<DrivableLanes> cutOverlappedLanes(
+  PathWithLaneId & path, const std::vector<DrivableLanes> & lanes);
+
+std::vector<DrivableLanes> generateDrivableLanes(const lanelet::ConstLanelets & current_lanes);
+
+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);
+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);
+
+void generateDrivableArea(
+  PathWithLaneId & path, const double vehicle_length, const double offset,
+  const bool is_driving_forward = true);
+
+lanelet::ConstLineStrings3d getMaximumDrivableArea(
+  const std::shared_ptr<const PlannerData> & planner_data);
+
+/**
+ * @brief Expand the borders of the given lanelets
+ * @param [in] drivable_lanes lanelets to expand
+ * @param [in] left_bound_offset [m] expansion distance of the left bound
+ * @param [in] right_bound_offset [m] expansion distance of the right bound
+ * @param [in] types_to_skip linestring types that will not be expanded
+ * @return expanded lanelets
+ */
+std::vector<DrivableLanes> expandLanelets(
+  const std::vector<DrivableLanes> & drivable_lanes, const double left_bound_offset,
+  const double right_bound_offset, const std::vector<std::string> & types_to_skip = {});
+
+void extractObstaclesFromDrivableArea(
+  PathWithLaneId & path, const std::vector<DrivableAreaInfo::Obstacle> & obstacles);
+
+std::vector<lanelet::ConstPoint3d> getBoundWithHatchedRoadMarkings(
+  const std::vector<lanelet::ConstPoint3d> & original_bound,
+  const std::shared_ptr<RouteHandler> & route_handler);
+
+std::vector<lanelet::ConstPoint3d> getBoundWithIntersectionAreas(
+  const std::vector<lanelet::ConstPoint3d> & original_bound,
+  const std::shared_ptr<RouteHandler> & route_handler,
+  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 std::vector<DrivableLanes> & drivable_lanes,
+  const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas,
+  const bool is_left);
+
+void makeBoundLongitudinallyMonotonic(
+  PathWithLaneId & path, const std::shared_ptr<const PlannerData> & planner_data,
+  const bool is_bound_left);
+
+DrivableAreaInfo combineDrivableAreaInfo(
+  const DrivableAreaInfo & drivable_area_info1, const DrivableAreaInfo & drivable_area_info2);
+
+lanelet::ConstLanelets combineLanelets(
+  const lanelet::ConstLanelets & base_lanes, const lanelet::ConstLanelets & added_lanes);
+
+std::vector<DrivableLanes> combineDrivableLanes(
+  const std::vector<DrivableLanes> & original_drivable_lanes_vec,
+  const std::vector<DrivableLanes> & new_drivable_lanes_vec);
+
+}  // namespace behavior_path_planner::utils
+
+#endif  // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__STATIC_DRIVABLE_AREA_HPP_

planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp

@@ -15,6 +15,7 @@
 #ifndef BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__UTIL_HPP_
 #define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__UTIL_HPP_
 
+#include "behavior_path_planner/utils/drivable_area_expansion/static_drivable_area.hpp"
 #include "behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp"
 #include "behavior_path_planner/utils/utils.hpp"
 

planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp

@@ -106,6 +106,13 @@ PathWithLaneId calcCenterLinePath(
 PathWithLaneId combinePath(const PathWithLaneId & path1, const PathWithLaneId & path2);
 
 boost::optional<Pose> getFirstStopPoseFromPath(const PathWithLaneId & path);
+
+BehaviorModuleOutput getReferencePath(
+  const lanelet::ConstLanelet & current_lane,
+  const std::shared_ptr<const PlannerData> & planner_data);
+
+BehaviorModuleOutput createGoalAroundPath(const std::shared_ptr<const PlannerData> & planner_data);
+
 }  // namespace behavior_path_planner::utils
 
 #endif  // BEHAVIOR_PATH_PLANNER__UTILS__PATH_UTILS_HPP_

planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp

@@ -16,6 +16,7 @@
 #define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__UTIL_HPP_
 
 #include "behavior_path_planner/data_manager.hpp"
+#include "behavior_path_planner/utils/drivable_area_expansion/static_drivable_area.hpp"
 #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp"
 #include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp"
 

planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp

@@ -59,7 +59,6 @@ using autoware_auto_perception_msgs::msg::Shape;
 using autoware_auto_planning_msgs::msg::Path;
 using autoware_auto_planning_msgs::msg::PathPointWithLaneId;
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
-using drivable_area_expansion::DrivableAreaExpansionParameters;
 using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::PoseArray;
@@ -209,56 +208,6 @@ boost::optional<lanelet::ConstLanelet> getRightLanelet(
   const lanelet::ConstLanelet & current_lane, const lanelet::ConstLanelets & shoulder_lanes);
 boost::optional<lanelet::ConstLanelet> getLeftLanelet(
   const lanelet::ConstLanelet & current_lane, const lanelet::ConstLanelets & shoulder_lanes);
-std::vector<DrivableLanes> generateDrivableLanes(const lanelet::ConstLanelets & current_lanes);
-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);
-std::vector<geometry_msgs::msg::Point> calcBound(
-  const std::shared_ptr<RouteHandler> route_handler,
-  const std::vector<DrivableLanes> & drivable_lanes,
-  const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas,
-  const bool is_left);
-
-std::vector<lanelet::ConstPoint3d> getBoundWithHatchedRoadMarkings(
-  const std::vector<lanelet::ConstPoint3d> & original_bound,
-  const std::shared_ptr<RouteHandler> & route_handler);
-
-std::vector<lanelet::ConstPoint3d> getBoundWithIntersectionAreas(
-  const std::vector<lanelet::ConstPoint3d> & original_bound,
-  const std::shared_ptr<RouteHandler> & route_handler,
-  const std::vector<DrivableLanes> & drivable_lanes, const bool is_left);
-
-boost::optional<size_t> getOverlappedLaneletId(const std::vector<DrivableLanes> & lanes);
-std::vector<DrivableLanes> cutOverlappedLanes(
-  PathWithLaneId & path, const std::vector<DrivableLanes> & lanes);
-
-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);
-
-void generateDrivableArea(
-  PathWithLaneId & path, const double vehicle_length, const double offset,
-  const bool is_driving_forward = true);
-
-lanelet::ConstLineStrings3d getMaximumDrivableArea(
-  const std::shared_ptr<const PlannerData> & planner_data);
-
-/**
- * @brief Expand the borders of the given lanelets
- * @param [in] drivable_lanes lanelets to expand
- * @param [in] left_bound_offset [m] expansion distance of the left bound
- * @param [in] right_bound_offset [m] expansion distance of the right bound
- * @param [in] types_to_skip linestring types that will not be expanded
- * @return expanded lanelets
- */
-std::vector<DrivableLanes> expandLanelets(
-  const std::vector<DrivableLanes> & drivable_lanes, const double left_bound_offset,
-  const double right_bound_offset, const std::vector<std::string> & types_to_skip = {});
-
 // goal management
 
 /**
@@ -294,8 +243,6 @@ PathWithLaneId refinePathForGoal(
 
 bool containsGoal(const lanelet::ConstLanelets & lanes, const lanelet::Id & goal_id);
 
-BehaviorModuleOutput createGoalAroundPath(const std::shared_ptr<const PlannerData> & planner_data);
-
 bool isInLanelets(const Pose & pose, const lanelet::ConstLanelets & lanes);
 
 bool isInLaneletWithYawThreshold(
@@ -349,10 +296,6 @@ PathWithLaneId setDecelerationVelocity(
   const lanelet::ConstLanelets & lanelet_sequence, const double lane_change_prepare_duration,
   const double lane_change_buffer);
 
-BehaviorModuleOutput getReferencePath(
-  const lanelet::ConstLanelet & current_lane,
-  const std::shared_ptr<const PlannerData> & planner_data);
-
 // object label
 std::uint8_t getHighestProbLabel(const std::vector<ObjectClassification> & classification);
 
@@ -399,26 +342,23 @@ lanelet::ConstLanelets getLaneletsFromPath(
 
 std::string convertToSnakeCase(const std::string & input_str);
 
-std::vector<DrivableLanes> combineDrivableLanes(
-  const std::vector<DrivableLanes> & original_drivable_lanes_vec,
-  const std::vector<DrivableLanes> & new_drivable_lanes_vec);
-
-DrivableAreaInfo combineDrivableAreaInfo(
-  const DrivableAreaInfo & drivable_area_info1, const DrivableAreaInfo & drivable_area_info2);
-
-void extractObstaclesFromDrivableArea(
-  PathWithLaneId & path, const std::vector<DrivableAreaInfo::Obstacle> & obstacles);
-
-void makeBoundLongitudinallyMonotonic(
-  PathWithLaneId & path, const std::shared_ptr<const PlannerData> & planner_data,
-  const bool is_bound_left);
-
 std::optional<lanelet::Polygon3d> getPolygonByPoint(
   const std::shared_ptr<RouteHandler> & route_handler, const lanelet::ConstPoint3d & point,
   const std::string & polygon_name);
 
-lanelet::ConstLanelets combineLanelets(
-  const lanelet::ConstLanelets & base_lanes, const lanelet::ConstLanelets & added_lanes);
+template <class T>
+size_t findNearestSegmentIndex(
+  const std::vector<T> & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold,
+  const double yaw_threshold)
+{
+  const auto nearest_idx =
+    motion_utils::findNearestSegmentIndex(points, pose, dist_threshold, yaw_threshold);
+  if (nearest_idx) {
+    return nearest_idx.get();
+  }
+
+  return motion_utils::findNearestSegmentIndex(points, pose.position);
+}
 }  // namespace behavior_path_planner::utils
 
 #endif  // BEHAVIOR_PATH_PLANNER__UTILS__UTILS_HPP_

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -22,6 +22,7 @@
 #include "behavior_path_planner/scene_module/lane_change/manager.hpp"
 #include "behavior_path_planner/scene_module/side_shift/manager.hpp"
 #include "behavior_path_planner/scene_module/start_planner/manager.hpp"
+#include "behavior_path_planner/utils/drivable_area_expansion/static_drivable_area.hpp"
 #include "behavior_path_planner/utils/path_utils.hpp"
 
 #include <tier4_autoware_utils/ros/update_param.hpp>

planning/behavior_path_planner/src/planner_manager.cpp

@@ -14,6 +14,8 @@
 
 #include "behavior_path_planner/planner_manager.hpp"
 
+#include "behavior_path_planner/utils/drivable_area_expansion/static_drivable_area.hpp"
+#include "behavior_path_planner/utils/path_utils.hpp"
 #include "behavior_path_planner/utils/utils.hpp"
 #include "tier4_autoware_utils/ros/debug_publisher.hpp"
 #include "tier4_autoware_utils/system/stop_watch.hpp"

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

@@ -18,6 +18,7 @@
 #include "behavior_path_planner/scene_module/scene_module_visitor.hpp"
 #include "behavior_path_planner/utils/avoidance/utils.hpp"
 #include "behavior_path_planner/utils/create_vehicle_footprint.hpp"
+#include "behavior_path_planner/utils/drivable_area_expansion/static_drivable_area.hpp"
 #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp"
 #include "behavior_path_planner/utils/path_utils.hpp"
 #include "behavior_path_planner/utils/utils.hpp"

planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp

@@ -14,6 +14,7 @@
 
 #include "behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp"
 
+#include "behavior_path_planner/utils/drivable_area_expansion/static_drivable_area.hpp"
 #include "behavior_path_planner/utils/utils.hpp"
 #include "object_recognition_utils/predicted_path_utils.hpp"
 #include "signal_processing/lowpass_filter_1d.hpp"

planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp

@@ -15,6 +15,7 @@
 #include "behavior_path_planner/scene_module/lane_change/normal.hpp"
 
 #include "behavior_path_planner/marker_utils/utils.hpp"
+#include "behavior_path_planner/utils/drivable_area_expansion/static_drivable_area.hpp"
 #include "behavior_path_planner/utils/lane_change/utils.hpp"
 #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp"
 #include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp"

planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp

@@ -15,6 +15,7 @@
 #include "behavior_path_planner/scene_module/side_shift/side_shift_module.hpp"
 
 #include "behavior_path_planner/marker_utils/utils.hpp"
+#include "behavior_path_planner/utils/drivable_area_expansion/static_drivable_area.hpp"
 #include "behavior_path_planner/utils/path_utils.hpp"
 #include "behavior_path_planner/utils/side_shift/util.hpp"
 #include "behavior_path_planner/utils/utils.hpp"