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_;