fix(bpp): keep publishing rtc cooperate status (#6953)

Signed-off-by: satoshi-ota <[email protected]>
autowarefoundation · May 9, 2024 · 569d56c · 569d56c
1 parent 473f238
commit 569d56c
Show file tree

Hide file tree

Showing 4 changed files with 13 additions and 20 deletions.
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/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();
   }

diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/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);
 

diff --git a/..._planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp b/..._planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp
@@ -186,7 +186,6 @@ class SceneModuleInterface
 
     clearWaitingApproval();
     removeRTCStatus();
-    publishRTCStatus();
     unlockNewModuleLaunch();
     unlockOutputPath();
     steering_factor_interface_ptr_->clearSteeringFactors();
@@ -196,18 +195,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_) {

diff --git a/..._common/include/behavior_path_planner_common/interface/scene_module_manager_interface.hpp b/..._common/include/behavior_path_planner_common/interface/scene_module_manager_interface.hpp
@@ -99,6 +99,15 @@ class SceneModuleManagerInterface
     observers_.erase(itr, observers_.end());
   }
 
+  void publishRTCStatus()
+  {
+    for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) {
+      if (ptr) {
+        ptr->publishCooperateStatus(rclcpp::Clock(RCL_ROS_TIME).now());
+      }
+    }
+  }
+
   void publishVirtualWall() const
   {
     using tier4_autoware_utils::appendMarkerArray;
@@ -235,15 +244,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();
     }