perf(processing_time/behavior_path_planner): add processing time publ…

…ishers to output results from the timer (autowarefoundation#5136) * perf(processing_time/behavior_path_planner): add processing time publishers to output results from the timer Signed-off-by: Owen-Liuyuxuan <[email protected]> * style(pre-commit): autofix --------- Signed-off-by: Owen-Liuyuxuan <[email protected]> Co-authored-by: Owen-Liuyuxuan <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
zulfaqar-azmi-t4 · Sep 26, 2023 · 9a8a043 · 9a8a043
1 parent 68ee0f2
commit 9a8a043
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 0 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
@@ -19,10 +19,12 @@
 #include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp"
 #include "behavior_path_planner/scene_module/scene_module_visitor.hpp"
 #include "behavior_path_planner/utils/lane_following/module_data.hpp"
+#include "tier4_autoware_utils/ros/debug_publisher.hpp"
 #include "tier4_autoware_utils/system/stop_watch.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 
+#include "tier4_debug_msgs/msg/float64_stamped.hpp"
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 #include <tier4_planning_msgs/msg/stop_reason_array.hpp>
 
@@ -44,6 +46,8 @@ using tier4_autoware_utils::StopWatch;
 using tier4_planning_msgs::msg::StopReasonArray;
 using SceneModulePtr = std::shared_ptr<SceneModuleInterface>;
 using SceneModuleManagerPtr = std::shared_ptr<SceneModuleManagerInterface>;
+using DebugPublisher = tier4_autoware_utils::DebugPublisher;
+using DebugDoubleMsg = tier4_debug_msgs::msg::Float64Stamped;
 
 enum Action {
   ADD = 0,
@@ -421,6 +425,8 @@ class PlannerManager
 
   std::vector<SceneModulePtr> candidate_module_ptrs_;
 
+  std::unique_ptr<DebugPublisher> debug_publisher_ptr_;
+
   mutable rclcpp::Logger logger_;
 
   mutable rclcpp::Clock clock_;

diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp
@@ -16,6 +16,7 @@
 
 #include "behavior_path_planner/utils/path_utils.hpp"
 #include "behavior_path_planner/utils/utils.hpp"
+#include "tier4_autoware_utils/ros/debug_publisher.hpp"
 #include "tier4_autoware_utils/system/stop_watch.hpp"
 
 #include <lanelet2_extension/utility/query.hpp>
@@ -34,6 +35,7 @@ PlannerManager::PlannerManager(rclcpp::Node & node, const bool verbose)
   verbose_{verbose}
 {
   processing_time_.emplace("total_time", 0.0);
+  debug_publisher_ptr_ = std::make_unique<DebugPublisher>(&node, "behavior_planner_manager/debug");
 }
 
 BehaviorModuleOutput PlannerManager::run(const std::shared_ptr<PlannerData> & data)
@@ -884,6 +886,8 @@ 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());