feat(diagnostic_graph_utils): publish error graph instead of the term…

…inal log

Signed-off-by: Takayuki Murooka <[email protected]>

system/diagnostic_graph_utils/package.xml

@@ -13,6 +13,7 @@
   <depend>diagnostic_msgs</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
+  <depend>tier4_debug_msgs</depend>
   <depend>tier4_system_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>

system/diagnostic_graph_utils/src/node/logging.cpp

@@ -33,6 +33,9 @@ LoggingNode::LoggingNode(const rclcpp::NodeOptions & options) : Node("logging",
   sub_graph_.register_create_callback(std::bind(&LoggingNode::on_create, this, _1));
   sub_graph_.subscribe(*this, 1);
 
+  pub_error_graph_ =
+    create_publisher<tier4_debug_msgs::msg::StringStamped>("~/debug/error_graph", rclcpp::QoS(1));
+
   const auto period = rclcpp::Rate(declare_parameter<double>("show_rate")).period();
   timer_ = rclcpp::create_timer(this, get_clock(), period, [this]() { on_timer(); });
 }
@@ -52,12 +55,15 @@ void LoggingNode::on_create(DiagGraph::ConstSharedPtr graph)
 
 void LoggingNode::on_timer()
 {
-  static const auto message = "The target mode is not available for the following reasons:";
   if (root_unit_ && root_unit_->level() != DiagUnit::DiagnosticStatus::OK) {
     dump_text_.str("");
     dump_text_.clear(std::stringstream::goodbit);
-    dump_unit(root_unit_, 0, "  ");
-    RCLCPP_WARN_STREAM(get_logger(), message << std::endl << dump_text_.str());
+    dump_unit(root_unit_, 0, "    ");
+
+    tier4_debug_msgs::msg::StringStamped message;
+    message.stamp = now();
+    message.data = dump_text_.str();
+    pub_error_graph_->publish(message);
   }
 }
 
@@ -85,7 +91,7 @@ void LoggingNode::dump_unit(DiagUnit * unit, int depth, const std::string & inde
 
   dump_text_ << indent << "- " + path << " " << text_level(unit->level()) << std::endl;
   for (const auto & child : unit->children()) {
-    dump_unit(child.unit, depth + 1, indent + "  ");
+    dump_unit(child.unit, depth + 1, indent + "    ");
   }
 }
 

system/diagnostic_graph_utils/src/node/logging.hpp

@@ -19,6 +19,8 @@
 
 #include <rclcpp/rclcpp.hpp>
 
+#include "tier4_debug_msgs/msg/string_stamped.hpp"
+
 #include <sstream>
 #include <string>
 
@@ -35,6 +37,7 @@ class LoggingNode : public rclcpp::Node
   void on_timer();
   void dump_unit(DiagUnit * unit, int depth, const std::string & indent);
   DiagGraphSubscription sub_graph_;
+  rclcpp::Publisher<tier4_debug_msgs::msg::StringStamped>::SharedPtr pub_error_graph_;
   rclcpp::TimerBase::SharedPtr timer_;
 
   DiagUnit * root_unit_;