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

…inal log (autowarefoundation#9421)

* feat(diagnostic_graph_utils): publish error graph instead of the terminal log

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* update

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* Update system/diagnostic_graph_utils/src/node/logging.cpp

Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com>

* error_graph -> error_graph_text

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

---------

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com>

system/diagnostic_graph_utils/launch/logging.launch.xml

@@ -6,5 +6,6 @@
     <param name="root_path" value="$(var root_path)"/>
     <param name="max_depth" value="$(var max_depth)"/>
     <param name="show_rate" value="$(var show_rate)"/>
+    <param name="enable_terminal_log" value="$(var enable_terminal_log)"/>
   </node>
 </launch>

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

@@ -34,8 +34,13 @@ 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_text_ = create_publisher<tier4_debug_msgs::msg::StringStamped>(
+    "~/debug/error_graph_text", 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(); });
+
+  enable_terminal_log_ = declare_parameter<bool>("enable_terminal_log");
 }
 
 void LoggingNode::on_create(DiagGraph::ConstSharedPtr graph)
@@ -53,12 +58,24 @@ 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:";
+  static const auto prefix_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, "");
+
+    if (enable_terminal_log_) {
+      RCLCPP_WARN_STREAM(get_logger(), prefix_message << std::endl << dump_text_.str());
+    }
+
+    tier4_debug_msgs::msg::StringStamped message;
+    message.stamp = now();
+    message.data = dump_text_.str();
+    pub_error_graph_text_->publish(message);
+  } else {
+    tier4_debug_msgs::msg::StringStamped message;
+    message.stamp = now();
+    pub_error_graph_text_->publish(message);
   }
 }
 
@@ -86,7 +103,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,12 +37,14 @@ 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_text_;
   rclcpp::TimerBase::SharedPtr timer_;
 
   DiagUnit * root_unit_;
   int max_depth_;
   std::string root_path_;
   std::ostringstream dump_text_;
+  bool enable_terminal_log_;
 };
 
 }  // namespace diagnostic_graph_utils