feat(goal_planner): add deciding status to check collision for for a …

…certain period of time

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

planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp

@@ -230,6 +230,13 @@ struct LastApprovalData
   Pose pose{};
 };
 
+enum class DecidingPathStatus {
+  NOT_DECIDED,
+  DECIDING,
+  DECIDED,
+};
+using DecidingPathStatusWithStamp = std::pair<DecidingPathStatus, rclcpp::Time>;
+
 struct PreviousPullOverData
 {
   struct SafetyStatus
@@ -242,12 +249,13 @@ struct PreviousPullOverData
   {
     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};
+  // bool has_decided_path{false};
+  DecidingPathStatusWithStamp deciding_path_status{};
 };
 
 // store stop_pose_ pointer with reason string
@@ -439,6 +447,8 @@ class GoalPlannerModule : public SceneModuleInterface
   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/src/goal_planner_module.cpp

@@ -903,36 +903,74 @@ void GoalPlannerModule::updateSteeringFactor(
 
 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;
+    return {DecidingPathStatus::NOT_DECIDED, clock_->now()};
+  }
+
+  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.
+    if (checkObjectsCollision(
+          current_path, parameters_->object_recognition_collision_check_margin,
+          true /*update_debug_data*/)) {
+      return {DecidingPathStatus::NOT_DECIDED, clock_->now()};
+    }
+
+    constexpr double check_collision_duration = 1.0;
+    if ((clock_->now() - prev_status.second).seconds() > check_collision_duration) {
+      return {DecidingPathStatus::DECIDED, clock_->now()};
+    }
+
+    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.
+  return {
+    parameters_->use_object_recognition ? DecidingPathStatus::DECIDING
+                                        : DecidingPathStatus::DECIDED,
+    clock_->now()};
 }
 
 void GoalPlannerModule::decideVelocity()
@@ -993,7 +1031,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput()
     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_
@@ -1083,7 +1121,7 @@ void GoalPlannerModule::updatePreviousData()
   // 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) {