refactor(behavior_path_planner): separate drivable area functions

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_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 @@
   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 @@
 
 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 @@
   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 @@
 
 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"
@@ -224,7 +225,7 @@
 
   // If the vehicle is on the shift line, keep RUNNING.
   {
     const size_t idx = planner_data_->findEgoIndex(path_shifter_.getReferencePath().points);
     const auto within = [](const auto & line, const size_t idx) {
       return line.start_idx < idx && idx < line.end_idx;
     };
@@ -452,7 +453,7 @@
         getLogger(),
         "Distance to shift start point is less than minimum prepare distance. The distance is not "
         "enough.");
       return false;
     }
   }
 
@@ -938,7 +939,7 @@
 
   // Calculate feasible shift length
   const auto get_shift_profile =
     [&](
       auto & object, const auto & desire_shift_length) -> std::optional<std::pair<double, double>> {
     // use each object param
     const auto object_type = utils::getHighestProbLabel(object.object.classification);
@@ -952,13 +953,13 @@
     // calculate remaining distance.
     const auto prepare_distance = helper_.getNominalPrepareDistance();
     const auto & additional_buffer_longitudinal =
       object_parameter.use_conservative_buffer_longitudinal
         ? planner_data_->parameters.base_link2front
         : 0.0;
     const auto constant = object_parameter.safety_buffer_longitudinal +
                           additional_buffer_longitudinal + prepare_distance;
     const auto has_enough_distance = object.longitudinal > constant + nominal_avoid_distance;
     const auto remaining_distance = object.longitudinal - constant;
     const auto avoidance_distance =
       has_enough_distance ? nominal_avoid_distance : remaining_distance;
 
@@ -999,7 +1000,7 @@
     // prepare distance is not enough. unavoidable.
     if (remaining_distance < 1e-3) {
       object.reason = AvoidanceDebugFactor::REMAINING_DISTANCE_LESS_THAN_ZERO;
       return std::nullopt;
     }
 
     // calculate lateral jerk.
@@ -1014,7 +1015,7 @@
     // avoidance distance is not enough. unavoidable.
     if (!isBestEffort(parameters_->policy_deceleration)) {
       object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK;
       return std::nullopt;
     }
 
     // output avoidance path under lateral jerk constraints.
@@ -1023,7 +1024,7 @@
 
     if (std::abs(feasible_relative_shift_length) < parameters_->lateral_execution_threshold) {
       object.reason = "LessThanExecutionThreshold";
       return std::nullopt;
     }
 
     const auto feasible_shift_length =
@@ -1036,8 +1037,8 @@
     if (infeasible) {
       RCLCPP_WARN_THROTTLE(
         getLogger(), *clock_, 1000, "feasible shift length is not enough to avoid. ");
       object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK;
       return std::nullopt;
     }
 
     {
@@ -1117,7 +1118,7 @@
         }
 
         const auto minimum_avoid_distance =
           helper_.getMinAvoidanceDistance(feasible_shift_profile.value().first - current_ego_shift);
         const auto furthest_avoid_distance = std::max(to_shift_end - minimum_avoid_distance, 1e-3);
 
         return std::clamp(data.to_start_point, nearest_avoid_distance, furthest_avoid_distance);
@@ -1129,7 +1130,7 @@
       al_avoid.start_shift_length = helper_.getLinearShift(al_avoid.start.position);
 
       // end point
       al_avoid.end_shift_length = feasible_shift_profile.value().first;
       al_avoid.end_longitudinal = to_shift_end;
 
       // misc
@@ -1144,7 +1145,7 @@
       const auto to_shift_start = o.longitudinal + offset;
 
       // start point
       al_return.start_shift_length = feasible_shift_profile.value().first;
       al_return.start_longitudinal = to_shift_start;
 
       // end point
@@ -1672,7 +1673,7 @@
     }
 
     if (s.start_longitudinal < helper_.getMinimumPrepareDistance()) {
       continue;
     }
 
     shift_lines.push_back(s);

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"
@@ -1088,14 +1089,14 @@
   return true;
 }
 
 bool NormalLaneChange::hasEnoughLengthToTrafficLight(
   const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const
 {
   const auto current_pose = getEgoPose();
   const auto dist_to_next_traffic_light =
     getDistanceToNextTrafficLight(current_pose, current_lanes);
   const auto dist_to_next_traffic_light_from_lc_start_pose =
     dist_to_next_traffic_light - path.info.length.prepare;
 
   return dist_to_next_traffic_light_from_lc_start_pose <= 0.0 ||
          dist_to_next_traffic_light_from_lc_start_pose >= path.info.length.lane_changing;
@@ -1360,10 +1361,10 @@
             logger_, "Stop time is over threshold. Allow lane change in intersection.");
         }
 
         if (
           lane_change_parameters_->regulate_on_traffic_light &&
           !hasEnoughLengthToTrafficLight(*candidate_path, current_lanes)) {
           continue;
         }
 
         candidate_paths->push_back(*candidate_path);
@@ -1638,7 +1639,7 @@
 
     if (
       !reference_lanelets.empty() &&
       route_handler->isInGoalRouteSection(reference_lanelets.back())) {
       const auto goal_arc_coordinates =
         lanelet::utils::getArcCoordinates(reference_lanelets, route_handler->getGoalPose());
       const double forward_length = std::max(goal_arc_coordinates.length, s_start);

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"