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_