fix(planner_manager): don't back waiting approval besides last one in…

… approved modules (#4588)

* fix(planner_manager): back waiting approval only when it's last module

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(avoidance): lock output path

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(avoidance): remove unnecessary process

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

---------

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

planning/behavior_path_planner/config/avoidance/avoidance.param.yaml

@@ -145,7 +145,6 @@
         time_horizon: 10.0                               # [s]
         safety_check_backward_distance: 100.0            # [m]
         safety_check_hysteresis_factor: 2.0              # [-]
-        safety_check_ego_offset: 1.0                     # [m]
 
       # For avoidance maneuver
       avoidance:

planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp

@@ -126,10 +126,10 @@ class PlannerManager
    */
   void reset()
   {
+    std::for_each(manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->reset(); });
     approved_module_ptrs_.clear();
     candidate_module_ptrs_.clear();
     root_lanelet_ = boost::none;
-    std::for_each(manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->reset(); });
     resetProcessingTime();
   }
 

planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp

@@ -161,7 +161,9 @@ class SceneModuleInterface
 
     clearWaitingApproval();
     removeRTCStatus();
+    publishRTCStatus();
     unlockNewModuleLaunch();
+    unlockOutputPath();
     steering_factor_interface_ptr_->clearSteeringFactors();
 
     stop_reason_ = StopReason();
@@ -220,6 +222,10 @@ class SceneModuleInterface
    */
   virtual void setData(const std::shared_ptr<const PlannerData> & data) { planner_data_ = data; }
 
+  void lockOutputPath() { is_locked_output_path_ = true; }
+
+  void unlockOutputPath() { is_locked_output_path_ = false; }
+
   bool isWaitingApproval() const
   {
     return is_waiting_approval_ || current_state_ == ModuleStatus::WAITING_APPROVAL;
@@ -340,6 +346,8 @@ class SceneModuleInterface
 
   bool is_locked_new_module_launch_{false};
 
+  bool is_locked_output_path_{false};
+
 protected:
   /**
    * @brief State transition condition ANY -> SUCCESS
@@ -507,6 +515,8 @@ class SceneModuleInterface
 
   BehaviorModuleOutput getPreviousModuleOutput() const { return previous_module_output_; }
 
+  bool isOutputPathLocked() const { return is_locked_output_path_; }
+
   void lockNewModuleLaunch() { is_locked_new_module_launch_ = true; }
 
   void unlockNewModuleLaunch() { is_locked_new_module_launch_ = false; }

planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp

@@ -201,9 +201,6 @@ struct AvoidanceParameters
   // transit hysteresis (unsafe to safe)
   double safety_check_hysteresis_factor;
 
-  // don't output new candidate path if the offset between ego and path is larger than this.
-  double safety_check_ego_offset;
-
   // keep target velocity in yield maneuver
   double yield_velocity;
 

planning/behavior_path_planner/src/planner_manager.cpp

@@ -129,6 +129,9 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr<PlannerData> & da
     return BehaviorModuleOutput{};
   }();
 
+  std::for_each(
+    manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->updateObserver(); });
+
   generateCombinedDrivableArea(result_output, data);
 
   return result_output;
@@ -449,6 +452,16 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptr<Pl
   BehaviorModuleOutput output = getReferencePath(data);
   results.emplace("root", output);
 
+  if (approved_module_ptrs_.empty()) {
+    return output;
+  }
+
+  // lock approved modules besides last one
+  std::for_each(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), [&](const auto & m) {
+    m->lockOutputPath();
+  });
+  approved_module_ptrs_.back()->unlockOutputPath();
+
   /**
    * execute all approved modules.
    */
@@ -468,21 +481,22 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptr<Pl
     const auto waiting_approval_modules = [](const auto & m) { return m->isWaitingApproval(); };
 
     const auto itr = std::find_if(
-      approved_module_ptrs_.begin(), approved_module_ptrs_.end(), waiting_approval_modules);
+      approved_module_ptrs_.rbegin(), approved_module_ptrs_.rend(), waiting_approval_modules);
 
-    if (itr != approved_module_ptrs_.end()) {
-      clearCandidateModules();
-      candidate_module_ptrs_.push_back(*itr);
+    if (itr != approved_module_ptrs_.rend()) {
+      const auto is_last_module = std::distance(approved_module_ptrs_.rbegin(), itr) == 0;
+      if (is_last_module) {
+        clearCandidateModules();
+        candidate_module_ptrs_.push_back(*itr);
+
+        debug_info_.emplace_back(*itr, Action::MOVE, "Back To Waiting Approval");
+
+        approved_module_ptrs_.pop_back();
+      }
 
-      std::for_each(
-        std::next(itr), approved_module_ptrs_.end(), [this](auto & m) { deleteExpiredModules(m); });
       std::for_each(
         manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->updateObserver(); });
-
-      debug_info_.emplace_back(*itr, Action::MOVE, "Back To Waiting Approval");
     }
-
-    approved_module_ptrs_.erase(itr, approved_module_ptrs_.end());
   }
 
   /**

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

@@ -480,18 +480,14 @@ void AvoidanceModule::fillEgoStatus(
 
   const auto can_yield_maneuver = canYieldManeuver(data);
 
-  const size_t ego_seg_idx =
-    planner_data_->findEgoSegmentIndex(helper_.getPreviousSplineShiftPath().path.points);
-  const auto offset = std::abs(motion_utils::calcLateralOffset(
-    helper_.getPreviousSplineShiftPath().path.points, getEgoPosition(), ego_seg_idx));
-
-  // don't output new candidate path if the offset between the ego and previous output path is
-  // larger than threshold.
-  // TODO(Satoshi OTA): remove this workaround
-  if (offset > parameters_->safety_check_ego_offset) {
+  /**
+   * If the output path is locked by outside of this module, don't update output path.
+   */
+  if (isOutputPathLocked()) {
     data.safe_new_sl.clear();
     data.candidate_path = helper_.getPreviousSplineShiftPath();
-    RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 500, "unsafe. canceling candidate path...");
+    RCLCPP_WARN_THROTTLE(
+      getLogger(), *clock_, 500, "this module is locked now. keep current path.");
     return;
   }
 
@@ -2172,8 +2168,6 @@ BehaviorModuleOutput AvoidanceModule::plan()
   generateExtendedDrivableArea(output);
   setDrivableLanes(output.drivable_area_info.drivable_lanes);
 
-  // updateRegisteredRTCStatus(spline_shift_path.path);
-
   return output;
 }
 
@@ -2515,8 +2509,6 @@ void AvoidanceModule::initVariables()
 
 void AvoidanceModule::initRTCStatus()
 {
-  removeRTCStatus();
-  clearWaitingApproval();
   left_shift_array_.clear();
   right_shift_array_.clear();
   uuid_map_.at("left") = generateUUID();

planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp

@@ -151,7 +151,6 @@ AvoidanceModuleManager::AvoidanceModuleManager(
       get_parameter<double>(node, ns + "safety_check_backward_distance");
     p.safety_check_hysteresis_factor =
       get_parameter<double>(node, ns + "safety_check_hysteresis_factor");
-    p.safety_check_ego_offset = get_parameter<double>(node, ns + "safety_check_ego_offset");
   }
 
   // avoidance maneuver (lateral)