feat(goal_planner): extend pull over lanes inward to extract objects (autowarefoundation#8714)

* feat(goal_planner): extend pull over lanes inward to extract objects

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* update from review

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* use optionale

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* rename lamda

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* return nullopt

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* Update planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp

Co-authored-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

* pre-commit

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

---------

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
Co-authored-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

Author: kosuke55

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp

@@ -275,6 +275,7 @@ struct GoalPlannerDebugData
   FreespacePlannerDebugData freespace_planner{};
   std::vector<Polygon2d> ego_polygons_expanded{};
   lanelet::ConstLanelet expanded_pull_over_lane_between_ego{};
+  Polygon2d objects_extraction_polygon{};
 };
 
 struct LastApprovalData

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp

@@ -31,13 +31,12 @@ class GoalSearcher : public GoalSearcherBase
 public:
   GoalSearcher(const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint);
 
-  GoalCandidates search(
-    const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
-    const std::shared_ptr<const PlannerData> & planner_data) override;
+  GoalCandidates search(const std::shared_ptr<const PlannerData> & planner_data) override;
   void update(
     GoalCandidates & goal_candidates,
     const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
-    const std::shared_ptr<const PlannerData> & planner_data) const override;
+    const std::shared_ptr<const PlannerData> & planner_data,
+    const PredictedObjects & objects) const override;
 
   // todo(kosuke55): Functions for this specific use should not be in the interface,
   // so it is better to consider interface design when we implement other goal searchers.
@@ -47,7 +46,8 @@ class GoalSearcher : public GoalSearcherBase
   bool isSafeGoalWithMarginScaleFactor(
     const GoalCandidate & goal_candidate, const double margin_scale_factor,
     const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
-    const std::shared_ptr<const PlannerData> & planner_data) const override;
+    const std::shared_ptr<const PlannerData> & planner_data,
+    const PredictedObjects & objects) const override;
 
 private:
   void countObjectsToAvoid(

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp

@@ -53,13 +53,12 @@ class GoalSearcherBase
   const Pose & getReferenceGoal() const { return reference_goal_pose_; }
 
   MultiPolygon2d getAreaPolygons() const { return area_polygons_; }
-  virtual GoalCandidates search(
-    const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
-    const std::shared_ptr<const PlannerData> & planner_data) = 0;
+  virtual GoalCandidates search(const std::shared_ptr<const PlannerData> & planner_data) = 0;
   virtual void update(
     [[maybe_unused]] GoalCandidates & goal_candidates,
     [[maybe_unused]] const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
-    [[maybe_unused]] const std::shared_ptr<const PlannerData> & planner_data) const
+    [[maybe_unused]] const std::shared_ptr<const PlannerData> & planner_data,
+    [[maybe_unused]] const PredictedObjects & objects) const
   {
     return;
   }
@@ -69,7 +68,8 @@ class GoalSearcherBase
   virtual bool isSafeGoalWithMarginScaleFactor(
     const GoalCandidate & goal_candidate, const double margin_scale_factor,
     const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
-    const std::shared_ptr<const PlannerData> & planner_data) const = 0;
+    const std::shared_ptr<const PlannerData> & planner_data,
+    const PredictedObjects & objects) const = 0;
 
 protected:
   GoalPlannerParameters parameters_{};

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp

@@ -64,6 +64,19 @@ lanelet::ConstLanelets generateBetweenEgoAndExpandedPullOverLanes(
   const geometry_msgs::msg::Pose ego_pose,
   const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double outer_road_offset,
   const double inner_road_offset);
+
+/*
+ * @brief generate polygon to extract objects
+ * @param pull_over_lanes pull over lanes
+ * @param left_side left side or right side
+ * @param outer_offset outer offset from pull over lane boundary
+ * @param inner_offset inner offset from pull over lane boundary
+ * @return polygon to extract objects
+ */
+std::optional<Polygon2d> generateObjectExtractionPolygon(
+  const lanelet::ConstLanelets & pull_over_lanes, const bool left_side, const double outer_offset,
+  const double inner_offset);
+
 PredictedObjects extractObjectsInExpandedPullOverLanes(
   const RouteHandler & route_handler, const bool left_side, const double backward_distance,
   const double forward_distance, double bound_offset, const PredictedObjects & objects);

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp

@@ -459,18 +459,33 @@ void GoalPlannerModule::updateData()
 {
   universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
 
-  // extract static and dynamic objects in expanded pull over lanes
+  // extract static and dynamic objects in extraction polygon for path coliision check
   {
     const auto & p = parameters_;
     const auto & rh = *(planner_data_->route_handler);
     const auto objects = *(planner_data_->dynamic_object);
-    const auto static_target_objects =
-      goal_planner_utils::extractStaticObjectsInExpandedPullOverLanes(
-        rh, left_side_parking_, p->backward_goal_search_length, p->forward_goal_search_length,
-        p->detection_bound_offset, objects, p->th_moving_object_velocity);
-    const auto dynamic_target_objects = goal_planner_utils::extractObjectsInExpandedPullOverLanes(
-      rh, left_side_parking_, p->backward_goal_search_length, p->forward_goal_search_length,
-      p->detection_bound_offset, objects);
+    const double vehicle_width = planner_data_->parameters.vehicle_width;
+    const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(
+      rh, left_side_parking_, p->backward_goal_search_length, p->forward_goal_search_length);
+    const auto objects_extraction_polygon = goal_planner_utils::generateObjectExtractionPolygon(
+      pull_over_lanes, left_side_parking_, p->detection_bound_offset,
+      p->margin_from_boundary + p->max_lateral_offset + vehicle_width);
+    if (objects_extraction_polygon.has_value()) {
+      debug_data_.objects_extraction_polygon = objects_extraction_polygon.value();
+    }
+    PredictedObjects dynamic_target_objects{};
+    for (const auto & object : objects.objects) {
+      const auto object_polygon = universe_utils::toPolygon2d(object);
+      if (
+        objects_extraction_polygon.has_value() &&
+        boost::geometry::intersects(object_polygon, objects_extraction_polygon.value())) {
+        dynamic_target_objects.objects.push_back(object);
+      }
+    }
+    const auto static_target_objects = utils::path_safety_checker::filterObjectsByVelocity(
+      dynamic_target_objects, p->th_moving_object_velocity);
+
+    // these objects are used for path collision check not for safety check
     thread_safe_data_.set_static_target_objects(static_target_objects);
     thread_safe_data_.set_dynamic_target_objects(dynamic_target_objects);
   }
@@ -739,7 +754,9 @@ bool GoalPlannerModule::planFreespacePath(
 {
   GoalCandidates goal_candidates{};
   goal_candidates = thread_safe_data_.get_goal_candidates();
-  goal_searcher->update(goal_candidates, occupancy_grid_map, planner_data);
+  goal_searcher->update(
+    goal_candidates, occupancy_grid_map, planner_data,
+    thread_safe_data_.get_static_target_objects());
   thread_safe_data_.set_goal_candidates(goal_candidates);
   debug_data_.freespace_planner.num_goal_candidates = goal_candidates.size();
   debug_data_.freespace_planner.is_planning = true;
@@ -824,7 +841,7 @@ GoalCandidates GoalPlannerModule::generateGoalCandidates() const
   // calculate goal candidates
   const auto & route_handler = planner_data_->route_handler;
   if (utils::isAllowedGoalModification(route_handler)) {
-    return goal_searcher_->search(occupancy_grid_map_, planner_data_);
+    return goal_searcher_->search(planner_data_);
   }
 
   // NOTE:
@@ -1282,9 +1299,9 @@ DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus(
     // check goal pose collision
     const auto modified_goal_opt = thread_safe_data_.get_modified_goal_pose();
     if (
-      modified_goal_opt &&
-      !goal_searcher->isSafeGoalWithMarginScaleFactor(
-        modified_goal_opt.value(), hysteresis_factor, occupancy_grid_map, planner_data)) {
+      modified_goal_opt && !goal_searcher->isSafeGoalWithMarginScaleFactor(
+                             modified_goal_opt.value(), hysteresis_factor, occupancy_grid_map,
+                             planner_data, thread_safe_data_.get_static_target_objects())) {
       RCLCPP_DEBUG(getLogger(), "[DecidingPathStatus]: DECIDING->NOT_DECIDED. goal is not safe");
       return {DecidingPathStatus::NOT_DECIDED, clock_->now()};
     }
@@ -1443,7 +1460,9 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput()
 
     // update goal candidates
     auto goal_candidates = thread_safe_data_.get_goal_candidates();
-    goal_searcher_->update(goal_candidates, occupancy_grid_map_, planner_data_);
+    goal_searcher_->update(
+      goal_candidates, occupancy_grid_map_, planner_data_,
+      thread_safe_data_.get_static_target_objects());
 
     // Select a path that is as safe as possible and has a high priority.
     const auto pull_over_path_candidates = thread_safe_data_.get_pull_over_path_candidates();
@@ -2518,6 +2537,24 @@ void GoalPlannerModule::setDebugData()
     // Visualize goal candidates
     const auto goal_candidates = thread_safe_data_.get_goal_candidates();
     add(goal_planner_utils::createGoalCandidatesMarkerArray(goal_candidates, color));
+
+    // Visualize objects extraction polygon
+    auto marker = autoware::universe_utils::createDefaultMarker(
+      "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "objects_extraction_polygon", 0, Marker::LINE_LIST,
+      autoware::universe_utils::createMarkerScale(0.1, 0.0, 0.0),
+      autoware::universe_utils::createMarkerColor(0.0, 1.0, 1.0, 0.999));
+    const double ego_z = planner_data_->self_odometry->pose.pose.position.z;
+    for (size_t i = 0; i < debug_data_.objects_extraction_polygon.outer().size(); ++i) {
+      const auto & current_point = debug_data_.objects_extraction_polygon.outer().at(i);
+      const auto & next_point = debug_data_.objects_extraction_polygon.outer().at(
+        (i + 1) % debug_data_.objects_extraction_polygon.outer().size());
+      marker.points.push_back(
+        autoware::universe_utils::createPoint(current_point.x(), current_point.y(), ego_z));
+      marker.points.push_back(
+        autoware::universe_utils::createPoint(next_point.x(), next_point.y(), ego_z));
+    }
+
+    debug_marker_.markers.push_back(marker);
   }
 
   // Visualize previous module output

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp

@@ -97,9 +97,7 @@ GoalSearcher::GoalSearcher(
 {
 }
 
-GoalCandidates GoalSearcher::search(
-  const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
-  const std::shared_ptr<const PlannerData> & planner_data)
+GoalCandidates GoalSearcher::search(const std::shared_ptr<const PlannerData> & planner_data)
 {
   GoalCandidates goal_candidates{};
 
@@ -193,8 +191,6 @@ GoalCandidates GoalSearcher::search(
   }
   createAreaPolygons(original_search_poses, planner_data);
 
-  update(goal_candidates, occupancy_grid_map, planner_data);
-
   return goal_candidates;
 }
 
@@ -268,16 +264,10 @@ void GoalSearcher::countObjectsToAvoid(
 void GoalSearcher::update(
   GoalCandidates & goal_candidates,
   const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
-  const std::shared_ptr<const PlannerData> & planner_data) const
+  const std::shared_ptr<const PlannerData> & planner_data, const PredictedObjects & objects) const
 {
-  const auto pull_over_lane_stop_objects =
-    goal_planner_utils::extractStaticObjectsInExpandedPullOverLanes(
-      *(planner_data->route_handler), left_side_parking_, parameters_.backward_goal_search_length,
-      parameters_.forward_goal_search_length, parameters_.detection_bound_offset,
-      *(planner_data->dynamic_object), parameters_.th_moving_object_velocity);
-
   if (parameters_.prioritize_goals_before_objects) {
-    countObjectsToAvoid(goal_candidates, pull_over_lane_stop_objects, planner_data);
+    countObjectsToAvoid(goal_candidates, objects, planner_data);
   }
 
   if (parameters_.goal_priority == "minimum_weighted_distance") {
@@ -297,15 +287,15 @@ void GoalSearcher::update(
     const Pose goal_pose = goal_candidate.goal_pose;
 
     // check collision with footprint
-    if (checkCollision(goal_pose, pull_over_lane_stop_objects, occupancy_grid_map)) {
+    if (checkCollision(goal_pose, objects, occupancy_grid_map)) {
       goal_candidate.is_safe = false;
       continue;
     }
 
     // check longitudinal margin with pull over lane objects
     constexpr bool filter_inside = true;
     const auto target_objects = goal_planner_utils::filterObjectsByLateralDistance(
-      goal_pose, planner_data->parameters.vehicle_width, pull_over_lane_stop_objects,
+      goal_pose, planner_data->parameters.vehicle_width, objects,
       parameters_.object_recognition_collision_check_hard_margins.back(), filter_inside);
     if (checkCollisionWithLongitudinalDistance(
           goal_pose, target_objects, occupancy_grid_map, planner_data)) {
@@ -323,33 +313,25 @@ void GoalSearcher::update(
 bool GoalSearcher::isSafeGoalWithMarginScaleFactor(
   const GoalCandidate & goal_candidate, const double margin_scale_factor,
   const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
-  const std::shared_ptr<const PlannerData> & planner_data) const
+  const std::shared_ptr<const PlannerData> & planner_data, const PredictedObjects & objects) const
 {
   if (!parameters_.use_object_recognition) {
     return true;
   }
 
   const Pose goal_pose = goal_candidate.goal_pose;
-
-  const auto pull_over_lane_stop_objects =
-    goal_planner_utils::extractStaticObjectsInExpandedPullOverLanes(
-      *(planner_data->route_handler), left_side_parking_, parameters_.backward_goal_search_length,
-      parameters_.forward_goal_search_length, parameters_.detection_bound_offset,
-      *(planner_data->dynamic_object), parameters_.th_moving_object_velocity);
-
   const double margin =
     parameters_.object_recognition_collision_check_hard_margins.back() * margin_scale_factor;
 
   if (utils::checkCollisionBetweenFootprintAndObjects(
-        vehicle_footprint_, goal_pose, pull_over_lane_stop_objects, margin)) {
+        vehicle_footprint_, goal_pose, objects, margin)) {
     return false;
   }
 
   // check longitudinal margin with pull over lane objects
   constexpr bool filter_inside = true;
   const auto target_objects = goal_planner_utils::filterObjectsByLateralDistance(
-    goal_pose, planner_data->parameters.vehicle_width, pull_over_lane_stop_objects, margin,
-    filter_inside);
+    goal_pose, planner_data->parameters.vehicle_width, objects, margin, filter_inside);
   if (checkCollisionWithLongitudinalDistance(
         goal_pose, target_objects, occupancy_grid_map, planner_data)) {
     return false;