fix(bpp): don't remove rtc cooperate status

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

Author: satoshi-ota

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp

@@ -376,7 +376,6 @@ class AvoidanceModule : public SceneModuleInterface
     if (!path_shifter_.getShiftLines().empty()) {
       left_shift_array_.clear();
       right_shift_array_.clear();
-      removeRTCStatus();
     }
 
     generator_.reset();

planning/behavior_path_avoidance_module/src/scene.cpp

@@ -1296,7 +1296,6 @@ void AvoidanceModule::updateData()
 void AvoidanceModule::processOnEntry()
 {
   initVariables();
-  removeRTCStatus();
 }
 
 void AvoidanceModule::processOnExit()

planning/behavior_path_lane_change_module/src/interface.cpp

@@ -117,7 +117,6 @@ BehaviorModuleOutput LaneChangeInterface::plan()
   updateSteeringFactorPtr(output);
   if (module_type_->isAbortState()) {
     waitApproval();
-    removeRTCStatus();
     const auto candidate = planCandidate();
     path_candidate_ = std::make_shared<PathWithLaneId>(candidate.path_candidate);
     updateRTCStatus(
@@ -148,7 +147,6 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval()
   stop_pose_ = module_type_->getStopPose();
 
   if (!module_type_->isValidPath()) {
-    removeRTCStatus();
     path_candidate_ = std::make_shared<PathWithLaneId>();
     return out;
   }

planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp

@@ -290,8 +290,6 @@ class PlannerManager
 
     module_ptr->updateCurrentState();
 
-    module_ptr->publishRTCStatus();
-
     module_ptr->publishSteeringFactor();
 
     module_ptr->publishObjectsOfInterestMarker();
@@ -323,7 +321,6 @@ class PlannerManager
   void deleteExpiredModules(SceneModulePtr & module_ptr) const
   {
     module_ptr->onExit();
-    module_ptr->publishRTCStatus();
     module_ptr->publishObjectsOfInterestMarker();
     module_ptr.reset();
   }

planning/behavior_path_planner/src/planner_manager.cpp

@@ -191,8 +191,10 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr<PlannerData> & da
     return BehaviorModuleOutput{};  // something wrong.
   }();
 
-  std::for_each(
-    manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->updateObserver(); });
+  std::for_each(manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) {
+    m->updateObserver();
+    m->publishRTCStatus();
+  });
 
   generateCombinedDrivableArea(result_output, data);
 

planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp

@@ -187,8 +187,6 @@ class SceneModuleInterface
     RCLCPP_DEBUG(getLogger(), "%s %s", name_.c_str(), __func__);
 
     clearWaitingApproval();
-    removeRTCStatus();
-    publishRTCStatus();
     unlockNewModuleLaunch();
     unlockOutputPath();
     steering_factor_interface_ptr_->clearSteeringFactors();
@@ -198,18 +196,6 @@ class SceneModuleInterface
     processOnExit();
   }
 
-  /**
-   * @brief Publish status if the module is requested to run
-   */
-  void publishRTCStatus()
-  {
-    for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) {
-      if (ptr) {
-        ptr->publishCooperateStatus(clock_->now());
-      }
-    }
-  }
-
   void publishObjectsOfInterestMarker()
   {
     for (const auto & [module_name, ptr] : objects_of_interest_marker_interface_ptr_map_) {
@@ -540,15 +526,6 @@ class SceneModuleInterface
     return existApprovedRequest();
   }
 
-  void removeRTCStatus()
-  {
-    for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) {
-      if (ptr) {
-        ptr->clearCooperateStatus();
-      }
-    }
-  }
-
   void setStopReason(const std::string & stop_reason, const PathWithLaneId & path)
   {
     stop_reason_.reason = stop_reason;

planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_manager_interface.hpp

@@ -99,6 +99,16 @@ class SceneModuleManagerInterface
     observers_.erase(itr, observers_.end());
   }
 
+  void publishRTCStatus()
+  {
+    for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) {
+      if (ptr) {
+        ptr->removeExpiredCooperateStatus();
+        ptr->publishCooperateStatus(rclcpp::Clock(RCL_ROS_TIME).now());
+      }
+    }
+  }
+
   void publishVirtualWall() const
   {
     using tier4_autoware_utils::appendMarkerArray;
@@ -235,15 +245,13 @@ class SceneModuleManagerInterface
     std::for_each(observers_.begin(), observers_.end(), [](const auto & observer) {
       if (!observer.expired()) {
         observer.lock()->onExit();
-        observer.lock()->publishRTCStatus();
       }
     });
 
     observers_.clear();
 
     if (idle_module_ptr_ != nullptr) {
       idle_module_ptr_->onExit();
-      idle_module_ptr_->publishRTCStatus();
       idle_module_ptr_.reset();
     }
 

planning/behavior_path_start_planner_module/src/start_planner_module.cpp

@@ -1051,7 +1051,6 @@ void StartPlannerModule::updateStatusAfterBackwardDriving()
   // request start_planner approval
   waitApproval();
   // To enable approval of the forward path, the RTC status is removed.
-  removeRTCStatus();
   for (auto itr = uuid_map_.begin(); itr != uuid_map_.end(); ++itr) {
     itr->second = generateUUID();
   }