refactor(behavior_path_planner): separate processing time function (#…

…5256) Signed-off-by: Takamasa Horibe <[email protected]>
autowarefoundation · Oct 11, 2023 · 82d9840 · 82d9840
1 parent 45339f6
commit 82d9840
Show file tree

Hide file tree

Showing 3 changed files with 14 additions and 2 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
@@ -242,6 +242,11 @@ class PlannerManager
    */
   void print() const;
 
+  /**
+   * @brief publish processing time of each module.
+   */
+  void publishProcessingTime() const;
+
   /**
    * @brief visit each module and get debug information.
    */

diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -579,6 +579,7 @@ void BehaviorPathPlannerNode::run()
   lk_pd.unlock();  // release planner_data_
 
   planner_manager_->print();
+  planner_manager_->publishProcessingTime();
   planner_manager_->publishMarker();
   planner_manager_->publishVirtualWall();
   lk_manager.unlock();  // release planner_manager_

diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp
@@ -886,13 +886,19 @@ void PlannerManager::print() const
     string_stream << std::right << "[" << std::setw(max_string_num + 1) << std::left << t.first
                   << ":" << std::setw(4) << std::right << t.second << "ms]\n"
                   << std::setw(21);
-    std::string name = std::string("processing_time/") + t.first;
-    debug_publisher_ptr_->publish<DebugDoubleMsg>(name, t.second);
   }
 
   RCLCPP_INFO_STREAM(logger_, string_stream.str());
 }
 
+void PlannerManager::publishProcessingTime() const
+{
+  for (const auto & t : processing_time_) {
+    std::string name = std::string("processing_time/") + t.first;
+    debug_publisher_ptr_->publish<DebugDoubleMsg>(name, t.second);
+  }
+}
+
 std::shared_ptr<SceneModuleVisitor> PlannerManager::getDebugMsg()
 {
   debug_msg_ptr_ = std::make_shared<SceneModuleVisitor>();