diff --git a/system/system_diagnostic_graph/CMakeLists.txt b/system/system_diagnostic_graph/CMakeLists.txt index 142aa94eeef69..be0fc19188a0b 100644 --- a/system/system_diagnostic_graph/CMakeLists.txt +++ b/system/system_diagnostic_graph/CMakeLists.txt @@ -20,9 +20,18 @@ ament_auto_add_executable(converter src/tool.cpp ) +ament_auto_add_executable(tool + src/tool/main.cpp +) +target_include_directories(tool PRIVATE src/core) + if(BUILD_TESTING) get_filename_component(RESOURCE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/test/files ABSOLUTE) - ament_auto_add_gtest(gtest_${PROJECT_NAME} test/src/test.cpp) + ament_auto_add_gtest(gtest_${PROJECT_NAME} + test/src/test1.cpp + test/src/test2.cpp + test/src/utils.cpp + ) target_compile_definitions(gtest_${PROJECT_NAME} PRIVATE TEST_RESOURCE_PATH="${RESOURCE_PATH}") target_include_directories(gtest_${PROJECT_NAME} PRIVATE src) endif() diff --git a/system/system_diagnostic_graph/config/default.param.yaml b/system/system_diagnostic_graph/config/default.param.yaml index 9fd572c7926fa..e91664c5b82d8 100644 --- a/system/system_diagnostic_graph/config/default.param.yaml +++ b/system/system_diagnostic_graph/config/default.param.yaml @@ -1,7 +1,7 @@ /**: ros__parameters: - mode_availability: true - mode: psim + use_operation_mode_availability: true + use_debug_mode: false rate: 1.0 input_qos_depth: 1000 graph_qos_depth: 1 diff --git a/system/system_diagnostic_graph/example/example_0.yaml b/system/system_diagnostic_graph/example/example_0.yaml index fc4436bba1595..ed92259b50721 100644 --- a/system/system_diagnostic_graph/example/example_0.yaml +++ b/system/system_diagnostic_graph/example/example_0.yaml @@ -22,15 +22,15 @@ nodes: list: - { type: link, link: /external/remote_command } - - path: /autoware/modes/emergency-stop + - path: /autoware/modes/emergency_stop type: ok - - path: /autoware/modes/comfortable-stop + - path: /autoware/modes/comfortable_stop type: and list: - { type: link, link: /functions/obstacle_detection } - - path: /autoware/modes/pull-over + - path: /autoware/modes/pull_over type: and list: - { type: link, link: /functions/pose_estimation } diff --git a/system/system_diagnostic_graph/src/core/config.cpp b/system/system_diagnostic_graph/src/core/config.cpp index 2339e96f3951f..4a32398a1b08f 100644 --- a/system/system_diagnostic_graph/src/core/config.cpp +++ b/system/system_diagnostic_graph/src/core/config.cpp @@ -22,6 +22,8 @@ #include #include #include +#include +#include #include #include @@ -66,15 +68,24 @@ ConfigData ConfigData::load(YAML::Node yaml) ConfigData ConfigData::type(const std::string & name) const { ConfigData data(file); - data.mark = name; + data.mark = mark.empty() ? name : mark + "-" + name; return data; } ConfigData ConfigData::node(const size_t index) const { - ConfigData data(file); - data.mark = mark + "-" + std::to_string(index); - return data; + return type(std::to_string(index)); +} + +std::optional ConfigData::take_yaml(const std::string & name) +{ + if (!object.count(name)) { + return std::nullopt; + } + + const auto yaml = object.at(name); + object.erase(name); + return yaml; } std::string ConfigData::take_text(const std::string & name) @@ -114,50 +125,82 @@ std::vector ConfigData::take_list(const std::string & name) return std::vector(yaml.begin(), yaml.end()); } -void check_config_nodes(const std::vector & nodes) +void resolve_link_nodes(RootConfig & root) { - std::unordered_map path_count; - for (const auto & node : nodes) { - path_count[node->path] += 1; + std::unordered_map paths; + for (const auto & node : root.nodes) { + if (node->path.empty()) { + continue; + } + if (paths.count(node->path)) { + throw error("object path is not unique", node->path); + } + paths[node->path] = node; } - path_count.erase(""); - for (const auto & [path, count] : path_count) { - if (1 < count) { - throw error("object path is not unique", path); + std::vector nodes; + std::vector links; + for (const auto & node : root.nodes) { + if (node->type == "link") { + links.push_back(node); + } else { + nodes.push_back(node); } } + + std::unordered_map targets; + for (const auto & node : nodes) { + targets[node] = node; + } + for (const auto & node : links) { + const auto path = node->data.take_text("link"); + if (!paths.count(path)) { + throw error("link path is not found", path, node->data); + } + const auto link = paths.at(path); + if (link->type == "link") { + throw error("link target is link type", path, node->data); + } + targets[node] = link; + } + for (const auto & node : nodes) { + for (auto & child : node->children) { + child = targets.at(child); + } + } + root.nodes = nodes; } -void resolve_link_nodes(std::vector & nodes) +void resolve_remove_edits(RootConfig & root) { - std::vector filtered; - std::unordered_map links; std::unordered_map paths; - - for (const auto & node : nodes) { - links[node] = node; + for (const auto & node : root.nodes) { paths[node->path] = node; } - for (const auto & node : nodes) { - if (node->type == "link" && node->path == "") { - const auto link = node->data.take_text("link"); - if (!paths.count(link)) { - throw error("link path is not found", link, node->data); + std::unordered_set removes; + for (const auto & edit : root.edits) { + if (edit->type == "remove") { + if (!paths.count(edit->path)) { + throw error("remove path is not found", edit->path, edit->data); } - links[node] = paths.at(link); - } else { - filtered.push_back(node); + removes.insert(paths.at(edit->path)); } } - nodes = filtered; - for (const auto & node : nodes) { - for (auto & child : node->children) { - child = links.at(child); + const auto filter = [removes](const std::vector & nodes) { + std::vector result; + for (const auto & node : nodes) { + if (!removes.count(node)) { + result.push_back(node); + } } + return result; + }; + for (const auto & node : root.nodes) { + node->children = filter(node->children); } + root.nodes = filter(root.nodes); } std::string complement_node_type(ConfigData & data) @@ -224,13 +267,23 @@ UnitConfig::SharedPtr parse_node_config(const ConfigData & data) return node; } +EditConfig::SharedPtr parse_edit_config(const ConfigData & data) +{ + const auto edit = std::make_shared(data); + edit->path = edit->data.take_text("path", ""); + edit->type = edit->data.take_text("type", ""); + return edit; +} + FileConfig::SharedPtr parse_file_config(const ConfigData & data) { const auto file = std::make_shared(data); const auto path_data = data.type("file"); const auto node_data = data.type("node"); + const auto edit_data = data.type("edit"); const auto paths = file->data.take_list("files"); const auto nodes = file->data.take_list("nodes"); + const auto edits = file->data.take_list("edits"); for (const auto & [index, yaml] : enumerate(paths)) { const auto path = path_data.node(index).load(yaml); @@ -240,6 +293,10 @@ FileConfig::SharedPtr parse_file_config(const ConfigData & data) const auto node = node_data.node(index).load(yaml); file->nodes.push_back(parse_node_config(node)); } + for (const auto & [index, yaml] : enumerate(edits)) { + const auto edit = edit_data.node(index).load(yaml); + file->edits.push_back(parse_edit_config(edit)); + } return file; } @@ -267,20 +324,22 @@ RootConfig load_root_config(const PathConfig::SharedPtr root) } std::vector nodes; + std::vector edits; for (const auto & file : files) { extend(nodes, file->nodes); + extend(edits, file->edits); } for (size_t i = 0; i < nodes.size(); ++i) { const auto node = nodes[i]; extend(nodes, node->children); } - check_config_nodes(nodes); - resolve_link_nodes(nodes); - RootConfig config; config.files = files; config.nodes = nodes; + config.edits = edits; + resolve_link_nodes(config); + resolve_remove_edits(config); return config; } diff --git a/system/system_diagnostic_graph/src/core/config.hpp b/system/system_diagnostic_graph/src/core/config.hpp index d959f5b6be8aa..d7cdd0ec3c5f7 100644 --- a/system/system_diagnostic_graph/src/core/config.hpp +++ b/system/system_diagnostic_graph/src/core/config.hpp @@ -18,6 +18,7 @@ #include #include +#include #include #include #include @@ -32,6 +33,14 @@ struct ConfigData ConfigData type(const std::string & name) const; ConfigData node(const size_t index) const; + template + T take(const std::string & name, const T & fail) + { + const auto yaml = take_yaml(name); + return yaml ? yaml.value().as() : fail; + } + + std::optional take_yaml(const std::string & name); std::string take_text(const std::string & name); std::string take_text(const std::string & name, const std::string & fail); std::vector take_list(const std::string & name); @@ -64,18 +73,28 @@ struct UnitConfig : public BaseConfig std::vector children; }; +struct EditConfig : public BaseConfig +{ + using SharedPtr = std::shared_ptr; + using BaseConfig::BaseConfig; + std::string type; + std::string path; +}; + struct FileConfig : public BaseConfig { using SharedPtr = std::shared_ptr; using BaseConfig::BaseConfig; std::vector paths; std::vector nodes; + std::vector edits; }; struct RootConfig { std::vector files; std::vector nodes; + std::vector edits; }; template diff --git a/system/system_diagnostic_graph/src/core/debug.cpp b/system/system_diagnostic_graph/src/core/debug.cpp index f14177f4571ad..e2771f08f9f1e 100644 --- a/system/system_diagnostic_graph/src/core/debug.cpp +++ b/system/system_diagnostic_graph/src/core/debug.cpp @@ -26,19 +26,32 @@ namespace system_diagnostic_graph { -const std::unordered_map level_names = { - {DiagnosticStatus::OK, "OK"}, - {DiagnosticStatus::WARN, "WARN"}, - {DiagnosticStatus::ERROR, "ERROR"}, - {DiagnosticStatus::STALE, "STALE"}}; +std::string get_level_text(DiagnosticLevel level) +{ + switch (level) { + case DiagnosticStatus::OK: + return "OK"; + case DiagnosticStatus::WARN: + return "WARN"; + case DiagnosticStatus::ERROR: + return "ERROR"; + case DiagnosticStatus::STALE: + return "STALE"; + } + return "UNKNOWN"; +} void Graph::debug() { std::vector lines; for (const auto & node : nodes_) { - const auto level_name = level_names.at(node->level()); + const auto level_name = get_level_text(node->level()); const auto index_name = std::to_string(node->index()); - lines.push_back({"unit", index_name, level_name, node->path(), "-----"}); + lines.push_back({index_name, level_name, node->path(), node->type()}); + } + for (const auto & [name, level] : unknowns_) { + const auto level_name = get_level_text(level); + lines.push_back({"*", level_name, name, "unknown"}); } std::array widths = {}; diff --git a/system/system_diagnostic_graph/src/core/debug.hpp b/system/system_diagnostic_graph/src/core/debug.hpp index bb1bbc6f14230..297356cd36bab 100644 --- a/system/system_diagnostic_graph/src/core/debug.hpp +++ b/system/system_diagnostic_graph/src/core/debug.hpp @@ -21,7 +21,7 @@ namespace system_diagnostic_graph { -constexpr size_t diag_debug_size = 5; +constexpr size_t diag_debug_size = 4; using DiagDebugData = std::array; } // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/error.hpp b/system/system_diagnostic_graph/src/core/error.hpp index 2c10d659f2df4..7e110654bf475 100644 --- a/system/system_diagnostic_graph/src/core/error.hpp +++ b/system/system_diagnostic_graph/src/core/error.hpp @@ -40,6 +40,11 @@ class InvalidType : public Exception using Exception::Exception; }; +class InvalidValue : public Exception +{ + using Exception::Exception; +}; + class FieldNotFound : public Exception { using Exception::Exception; diff --git a/system/system_diagnostic_graph/src/core/graph.cpp b/system/system_diagnostic_graph/src/core/graph.cpp index 96e9bcff5bfd9..5ad7df09de1d1 100644 --- a/system/system_diagnostic_graph/src/core/graph.cpp +++ b/system/system_diagnostic_graph/src/core/graph.cpp @@ -103,6 +103,12 @@ BaseUnit::UniquePtr make_node(const UnitConfig::SharedPtr & config) if (config->type == "or") { return std::make_unique(config->path); } + if (config->type == "warn-to-ok") { + return std::make_unique(config->path, DiagnosticStatus::OK); + } + if (config->type == "warn-to-error") { + return std::make_unique(config->path, DiagnosticStatus::ERROR); + } if (config->type == "ok") { return std::make_unique(config->path, DiagnosticStatus::OK); } @@ -128,10 +134,8 @@ Graph::~Graph() // for unique_ptr } -void Graph::init(const std::string & file, const std::string & mode) +void Graph::init(const std::string & file) { - (void)mode; // TODO(Takagi, Isamu) - BaseUnit::UniquePtrList nodes; BaseUnit::NodeDict dict; @@ -149,17 +153,26 @@ void Graph::init(const std::string & file, const std::string & mode) // Sort units in topological order for update dependencies. nodes = topological_sort(std::move(nodes)); - for (size_t index = 0; index < nodes.size(); ++index) { - nodes[index]->set_index(index); - } - + // List diag nodes that have diag name. for (const auto & node : nodes) { const auto diag = dynamic_cast(node.get()); if (diag) { diags_[diag->name()] = diag; - std::cout << diag->name() << std::endl; } } + + // List unit nodes that have path name. + for (const auto & node : nodes) { + if (!node->path().empty()) { + units_.push_back(node.get()); + } + } + + // Set unit index. + for (size_t index = 0; index < units_.size(); ++index) { + units_[index]->set_index(index); + } + nodes_ = std::move(nodes); } @@ -170,8 +183,7 @@ void Graph::callback(const rclcpp::Time & stamp, const DiagnosticArray & array) if (iter != diags_.end()) { iter->second->callback(stamp, status); } else { - // TODO(Takagi, Isamu) - std::cout << "unknown diag: " << status.name << std::endl; + unknowns_[status.name] = status.level; } } } @@ -184,8 +196,8 @@ DiagnosticGraph Graph::report(const rclcpp::Time & stamp) DiagnosticGraph message; message.stamp = stamp; - message.nodes.reserve(nodes_.size()); - for (const auto & node : nodes_) { + message.nodes.reserve(units_.size()); + for (const auto & node : units_) { const auto report = node->report(); DiagnosticNode temp; temp.status.name = node->path(); diff --git a/system/system_diagnostic_graph/src/core/graph.hpp b/system/system_diagnostic_graph/src/core/graph.hpp index 366f6b457e272..7b0a5bf563106 100644 --- a/system/system_diagnostic_graph/src/core/graph.hpp +++ b/system/system_diagnostic_graph/src/core/graph.hpp @@ -34,16 +34,17 @@ class Graph final Graph(); ~Graph(); - void init(const std::string & file, const std::string & mode = ""); + void init(const std::string & file); void callback(const rclcpp::Time & stamp, const DiagnosticArray & array); + void debug(); DiagnosticGraph report(const rclcpp::Time & stamp); std::vector nodes() const; - void debug(); - private: std::vector> nodes_; + std::vector units_; std::unordered_map diags_; + std::unordered_map unknowns_; }; } // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/modes.cpp b/system/system_diagnostic_graph/src/core/modes.cpp index 96944bd50f81a..1488387811d67 100644 --- a/system/system_diagnostic_graph/src/core/modes.cpp +++ b/system/system_diagnostic_graph/src/core/modes.cpp @@ -48,9 +48,9 @@ OperationModes::OperationModes(rclcpp::Node & node, const std::vector #include #include @@ -40,13 +42,6 @@ auto resolve(const BaseUnit::NodeDict & dict, const std::vector result; - result.push_back(dict.paths.at(path)); - return result; -} - BaseUnit::BaseUnit(const std::string & path) : path_(path) { index_ = 0; @@ -69,8 +64,8 @@ BaseUnit::NodeData BaseUnit::report() const void DiagUnit::init(const UnitConfig::SharedPtr & config, const NodeDict &) { - timeout_ = 3.0; // TODO(Takagi, Isamu): parameterize name_ = config->data.take_text("diag"); + timeout_ = config->data.take("timeout", 1.0); } void DiagUnit::update(const rclcpp::Time & stamp) @@ -148,6 +143,28 @@ void OrUnit::update(const rclcpp::Time &) level_ = std::min(level_, DiagnosticStatus::ERROR); } +RemapUnit::RemapUnit(const std::string & path, DiagnosticLevel remap_warn) : BaseUnit(path) +{ + remap_warn_ = remap_warn; +} + +void RemapUnit::init(const UnitConfig::SharedPtr & config, const NodeDict & dict) +{ + if (config->children.size() != 1) { + throw error("list size must be 1", config->data); + } + children_ = resolve(dict, config->children); +} + +void RemapUnit::update(const rclcpp::Time &) +{ + const auto status = children_.front()->status(); + level_ = status.level; + links_ = status.links; + + if (level_ == DiagnosticStatus::WARN) level_ = remap_warn_; +} + DebugUnit::DebugUnit(const std::string & path, DiagnosticLevel level) : BaseUnit(path) { level_ = level; // overwrite diff --git a/system/system_diagnostic_graph/src/core/units.hpp b/system/system_diagnostic_graph/src/core/units.hpp index ad5fa4c4bc090..2b94b5f83a216 100644 --- a/system/system_diagnostic_graph/src/core/units.hpp +++ b/system/system_diagnostic_graph/src/core/units.hpp @@ -50,6 +50,7 @@ class BaseUnit virtual ~BaseUnit() = default; virtual void init(const UnitConfig::SharedPtr & config, const NodeDict & dict) = 0; virtual void update(const rclcpp::Time & stamp) = 0; + virtual std::string type() const = 0; NodeData status() const; NodeData report() const; @@ -77,6 +78,7 @@ class DiagUnit : public BaseUnit using BaseUnit::BaseUnit; void init(const UnitConfig::SharedPtr & config, const NodeDict & dict) override; void update(const rclcpp::Time & stamp) override; + std::string type() const override { return "diag"; } std::string name() const { return name_; } void callback(const rclcpp::Time & stamp, const DiagnosticStatus & status); @@ -93,6 +95,7 @@ class AndUnit : public BaseUnit AndUnit(const std::string & path, bool short_circuit); void init(const UnitConfig::SharedPtr & config, const NodeDict & dict) override; void update(const rclcpp::Time & stamp) override; + std::string type() const override { return short_circuit_ ? "short-circuit-and" : "and"; } private: bool short_circuit_; @@ -104,6 +107,19 @@ class OrUnit : public BaseUnit using BaseUnit::BaseUnit; void init(const UnitConfig::SharedPtr & config, const NodeDict & dict) override; void update(const rclcpp::Time & stamp) override; + std::string type() const override { return "or"; } +}; + +class RemapUnit : public BaseUnit +{ +public: + RemapUnit(const std::string & path, DiagnosticLevel remap_warn); + void init(const UnitConfig::SharedPtr & config, const NodeDict & dict) override; + void update(const rclcpp::Time & stamp) override; + std::string type() const override { return "remap"; } + +private: + DiagnosticLevel remap_warn_; }; class DebugUnit : public BaseUnit @@ -112,6 +128,7 @@ class DebugUnit : public BaseUnit DebugUnit(const std::string & path, DiagnosticLevel level); void init(const UnitConfig::SharedPtr & config, const NodeDict & dict) override; void update(const rclcpp::Time & stamp) override; + std::string type() const override { return "const"; } }; } // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/main.cpp b/system/system_diagnostic_graph/src/main.cpp index 7db35d1fd2551..7393d9fb086f6 100644 --- a/system/system_diagnostic_graph/src/main.cpp +++ b/system/system_diagnostic_graph/src/main.cpp @@ -25,13 +25,11 @@ MainNode::MainNode() : Node("system_diagnostic_graph_aggregator") // Init diagnostics graph. { const auto file = declare_parameter("graph_file"); - const auto mode = declare_parameter("mode"); - graph_.init(file, mode); - graph_.debug(); + graph_.init(file); } - // Init plugins - if (declare_parameter("mode_availability")) { + // Init plugins. + if (declare_parameter("use_operation_mode_availability")) { modes_ = std::make_unique(*this, graph_.nodes()); } @@ -48,6 +46,9 @@ MainNode::MainNode() : Node("system_diagnostic_graph_aggregator") const auto rate = rclcpp::Rate(declare_parameter("rate")); timer_ = rclcpp::create_timer(this, get_clock(), rate.period(), [this]() { on_timer(); }); } + + // Init debug mode. + debug_ = declare_parameter("use_debug_mode"); } MainNode::~MainNode() @@ -59,7 +60,7 @@ void MainNode::on_timer() { const auto stamp = now(); pub_graph_->publish(graph_.report(stamp)); - graph_.debug(); + if (debug_) graph_.debug(); if (modes_) modes_->update(stamp); } diff --git a/system/system_diagnostic_graph/src/main.hpp b/system/system_diagnostic_graph/src/main.hpp index 6deb0518cd9d0..495c51218cabc 100644 --- a/system/system_diagnostic_graph/src/main.hpp +++ b/system/system_diagnostic_graph/src/main.hpp @@ -40,6 +40,8 @@ class MainNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_graph_; void on_timer(); void on_diag(const DiagnosticArray::ConstSharedPtr msg); + + bool debug_; }; } // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/tool/main.cpp b/system/system_diagnostic_graph/src/tool/main.cpp new file mode 100644 index 0000000000000..6efb911d8e2d3 --- /dev/null +++ b/system/system_diagnostic_graph/src/tool/main.cpp @@ -0,0 +1,140 @@ +// Copyright 2023 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 "graph.hpp" +#include "types.hpp" +#include "units.hpp" + +#include +#include +#include + +namespace system_diagnostic_graph +{ + +struct GraphNode +{ + using UniquePtr = std::unique_ptr; + std::string type; + std::string path; + std::vector children; + std::vector parents; +}; + +struct GraphRoot +{ + std::vector owner; + std::vector nodes; +}; + +GraphRoot load_graph_nodes(const std::string & path) +{ + GraphRoot result; + { + std::unordered_map mapping; + Graph graph; + graph.init(path); + + for (const auto & node : graph.nodes()) { + auto data = std::make_unique(); + data->path = node->path(); + data->type = node->type(); + mapping[node] = std::move(data); + } + + for (const auto & [node, data] : mapping) { + for (const auto & link : node->children()) { + const auto parent = data.get(); + const auto child = mapping.at(link).get(); + child->parents.push_back(parent); + parent->children.push_back(child); + } + } + + for (auto & [node, data] : mapping) { + result.owner.push_back(std::move(data)); + } + for (const auto & node : result.owner) { + result.nodes.push_back(node.get()); + } + } + return result; +} + +void dump_plantuml_path(const std::string & path) +{ + const auto graph = load_graph_nodes(path); + const auto color = "#FFFFFF"; + + for (const auto & node : graph.nodes) { + std::cout << "card " << node << " " << color << " [" << std::endl; + std::cout << node->path << std::endl; + std::cout << "]" << std::endl; + } + + for (const auto & node : graph.nodes) { + for (const auto & child : node->children) { + std::cout << node << " --> " << child << std::endl; + } + } +} + +void dump_tree_node(const GraphNode * node, const std::string & indent = "", bool root = true) +{ + const auto path = node->path.empty() ? "" : node->path + " "; + const auto type = "(" + node->type + ")"; + std::cout << indent << "- " << path << type << std::endl; + + if (root || node->parents.size() == 1) { + for (const auto child : node->children) { + dump_tree_node(child, indent + " ", false); + } + } +} + +void dump_tree_path(const std::string & path) +{ + const auto graph = load_graph_nodes(path); + + std::cout << "===== root nodes =================================" << std::endl; + for (const auto & node : graph.nodes) { + if (node->parents.size() == 0 && node->children.size() != 0) { + dump_tree_node(node); + } + } + std::cout << "===== intermediate nodes =========================" << std::endl; + for (const auto & node : graph.nodes) { + if (node->parents.size() >= 2) { + dump_tree_node(node); + } + } + + std::cout << "===== isolated nodes =============================" << std::endl; + for (const auto & node : graph.nodes) { + if (node->parents.size() == 0 && node->children.size() == 0) { + dump_tree_node(node); + } + } +} + +} // namespace system_diagnostic_graph + +int main(int argc, char ** argv) +{ + if (argc != 2) { + std::cerr << "usage: " << argv[0] << " " << std::endl; + return 1; + } + system_diagnostic_graph::dump_tree_path(argv[1]); +} diff --git a/system/system_diagnostic_graph/test/files/field-not-found.yaml b/system/system_diagnostic_graph/test/files/test1/field-not-found.yaml similarity index 100% rename from system/system_diagnostic_graph/test/files/field-not-found.yaml rename to system/system_diagnostic_graph/test/files/test1/field-not-found.yaml diff --git a/system/system_diagnostic_graph/test/files/file-not-found.yaml b/system/system_diagnostic_graph/test/files/test1/file-not-found.yaml similarity index 100% rename from system/system_diagnostic_graph/test/files/file-not-found.yaml rename to system/system_diagnostic_graph/test/files/test1/file-not-found.yaml diff --git a/system/system_diagnostic_graph/test/files/graph-circulation.yaml b/system/system_diagnostic_graph/test/files/test1/graph-circulation.yaml similarity index 100% rename from system/system_diagnostic_graph/test/files/graph-circulation.yaml rename to system/system_diagnostic_graph/test/files/test1/graph-circulation.yaml diff --git a/system/system_diagnostic_graph/test/files/invalid-dict-type.yaml b/system/system_diagnostic_graph/test/files/test1/invalid-dict-type.yaml similarity index 100% rename from system/system_diagnostic_graph/test/files/invalid-dict-type.yaml rename to system/system_diagnostic_graph/test/files/test1/invalid-dict-type.yaml diff --git a/system/system_diagnostic_graph/test/files/invalid-list-type.yaml b/system/system_diagnostic_graph/test/files/test1/invalid-list-type.yaml similarity index 100% rename from system/system_diagnostic_graph/test/files/invalid-list-type.yaml rename to system/system_diagnostic_graph/test/files/test1/invalid-list-type.yaml diff --git a/system/system_diagnostic_graph/test/files/path-conflict.yaml b/system/system_diagnostic_graph/test/files/test1/path-conflict.yaml similarity index 100% rename from system/system_diagnostic_graph/test/files/path-conflict.yaml rename to system/system_diagnostic_graph/test/files/test1/path-conflict.yaml diff --git a/system/system_diagnostic_graph/test/files/path-not-found.yaml b/system/system_diagnostic_graph/test/files/test1/path-not-found.yaml similarity index 100% rename from system/system_diagnostic_graph/test/files/path-not-found.yaml rename to system/system_diagnostic_graph/test/files/test1/path-not-found.yaml diff --git a/system/system_diagnostic_graph/test/files/unknown-node-type.yaml b/system/system_diagnostic_graph/test/files/test1/unknown-node-type.yaml similarity index 100% rename from system/system_diagnostic_graph/test/files/unknown-node-type.yaml rename to system/system_diagnostic_graph/test/files/test1/unknown-node-type.yaml diff --git a/system/system_diagnostic_graph/test/files/unknown-substitution.yaml b/system/system_diagnostic_graph/test/files/test1/unknown-substitution.yaml similarity index 100% rename from system/system_diagnostic_graph/test/files/unknown-substitution.yaml rename to system/system_diagnostic_graph/test/files/test1/unknown-substitution.yaml diff --git a/system/system_diagnostic_graph/test/files/test2/and.yaml b/system/system_diagnostic_graph/test/files/test2/and.yaml new file mode 100644 index 0000000000000..0c9b651f89b2e --- /dev/null +++ b/system/system_diagnostic_graph/test/files/test2/and.yaml @@ -0,0 +1,10 @@ +nodes: + - path: output + type: and + list: + - path: input-0 + type: diag + diag: "test: input-0" + - path: input-1 + type: diag + diag: "test: input-1" diff --git a/system/system_diagnostic_graph/test/files/test2/or.yaml b/system/system_diagnostic_graph/test/files/test2/or.yaml new file mode 100644 index 0000000000000..c7f37a6c32064 --- /dev/null +++ b/system/system_diagnostic_graph/test/files/test2/or.yaml @@ -0,0 +1,10 @@ +nodes: + - path: output + type: or + list: + - path: input-0 + type: diag + diag: "test: input-0" + - path: input-1 + type: diag + diag: "test: input-1" diff --git a/system/system_diagnostic_graph/test/files/test2/warn-to-error.yaml b/system/system_diagnostic_graph/test/files/test2/warn-to-error.yaml new file mode 100644 index 0000000000000..816bf4fd6e1bf --- /dev/null +++ b/system/system_diagnostic_graph/test/files/test2/warn-to-error.yaml @@ -0,0 +1,7 @@ +nodes: + - path: output + type: warn-to-error + list: + - path: input-0 + type: diag + diag: "test: input-0" diff --git a/system/system_diagnostic_graph/test/files/test2/warn-to-ok.yaml b/system/system_diagnostic_graph/test/files/test2/warn-to-ok.yaml new file mode 100644 index 0000000000000..1f5cf18978e3c --- /dev/null +++ b/system/system_diagnostic_graph/test/files/test2/warn-to-ok.yaml @@ -0,0 +1,7 @@ +nodes: + - path: output + type: warn-to-ok + list: + - path: input-0 + type: diag + diag: "test: input-0" diff --git a/system/system_diagnostic_graph/test/src/test.cpp b/system/system_diagnostic_graph/test/src/test1.cpp similarity index 57% rename from system/system_diagnostic_graph/test/src/test.cpp rename to system/system_diagnostic_graph/test/src/test1.cpp index b763179be0791..d01822e93c2eb 100644 --- a/system/system_diagnostic_graph/test/src/test.cpp +++ b/system/system_diagnostic_graph/test/src/test1.cpp @@ -14,75 +14,68 @@ #include "core/error.hpp" #include "core/graph.hpp" +#include "utils.hpp" #include -#include -#include - using namespace system_diagnostic_graph; // NOLINT(build/namespaces) -std::filesystem::path resource(const std::string & path) -{ - return std::filesystem::path(TEST_RESOURCE_PATH) / path; -} - TEST(ConfigFile, RootNotFound) { Graph graph; - EXPECT_THROW(graph.init(resource("fake-file-name.yaml")), FileNotFound); + EXPECT_THROW(graph.init(resource("test1/fake-file-name.yaml")), FileNotFound); } TEST(ConfigFile, FileNotFound) { Graph graph; - EXPECT_THROW(graph.init(resource("file-not-found.yaml")), FileNotFound); + EXPECT_THROW(graph.init(resource("test1/file-not-found.yaml")), FileNotFound); } TEST(ConfigFile, UnknownSubstitution) { Graph graph; - EXPECT_THROW(graph.init(resource("unknown-substitution.yaml")), UnknownType); + EXPECT_THROW(graph.init(resource("test1/unknown-substitution.yaml")), UnknownType); } TEST(ConfigFile, UnknownNodeType) { Graph graph; - EXPECT_THROW(graph.init(resource("unknown-node-type.yaml")), UnknownType); + EXPECT_THROW(graph.init(resource("test1/unknown-node-type.yaml")), UnknownType); } TEST(ConfigFile, InvalidDictType) { Graph graph; - EXPECT_THROW(graph.init(resource("invalid-dict-type.yaml")), InvalidType); + EXPECT_THROW(graph.init(resource("test1/invalid-dict-type.yaml")), InvalidType); } TEST(ConfigFile, InvalidListType) { Graph graph; - EXPECT_THROW(graph.init(resource("invalid-list-type.yaml")), InvalidType); + EXPECT_THROW(graph.init(resource("test1/invalid-list-type.yaml")), InvalidType); } TEST(ConfigFile, FieldNotFound) { Graph graph; - EXPECT_THROW(graph.init(resource("field-not-found.yaml")), FieldNotFound); + EXPECT_THROW(graph.init(resource("test1/field-not-found.yaml")), FieldNotFound); } TEST(ConfigFile, PathConflict) { Graph graph; - EXPECT_THROW(graph.init(resource("path-conflict.yaml")), PathConflict); + EXPECT_THROW(graph.init(resource("test1/path-conflict.yaml")), PathConflict); } TEST(ConfigFile, PathNotFound) { Graph graph; - EXPECT_THROW(graph.init(resource("path-not-found.yaml")), PathNotFound); + EXPECT_THROW(graph.init(resource("test1/path-not-found.yaml")), PathNotFound); } TEST(ConfigFile, GraphCirculation) { Graph graph; - EXPECT_THROW(graph.init(resource("graph-circulation.yaml")), GraphStructure); + EXPECT_THROW(graph.init(resource("test1/graph-circulation.yaml")), GraphStructure); } diff --git a/system/system_diagnostic_graph/test/src/test2.cpp b/system/system_diagnostic_graph/test/src/test2.cpp new file mode 100644 index 0000000000000..3e4c946f73fb7 --- /dev/null +++ b/system/system_diagnostic_graph/test/src/test2.cpp @@ -0,0 +1,143 @@ +// Copyright 2023 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 "core/error.hpp" +#include "core/graph.hpp" +#include "utils.hpp" + +#include +#include +#include + +#include + +using namespace system_diagnostic_graph; // NOLINT(build/namespaces) + +using diagnostic_msgs::msg::DiagnosticArray; +using diagnostic_msgs::msg::DiagnosticStatus; +using tier4_system_msgs::msg::DiagnosticGraph; + +constexpr auto OK = DiagnosticStatus::OK; +constexpr auto WARN = DiagnosticStatus::WARN; +constexpr auto ERROR = DiagnosticStatus::ERROR; +constexpr auto STALE = DiagnosticStatus::STALE; + +struct GraphTestParam +{ + std::string config; + std::vector inputs; + uint8_t result; +}; + +class GraphTest : public testing::TestWithParam +{ +}; + +DiagnosticArray create_input(const std::vector & levels) +{ + DiagnosticArray array; + for (size_t i = 0; i < levels.size(); ++i) { + DiagnosticStatus status; + status.level = levels[i]; + status.name = "test: input-" + std::to_string(i); + array.status.push_back(status); + } + return array; +}; + +uint8_t get_output(const DiagnosticGraph & graph) +{ + for (const auto & node : graph.nodes) { + if (node.status.name == "output") { + return node.status.level; + } + } + throw std::runtime_error("output node is not found"); +} + +TEST_P(GraphTest, Aggregation) +{ + const auto param = GetParam(); + const auto stamp = rclcpp::Clock().now(); + Graph graph; + graph.init(resource(param.config)); + graph.callback(stamp, create_input(param.inputs)); + + const auto output = get_output(graph.report(stamp)); + EXPECT_EQ(output, param.result); +} + +// clang-format off + +INSTANTIATE_TEST_SUITE_P(And, GraphTest, + testing::Values( + GraphTestParam{"test2/and.yaml", {OK, OK }, OK }, + GraphTestParam{"test2/and.yaml", {OK, WARN }, WARN }, + GraphTestParam{"test2/and.yaml", {OK, ERROR}, ERROR}, + GraphTestParam{"test2/and.yaml", {OK, STALE}, ERROR}, + GraphTestParam{"test2/and.yaml", {WARN, OK }, WARN }, + GraphTestParam{"test2/and.yaml", {WARN, WARN }, WARN }, + GraphTestParam{"test2/and.yaml", {WARN, ERROR}, ERROR}, + GraphTestParam{"test2/and.yaml", {WARN, STALE}, ERROR}, + GraphTestParam{"test2/and.yaml", {ERROR, OK }, ERROR}, + GraphTestParam{"test2/and.yaml", {ERROR, WARN }, ERROR}, + GraphTestParam{"test2/and.yaml", {ERROR, ERROR}, ERROR}, + GraphTestParam{"test2/and.yaml", {ERROR, STALE}, ERROR}, + GraphTestParam{"test2/and.yaml", {STALE, OK }, ERROR}, + GraphTestParam{"test2/and.yaml", {STALE, WARN }, ERROR}, + GraphTestParam{"test2/and.yaml", {STALE, ERROR}, ERROR}, + GraphTestParam{"test2/and.yaml", {STALE, STALE}, ERROR} + ) +); + +INSTANTIATE_TEST_SUITE_P(Or, GraphTest, + testing::Values( + GraphTestParam{"test2/or.yaml", {OK, OK }, OK }, + GraphTestParam{"test2/or.yaml", {OK, WARN }, OK }, + GraphTestParam{"test2/or.yaml", {OK, ERROR}, OK }, + GraphTestParam{"test2/or.yaml", {OK, STALE}, OK }, + GraphTestParam{"test2/or.yaml", {WARN, OK }, OK }, + GraphTestParam{"test2/or.yaml", {WARN, WARN }, WARN }, + GraphTestParam{"test2/or.yaml", {WARN, ERROR}, WARN }, + GraphTestParam{"test2/or.yaml", {WARN, STALE}, WARN }, + GraphTestParam{"test2/or.yaml", {ERROR, OK }, OK }, + GraphTestParam{"test2/or.yaml", {ERROR, WARN }, WARN }, + GraphTestParam{"test2/or.yaml", {ERROR, ERROR}, ERROR}, + GraphTestParam{"test2/or.yaml", {ERROR, STALE}, ERROR}, + GraphTestParam{"test2/or.yaml", {STALE, OK }, OK }, + GraphTestParam{"test2/or.yaml", {STALE, WARN }, WARN }, + GraphTestParam{"test2/or.yaml", {STALE, ERROR}, ERROR}, + GraphTestParam{"test2/or.yaml", {STALE, STALE}, ERROR} + ) +); + +INSTANTIATE_TEST_SUITE_P(WarnToOk, GraphTest, + testing::Values( + GraphTestParam{"test2/warn-to-ok.yaml", {OK }, OK }, + GraphTestParam{"test2/warn-to-ok.yaml", {WARN }, OK}, + GraphTestParam{"test2/warn-to-ok.yaml", {ERROR}, ERROR}, + GraphTestParam{"test2/warn-to-ok.yaml", {STALE}, STALE} + ) +); + +INSTANTIATE_TEST_SUITE_P(WarnToError, GraphTest, + testing::Values( + GraphTestParam{"test2/warn-to-error.yaml", {OK }, OK }, + GraphTestParam{"test2/warn-to-error.yaml", {WARN }, ERROR}, + GraphTestParam{"test2/warn-to-error.yaml", {ERROR}, ERROR}, + GraphTestParam{"test2/warn-to-error.yaml", {STALE}, STALE} + ) +); + +// clang-format on diff --git a/system/system_diagnostic_graph/test/src/utils.cpp b/system/system_diagnostic_graph/test/src/utils.cpp new file mode 100644 index 0000000000000..c64812afa649a --- /dev/null +++ b/system/system_diagnostic_graph/test/src/utils.cpp @@ -0,0 +1,20 @@ +// Copyright 2023 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 "utils.hpp" + +std::filesystem::path resource(const std::string & path) +{ + return std::filesystem::path(TEST_RESOURCE_PATH) / path; +} diff --git a/system/system_diagnostic_graph/test/src/utils.hpp b/system/system_diagnostic_graph/test/src/utils.hpp new file mode 100644 index 0000000000000..27f0b293d72dc --- /dev/null +++ b/system/system_diagnostic_graph/test/src/utils.hpp @@ -0,0 +1,23 @@ +// Copyright 2023 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 UTILS_HPP_ +#define UTILS_HPP_ + +#include +#include + +std::filesystem::path resource(const std::string & path); + +#endif // UTILS_HPP_