check goal longitudinal margin when deciding

Signed-off-by: kosuke55 <[email protected]>

planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher.hpp

@@ -35,8 +35,13 @@ class GoalSearcher : public GoalSearcherBase
 
   GoalCandidates search() override;
   void update(GoalCandidates & goal_candidates) 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.
   GoalCandidate getClosetGoalCandidateAlongLanes(
     const GoalCandidates & goal_candidates) const override;
+  bool isSafeGoalWithMarginScaleFactor(
+    const GoalCandidate & goal_candidate, const double margin_scale_factor) const override;
 
 private:
   void countObjectsToAvoid(

planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher_base.hpp

@@ -60,6 +60,8 @@ class GoalSearcherBase
   virtual void update([[maybe_unused]] GoalCandidates & goal_candidates) const { return; }
   virtual GoalCandidate getClosetGoalCandidateAlongLanes(
     const GoalCandidates & goal_candidates) const = 0;
+  virtual bool isSafeGoalWithMarginScaleFactor(
+    const GoalCandidate & goal_candidate, const double margin_scale_factor) const = 0;
 
 protected:
   GoalPlannerParameters parameters_{};

planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp

@@ -936,21 +936,35 @@ DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus() const
     return {DecidingPathStatus::NOT_DECIDED, clock_->now()};
   }
 
+  // if object recognition for path collision check is enabled, transition to DECIDED after
+  // DECIDING for a certain period of time if there is no collision.
   const auto pull_over_path = thread_safe_data_.get_pull_over_path();
   const auto current_path = pull_over_path->getCurrentPath();
   if (prev_status.first == DecidingPathStatus::DECIDING) {
-    // if object recognition for path collision check is enabled, transition to DECIDED after
-    // DECIDING for a certain period of time if there is no collision.
+    const double hysteresis_factor = 0.9;
+
+    // check goal pose collision
+    goal_searcher_->setPlannerData(planner_data_);
+    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)) {
+      RCLCPP_DEBUG(getLogger(), "[DecidingPathStatus]: DECIDING->NOT_DECIDED. goal is not safe");
+      return {DecidingPathStatus::NOT_DECIDED, clock_->now()};
+    }
+
+    // check current parking path collision
     const auto parking_path = utils::resamplePathWithSpline(pull_over_path->getParkingPath(), 0.5);
     const double margin =
-      parameters_->object_recognition_collision_check_margin * 0.9;  // hysteresis factor
+      parameters_->object_recognition_collision_check_margin * hysteresis_factor;
     if (checkObjectsCollision(parking_path, margin)) {
       RCLCPP_DEBUG(
         getLogger(),
         "[DecidingPathStatus]: DECIDING->NOT_DECIDED. path has collision with objects");
       return {DecidingPathStatus::NOT_DECIDED, clock_->now()};
     }
 
+    // if enough time has passed since deciding status starts, transition to DECIDED
     constexpr double check_collision_duration = 1.0;
     const double elapsed_time_from_deciding = (clock_->now() - prev_status.second).seconds();
     if (elapsed_time_from_deciding > check_collision_duration) {
@@ -959,6 +973,7 @@ DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus() const
       return {DecidingPathStatus::DECIDED, clock_->now()};
     }
 
+    // if enough time has NOT passed since deciding status starts, keep DECIDING
     RCLCPP_DEBUG(
       getLogger(), "[DecidingPathStatus]: keep DECIDING. elapsed_time_from_deciding: %f",
       elapsed_time_from_deciding);

planning/behavior_path_goal_planner_module/src/goal_searcher.cpp

@@ -312,6 +312,43 @@ void GoalSearcher::update(GoalCandidates & goal_candidates) const
   }
 }
 
+// Note: this function is not just return goal_candidate.is_safe but check collision with
+// current planner_data_ and margin scale factor.
+// And is_safe is not updated in this function.
+bool GoalSearcher::isSafeGoalWithMarginScaleFactor(
+  const GoalCandidate & goal_candidate, const double margin_scale_factor) 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_margin * margin_scale_factor;
+
+  if (utils::checkCollisionBetweenFootprintAndObjects(
+        vehicle_footprint_, goal_pose, pull_over_lane_stop_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);
+  if (checkCollisionWithLongitudinalDistance(goal_pose, target_objects)) {
+    return false;
+  }
+
+  return true;
+}
+
 bool GoalSearcher::checkCollision(const Pose & pose, const PredictedObjects & objects) const
 {
   if (parameters_.use_occupancy_grid_for_goal_search) {