feat(goal_planner): add deciding status to check collision for for a certain period of time

planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp

@@ -230,6 +230,13 @@
   Pose pose{};
 };
 
+enum class DecidingPathStatus {
+  NOT_DECIDED,
+  DECIDING,
+  DECIDED,
+};
+using DecidingPathStatusWithStamp = std::pair<DecidingPathStatus, rclcpp::Time>;
+
 struct PreviousPullOverData
 {
   struct SafetyStatus
@@ -242,12 +249,12 @@
   {
     found_path = false;
     safety_status = SafetyStatus{};
-    has_decided_path = false;
+    deciding_path_status = DecidingPathStatusWithStamp{};
   }
 
   bool found_path{false};
   SafetyStatus safety_status{};
-  bool has_decided_path{false};
+  DecidingPathStatusWithStamp deciding_path_status{};
 };
 
 // store stop_pose_ pointer with reason string
@@ -439,6 +446,8 @@
   bool needPathUpdate(const double path_update_duration) const;
   bool isStuck();
   bool hasDecidedPath() const;
+  bool hasNotDecidedPath() const;
+  DecidingPathStatusWithStamp checkDecidingPathStatus() const;
   void decideVelocity();
   bool foundPullOverPath() const;
   void updateStatus(const BehaviorModuleOutput & output);

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

@@ -1,4 +1,4 @@
 // Copyright 2021 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.
@@ -806,8 +806,8 @@
 
   if (!thread_safe_data_.foundPullOverPath()) {
     // situation : not safe against static objects use stop_path
     output.path = generateStopPath();
     RCLCPP_WARN_THROTTLE(
       getLogger(), *clock_, 5000, "Not found safe pull_over path, generate stop path");
     setDrivableAreaInfo(output);
     return;
@@ -819,10 +819,10 @@
     // insert stop point in current path if ego is able to stop with acceleration and jerk
     // constraints
     output.path =
       generateFeasibleStopPath(thread_safe_data_.get_pull_over_path()->getCurrentPath());
     RCLCPP_WARN_THROTTLE(
       getLogger(), *clock_, 5000, "Not safe against dynamic objects, generate stop path");
     debug_stop_pose_with_info_.set(std::string("feasible stop after approval"));
   } else {
     // situation : (safe against static and dynamic objects) or (safe against static objects and
     // before approval) don't stop
@@ -903,36 +903,108 @@
 
 bool GoalPlannerModule::hasDecidedPath() const
 {
+  return checkDecidingPathStatus().first == DecidingPathStatus::DECIDED;
+}
+
+bool GoalPlannerModule::hasNotDecidedPath() const
+{
+  return checkDecidingPathStatus().first == DecidingPathStatus::NOT_DECIDED;
+}
+
+DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus() const
+{
+  const auto & prev_status = prev_data_.deciding_path_status;
+
   // Once this function returns true, it will continue to return true thereafter
-  if (prev_data_.has_decided_path) {
-    return true;
+  if (prev_status.first == DecidingPathStatus::DECIDED) {
+    return prev_status;
   }
 
   // if path is not safe, not decided
   if (!thread_safe_data_.foundPullOverPath()) {
-    return false;
+    return {DecidingPathStatus::NOT_DECIDED, clock_->now()};
   }
 
-  // If it is dangerous before approval, do not determine the path.
+  // If it is dangerous against dynamic objects before approval, do not determine the path.
   // This eliminates a unsafe path to be approved
   if (
     parameters_->safety_check_params.enable_safety_check && !isSafePath().first && !isActivated()) {
-    return false;
+    RCLCPP_DEBUG(
+      getLogger(),
+      "[DecidingPathStatus]: NOT_DECIDED. path is not safe against dynamic objects before "
+      "approval");
+    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) {
+    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 * 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) {
+      RCLCPP_DEBUG(
+        getLogger(), "[DecidingPathStatus]: DECIDING->DECIDED. has enough safe time passed");
+      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);
+    return prev_status;
   }
 
   // if ego is sufficiently close to the start of the nearest candidate path, the path is decided
   const auto & current_pose = planner_data_->self_odometry->pose.pose;
-  const size_t ego_segment_idx = motion_utils::findNearestSegmentIndex(
-    thread_safe_data_.get_pull_over_path()->getCurrentPath().points, current_pose.position);
+  const size_t ego_segment_idx =
+    motion_utils::findNearestSegmentIndex(current_path.points, current_pose.position);
 
-  const size_t start_pose_segment_idx = motion_utils::findNearestSegmentIndex(
-    thread_safe_data_.get_pull_over_path()->getCurrentPath().points,
-    thread_safe_data_.get_pull_over_path()->start_pose.position);
+  const size_t start_pose_segment_idx =
+    motion_utils::findNearestSegmentIndex(current_path.points, pull_over_path->start_pose.position);
   const double dist_to_parking_start_pose = calcSignedArcLength(
-    thread_safe_data_.get_pull_over_path()->getCurrentPath().points, current_pose.position,
-    ego_segment_idx, thread_safe_data_.get_pull_over_path()->start_pose.position,
-    start_pose_segment_idx);
-  return dist_to_parking_start_pose < parameters_->decide_path_distance;
+    current_path.points, current_pose.position, ego_segment_idx,
+    pull_over_path->start_pose.position, start_pose_segment_idx);
+  if (dist_to_parking_start_pose > parameters_->decide_path_distance) {
+    return {DecidingPathStatus::NOT_DECIDED, clock_->now()};
+  }
+
+  // if object recognition for path collision check is enabled, transition to DECIDING to check
+  // collision for a certain period of time. Otherwise, transition to DECIDED directly.
+  if (parameters_->use_object_recognition) {
+    RCLCPP_DEBUG(
+      getLogger(),
+      "[DecidingPathStatus]: NOT_DECIDED->DECIDING. start checking collision for certain "
+      "period of time");
+    return {DecidingPathStatus::DECIDING, clock_->now()};
+  }
+  RCLCPP_DEBUG(getLogger(), "[DecidingPathStatus]: NOT_DECIDED->DECIDED");
+  return {DecidingPathStatus::DECIDED, clock_->now()};
 }
 
 void GoalPlannerModule::decideVelocity()
@@ -993,10 +1065,11 @@
     return getPreviousModuleOutput();
   }
 
-  if (!hasDecidedPath() && needPathUpdate(1.0 /*path_update_duration*/)) {
+  if (hasNotDecidedPath() && needPathUpdate(1.0 /*path_update_duration*/)) {
     // if the final path is not decided and enough time has passed since last path update,
     // select safe path from lane parking pull over path candidates
     // and set it to thread_safe_data_
+    RCLCPP_DEBUG(getLogger(), "Update pull over path candidates");
 
     thread_safe_data_.clearPullOverPath();
 
@@ -1050,7 +1123,7 @@
   path_candidate_ =
     std::make_shared<PathWithLaneId>(thread_safe_data_.get_pull_over_path()->getFullPath());
 
   updatePreviousData();
 
   return output;
 }
@@ -1077,13 +1150,13 @@
   setStopReason(StopReason::GOAL_PLANNER, thread_safe_data_.get_pull_over_path()->getFullPath());
 }
 
 void GoalPlannerModule::updatePreviousData()
 {
   // for the next loop setOutput().
   // this is used to determine whether to generate a new stop path or keep the current stop path.
   prev_data_.found_path = thread_safe_data_.foundPullOverPath();
 
-  prev_data_.has_decided_path = hasDecidedPath();
+  prev_data_.deciding_path_status = checkDecidingPathStatus();
 
   // This is related to safety check, so if it is disabled, return here.
   if (!parameters_->safety_check_params.enable_safety_check) {
@@ -1208,7 +1281,7 @@
       return std::make_pair(decel_pose.value(), "stop at search start pose");
     });
   if (!stop_pose_with_info) {
     const auto feasible_stop_path = generateFeasibleStopPath(getPreviousModuleOutput().path);
     // override stop pose info debug string
     debug_stop_pose_with_info_.set(std::string("feasible stop: not calculate stop pose"));
     return feasible_stop_path;
@@ -1223,7 +1296,7 @@
   const bool is_stopped = std::abs(current_vel) < eps_vel;
   const double buffer = is_stopped ? stop_distance_buffer_ : 0.0;
   if (min_stop_distance && ego_to_stop_distance + buffer < *min_stop_distance) {
     const auto feasible_stop_path = generateFeasibleStopPath(getPreviousModuleOutput().path);
     debug_stop_pose_with_info_.set(
       std::string("feasible stop: stop pose is closer than min_stop_distance"));
     return feasible_stop_path;
@@ -1255,17 +1328,17 @@
   return stop_path;
 }
 
 PathWithLaneId GoalPlannerModule::generateFeasibleStopPath(const PathWithLaneId & path) const
 {
   // calc minimum stop distance under maximum deceleration
   const auto min_stop_distance = calcFeasibleDecelDistance(
     planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0);
   if (!min_stop_distance) {
     return path;
   }
 
   // set stop point
   auto stop_path = path;
   const auto & current_pose = planner_data_->self_odometry->pose.pose;
   const auto stop_idx =
     motion_utils::insertStopPoint(current_pose, *min_stop_distance, stop_path.points);
@@ -1450,9 +1523,9 @@
 {
   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);
 
   std::vector<Polygon2d> obj_polygons;
   for (const auto & object : pull_over_lane_stop_objects.objects) {

planning/behavior_path_goal_planner_module/src/goal_searcher.cpp

@@ -1,4 +1,4 @@
 // Copyright 2022 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.
@@ -268,9 +268,9 @@
 {
   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);
@@ -312,6 +312,43 @@
   }
 }
 
+// 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) {