diff --git a/system/diagnostic_graph_utils/CMakeLists.txt b/system/diagnostic_graph_utils/CMakeLists.txt
index b68e7bedb57ed..0c36964f49237 100644
--- a/system/diagnostic_graph_utils/CMakeLists.txt
+++ b/system/diagnostic_graph_utils/CMakeLists.txt
@@ -12,6 +12,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
ament_auto_add_library(${PROJECT_NAME}_tools SHARED
src/node/dump.cpp
src/node/converter.cpp
+ src/node/logging.cpp
)
rclcpp_components_register_node(${PROJECT_NAME}_tools
@@ -24,4 +25,9 @@ rclcpp_components_register_node(${PROJECT_NAME}_tools
EXECUTABLE converter_node
)
-ament_auto_package()
+rclcpp_components_register_node(${PROJECT_NAME}_tools
+ PLUGIN "diagnostic_graph_utils::LoggingNode"
+ EXECUTABLE logging_node
+)
+
+ament_auto_package(INSTALL_TO_SHARE launch)
diff --git a/system/diagnostic_graph_utils/launch/logging.launch.xml b/system/diagnostic_graph_utils/launch/logging.launch.xml
new file mode 100644
index 0000000000000..12636574eb8be
--- /dev/null
+++ b/system/diagnostic_graph_utils/launch/logging.launch.xml
@@ -0,0 +1,10 @@
+
+
+
+
+
+
+
+
+
+
diff --git a/system/diagnostic_graph_utils/src/node/logging.cpp b/system/diagnostic_graph_utils/src/node/logging.cpp
new file mode 100644
index 0000000000000..009af51e949cd
--- /dev/null
+++ b/system/diagnostic_graph_utils/src/node/logging.cpp
@@ -0,0 +1,95 @@
+// Copyright 2024 The Autoware Contributors
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "logging.hpp"
+
+#include
+#include
+#include
+#include
+#include
+#include
+
+namespace diagnostic_graph_utils
+{
+
+LoggingNode::LoggingNode(const rclcpp::NodeOptions & options) : Node("logging", options)
+{
+ root_path_ = declare_parameter("root_path");
+ max_depth_ = declare_parameter("max_depth");
+
+ using std::placeholders::_1;
+ sub_graph_.register_create_callback(std::bind(&LoggingNode::on_create, this, _1));
+ sub_graph_.subscribe(*this, 1);
+
+ const auto period = rclcpp::Rate(declare_parameter("show_rate")).period();
+ timer_ = rclcpp::create_timer(this, get_clock(), period, [this]() { on_timer(); });
+}
+
+void LoggingNode::on_create(DiagGraph::ConstSharedPtr graph)
+{
+ // Search root node.
+ root_unit_ = nullptr;
+ for (const auto & unit : graph->units()) {
+ if (unit->path() == root_path_) {
+ root_unit_ = unit;
+ return;
+ }
+ }
+ RCLCPP_ERROR_STREAM(get_logger(), "root unit is not found: " << root_path_);
+}
+
+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());
+ }
+}
+
+void LoggingNode::dump_unit(DiagUnit * unit, int depth, const std::string & indent)
+{
+ const auto text_level = [](DiagUnit::DiagnosticLevel level) {
+ if (level == DiagUnit::DiagnosticStatus::OK) return "OK ";
+ if (level == DiagUnit::DiagnosticStatus::WARN) return "WARN ";
+ if (level == DiagUnit::DiagnosticStatus::ERROR) return "ERROR";
+ if (level == DiagUnit::DiagnosticStatus::STALE) return "STALE";
+ return "-----";
+ };
+
+ if (max_depth_ < depth) {
+ return;
+ }
+ if (unit->level() == DiagUnit::DiagnosticStatus::OK) {
+ return;
+ }
+
+ std::string path = unit->path();
+ if (path.empty()) {
+ path = "[anonymous group]";
+ }
+
+ dump_text_ << indent << "- " + path << " " << text_level(unit->level()) << std::endl;
+ for (const auto & child : unit->children()) {
+ dump_unit(child.unit, depth + 1, indent + " ");
+ }
+}
+
+} // namespace diagnostic_graph_utils
+
+#include
+RCLCPP_COMPONENTS_REGISTER_NODE(diagnostic_graph_utils::LoggingNode)
diff --git a/system/diagnostic_graph_utils/src/node/logging.hpp b/system/diagnostic_graph_utils/src/node/logging.hpp
new file mode 100644
index 0000000000000..b2d305c81ca99
--- /dev/null
+++ b/system/diagnostic_graph_utils/src/node/logging.hpp
@@ -0,0 +1,48 @@
+// Copyright 2024 The Autoware Contributors
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NODE__LOGGING_HPP_
+#define NODE__LOGGING_HPP_
+
+#include "diagnostic_graph_utils/subscription.hpp"
+
+#include
+
+#include
+#include
+
+namespace diagnostic_graph_utils
+{
+
+class LoggingNode : public rclcpp::Node
+{
+public:
+ explicit LoggingNode(const rclcpp::NodeOptions & options);
+
+private:
+ void on_create(DiagGraph::ConstSharedPtr graph);
+ void on_timer();
+ void dump_unit(DiagUnit * unit, int depth, const std::string & indent);
+ DiagGraphSubscription sub_graph_;
+ rclcpp::TimerBase::SharedPtr timer_;
+
+ DiagUnit * root_unit_;
+ int max_depth_;
+ std::string root_path_;
+ std::ostringstream dump_text_;
+};
+
+} // namespace diagnostic_graph_utils
+
+#endif // NODE__LOGGING_HPP_