diff --git a/system/diagnostic_graph_utils/package.xml b/system/diagnostic_graph_utils/package.xml
index 2b393b4191581..044a29423db22 100644
--- a/system/diagnostic_graph_utils/package.xml
+++ b/system/diagnostic_graph_utils/package.xml
@@ -13,6 +13,7 @@
diagnostic_msgs
rclcpp
rclcpp_components
+ tier4_debug_msgs
tier4_system_msgs
ament_lint_auto
diff --git a/system/diagnostic_graph_utils/src/node/logging.cpp b/system/diagnostic_graph_utils/src/node/logging.cpp
index 009af51e949cd..56a6c6756d071 100644
--- a/system/diagnostic_graph_utils/src/node/logging.cpp
+++ b/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("~/debug/error_graph", rclcpp::QoS(1));
+
const auto period = rclcpp::Rate(declare_parameter("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 + " ");
}
}
diff --git a/system/diagnostic_graph_utils/src/node/logging.hpp b/system/diagnostic_graph_utils/src/node/logging.hpp
index b2d305c81ca99..6ab19ca32352e 100644
--- a/system/diagnostic_graph_utils/src/node/logging.hpp
+++ b/system/diagnostic_graph_utils/src/node/logging.hpp
@@ -19,6 +19,8 @@
#include
+#include "tier4_debug_msgs/msg/string_stamped.hpp"
+
#include
#include
@@ -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::SharedPtr pub_error_graph_;
rclcpp::TimerBase::SharedPtr timer_;
DiagUnit * root_unit_;