From 82d9840bc16dd380e2509e4fdac6d0be041f1796 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Wed, 11 Oct 2023 09:05:36 +0900 Subject: [PATCH] refactor(behavior_path_planner): separate processing time function (#5256) Signed-off-by: Takamasa Horibe --- .../include/behavior_path_planner/planner_manager.hpp | 5 +++++ .../src/behavior_path_planner_node.cpp | 1 + planning/behavior_path_planner/src/planner_manager.cpp | 10 ++++++++-- 3 files changed, 14 insertions(+), 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 index ee5771c98217e..72c32039da627 100644 --- 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 index 6e37bd0ec4b2c..755817aa22ae7 100644 --- 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 index 5963ddb778874..24967608775f3 100644 --- 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(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(name, t.second); + } +} + std::shared_ptr PlannerManager::getDebugMsg() { debug_msg_ptr_ = std::make_shared();