feat(behavior_path_planner): output manager internal state as topic (a…

…utowarefoundation#6099)

Signed-off-by: satoshi-ota <[email protected]>

planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp

@@ -46,6 +46,7 @@ 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;
+using DebugStringMsg = tier4_debug_msgs::msg::StringStamped;
 
 enum Action {
   ADD = 0,
@@ -456,6 +457,8 @@ class PlannerManager
 
   std::unique_ptr<DebugPublisher> debug_publisher_ptr_;
 
+  std::unique_ptr<DebugPublisher> state_publisher_ptr_;
+
   pluginlib::ClassLoader<SceneModuleManagerInterface> plugin_loader_;
 
   mutable rclcpp::Logger logger_;

planning/behavior_path_planner/src/planner_manager.cpp

@@ -40,6 +40,7 @@ PlannerManager::PlannerManager(
 {
   processing_time_.emplace("total_time", 0.0);
   debug_publisher_ptr_ = std::make_unique<DebugPublisher>(&node, "~/debug");
+  state_publisher_ptr_ = std::make_unique<DebugPublisher>(&node, "~/debug");
 }
 
 void PlannerManager::launchScenePlugin(rclcpp::Node & node, const std::string & name)
@@ -908,10 +909,6 @@ void PlannerManager::resetRootLanelet(const std::shared_ptr<PlannerData> & data)
 
 void PlannerManager::print() const
 {
-  if (!verbose_) {
-    return;
-  }
-
   const auto get_status = [](const auto & m) {
     return magic_enum::enum_name(m->getCurrentStatus());
   };
@@ -961,6 +958,12 @@ void PlannerManager::print() const
                   << std::setw(21);
   }
 
+  state_publisher_ptr_->publish<DebugStringMsg>("internal_state", string_stream.str());
+
+  if (!verbose_) {
+    return;
+  }
+
   RCLCPP_INFO_STREAM(logger_, string_stream.str());
 }