From 62bc9e1f996c20aac1c8e72926ef20819bf32169 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Wed, 25 Oct 2023 05:42:15 +0900 Subject: [PATCH 01/12] replace with new class Signed-off-by: Takagi, Isamu --- system/system_diagnostic_graph/CMakeLists.txt | 3 +- .../example/example_0.yaml | 8 +- .../example/example_1.yaml | 6 +- .../src/{core => back}/exprs.cpp | 0 .../src/{core => back}/exprs.hpp | 6 +- .../src/{core => back}/nodes.cpp | 0 .../src/{core => back}/nodes.hpp | 6 +- .../src/core/config.cpp | 285 +++++++++--------- .../src/core/config.hpp | 81 ++--- .../src/core/debug.cpp | 5 +- .../src/core/error.hpp | 30 ++ .../src/core/graph.cpp | 117 +++---- .../src/core/graph.hpp | 7 +- .../src/core/modes.cpp | 13 +- .../src/core/modes.hpp | 16 +- .../src/core/types.hpp | 9 +- .../src/core/units.cpp | 87 ++++++ .../src/core/units.hpp | 115 +++++++ system/system_diagnostic_graph/src/main.cpp | 1 + 19 files changed, 494 insertions(+), 301 deletions(-) rename system/system_diagnostic_graph/src/{core => back}/exprs.cpp (100%) rename system/system_diagnostic_graph/src/{core => back}/exprs.hpp (96%) rename system/system_diagnostic_graph/src/{core => back}/nodes.cpp (100%) rename system/system_diagnostic_graph/src/{core => back}/nodes.hpp (97%) create mode 100644 system/system_diagnostic_graph/src/core/error.hpp create mode 100644 system/system_diagnostic_graph/src/core/units.cpp create mode 100644 system/system_diagnostic_graph/src/core/units.hpp diff --git a/system/system_diagnostic_graph/CMakeLists.txt b/system/system_diagnostic_graph/CMakeLists.txt index 81d8ba39b3d1b..1166f441890f5 100644 --- a/system/system_diagnostic_graph/CMakeLists.txt +++ b/system/system_diagnostic_graph/CMakeLists.txt @@ -8,8 +8,7 @@ ament_auto_add_executable(aggregator src/core/config.cpp src/core/debug.cpp src/core/graph.cpp - src/core/nodes.cpp - src/core/exprs.cpp + src/core/units.cpp src/core/modes.cpp src/main.cpp ) diff --git a/system/system_diagnostic_graph/example/example_0.yaml b/system/system_diagnostic_graph/example/example_0.yaml index 0ee6e59c8e121..cdfef990fd31c 100644 --- a/system/system_diagnostic_graph/example/example_0.yaml +++ b/system/system_diagnostic_graph/example/example_0.yaml @@ -9,18 +9,18 @@ nodes: - path: /autoware/modes/autonomous type: and list: - - { type: link, path: /functions/pose_estimation } - - { type: link, path: /functions/obstacle_detection } + - { type: link, link: /functions/pose_estimation } + - { type: link, link: /functions/obstacle_detection } - path: /autoware/modes/local type: and list: - - { type: link, path: /external/joystick_command } + - { type: link, link: /external/joystick_command } - path: /autoware/modes/remote type: and list: - - { type: link, path: /external/remote_command } + - { type: link, link: /external/remote_command } - path: /autoware/modes/emergency-stop type: debug-ok diff --git a/system/system_diagnostic_graph/example/example_1.yaml b/system/system_diagnostic_graph/example/example_1.yaml index 5ba93ec3059e4..8851bc15b93fd 100644 --- a/system/system_diagnostic_graph/example/example_1.yaml +++ b/system/system_diagnostic_graph/example/example_1.yaml @@ -2,13 +2,13 @@ nodes: - path: /functions/pose_estimation type: and list: - - { type: link, path: /sensing/lidars/top } + - { type: link, link: /sensing/lidars/top } - path: /functions/obstacle_detection type: or list: - - { type: link, path: /sensing/lidars/front } - - { type: link, path: /sensing/radars/front } + - { type: link, link: /sensing/lidars/front } + - { type: link, link: /sensing/radars/front } - path: /sensing/lidars/top type: diag diff --git a/system/system_diagnostic_graph/src/core/exprs.cpp b/system/system_diagnostic_graph/src/back/exprs.cpp similarity index 100% rename from system/system_diagnostic_graph/src/core/exprs.cpp rename to system/system_diagnostic_graph/src/back/exprs.cpp diff --git a/system/system_diagnostic_graph/src/core/exprs.hpp b/system/system_diagnostic_graph/src/back/exprs.hpp similarity index 96% rename from system/system_diagnostic_graph/src/core/exprs.hpp rename to system/system_diagnostic_graph/src/back/exprs.hpp index f582e019d5691..9775ea4cd870e 100644 --- a/system/system_diagnostic_graph/src/core/exprs.hpp +++ b/system/system_diagnostic_graph/src/back/exprs.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef CORE__EXPRS_HPP_ -#define CORE__EXPRS_HPP_ +#ifndef BACK__EXPRS_HPP_ +#define BACK__EXPRS_HPP_ #include "config.hpp" #include "types.hpp" @@ -101,4 +101,4 @@ class ExprInit } // namespace system_diagnostic_graph -#endif // CORE__EXPRS_HPP_ +#endif // BACK__EXPRS_HPP_ diff --git a/system/system_diagnostic_graph/src/core/nodes.cpp b/system/system_diagnostic_graph/src/back/nodes.cpp similarity index 100% rename from system/system_diagnostic_graph/src/core/nodes.cpp rename to system/system_diagnostic_graph/src/back/nodes.cpp diff --git a/system/system_diagnostic_graph/src/core/nodes.hpp b/system/system_diagnostic_graph/src/back/nodes.hpp similarity index 97% rename from system/system_diagnostic_graph/src/core/nodes.hpp rename to system/system_diagnostic_graph/src/back/nodes.hpp index 1a849cf58b268..9c4fb8141bed0 100644 --- a/system/system_diagnostic_graph/src/core/nodes.hpp +++ b/system/system_diagnostic_graph/src/back/nodes.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef CORE__NODES_HPP_ -#define CORE__NODES_HPP_ +#ifndef BACK__NODES_HPP_ +#define BACK__NODES_HPP_ #include "config.hpp" #include "debug.hpp" @@ -111,4 +111,4 @@ class UnknownNode : public BaseNode } // namespace system_diagnostic_graph -#endif // CORE__NODES_HPP_ +#endif // BACK__NODES_HPP_ diff --git a/system/system_diagnostic_graph/src/core/config.cpp b/system/system_diagnostic_graph/src/core/config.cpp index 305860af32840..8b419dfcc1c22 100644 --- a/system/system_diagnostic_graph/src/core/config.cpp +++ b/system/system_diagnostic_graph/src/core/config.cpp @@ -14,11 +14,14 @@ #include "config.hpp" +#include "error.hpp" + #include #include #include #include +#include #include // DEBUG @@ -27,95 +30,63 @@ namespace system_diagnostic_graph { -ErrorMarker::ErrorMarker(const std::string & file) -{ - file_ = file; -} - -std::string ErrorMarker::str() const -{ - if (type_.empty()) { - return file_; - } - - std::string result = type_; - for (const auto & index : indices_) { - result += "-" + std::to_string(index); - } - return result + " in " + file_; -} - -ErrorMarker ErrorMarker::type(const std::string & type) const -{ - ErrorMarker mark = *this; - mark.type_ = type; - return mark; -} - -ErrorMarker ErrorMarker::index(size_t index) const -{ - ErrorMarker mark = *this; - mark.indices_.push_back(index); - return mark; -} - -ConfigObject::ConfigObject(const ErrorMarker & mark, YAML::Node yaml, const std::string & type) +ConfigData ConfigData::load(YAML::Node yaml) { if (!yaml.IsMap()) { - throw create_error(mark, type + " is not a dict type"); + throw ConfigError(std::string("TODO") + " is not a dict type"); } for (const auto & kv : yaml) { - dict_[kv.first.as()] = kv.second; + object[kv.first.as()] = kv.second; } - mark_ = mark; - type_ = type; + return *this; } -ErrorMarker ConfigObject::mark() const +ConfigData ConfigData::type(const std::string & name) const { - return mark_; + ConfigData data; + data.file = file; + data.mark = name; + return data; } -std::optional ConfigObject::take_yaml(const std::string & name) +ConfigData ConfigData::node(const size_t index) const { - if (!dict_.count(name)) { - return std::nullopt; - } - const auto yaml = dict_.at(name); - dict_.erase(name); - return yaml; + ConfigData data; + data.file = file; + data.mark = mark + "-" + std::to_string(index); + return data; } -std::string ConfigObject::take_text(const std::string & name) +std::string ConfigData::take_text(const std::string & name) { - if (!dict_.count(name)) { - throw create_error(mark_, "object has no '" + name + "' field"); + if (!object.count(name)) { + throw ConfigError("object has no '" + name + "' field"); } - const auto yaml = dict_.at(name); - dict_.erase(name); + const auto yaml = object.at(name); + object.erase(name); return yaml.as(); } -std::string ConfigObject::take_text(const std::string & name, const std::string & fail) +std::string ConfigData::take_text(const std::string & name, const std::string & fail) { - if (!dict_.count(name)) { + if (!object.count(name)) { return fail; } - const auto yaml = dict_.at(name); - dict_.erase(name); + const auto yaml = object.at(name); + object.erase(name); return yaml.as(); } -std::vector ConfigObject::take_list(const std::string & name) +std::vector ConfigData::take_list(const std::string & name) { - if (!dict_.count(name)) { + if (!object.count(name)) { return std::vector(); } - const auto yaml = dict_.at(name); - dict_.erase(name); + const auto yaml = object.at(name); + object.erase(name); if (!yaml.IsSequence()) { throw ConfigError("the '" + name + "' field is not a list type"); @@ -123,139 +94,157 @@ std::vector ConfigObject::take_list(const std::string & name) return std::vector(yaml.begin(), yaml.end()); } -bool ConfigFilter::check(const std::string & mode) const +template +void extend(std::vector & u, const std::vector & v) { - if (!excludes.empty() && excludes.count(mode) != 0) return false; - if (!includes.empty() && includes.count(mode) == 0) return false; - return true; + u.insert(u.end(), v.begin(), v.end()); } -ConfigError create_error(const ErrorMarker & mark, const std::string & message) +template +auto enumerate(const std::vector & v) { - (void)mark; - return ConfigError(message); + std::vector> result; + for (size_t i = 0; i < v.size(); ++i) { + result.push_back(std::make_pair(i, v[i])); + } + return result; } -ConfigFilter parse_mode_filter(const ErrorMarker & mark, std::optional yaml) +UnitConfig::SharedPtr parse_node_config(const ConfigData & data) { - std::unordered_set excludes; - std::unordered_set includes; - if (yaml) { - ConfigObject dict(mark, yaml.value(), "mode filter"); + const auto node = std::make_shared(); + node->data = data; + node->path = node->data.take_text("path", ""); + node->type = node->data.take_text("type"); - for (const auto & mode : dict.take_list("except")) { - excludes.emplace(mode.as()); - } - for (const auto & mode : dict.take_list("only")) { - includes.emplace(mode.as()); - } + for (const auto & [index, yaml] : enumerate(node->data.take_list("list"))) { + const auto child = data.node(index).load(yaml); + node->children.push_back(parse_node_config(child)); } - return ConfigFilter{excludes, includes}; + return node; } -FileConfig parse_file_config(const ErrorMarker & mark, YAML::Node yaml) +FileConfig::SharedPtr parse_file_config(const ConfigData & data) { - ConfigObject dict(mark, yaml, "file object"); - const auto relative_path = dict.take_text("path"); - const auto package_name = dict.take_text("package"); - const auto package_path = ament_index_cpp::get_package_share_directory(package_name); - return FileConfig{mark, package_path + "/" + relative_path}; + const auto file = std::make_shared(); + file->data = data; + file->package_name = file->data.take_text("package"); + file->package_path = file->data.take_text("path"); + return file; } -NodeConfig parse_node_config(const ErrorMarker & mark, YAML::Node yaml) +void dump_node(const UnitConfig & config, const std::string & indent = "") { - ConfigObject dict(mark, yaml, "node object"); - const auto path = dict.take_text("path"); - const auto mode = parse_mode_filter(dict.mark(), dict.take_yaml("mode")); - return NodeConfig{path, mode, dict}; + std::cout << indent << " - node: " << config.type << " (" << config.path << ")" << std::endl; + for (const auto & child : config.children) dump_node(*child, indent + " "); } -ExprConfig parse_expr_config(const ErrorMarker & mark, YAML::Node yaml) +void dump_file(const FileConfig & config) { - ConfigObject dict(mark, yaml, "expr object"); - return parse_expr_config(dict); + std::cout << "=================================================================" << std::endl; + std::cout << config.path << std::endl; + for (const auto & file : config.files) { + std::cout << " - file: " << file->package_name << "/" << file->package_path << std::endl; + } + for (const auto & node : config.nodes) { + dump_node(*node); + } } -ExprConfig parse_expr_config(ConfigObject & dict) +void load_config_file(FileConfig & config, const ConfigData & parent) { - const auto type = dict.take_text("type"); - const auto mode = parse_mode_filter(dict.mark(), dict.take_yaml("mode")); - return ExprConfig{type, mode, dict}; -} + if (config.path.empty()) { + const auto package_base = ament_index_cpp::get_package_share_directory(config.package_name); + config.path = package_base + "/" + config.package_path; + } -void dump(const ConfigFile & config) -{ - std::cout << "=================================================================" << std::endl; - std::cout << config.mark.str() << std::endl; - for (const auto & file : config.files) { - std::cout << " - f: " << file.path << " (" << file.mark.str() << ")" << std::endl; + if (!std::filesystem::exists(config.path)) { + (void)parent; + throw ConfigError("TODO"); } - for (const auto & unit : config.units) { - std::cout << " - u: " << unit.path << " (" << unit.dict.mark().str() << ")" << std::endl; + + ConfigData data; + data.file = config.path.empty() ? config.package_path + "/" + config.package_path : "root"; + data.load(YAML::LoadFile(config.path)); + const auto files = data.take_list("files"); + const auto nodes = data.take_list("nodes"); + + for (const auto & [index, yaml] : enumerate(files)) { + const auto file = data.type("file").node(index).load(yaml); + config.files.push_back(parse_file_config(file)); } - for (const auto & diag : config.diags) { - std::cout << " - d: " << diag.path << " (" << diag.dict.mark().str() << ")" << std::endl; + for (const auto & [index, yaml] : enumerate(nodes)) { + const auto node = data.type("file").node(index).load(yaml); + config.nodes.push_back(parse_node_config(node)); } } -template -auto apply(const ErrorMarker & mark, F & func, const std::vector & list) +void check_config_nodes(const std::vector & nodes) { - std::vector result; - for (size_t i = 0; i < list.size(); ++i) { - result.push_back(func(mark.index(i), list[i])); + std::unordered_map path_count; + for (const auto & node : nodes) { + path_count[node->path] += 1; + } + + path_count.erase(""); + for (const auto & [path, count] : path_count) { + if (1 < count) { + throw ConfigError("TODO: path conflict"); + } } - return result; } -ConfigFile load_config_file(const FileConfig & file) +void resolve_link_nodes(std::vector & nodes) { - if (!std::filesystem::exists(file.path)) { - throw create_error(file.mark, "config file '" + file.path + "' does not exist"); - } + std::vector filtered; + std::unordered_map links; + std::unordered_map paths; - const auto yaml = YAML::LoadFile(file.path); - const auto mark = ErrorMarker(file.path); - auto dict = ConfigObject(mark, yaml, "config file"); + for (const auto & node : nodes) { + links[node] = node; + paths[node->path] = node; + } - std::vector units; - std::vector diags; - for (const auto & node : dict.take_list("nodes")) { - const auto type = node["type"].as(); - if (type == "diag") { - diags.push_back(node); + for (const auto & node : nodes) { + if (node->type == "link" && node->path == "") { + links[node] = paths.at(node->data.take_text("link")); } else { - units.push_back(node); + filtered.push_back(node); } } + nodes = filtered; - ConfigFile config(mark); - config.files = apply(mark.type("file"), parse_file_config, dict.take_list("files")); - config.units = apply(mark.type("unit"), parse_node_config, units); - config.diags = apply(mark.type("diag"), parse_node_config, diags); - return config; + for (const auto & node : nodes) { + for (auto & child : node->children) { + child = links.at(child); + } + } } -ConfigFile load_config_root(const std::string & path) +RootConfig load_config_root(const std::string & path) { - const auto mark = ErrorMarker("root file"); - std::vector configs; - configs.push_back(load_config_file(FileConfig{mark, path})); - - // Use an index because updating the vector invalidates the iterator. - for (size_t i = 0; i < configs.size(); ++i) { - for (const auto & file : configs[i].files) { - configs.push_back(load_config_file(file)); - } + RootConfig config; + config.files.push_back(std::make_shared()); + config.files.back()->path = path; + + for (size_t i = 0; i < config.files.size(); ++i) { + const auto & file = config.files[i]; + load_config_file(*file, {}); + extend(config.files, file->files); } - ConfigFile result(mark); - for (const auto & config : configs) { - result.files.insert(result.files.end(), config.files.begin(), config.files.end()); - result.units.insert(result.units.end(), config.units.begin(), config.units.end()); - result.diags.insert(result.diags.end(), config.diags.begin(), config.diags.end()); + for (const auto & file : config.files) { + extend(config.nodes, file->nodes); } - return result; + + for (size_t i = 0; i < config.nodes.size(); ++i) { + const auto & node = config.nodes[i]; + extend(config.nodes, node->children); + } + + check_config_nodes(config.nodes); + resolve_link_nodes(config.nodes); + return config; } } // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/config.hpp b/system/system_diagnostic_graph/src/core/config.hpp index 4d679ef575944..e9f9165a08c12 100644 --- a/system/system_diagnostic_graph/src/core/config.hpp +++ b/system/system_diagnostic_graph/src/core/config.hpp @@ -18,94 +18,55 @@ #include #include -#include -#include #include #include -#include #include namespace system_diagnostic_graph { -struct ConfigError : public std::runtime_error +struct ConfigData { - using runtime_error::runtime_error; -}; - -class ErrorMarker -{ -public: - explicit ErrorMarker(const std::string & file = ""); - std::string str() const; - ErrorMarker type(const std::string & type) const; - ErrorMarker index(size_t index) const; + ConfigData load(YAML::Node yaml); + ConfigData type(const std::string & name) const; + ConfigData node(const size_t index) const; -private: - std::string file_; - std::string type_; - std::vector indices_; -}; - -class ConfigObject -{ -public: - ConfigObject(const ErrorMarker & mark, YAML::Node yaml, const std::string & type); - ErrorMarker mark() const; - 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); -private: - ErrorMarker mark_; - std::string type_; - std::unordered_map dict_; -}; - -struct ConfigFilter -{ - bool check(const std::string & mode) const; - std::unordered_set excludes; - std::unordered_set includes; + std::string file; + std::string mark; + std::unordered_map object; }; -struct ExprConfig +struct UnitConfig { + using SharedPtr = std::shared_ptr; std::string type; - ConfigFilter mode; - ConfigObject dict; -}; - -struct NodeConfig -{ std::string path; - ConfigFilter mode; - ConfigObject dict; + std::vector children; + ConfigData data; }; struct FileConfig { - ErrorMarker mark; + using SharedPtr = std::shared_ptr; + std::string package_name; + std::string package_path; std::string path; + std::vector files; + std::vector nodes; + ConfigData data; }; -struct ConfigFile +struct RootConfig { - explicit ConfigFile(const ErrorMarker & mark) : mark(mark) {} - ErrorMarker mark; - std::vector files; - std::vector units; - std::vector diags; + std::vector files; + std::vector nodes; }; -using ConfigDict = std::unordered_map; - -ConfigError create_error(const ErrorMarker & mark, const std::string & message); -ConfigFile load_config_root(const std::string & path); - -ExprConfig parse_expr_config(const ErrorMarker & mark, YAML::Node yaml); -ExprConfig parse_expr_config(ConfigObject & dict); +RootConfig load_config_root(const std::string & path); } // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/debug.cpp b/system/system_diagnostic_graph/src/core/debug.cpp index ed696573521a7..188eb75ea9645 100644 --- a/system/system_diagnostic_graph/src/core/debug.cpp +++ b/system/system_diagnostic_graph/src/core/debug.cpp @@ -15,7 +15,6 @@ #include "debug.hpp" #include "graph.hpp" -#include "nodes.hpp" #include "types.hpp" #include @@ -34,6 +33,7 @@ const std::unordered_map level_names = { void Graph::debug() { + /* std::vector lines; for (const auto & node : nodes_) { lines.push_back(node->debug()); @@ -55,8 +55,10 @@ void Graph::debug() } std::cout << "|" << std::endl; } + */ } +/* DiagDebugData UnitNode::debug() const { const auto level_name = level_names.at(level()); @@ -77,5 +79,6 @@ DiagDebugData UnknownNode::debug() const const auto index_name = std::to_string(index()); return {"test", index_name, level_name, path_, "-----"}; } +*/ } // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/error.hpp b/system/system_diagnostic_graph/src/core/error.hpp new file mode 100644 index 0000000000000..1aa4b88eb91f8 --- /dev/null +++ b/system/system_diagnostic_graph/src/core/error.hpp @@ -0,0 +1,30 @@ +// 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 CORE__ERROR_HPP_ +#define CORE__ERROR_HPP_ + +#include + +namespace system_diagnostic_graph +{ + +struct ConfigError : public std::runtime_error +{ + using runtime_error::runtime_error; +}; + +} // namespace system_diagnostic_graph + +#endif // CORE__ERROR_HPP_ diff --git a/system/system_diagnostic_graph/src/core/graph.cpp b/system/system_diagnostic_graph/src/core/graph.cpp index b4fd1d15827f3..0806219c22240 100644 --- a/system/system_diagnostic_graph/src/core/graph.cpp +++ b/system/system_diagnostic_graph/src/core/graph.cpp @@ -15,8 +15,8 @@ #include "graph.hpp" #include "config.hpp" -#include "exprs.hpp" -#include "nodes.hpp" +#include "error.hpp" +#include "units.hpp" #include #include @@ -31,8 +31,11 @@ namespace system_diagnostic_graph { -void topological_sort(std::vector> & input) +BaseUnit::UniquePtrList topological_sort(BaseUnit::UniquePtrList && input) { + return std::move(input); + + /* std::unordered_map degrees; std::deque nodes; std::deque result; @@ -87,6 +90,39 @@ void topological_sort(std::vector> & input) sorted[indices[node.get()]] = std::move(node); } input = std::move(sorted); + */ +} + +BaseUnit::UniquePtr make_node(const UnitConfig::SharedPtr & config) +{ + if (config->type == "diag") { + return std::make_unique(config->path); + } + if (config->type == "link") { + return std::make_unique(config->path); + } + if (config->type == "and") { + return std::make_unique(config->path, false); + } + if (config->type == "short-circuit-and") { + return std::make_unique(config->path, true); + } + if (config->type == "or") { + return std::make_unique(config->path); + } + if (config->type == "debug-ok") { + return std::make_unique(config->path, DiagnosticStatus::OK); + } + if (config->type == "debug-warn") { + return std::make_unique(config->path, DiagnosticStatus::WARN); + } + if (config->type == "debug-error") { + return std::make_unique(config->path, DiagnosticStatus::ERROR); + } + if (config->type == "debug-stale") { + return std::make_unique(config->path, DiagnosticStatus::STALE); + } + throw ConfigError("unknown node type: " + config->type + " " + "TODO"); } Graph::Graph() @@ -101,63 +137,39 @@ Graph::~Graph() void Graph::init(const std::string & file, const std::string & mode) { - ConfigFile root = load_config_root(file); + (void)mode; // TODO(Takagi, Isamu) - std::vector> nodes; - std::unordered_map diags; - std::unordered_map paths; - ExprInit exprs(mode); + BaseUnit::UniquePtrList nodes; + BaseUnit::NodeDict dict; - for (auto & config : root.units) { - if (config.mode.check(mode)) { - auto node = std::make_unique(config.path); - nodes.push_back(std::move(node)); - } - } - for (auto & config : root.diags) { - if (config.mode.check(mode)) { - auto node = std::make_unique(config.path, config.dict); - diags[node->name()] = node.get(); - nodes.push_back(std::move(node)); - } + for (const auto & config : load_config_root(file).nodes) { + const auto node = nodes.emplace_back(make_node(config)).get(); + dict.configs[config] = node; + dict.paths[config->path] = node; } + dict.paths.erase(""); - // TODO(Takagi, Isamu): unknown diag names - { - auto node = std::make_unique("/unknown"); - unknown_ = node.get(); - nodes.push_back(std::move(node)); + for (const auto & [config, node] : dict.configs) { + node->init(config, dict); } - for (const auto & node : nodes) { - paths[node->path()] = node.get(); + // DEBUG + for (size_t index = 0; index < nodes.size(); ++index) { + nodes[index]->set_index(index); } - for (auto & config : root.units) { - if (paths.count(config.path)) { - paths.at(config.path)->create(config.dict, exprs); - } - } - for (auto & config : root.diags) { - if (paths.count(config.path)) { - paths.at(config.path)->create(config.dict, exprs); + for (const auto & [config, node] : dict.configs) { + std::cout << std::left << std::setw(3) << node->index(); + std::cout << std::left << std::setw(10) << config->type; + std::cout << std::left << std::setw(40) << node->path(); + for (const auto & child : node->children()) { + std::cout << " " << child->index(); } + std::cout << std::endl; } - for (auto & [link, config] : exprs.get()) { - link->init(config, paths); - } - - // Sort unit nodes in topological order for update dependencies. - topological_sort(nodes); - - // Set the link index for the ros message. - for (size_t i = 0; i < nodes.size(); ++i) { - nodes[i]->set_index(i); - } - - nodes_ = std::move(nodes); - diags_ = diags; + // Sort units in topological order for update dependencies. + nodes_ = topological_sort(std::move(nodes)); } void Graph::callback(const DiagnosticArray & array, const rclcpp::Time & stamp) @@ -167,7 +179,8 @@ void Graph::callback(const DiagnosticArray & array, const rclcpp::Time & stamp) if (iter != diags_.end()) { iter->second->callback(status, stamp); } else { - unknown_->callback(status, stamp); + // TODO(Takagi, Isamu) + // unknown_->callback(status, stamp); } } } @@ -191,9 +204,9 @@ DiagnosticGraph Graph::message() const return result; } -std::vector Graph::nodes() const +std::vector Graph::nodes() const { - std::vector result; + std::vector result; result.reserve(nodes_.size()); for (const auto & node : nodes_) { result.push_back(node.get()); diff --git a/system/system_diagnostic_graph/src/core/graph.hpp b/system/system_diagnostic_graph/src/core/graph.hpp index e0060c9111592..c8a155f73918c 100644 --- a/system/system_diagnostic_graph/src/core/graph.hpp +++ b/system/system_diagnostic_graph/src/core/graph.hpp @@ -38,14 +38,13 @@ class Graph final void callback(const DiagnosticArray & array, const rclcpp::Time & stamp); void update(const rclcpp::Time & stamp); DiagnosticGraph message() const; - std::vector nodes() const; + std::vector nodes() const; void debug(); private: - std::vector> nodes_; - std::unordered_map diags_; - UnknownNode * unknown_; + std::vector> nodes_; + std::unordered_map diags_; rclcpp::Time stamp_; }; diff --git a/system/system_diagnostic_graph/src/core/modes.cpp b/system/system_diagnostic_graph/src/core/modes.cpp index 0ca18f84b0407..a2e3d27791465 100644 --- a/system/system_diagnostic_graph/src/core/modes.cpp +++ b/system/system_diagnostic_graph/src/core/modes.cpp @@ -15,7 +15,8 @@ #include "modes.hpp" #include "config.hpp" -#include "nodes.hpp" +#include "error.hpp" +#include "units.hpp" #include #include @@ -24,17 +25,17 @@ namespace system_diagnostic_graph { -OperationModes::OperationModes(rclcpp::Node & node, const std::vector & graph) +OperationModes::OperationModes(rclcpp::Node & node, const std::vector & graph) { pub_ = node.create_publisher("/system/operation_mode/availability", rclcpp::QoS(1)); - using PathNodes = std::unordered_map; - PathNodes paths; + using PathDict = std::unordered_map; + PathDict paths; for (const auto & node : graph) { paths[node->path()] = node; } - const auto find_node = [](const PathNodes & paths, const std::string & name) { + const auto find_node = [](const PathDict & paths, const std::string & name) { const auto iter = paths.find(name); if (iter != paths.end()) { return iter->second; @@ -55,7 +56,7 @@ OperationModes::OperationModes(rclcpp::Node & node, const std::vectorlevel() == DiagnosticStatus::OK; }; + const auto is_ok = [](const BaseUnit * node) { return node->level() == DiagnosticStatus::OK; }; // clang-format off Availability message; diff --git a/system/system_diagnostic_graph/src/core/modes.hpp b/system/system_diagnostic_graph/src/core/modes.hpp index ead1ec10dce93..47a31b8d2a152 100644 --- a/system/system_diagnostic_graph/src/core/modes.hpp +++ b/system/system_diagnostic_graph/src/core/modes.hpp @@ -29,7 +29,7 @@ namespace system_diagnostic_graph class OperationModes { public: - explicit OperationModes(rclcpp::Node & node, const std::vector & graph); + OperationModes(rclcpp::Node & node, const std::vector & graph); void update(const rclcpp::Time & stamp) const; private: @@ -37,13 +37,13 @@ class OperationModes rclcpp::TimerBase::SharedPtr timer_; rclcpp::Publisher::SharedPtr pub_; - BaseNode * stop_mode_; - BaseNode * autonomous_mode_; - BaseNode * local_mode_; - BaseNode * remote_mode_; - BaseNode * emergency_stop_mrm_; - BaseNode * comfortable_stop_mrm_; - BaseNode * pull_over_mrm_; + BaseUnit * stop_mode_; + BaseUnit * autonomous_mode_; + BaseUnit * local_mode_; + BaseUnit * remote_mode_; + BaseUnit * emergency_stop_mrm_; + BaseUnit * comfortable_stop_mrm_; + BaseUnit * pull_over_mrm_; }; } // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/types.hpp b/system/system_diagnostic_graph/src/core/types.hpp index 2adfbb3f9d4ef..c0b901d1e4511 100644 --- a/system/system_diagnostic_graph/src/core/types.hpp +++ b/system/system_diagnostic_graph/src/core/types.hpp @@ -31,13 +31,8 @@ using tier4_system_msgs::msg::DiagnosticLink; using tier4_system_msgs::msg::DiagnosticNode; using DiagnosticLevel = DiagnosticStatus::_level_type; -class BaseNode; -class UnitNode; -class DiagNode; -class UnknownNode; - -class BaseExpr; -class ExprInit; +class BaseUnit; +class DiagUnit; } // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/units.cpp b/system/system_diagnostic_graph/src/core/units.cpp new file mode 100644 index 0000000000000..996742ec48c78 --- /dev/null +++ b/system/system_diagnostic_graph/src/core/units.cpp @@ -0,0 +1,87 @@ +// 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 "units.hpp" + +#include +#include + +namespace system_diagnostic_graph +{ + +auto resolve(const BaseUnit::NodeDict & dict, const std::vector & children) +{ + std::vector result; + for (const auto & child : children) { + result.push_back(dict.configs.at(child)); + } + return result; +} + +auto resolve(const BaseUnit::NodeDict & dict, const std::string & path) +{ + std::vector result; + result.push_back(dict.paths.at(path)); + return result; +} + +BaseUnit::BaseUnit(const std::string & path) : path_(path) +{ + index_ = 0; +} + +void DiagUnit::init(const UnitConfig::SharedPtr & config, const NodeDict & dict) +{ + (void)config; + (void)dict; +} + +void DiagUnit::callback(const DiagnosticStatus & status, const rclcpp::Time & stamp) +{ + (void)status; + (void)stamp; +} + +void LinkUnit::init(const UnitConfig::SharedPtr & config, const NodeDict & dict) +{ + children_ = resolve(dict, config->data.take_text("link")); +} + +AndUnit::AndUnit(const std::string & path, bool short_circuit) : BaseUnit(path) +{ + short_circuit_ = short_circuit; +} + +void AndUnit::init(const UnitConfig::SharedPtr & config, const NodeDict & dict) +{ + children_ = resolve(dict, config->children); +} + +void OrUnit::init(const UnitConfig::SharedPtr & config, const NodeDict & dict) +{ + children_ = resolve(dict, config->children); +} + +DebugUnit::DebugUnit(const std::string & path, const DiagnosticLevel level) : BaseUnit(path) +{ + level_ = level; +} + +void DebugUnit::init(const UnitConfig::SharedPtr & config, const NodeDict & dict) +{ + (void)config; + (void)dict; +} + +} // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/units.hpp b/system/system_diagnostic_graph/src/core/units.hpp new file mode 100644 index 0000000000000..f2afa98316686 --- /dev/null +++ b/system/system_diagnostic_graph/src/core/units.hpp @@ -0,0 +1,115 @@ +// 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 CORE__UNITS_HPP_ +#define CORE__UNITS_HPP_ + +#include "config.hpp" +#include "types.hpp" + +#include + +#include +#include +#include +#include + +namespace system_diagnostic_graph +{ + +class BaseUnit +{ +public: + struct NodeDict + { + std::unordered_map configs; + std::unordered_map paths; + }; + using UniquePtr = std::unique_ptr; + using UniquePtrList = std::vector>; + + explicit BaseUnit(const std::string & path); + virtual ~BaseUnit() = default; + virtual void init(const UnitConfig::SharedPtr & config, const NodeDict & dict) = 0; + + std::string path() const { return path_; } + std::vector children() const { return children_; } + + size_t index() const { return index_; } + void set_index(const size_t index) { index_ = index; } + + /* + virtual void update(const rclcpp::Time & stamp) = 0; + virtual DiagnosticNode report() const = 0; + virtual DiagnosticLevel level() const = 0; + */ + virtual void update(const rclcpp::Time & stamp) { (void)stamp; } + virtual DiagnosticNode report() const { return DiagnosticNode(); } + virtual DiagnosticLevel level() const { return DiagnosticStatus::OK; } + + /* + virtual std::vector links() const = 0; + */ + +protected: + const std::string path_; + size_t index_; + std::vector children_; +}; + +class DiagUnit : public BaseUnit +{ +public: + using BaseUnit::BaseUnit; + void init(const UnitConfig::SharedPtr & config, const NodeDict & dict) override; + void callback(const DiagnosticStatus & status, const rclcpp::Time & stamp); +}; + +class LinkUnit : public BaseUnit +{ +public: + using BaseUnit::BaseUnit; + void init(const UnitConfig::SharedPtr & config, const NodeDict & dict) override; +}; + +class AndUnit : public BaseUnit +{ +public: + AndUnit(const std::string & path, bool short_circuit); + void init(const UnitConfig::SharedPtr & config, const NodeDict & dict) override; + +private: + bool short_circuit_; +}; + +class OrUnit : public BaseUnit +{ +public: + using BaseUnit::BaseUnit; + void init(const UnitConfig::SharedPtr & config, const NodeDict & dict) override; +}; + +class DebugUnit : public BaseUnit +{ +public: + DebugUnit(const std::string & path, const DiagnosticLevel level); + void init(const UnitConfig::SharedPtr & config, const NodeDict & dict) override; + +private: + DiagnosticLevel level_; +}; + +} // namespace system_diagnostic_graph + +#endif // CORE__UNITS_HPP_ diff --git a/system/system_diagnostic_graph/src/main.cpp b/system/system_diagnostic_graph/src/main.cpp index 722ddcf833577..2b490fe5c6c0a 100644 --- a/system/system_diagnostic_graph/src/main.cpp +++ b/system/system_diagnostic_graph/src/main.cpp @@ -27,6 +27,7 @@ MainNode::MainNode() : Node("system_diagnostic_graph_aggregator") const auto file = declare_parameter("graph_file"); const auto mode = declare_parameter("mode"); graph_.init(file, mode); + return; graph_.debug(); } From ee4e2b1e5e6158cc20504f332b0021fc0fbd4ed9 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Wed, 25 Oct 2023 16:53:02 +0900 Subject: [PATCH 02/12] restore sort Signed-off-by: Takagi, Isamu --- system/system_diagnostic_graph/CMakeLists.txt | 5 - .../src/back/nodes.cpp | 157 ------------------ .../src/back/nodes.hpp | 114 ------------- .../src/core/debug.cpp | 7 +- .../src/core/graph.cpp | 68 +++----- .../src/core/graph.hpp | 4 +- .../src/core/units.cpp | 90 +++++++--- .../src/core/units.hpp | 59 ++++--- system/system_diagnostic_graph/src/main.cpp | 5 +- system/system_diagnostic_graph/util/debug.py | 85 ---------- 10 files changed, 138 insertions(+), 456 deletions(-) delete mode 100644 system/system_diagnostic_graph/src/back/nodes.cpp delete mode 100644 system/system_diagnostic_graph/src/back/nodes.hpp delete mode 100755 system/system_diagnostic_graph/util/debug.py diff --git a/system/system_diagnostic_graph/CMakeLists.txt b/system/system_diagnostic_graph/CMakeLists.txt index 1166f441890f5..9d2e1c0dd07c4 100644 --- a/system/system_diagnostic_graph/CMakeLists.txt +++ b/system/system_diagnostic_graph/CMakeLists.txt @@ -17,9 +17,4 @@ ament_auto_add_executable(converter src/tool.cpp ) -install( - PROGRAMS util/debug.py - DESTINATION lib/${PROJECT_NAME} -) - ament_auto_package(INSTALL_TO_SHARE config example launch) diff --git a/system/system_diagnostic_graph/src/back/nodes.cpp b/system/system_diagnostic_graph/src/back/nodes.cpp deleted file mode 100644 index bbc4bb4d42561..0000000000000 --- a/system/system_diagnostic_graph/src/back/nodes.cpp +++ /dev/null @@ -1,157 +0,0 @@ -// 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 "nodes.hpp" - -#include "exprs.hpp" - -#include - -#include -#include - -namespace system_diagnostic_graph -{ - -BaseNode::BaseNode(const std::string & path) : path_(path) -{ - index_ = 0; -} - -UnitNode::UnitNode(const std::string & path) : BaseNode(path) -{ - level_ = DiagnosticStatus::STALE; -} - -UnitNode::~UnitNode() -{ - // for unique_ptr -} - -void UnitNode::create(ConfigObject & config, ExprInit & exprs) -{ - expr_ = exprs.create(parse_expr_config(config)); -} - -void UnitNode::update(const rclcpp::Time &) -{ - const auto result = expr_->eval(); - level_ = result.level; - links_.clear(); - for (const auto & [node, used] : result.links) { - DiagnosticLink link; - link.index = node->index(); - link.used = used; - links_.push_back(link); - } -} - -DiagnosticNode UnitNode::report() const -{ - DiagnosticNode message; - message.status.level = level_; - message.status.name = path_; - message.links = links_; - return message; -} - -DiagnosticLevel UnitNode::level() const -{ - return level_; -} - -std::vector UnitNode::links() const -{ - return expr_->get_dependency(); -} - -DiagNode::DiagNode(const std::string & path, ConfigObject & config) : BaseNode(path) -{ - timeout_ = 3.0; // TODO(Takagi, Isamu): parameterize - name_ = config.take_text("name"); - - status_.level = DiagnosticStatus::STALE; -} - -void DiagNode::create(ConfigObject &, ExprInit &) -{ -} - -void DiagNode::update(const rclcpp::Time & stamp) -{ - if (time_) { - const auto elapsed = (stamp - time_.value()).seconds(); - if (timeout_ < elapsed) { - status_ = DiagnosticStatus(); - status_.level = DiagnosticStatus::STALE; - time_ = std::nullopt; - } - } -} - -DiagnosticNode DiagNode::report() const -{ - DiagnosticNode message; - message.status = status_; - message.status.name = path_; - return message; -} - -DiagnosticLevel DiagNode::level() const -{ - return status_.level; -} - -void DiagNode::callback(const DiagnosticStatus & status, const rclcpp::Time & stamp) -{ - status_ = status; - time_ = stamp; -} - -UnknownNode::UnknownNode(const std::string & path) : BaseNode(path) -{ -} - -void UnknownNode::create(ConfigObject &, ExprInit &) -{ -} - -void UnknownNode::update(const rclcpp::Time & stamp) -{ - (void)stamp; -} - -DiagnosticNode UnknownNode::report() const -{ - DiagnosticNode message; - message.status.name = path_; - for (const auto & diag : diagnostics_) { - diagnostic_msgs::msg::KeyValue kv; - kv.key = diag.first; - message.status.values.push_back(kv); - } - return message; -} - -DiagnosticLevel UnknownNode::level() const -{ - return DiagnosticStatus::WARN; -} - -void UnknownNode::callback(const DiagnosticStatus & status, const rclcpp::Time & stamp) -{ - diagnostics_[status.name] = stamp; -} - -} // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/back/nodes.hpp b/system/system_diagnostic_graph/src/back/nodes.hpp deleted file mode 100644 index 9c4fb8141bed0..0000000000000 --- a/system/system_diagnostic_graph/src/back/nodes.hpp +++ /dev/null @@ -1,114 +0,0 @@ -// 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 BACK__NODES_HPP_ -#define BACK__NODES_HPP_ - -#include "config.hpp" -#include "debug.hpp" -#include "types.hpp" - -#include - -#include -#include -#include -#include -#include -#include - -namespace system_diagnostic_graph -{ - -class BaseNode -{ -public: - explicit BaseNode(const std::string & path); - virtual ~BaseNode() = default; - virtual void create(ConfigObject & config, ExprInit & exprs) = 0; - virtual void update(const rclcpp::Time & stamp) = 0; - virtual DiagnosticNode report() const = 0; - virtual DiagnosticLevel level() const = 0; - virtual DiagDebugData debug() const = 0; - virtual std::vector links() const = 0; - - std::string path() const { return path_; } - - size_t index() const { return index_; } - void set_index(const size_t index) { index_ = index; } - -protected: - const std::string path_; - size_t index_; -}; - -class UnitNode : public BaseNode -{ -public: - explicit UnitNode(const std::string & path); - ~UnitNode() override; - void create(ConfigObject & config, ExprInit & exprs) override; - void update(const rclcpp::Time & stamp) override; - DiagnosticNode report() const override; - DiagnosticLevel level() const override; - DiagDebugData debug() const override; - std::vector links() const override; - -private: - std::unique_ptr expr_; - std::vector links_; - DiagnosticLevel level_; -}; - -class DiagNode : public BaseNode -{ -public: - DiagNode(const std::string & path, ConfigObject & config); - void create(ConfigObject & config, ExprInit & exprs) override; - void update(const rclcpp::Time & stamp) override; - DiagnosticNode report() const override; - DiagnosticLevel level() const override; - DiagDebugData debug() const override; - std::vector links() const override { return {}; } - std::string name() const { return name_; } - - void callback(const DiagnosticStatus & status, const rclcpp::Time & stamp); - -private: - double timeout_; - std::optional time_; - std::string name_; - DiagnosticStatus status_; -}; - -class UnknownNode : public BaseNode -{ -public: - explicit UnknownNode(const std::string & path); - void create(ConfigObject & config, ExprInit & exprs) override; - void update(const rclcpp::Time & stamp) override; - DiagnosticNode report() const override; - DiagnosticLevel level() const override; - DiagDebugData debug() const override; - std::vector links() const override { return {}; } - - void callback(const DiagnosticStatus & status, const rclcpp::Time & stamp); - -private: - std::map diagnostics_; -}; - -} // namespace system_diagnostic_graph - -#endif // BACK__NODES_HPP_ diff --git a/system/system_diagnostic_graph/src/core/debug.cpp b/system/system_diagnostic_graph/src/core/debug.cpp index 188eb75ea9645..45c097b8947b5 100644 --- a/system/system_diagnostic_graph/src/core/debug.cpp +++ b/system/system_diagnostic_graph/src/core/debug.cpp @@ -16,6 +16,7 @@ #include "graph.hpp" #include "types.hpp" +#include "units.hpp" #include #include @@ -33,10 +34,11 @@ const std::unordered_map level_names = { void Graph::debug() { - /* std::vector lines; for (const auto & node : nodes_) { - lines.push_back(node->debug()); + const auto level_name = level_names.at(node->level()); + const auto index_name = std::to_string(node->index()); + lines.push_back({"unit", index_name, level_name, node->path(), "-----"}); } std::array widths = {}; @@ -55,7 +57,6 @@ void Graph::debug() } std::cout << "|" << std::endl; } - */ } /* diff --git a/system/system_diagnostic_graph/src/core/graph.cpp b/system/system_diagnostic_graph/src/core/graph.cpp index 0806219c22240..31c39726e7f24 100644 --- a/system/system_diagnostic_graph/src/core/graph.cpp +++ b/system/system_diagnostic_graph/src/core/graph.cpp @@ -33,13 +33,10 @@ namespace system_diagnostic_graph BaseUnit::UniquePtrList topological_sort(BaseUnit::UniquePtrList && input) { - return std::move(input); - - /* - std::unordered_map degrees; - std::deque nodes; - std::deque result; - std::deque buffer; + std::unordered_map degrees; + std::deque nodes; + std::deque result; + std::deque buffer; // Create a list of raw pointer nodes. for (const auto & node : input) { @@ -48,7 +45,7 @@ BaseUnit::UniquePtrList topological_sort(BaseUnit::UniquePtrList && input) // Count degrees of each node. for (const auto & node : nodes) { - for (const auto & link : node->links()) { + for (const auto & [link, used] : node->links()) { ++degrees[link]; } } @@ -64,7 +61,7 @@ BaseUnit::UniquePtrList topological_sort(BaseUnit::UniquePtrList && input) while (!buffer.empty()) { const auto node = buffer.front(); buffer.pop_front(); - for (const auto & link : node->links()) { + for (const auto & [link, used] : node->links()) { if (--degrees[link] == 0) { buffer.push_back(link); } @@ -78,19 +75,18 @@ BaseUnit::UniquePtrList topological_sort(BaseUnit::UniquePtrList && input) } // Reverse the result to process from leaf node. - result = std::deque(result.rbegin(), result.rend()); + result = std::deque(result.rbegin(), result.rend()); // Replace node vector. - std::unordered_map indices; + std::unordered_map indices; for (size_t i = 0; i < result.size(); ++i) { indices[result[i]] = i; } - std::vector> sorted(input.size()); + std::vector> sorted(input.size()); for (auto & node : input) { sorted[indices[node.get()]] = std::move(node); } - input = std::move(sorted); - */ + return sorted; } BaseUnit::UniquePtr make_node(const UnitConfig::SharedPtr & config) @@ -98,9 +94,6 @@ BaseUnit::UniquePtr make_node(const UnitConfig::SharedPtr & config) if (config->type == "diag") { return std::make_unique(config->path); } - if (config->type == "link") { - return std::make_unique(config->path); - } if (config->type == "and") { return std::make_unique(config->path, false); } @@ -153,23 +146,21 @@ void Graph::init(const std::string & file, const std::string & mode) node->init(config, dict); } - // DEBUG + // 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); } - for (const auto & [config, node] : dict.configs) { - std::cout << std::left << std::setw(3) << node->index(); - std::cout << std::left << std::setw(10) << config->type; - std::cout << std::left << std::setw(40) << node->path(); - for (const auto & child : node->children()) { - std::cout << " " << child->index(); + for (const auto & node : nodes) { + const auto diag = dynamic_cast(node.get()); + if (diag) { + diags_[diag->name()] = diag; + std::cout << diag->name() << std::endl; } - std::cout << std::endl; } - - // Sort units in topological order for update dependencies. - nodes_ = topological_sort(std::move(nodes)); + nodes_ = std::move(nodes); } void Graph::callback(const DiagnosticArray & array, const rclcpp::Time & stamp) @@ -181,27 +172,20 @@ void Graph::callback(const DiagnosticArray & array, const rclcpp::Time & stamp) } else { // TODO(Takagi, Isamu) // unknown_->callback(status, stamp); + std::cout << "unknown diag: " << status.name << std::endl; } } } -void Graph::update(const rclcpp::Time & stamp) +DiagnosticGraph Graph::report(const rclcpp::Time & stamp) { + DiagnosticGraph message; + message.stamp = stamp; + message.nodes.reserve(nodes_.size()); for (const auto & node : nodes_) { - node->update(stamp); + message.nodes.push_back(node->report(stamp)); } - stamp_ = stamp; -} - -DiagnosticGraph Graph::message() const -{ - DiagnosticGraph result; - result.stamp = stamp_; - result.nodes.reserve(nodes_.size()); - for (const auto & node : nodes_) { - result.nodes.push_back(node->report()); - } - return result; + return message; } std::vector Graph::nodes() const diff --git a/system/system_diagnostic_graph/src/core/graph.hpp b/system/system_diagnostic_graph/src/core/graph.hpp index c8a155f73918c..fb137e27642c6 100644 --- a/system/system_diagnostic_graph/src/core/graph.hpp +++ b/system/system_diagnostic_graph/src/core/graph.hpp @@ -36,8 +36,7 @@ class Graph final void init(const std::string & file, const std::string & mode); void callback(const DiagnosticArray & array, const rclcpp::Time & stamp); - void update(const rclcpp::Time & stamp); - DiagnosticGraph message() const; + DiagnosticGraph report(const rclcpp::Time & stamp); std::vector nodes() const; void debug(); @@ -45,7 +44,6 @@ class Graph final private: std::vector> nodes_; std::unordered_map diags_; - rclcpp::Time stamp_; }; } // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/units.cpp b/system/system_diagnostic_graph/src/core/units.cpp index 996742ec48c78..b5a96e8aadb89 100644 --- a/system/system_diagnostic_graph/src/core/units.cpp +++ b/system/system_diagnostic_graph/src/core/units.cpp @@ -15,47 +15,80 @@ #include "units.hpp" #include +#include #include namespace system_diagnostic_graph { -auto resolve(const BaseUnit::NodeDict & dict, const std::vector & children) +auto resolve(const BaseUnit::NodeDict & dict, const std::vector & links) { - std::vector result; - for (const auto & child : children) { - result.push_back(dict.configs.at(child)); + std::vector> result; + for (const auto & link : links) { + result.push_back(std::make_pair(dict.configs.at(link), true)); } return result; } auto resolve(const BaseUnit::NodeDict & dict, const std::string & path) { - std::vector result; - result.push_back(dict.paths.at(path)); + std::vector> result; + result.push_back(std::make_pair(dict.paths.at(path), true)); return result; } BaseUnit::BaseUnit(const std::string & path) : path_(path) { index_ = 0; + level_ = DiagnosticStatus::STALE; } -void DiagUnit::init(const UnitConfig::SharedPtr & config, const NodeDict & dict) +DiagnosticNode BaseUnit::report(const rclcpp::Time & stamp) { - (void)config; - (void)dict; + const auto message = update(stamp); + level_ = message.status.level; + return message; +} + +void DiagUnit::init(const UnitConfig::SharedPtr & config, const NodeDict &) +{ + timeout_ = 3.0; // TODO(Takagi, Isamu): parameterize + name_ = config->data.take_text("name", path_); +} + +void DiagUnit::eval() +{ + if (diagnostics_) { + level_ = diagnostics_.value().first.status.level; + } else { + level_ = DiagnosticStatus::STALE; + } } void DiagUnit::callback(const DiagnosticStatus & status, const rclcpp::Time & stamp) { - (void)status; - (void)stamp; + diagnostics_ = std::make_pair(status, stamp); } -void LinkUnit::init(const UnitConfig::SharedPtr & config, const NodeDict & dict) +DiagnosticNode DiagUnit::update(const rclcpp::Time & stamp) { - children_ = resolve(dict, config->data.take_text("link")); + if (diagnostics_) { + const auto updated = diagnostics_.value().second; + const auto elapsed = (stamp - updated).seconds(); + if (timeout_ < elapsed) { + diagnostics_ = std::nullopt; + } + } + + DiagnosticNode message; + if (diagnostics_) { + message.status = diagnostics_.value().first; + message.status.name = path_; + } else { + message.status.level = DiagnosticStatus::STALE; + message.status.name = path_; + } + return message; } AndUnit::AndUnit(const std::string & path, bool short_circuit) : BaseUnit(path) @@ -65,23 +98,42 @@ AndUnit::AndUnit(const std::string & path, bool short_circuit) : BaseUnit(path) void AndUnit::init(const UnitConfig::SharedPtr & config, const NodeDict & dict) { - children_ = resolve(dict, config->children); + links_ = resolve(dict, config->children); +} + +DiagnosticNode AndUnit::update(const rclcpp::Time &) +{ + DiagnosticNode message; + message.status.name = path_; + return message; } void OrUnit::init(const UnitConfig::SharedPtr & config, const NodeDict & dict) { - children_ = resolve(dict, config->children); + links_ = resolve(dict, config->children); +} + +DiagnosticNode OrUnit::update(const rclcpp::Time &) +{ + DiagnosticNode message; + message.status.name = path_; + return message; } DebugUnit::DebugUnit(const std::string & path, const DiagnosticLevel level) : BaseUnit(path) { - level_ = level; + const_ = level; +} + +void DebugUnit::init(const UnitConfig::SharedPtr &, const NodeDict &) +{ } -void DebugUnit::init(const UnitConfig::SharedPtr & config, const NodeDict & dict) +DiagnosticNode DebugUnit::update(const rclcpp::Time &) { - (void)config; - (void)dict; + DiagnosticNode message; + message.status.name = path_; + return message; } } // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/units.hpp b/system/system_diagnostic_graph/src/core/units.hpp index f2afa98316686..a014d7f7b182a 100644 --- a/system/system_diagnostic_graph/src/core/units.hpp +++ b/system/system_diagnostic_graph/src/core/units.hpp @@ -21,8 +21,10 @@ #include #include +#include #include #include +#include #include namespace system_diagnostic_graph @@ -36,36 +38,35 @@ class BaseUnit std::unordered_map configs; std::unordered_map paths; }; + struct NodeData + { + DiagnosticLevel level; + std::vector> links; + }; using UniquePtr = std::unique_ptr; using UniquePtrList = std::vector>; explicit BaseUnit(const std::string & path); virtual ~BaseUnit() = default; virtual void init(const UnitConfig::SharedPtr & config, const NodeDict & dict) = 0; + virtual void eval() = 0; - std::string path() const { return path_; } - std::vector children() const { return children_; } + DiagnosticNode report(const rclcpp::Time & stamp); + auto level() const { return level_; } + auto links() const { return links_; } + auto path() const { return path_; } size_t index() const { return index_; } void set_index(const size_t index) { index_ = index; } - /* - virtual void update(const rclcpp::Time & stamp) = 0; - virtual DiagnosticNode report() const = 0; - virtual DiagnosticLevel level() const = 0; - */ - virtual void update(const rclcpp::Time & stamp) { (void)stamp; } - virtual DiagnosticNode report() const { return DiagnosticNode(); } - virtual DiagnosticLevel level() const { return DiagnosticStatus::OK; } - - /* - virtual std::vector links() const = 0; - */ - protected: - const std::string path_; + virtual DiagnosticNode update(const rclcpp::Time & stamp) = 0; + DiagnosticLevel level_; + std::vector> links_; + std::string path_; + +private: size_t index_; - std::vector children_; }; class DiagUnit : public BaseUnit @@ -73,14 +74,16 @@ class DiagUnit : public BaseUnit public: using BaseUnit::BaseUnit; void init(const UnitConfig::SharedPtr & config, const NodeDict & dict) override; + void eval() override; + + std::string name() const { return name_; } void callback(const DiagnosticStatus & status, const rclcpp::Time & stamp); -}; -class LinkUnit : public BaseUnit -{ -public: - using BaseUnit::BaseUnit; - void init(const UnitConfig::SharedPtr & config, const NodeDict & dict) override; +private: + DiagnosticNode update(const rclcpp::Time & stamp) override; + double timeout_; + std::optional> diagnostics_; + std::string name_; }; class AndUnit : public BaseUnit @@ -88,8 +91,10 @@ class AndUnit : public BaseUnit public: AndUnit(const std::string & path, bool short_circuit); void init(const UnitConfig::SharedPtr & config, const NodeDict & dict) override; + void eval() override; private: + DiagnosticNode update(const rclcpp::Time & stamp) override; bool short_circuit_; }; @@ -98,6 +103,10 @@ class OrUnit : public BaseUnit public: using BaseUnit::BaseUnit; void init(const UnitConfig::SharedPtr & config, const NodeDict & dict) override; + void eval() override; + +private: + DiagnosticNode update(const rclcpp::Time & stamp) override; }; class DebugUnit : public BaseUnit @@ -105,9 +114,11 @@ class DebugUnit : public BaseUnit public: DebugUnit(const std::string & path, const DiagnosticLevel level); void init(const UnitConfig::SharedPtr & config, const NodeDict & dict) override; + void eval() override; private: - DiagnosticLevel level_; + DiagnosticNode update(const rclcpp::Time & stamp) override; + DiagnosticLevel const_; }; } // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/main.cpp b/system/system_diagnostic_graph/src/main.cpp index 2b490fe5c6c0a..8a8d3a1bc2189 100644 --- a/system/system_diagnostic_graph/src/main.cpp +++ b/system/system_diagnostic_graph/src/main.cpp @@ -27,7 +27,6 @@ MainNode::MainNode() : Node("system_diagnostic_graph_aggregator") const auto file = declare_parameter("graph_file"); const auto mode = declare_parameter("mode"); graph_.init(file, mode); - return; graph_.debug(); } @@ -59,10 +58,8 @@ MainNode::~MainNode() void MainNode::on_timer() { const auto stamp = now(); - graph_.update(stamp); + pub_graph_->publish(graph_.report(stamp)); graph_.debug(); - pub_graph_->publish(graph_.message()); - if (modes_) modes_->update(stamp); } diff --git a/system/system_diagnostic_graph/util/debug.py b/system/system_diagnostic_graph/util/debug.py deleted file mode 100755 index dac917a26a2f6..0000000000000 --- a/system/system_diagnostic_graph/util/debug.py +++ /dev/null @@ -1,85 +0,0 @@ -#!/usr/bin/env python3 - -# 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. - -import rclpy -from rclpy.node import Node -from rclpy.qos import QoSDurabilityPolicy -from rclpy.qos import QoSProfile -from tier4_system_msgs.msg import DiagnosticGraph - - -class StructNode(Node): - def __init__(self): - super().__init__("system_diagnostic_graph_tools_struct") - qos_struct = QoSProfile(depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL) - self.sub_struct = self.create_subscription( - DiagnosticGraph, "/diagnostics_graph/struct", self.callback, qos_struct - ) - self.message = None - - def callback(self, msg): - self.message = msg - - -class NodeSpinner: - def __init__(self, time): - self.time = time - self.wait = True - - def on_timer(self): - self.wait = False - - def spin(self, node): - timer = node.create_timer(self.time, self.on_timer) - while self.wait: - rclpy.spin_once(node) - node.destroy_timer(timer) - - -class GraphNode: - def __init__(self, msg): - self.name = msg.name - self.links = msg.links - - -class GraphStruct: - def __init__(self, msg): - self.nodes = [GraphNode(node) for node in msg.nodes] - - @staticmethod - def Subscribe(): - spin = NodeSpinner(1.0) - node = StructNode() - spin.spin(node) - return GraphStruct(node.message) - - def visualize(self): - from graphviz import Digraph - - graph = Digraph() - graph.attr("node", shape="box") - for node in self.nodes: - graph.node(node.name) - for link in node.links: - graph.edge(node.name, self.nodes[link].name) - graph.view() - - -if __name__ == "__main__": - rclpy.init() - graph = GraphStruct.Subscribe() - rclpy.shutdown() - graph.visualize() From 74051e4096de628cebd4dce707097ab34393e48f Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Wed, 25 Oct 2023 19:37:08 +0900 Subject: [PATCH 03/12] restore status Signed-off-by: Takagi, Isamu --- .../src/back/exprs.cpp | 216 ------------------ .../src/back/exprs.hpp | 104 --------- .../src/core/graph.cpp | 19 +- .../src/core/graph.hpp | 2 +- .../src/core/units.cpp | 114 +++++---- .../src/core/units.hpp | 31 ++- system/system_diagnostic_graph/src/main.cpp | 2 +- 7 files changed, 95 insertions(+), 393 deletions(-) delete mode 100644 system/system_diagnostic_graph/src/back/exprs.cpp delete mode 100644 system/system_diagnostic_graph/src/back/exprs.hpp diff --git a/system/system_diagnostic_graph/src/back/exprs.cpp b/system/system_diagnostic_graph/src/back/exprs.cpp deleted file mode 100644 index 22281f939cad2..0000000000000 --- a/system/system_diagnostic_graph/src/back/exprs.cpp +++ /dev/null @@ -1,216 +0,0 @@ -// 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 "exprs.hpp" - -#include "nodes.hpp" - -#include -#include -#include -#include -#include - -// DEBUG -#include - -namespace system_diagnostic_graph -{ - -using LinkStatus = std::vector>; - -void extend(LinkStatus & a, const LinkStatus & b) -{ - a.insert(a.end(), b.begin(), b.end()); -} - -void extend_false(LinkStatus & a, const LinkStatus & b) -{ - for (const auto & p : b) { - a.push_back(std::make_pair(p.first, false)); - } -} - -auto create_expr_list(ExprInit & exprs, ConfigObject & config) -{ - std::vector> result; - const auto list = config.take_list("list"); - for (size_t i = 0; i < list.size(); ++i) { - auto dict = parse_expr_config(config.mark().index(i), list[i]); - auto expr = exprs.create(dict); - if (expr) { - result.push_back(std::move(expr)); - } - } - return result; -} - -ConstExpr::ConstExpr(const DiagnosticLevel level) -{ - level_ = level; -} - -ExprStatus ConstExpr::eval() const -{ - ExprStatus status; - status.level = level_; - return status; -} - -std::vector ConstExpr::get_dependency() const -{ - return {}; -} - -LinkExpr::LinkExpr(ExprInit & exprs, ConfigObject & config) -{ - // TODO(Takagi, Isamu): remove - (void)config; - (void)exprs; -} - -void LinkExpr::init(ConfigObject & config, std::unordered_map nodes) -{ - const auto path = config.take_text("path"); - if (!nodes.count(path)) { - throw ConfigError("node path '" + path + "' does not exist"); - } - node_ = nodes.at(path); -} - -ExprStatus LinkExpr::eval() const -{ - ExprStatus status; - status.level = node_->level(); - status.links.push_back(std::make_pair(node_, true)); - return status; -} - -std::vector LinkExpr::get_dependency() const -{ - return {node_}; -} - -AndExpr::AndExpr(ExprInit & exprs, ConfigObject & config, bool short_circuit) -{ - list_ = create_expr_list(exprs, config); - short_circuit_ = short_circuit; -} - -ExprStatus AndExpr::eval() const -{ - if (list_.empty()) { - ExprStatus status; - status.level = DiagnosticStatus::OK; - return status; - } - - ExprStatus status; - status.level = DiagnosticStatus::OK; - for (const auto & expr : list_) { - const auto result = expr->eval(); - status.level = std::max(status.level, result.level); - extend(status.links, result.links); - if (short_circuit_ && status.level != DiagnosticStatus::OK) { - break; - } - } - status.level = std::min(status.level, DiagnosticStatus::ERROR); - return status; -} - -std::vector AndExpr::get_dependency() const -{ - std::vector depends; - for (const auto & expr : list_) { - const auto nodes = expr->get_dependency(); - depends.insert(depends.end(), nodes.begin(), nodes.end()); - } - return depends; -} - -OrExpr::OrExpr(ExprInit & exprs, ConfigObject & config) -{ - list_ = create_expr_list(exprs, config); -} - -ExprStatus OrExpr::eval() const -{ - if (list_.empty()) { - ExprStatus status; - status.level = DiagnosticStatus::OK; - return status; - } - - ExprStatus status; - status.level = DiagnosticStatus::ERROR; - for (const auto & expr : list_) { - const auto result = expr->eval(); - status.level = std::min(status.level, result.level); - extend(status.links, result.links); - } - status.level = std::min(status.level, DiagnosticStatus::ERROR); - return status; -} - -std::vector OrExpr::get_dependency() const -{ - std::vector depends; - for (const auto & expr : list_) { - const auto nodes = expr->get_dependency(); - depends.insert(depends.end(), nodes.begin(), nodes.end()); - } - return depends; -} - -ExprInit::ExprInit(const std::string & mode) -{ - mode_ = mode; -} - -std::unique_ptr ExprInit::create(ExprConfig config) -{ - if (!config.mode.check(mode_)) { - return nullptr; - } - if (config.type == "link") { - auto expr = std::make_unique(*this, config.dict); - links_.push_back(std::make_pair(expr.get(), config.dict)); - return expr; - } - if (config.type == "and") { - return std::make_unique(*this, config.dict, false); - } - if (config.type == "short-circuit-and") { - return std::make_unique(*this, config.dict, true); - } - if (config.type == "or") { - return std::make_unique(*this, config.dict); - } - if (config.type == "debug-ok") { - return std::make_unique(DiagnosticStatus::OK); - } - if (config.type == "debug-warn") { - return std::make_unique(DiagnosticStatus::WARN); - } - if (config.type == "debug-error") { - return std::make_unique(DiagnosticStatus::ERROR); - } - if (config.type == "debug-stale") { - return std::make_unique(DiagnosticStatus::STALE); - } - throw ConfigError("unknown expr type: " + config.type + " " + config.dict.mark().str()); -} - -} // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/back/exprs.hpp b/system/system_diagnostic_graph/src/back/exprs.hpp deleted file mode 100644 index 9775ea4cd870e..0000000000000 --- a/system/system_diagnostic_graph/src/back/exprs.hpp +++ /dev/null @@ -1,104 +0,0 @@ -// 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 BACK__EXPRS_HPP_ -#define BACK__EXPRS_HPP_ - -#include "config.hpp" -#include "types.hpp" - -#include -#include -#include -#include -#include - -namespace system_diagnostic_graph -{ - -struct ExprStatus -{ - DiagnosticLevel level; - std::vector> links; -}; - -class BaseExpr -{ -public: - virtual ~BaseExpr() = default; - virtual ExprStatus eval() const = 0; - virtual std::vector get_dependency() const = 0; -}; - -class ConstExpr : public BaseExpr -{ -public: - explicit ConstExpr(const DiagnosticLevel level); - ExprStatus eval() const override; - std::vector get_dependency() const override; - -private: - DiagnosticLevel level_; -}; - -class LinkExpr : public BaseExpr -{ -public: - LinkExpr(ExprInit & exprs, ConfigObject & config); - void init(ConfigObject & config, std::unordered_map nodes); - ExprStatus eval() const override; - std::vector get_dependency() const override; - -private: - BaseNode * node_; -}; - -class AndExpr : public BaseExpr -{ -public: - AndExpr(ExprInit & exprs, ConfigObject & config, bool short_circuit); - ExprStatus eval() const override; - std::vector get_dependency() const override; - -private: - bool short_circuit_; - std::vector> list_; -}; - -class OrExpr : public BaseExpr -{ -public: - OrExpr(ExprInit & exprs, ConfigObject & config); - ExprStatus eval() const override; - std::vector get_dependency() const override; - -private: - std::vector> list_; -}; - -class ExprInit -{ -public: - explicit ExprInit(const std::string & mode); - std::unique_ptr create(ExprConfig config); - auto get() const { return links_; } - -private: - std::string mode_; - std::vector> links_; -}; - -} // namespace system_diagnostic_graph - -#endif // BACK__EXPRS_HPP_ diff --git a/system/system_diagnostic_graph/src/core/graph.cpp b/system/system_diagnostic_graph/src/core/graph.cpp index 31c39726e7f24..3d44473d643e2 100644 --- a/system/system_diagnostic_graph/src/core/graph.cpp +++ b/system/system_diagnostic_graph/src/core/graph.cpp @@ -45,8 +45,8 @@ BaseUnit::UniquePtrList topological_sort(BaseUnit::UniquePtrList && input) // Count degrees of each node. for (const auto & node : nodes) { - for (const auto & [link, used] : node->links()) { - ++degrees[link]; + for (const auto & child : node->children()) { + ++degrees[child]; } } @@ -61,9 +61,9 @@ BaseUnit::UniquePtrList topological_sort(BaseUnit::UniquePtrList && input) while (!buffer.empty()) { const auto node = buffer.front(); buffer.pop_front(); - for (const auto & [link, used] : node->links()) { - if (--degrees[link] == 0) { - buffer.push_back(link); + for (const auto & child : node->children()) { + if (--degrees[child] == 0) { + buffer.push_back(child); } } result.push_back(node); @@ -163,15 +163,14 @@ void Graph::init(const std::string & file, const std::string & mode) nodes_ = std::move(nodes); } -void Graph::callback(const DiagnosticArray & array, const rclcpp::Time & stamp) +void Graph::callback(const rclcpp::Time & stamp, const DiagnosticArray & array) { for (const auto & status : array.status) { const auto iter = diags_.find(status.name); if (iter != diags_.end()) { - iter->second->callback(status, stamp); + iter->second->callback(stamp, status); } else { // TODO(Takagi, Isamu) - // unknown_->callback(status, stamp); std::cout << "unknown diag: " << status.name << std::endl; } } @@ -179,6 +178,10 @@ void Graph::callback(const DiagnosticArray & array, const rclcpp::Time & stamp) DiagnosticGraph Graph::report(const rclcpp::Time & stamp) { + for (const auto & node : nodes_) { + node->update(stamp); + } + DiagnosticGraph message; message.stamp = stamp; message.nodes.reserve(nodes_.size()); diff --git a/system/system_diagnostic_graph/src/core/graph.hpp b/system/system_diagnostic_graph/src/core/graph.hpp index fb137e27642c6..70c3e09696a83 100644 --- a/system/system_diagnostic_graph/src/core/graph.hpp +++ b/system/system_diagnostic_graph/src/core/graph.hpp @@ -35,7 +35,7 @@ class Graph final ~Graph(); void init(const std::string & file, const std::string & mode); - void callback(const DiagnosticArray & array, const rclcpp::Time & stamp); + void callback(const rclcpp::Time & stamp, const DiagnosticArray & array); DiagnosticGraph report(const rclcpp::Time & stamp); std::vector nodes() const; diff --git a/system/system_diagnostic_graph/src/core/units.cpp b/system/system_diagnostic_graph/src/core/units.cpp index b5a96e8aadb89..c04e36fe92a41 100644 --- a/system/system_diagnostic_graph/src/core/units.cpp +++ b/system/system_diagnostic_graph/src/core/units.cpp @@ -14,6 +14,7 @@ #include "units.hpp" +#include #include #include #include @@ -21,33 +22,44 @@ namespace system_diagnostic_graph { -auto resolve(const BaseUnit::NodeDict & dict, const std::vector & links) +using LinkList = std::vector>; + +void merge(LinkList & a, const LinkList & b, bool uses) +{ + for (const auto & [node, used] : b) { + a.push_back(std::make_pair(node, used && uses)); + } +} + +auto resolve(const BaseUnit::NodeDict & dict, const std::vector & children) { - std::vector> result; - for (const auto & link : links) { - result.push_back(std::make_pair(dict.configs.at(link), true)); + std::vector result; + for (const auto & child : children) { + result.push_back(dict.configs.at(child)); } return result; } auto resolve(const BaseUnit::NodeDict & dict, const std::string & path) { - std::vector> result; - result.push_back(std::make_pair(dict.paths.at(path), true)); + std::vector result; + result.push_back(dict.paths.at(path)); return result; } BaseUnit::BaseUnit(const std::string & path) : path_(path) { index_ = 0; - level_ = DiagnosticStatus::STALE; + level_ = DiagnosticStatus::OK; } -DiagnosticNode BaseUnit::report(const rclcpp::Time & stamp) +BaseUnit::NodeData BaseUnit::status() { - const auto message = update(stamp); - level_ = message.status.level; - return message; + if (path_.empty()) { + return {level_, links_}; + } else { + return {level_, {std::make_pair(this, true)}}; + } } void DiagUnit::init(const UnitConfig::SharedPtr & config, const NodeDict &) @@ -56,39 +68,26 @@ void DiagUnit::init(const UnitConfig::SharedPtr & config, const NodeDict &) name_ = config->data.take_text("name", path_); } -void DiagUnit::eval() -{ - if (diagnostics_) { - level_ = diagnostics_.value().first.status.level; - } else { - level_ = DiagnosticStatus::STALE; - } -} - -void DiagUnit::callback(const DiagnosticStatus & status, const rclcpp::Time & stamp) -{ - diagnostics_ = std::make_pair(status, stamp); -} - -DiagnosticNode DiagUnit::update(const rclcpp::Time & stamp) +void DiagUnit::update(const rclcpp::Time & stamp) { if (diagnostics_) { - const auto updated = diagnostics_.value().second; + const auto updated = diagnostics_.value().first; const auto elapsed = (stamp - updated).seconds(); if (timeout_ < elapsed) { diagnostics_ = std::nullopt; } } - DiagnosticNode message; if (diagnostics_) { - message.status = diagnostics_.value().first; - message.status.name = path_; + level_ = diagnostics_.value().second.level; } else { - message.status.level = DiagnosticStatus::STALE; - message.status.name = path_; + level_ = DiagnosticStatus::STALE; } - return message; +} + +void DiagUnit::callback(const rclcpp::Time & stamp, const DiagnosticStatus & status) +{ + diagnostics_ = std::make_pair(stamp, status); } AndUnit::AndUnit(const std::string & path, bool short_circuit) : BaseUnit(path) @@ -98,26 +97,50 @@ AndUnit::AndUnit(const std::string & path, bool short_circuit) : BaseUnit(path) void AndUnit::init(const UnitConfig::SharedPtr & config, const NodeDict & dict) { - links_ = resolve(dict, config->children); + children_ = resolve(dict, config->children); } -DiagnosticNode AndUnit::update(const rclcpp::Time &) +void AndUnit::update(const rclcpp::Time &) { - DiagnosticNode message; - message.status.name = path_; - return message; + if (children_.empty()) { + return; + } + + bool uses = true; + level_ = DiagnosticStatus::OK; + links_ = LinkList(); + + for (const auto & child : children_) { + const auto status = child->status(); + level_ = std::max(level_, status.level); + merge(links_, status.links, uses); + if (short_circuit_ && level_ != DiagnosticStatus::OK) { + uses = false; + } + } + level_ = std::min(level_, DiagnosticStatus::ERROR); } void OrUnit::init(const UnitConfig::SharedPtr & config, const NodeDict & dict) { - links_ = resolve(dict, config->children); + children_ = resolve(dict, config->children); } -DiagnosticNode OrUnit::update(const rclcpp::Time &) +void OrUnit::update(const rclcpp::Time &) { - DiagnosticNode message; - message.status.name = path_; - return message; + if (children_.empty()) { + return; + } + + level_ = DiagnosticStatus::ERROR; + links_ = LinkList(); + + for (const auto & child : children_) { + const auto status = child->status(); + level_ = std::min(level_, status.level); + merge(links_, status.links, true); + } + level_ = std::min(level_, DiagnosticStatus::ERROR); } DebugUnit::DebugUnit(const std::string & path, const DiagnosticLevel level) : BaseUnit(path) @@ -129,11 +152,8 @@ void DebugUnit::init(const UnitConfig::SharedPtr &, const NodeDict &) { } -DiagnosticNode DebugUnit::update(const rclcpp::Time &) +void DebugUnit::update(const rclcpp::Time &) { - DiagnosticNode message; - message.status.name = path_; - return message; } } // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/units.hpp b/system/system_diagnostic_graph/src/core/units.hpp index a014d7f7b182a..7a1816a959b2c 100644 --- a/system/system_diagnostic_graph/src/core/units.hpp +++ b/system/system_diagnostic_graph/src/core/units.hpp @@ -49,21 +49,24 @@ class BaseUnit explicit BaseUnit(const std::string & path); virtual ~BaseUnit() = default; virtual void init(const UnitConfig::SharedPtr & config, const NodeDict & dict) = 0; - virtual void eval() = 0; + virtual void update(const rclcpp::Time & stamp) = 0; + + DiagnosticLevel level() const { return level_; } + NodeData status(); + // NodeData report(); + DiagnosticNode report(const rclcpp::Time &) const { return DiagnosticNode(); } - DiagnosticNode report(const rclcpp::Time & stamp); - auto level() const { return level_; } - auto links() const { return links_; } auto path() const { return path_; } + auto children() const { return children_; } size_t index() const { return index_; } void set_index(const size_t index) { index_ = index; } protected: - virtual DiagnosticNode update(const rclcpp::Time & stamp) = 0; DiagnosticLevel level_; - std::vector> links_; std::string path_; + std::vector children_; + std::vector> links_; private: size_t index_; @@ -74,15 +77,14 @@ class DiagUnit : public BaseUnit public: using BaseUnit::BaseUnit; void init(const UnitConfig::SharedPtr & config, const NodeDict & dict) override; - void eval() override; + void update(const rclcpp::Time & stamp) override; std::string name() const { return name_; } - void callback(const DiagnosticStatus & status, const rclcpp::Time & stamp); + void callback(const rclcpp::Time & stamp, const DiagnosticStatus & status); private: - DiagnosticNode update(const rclcpp::Time & stamp) override; double timeout_; - std::optional> diagnostics_; + std::optional> diagnostics_; std::string name_; }; @@ -91,10 +93,9 @@ class AndUnit : public BaseUnit public: AndUnit(const std::string & path, bool short_circuit); void init(const UnitConfig::SharedPtr & config, const NodeDict & dict) override; - void eval() override; + void update(const rclcpp::Time & stamp) override; private: - DiagnosticNode update(const rclcpp::Time & stamp) override; bool short_circuit_; }; @@ -103,10 +104,9 @@ class OrUnit : public BaseUnit public: using BaseUnit::BaseUnit; void init(const UnitConfig::SharedPtr & config, const NodeDict & dict) override; - void eval() override; + void update(const rclcpp::Time & stamp) override; private: - DiagnosticNode update(const rclcpp::Time & stamp) override; }; class DebugUnit : public BaseUnit @@ -114,10 +114,9 @@ class DebugUnit : public BaseUnit public: DebugUnit(const std::string & path, const DiagnosticLevel level); void init(const UnitConfig::SharedPtr & config, const NodeDict & dict) override; - void eval() override; + void update(const rclcpp::Time & stamp) override; private: - DiagnosticNode update(const rclcpp::Time & stamp) override; DiagnosticLevel const_; }; diff --git a/system/system_diagnostic_graph/src/main.cpp b/system/system_diagnostic_graph/src/main.cpp index 8a8d3a1bc2189..7db35d1fd2551 100644 --- a/system/system_diagnostic_graph/src/main.cpp +++ b/system/system_diagnostic_graph/src/main.cpp @@ -65,7 +65,7 @@ void MainNode::on_timer() void MainNode::on_diag(const DiagnosticArray::ConstSharedPtr msg) { - graph_.callback(*msg, now()); + graph_.callback(now(), *msg); } } // namespace system_diagnostic_graph From 034512ee626aa974a02fc2fad95c997c6040e429 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Wed, 25 Oct 2023 20:17:15 +0900 Subject: [PATCH 04/12] change report function Signed-off-by: Takagi, Isamu --- .../system_diagnostic_graph/src/core/graph.cpp | 12 +++++++++++- .../system_diagnostic_graph/src/core/units.cpp | 13 +++++++++---- .../system_diagnostic_graph/src/core/units.hpp | 16 +++++----------- 3 files changed, 25 insertions(+), 16 deletions(-) diff --git a/system/system_diagnostic_graph/src/core/graph.cpp b/system/system_diagnostic_graph/src/core/graph.cpp index 3d44473d643e2..5513a65003b2a 100644 --- a/system/system_diagnostic_graph/src/core/graph.cpp +++ b/system/system_diagnostic_graph/src/core/graph.cpp @@ -186,7 +186,17 @@ DiagnosticGraph Graph::report(const rclcpp::Time & stamp) message.stamp = stamp; message.nodes.reserve(nodes_.size()); for (const auto & node : nodes_) { - message.nodes.push_back(node->report(stamp)); + const auto report = node->report(); + DiagnosticNode temp; + temp.status.name = node->path(); + temp.status.level = report.level; + for (const auto & [ref, used] : report.links) { + DiagnosticLink link; + link.index = ref->index(); + link.used = used; + temp.links.push_back(link); + } + message.nodes.push_back(temp); } return message; } diff --git a/system/system_diagnostic_graph/src/core/units.cpp b/system/system_diagnostic_graph/src/core/units.cpp index c04e36fe92a41..67556c94974a3 100644 --- a/system/system_diagnostic_graph/src/core/units.cpp +++ b/system/system_diagnostic_graph/src/core/units.cpp @@ -22,7 +22,7 @@ namespace system_diagnostic_graph { -using LinkList = std::vector>; +using LinkList = std::vector>; void merge(LinkList & a, const LinkList & b, bool uses) { @@ -53,7 +53,7 @@ BaseUnit::BaseUnit(const std::string & path) : path_(path) level_ = DiagnosticStatus::OK; } -BaseUnit::NodeData BaseUnit::status() +BaseUnit::NodeData BaseUnit::status() const { if (path_.empty()) { return {level_, links_}; @@ -62,6 +62,11 @@ BaseUnit::NodeData BaseUnit::status() } } +BaseUnit::NodeData BaseUnit::report() const +{ + return {level_, links_}; +} + void DiagUnit::init(const UnitConfig::SharedPtr & config, const NodeDict &) { timeout_ = 3.0; // TODO(Takagi, Isamu): parameterize @@ -143,9 +148,9 @@ void OrUnit::update(const rclcpp::Time &) level_ = std::min(level_, DiagnosticStatus::ERROR); } -DebugUnit::DebugUnit(const std::string & path, const DiagnosticLevel level) : BaseUnit(path) +DebugUnit::DebugUnit(const std::string & path, DiagnosticLevel level) : BaseUnit(path) { - const_ = level; + level_ = level; // overwrite } void DebugUnit::init(const UnitConfig::SharedPtr &, const NodeDict &) diff --git a/system/system_diagnostic_graph/src/core/units.hpp b/system/system_diagnostic_graph/src/core/units.hpp index 7a1816a959b2c..ad5fa4c4bc090 100644 --- a/system/system_diagnostic_graph/src/core/units.hpp +++ b/system/system_diagnostic_graph/src/core/units.hpp @@ -41,7 +41,7 @@ class BaseUnit struct NodeData { DiagnosticLevel level; - std::vector> links; + std::vector> links; }; using UniquePtr = std::unique_ptr; using UniquePtrList = std::vector>; @@ -51,10 +51,9 @@ class BaseUnit virtual void init(const UnitConfig::SharedPtr & config, const NodeDict & dict) = 0; virtual void update(const rclcpp::Time & stamp) = 0; + NodeData status() const; + NodeData report() const; DiagnosticLevel level() const { return level_; } - NodeData status(); - // NodeData report(); - DiagnosticNode report(const rclcpp::Time &) const { return DiagnosticNode(); } auto path() const { return path_; } auto children() const { return children_; } @@ -66,7 +65,7 @@ class BaseUnit DiagnosticLevel level_; std::string path_; std::vector children_; - std::vector> links_; + std::vector> links_; private: size_t index_; @@ -105,19 +104,14 @@ class OrUnit : public BaseUnit using BaseUnit::BaseUnit; void init(const UnitConfig::SharedPtr & config, const NodeDict & dict) override; void update(const rclcpp::Time & stamp) override; - -private: }; class DebugUnit : public BaseUnit { public: - DebugUnit(const std::string & path, const DiagnosticLevel level); + DebugUnit(const std::string & path, DiagnosticLevel level); void init(const UnitConfig::SharedPtr & config, const NodeDict & dict) override; void update(const rclcpp::Time & stamp) override; - -private: - DiagnosticLevel const_; }; } // namespace system_diagnostic_graph From ba93044ccf49f37d8248d80d09ce0c9614892e93 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Thu, 26 Oct 2023 20:02:27 +0900 Subject: [PATCH 05/12] create tests Signed-off-by: Takagi, Isamu --- system/system_diagnostic_graph/CMakeLists.txt | 6 ++++ .../example/example_0.yaml | 9 ++++-- system/system_diagnostic_graph/package.xml | 1 + .../src/core/debug.cpp | 23 --------------- .../system_diagnostic_graph/test/src/test.cpp | 28 +++++++++++++++++++ 5 files changed, 42 insertions(+), 25 deletions(-) create mode 100644 system/system_diagnostic_graph/test/src/test.cpp diff --git a/system/system_diagnostic_graph/CMakeLists.txt b/system/system_diagnostic_graph/CMakeLists.txt index 9d2e1c0dd07c4..4b4f37891fa7b 100644 --- a/system/system_diagnostic_graph/CMakeLists.txt +++ b/system/system_diagnostic_graph/CMakeLists.txt @@ -17,4 +17,10 @@ ament_auto_add_executable(converter src/tool.cpp ) +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) + target_compile_definitions(gtest_${PROJECT_NAME} PRIVATE TEST_RESOURCE_PATH="${RESOURCE_PATH}") +endif() + ament_auto_package(INSTALL_TO_SHARE config example launch) diff --git a/system/system_diagnostic_graph/example/example_0.yaml b/system/system_diagnostic_graph/example/example_0.yaml index cdfef990fd31c..5e78a3e43a86e 100644 --- a/system/system_diagnostic_graph/example/example_0.yaml +++ b/system/system_diagnostic_graph/example/example_0.yaml @@ -26,7 +26,12 @@ nodes: type: debug-ok - path: /autoware/modes/comfortable-stop - type: debug-ok + type: and + list: + - { type: link, link: /functions/obstacle_detection } - path: /autoware/modes/pull-over - type: debug-ok + type: and + list: + - { type: link, link: /functions/pose_estimation } + - { type: link, link: /functions/obstacle_detection } diff --git a/system/system_diagnostic_graph/package.xml b/system/system_diagnostic_graph/package.xml index ffc0c6c7d5385..7855edcde9c62 100644 --- a/system/system_diagnostic_graph/package.xml +++ b/system/system_diagnostic_graph/package.xml @@ -15,6 +15,7 @@ tier4_system_msgs yaml_cpp_vendor + ament_cmake_gtest ament_lint_auto autoware_lint_common diff --git a/system/system_diagnostic_graph/src/core/debug.cpp b/system/system_diagnostic_graph/src/core/debug.cpp index 45c097b8947b5..f14177f4571ad 100644 --- a/system/system_diagnostic_graph/src/core/debug.cpp +++ b/system/system_diagnostic_graph/src/core/debug.cpp @@ -59,27 +59,4 @@ void Graph::debug() } } -/* -DiagDebugData UnitNode::debug() const -{ - const auto level_name = level_names.at(level()); - const auto index_name = std::to_string(index()); - return {"unit", index_name, level_name, path_, "-----"}; -} - -DiagDebugData DiagNode::debug() const -{ - const auto level_name = level_names.at(level()); - const auto index_name = std::to_string(index()); - return {"diag", index_name, level_name, path_, name_}; -} - -DiagDebugData UnknownNode::debug() const -{ - const auto level_name = level_names.at(level()); - const auto index_name = std::to_string(index()); - return {"test", index_name, level_name, path_, "-----"}; -} -*/ - } // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/test/src/test.cpp b/system/system_diagnostic_graph/test/src/test.cpp new file mode 100644 index 0000000000000..d5aa4cf5cacea --- /dev/null +++ b/system/system_diagnostic_graph/test/src/test.cpp @@ -0,0 +1,28 @@ +// 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 + +#include +#include + +std::filesystem::path resource(const std::string & path) +{ + return std::filesystem::path(TEST_RESOURCE_PATH) / path; +} + +TEST(TestSuite, TestName) +{ + std::cout << resource("main.yaml") << std::endl; +} From a37adf240b4b7212eb8dc7f06674164921fcee08 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Thu, 26 Oct 2023 20:29:22 +0900 Subject: [PATCH 06/12] WIP Signed-off-by: Takagi, Isamu --- .../system_diagnostic_graph/src/core/config.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/system/system_diagnostic_graph/src/core/config.cpp b/system/system_diagnostic_graph/src/core/config.cpp index 8b419dfcc1c22..439afc4a5a7a4 100644 --- a/system/system_diagnostic_graph/src/core/config.cpp +++ b/system/system_diagnostic_graph/src/core/config.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include #include @@ -194,6 +195,18 @@ void check_config_nodes(const std::vector & nodes) } } +std::string resolve_file_path(const std::string & path) +{ + static const std::regex pattern(R"(\$\([^()]*\))"); + std::smatch m; + std::regex_search(path, m, pattern); + + std::cout << path << std::endl; + std::cout << m.str() << std::endl; + + return path; +} + void resolve_link_nodes(std::vector & nodes) { std::vector filtered; @@ -242,6 +255,9 @@ RootConfig load_config_root(const std::string & path) extend(config.nodes, node->children); } + resolve_file_path("$(find-pkg-share system_diagnostic_graph)/aaa/bbb/ccc.yaml"); + resolve_file_path("$(dirname)/$(find-pkg-share aaa)/bbb/ccc.yaml"); + check_config_nodes(config.nodes); resolve_link_nodes(config.nodes); return config; From 8afff1acf39cfdf30c718d6d8bdf202b6daa1e03 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Fri, 27 Oct 2023 16:48:08 +0900 Subject: [PATCH 07/12] change config structure Signed-off-by: Takagi, Isamu --- .../example/example_0.yaml | 4 +- .../src/core/config.cpp | 216 +++++++++--------- .../src/core/config.hpp | 29 ++- 3 files changed, 136 insertions(+), 113 deletions(-) diff --git a/system/system_diagnostic_graph/example/example_0.yaml b/system/system_diagnostic_graph/example/example_0.yaml index 5e78a3e43a86e..eb39868fd40de 100644 --- a/system/system_diagnostic_graph/example/example_0.yaml +++ b/system/system_diagnostic_graph/example/example_0.yaml @@ -1,6 +1,6 @@ files: - - { package: system_diagnostic_graph, path: example/example_1.yaml } - - { package: system_diagnostic_graph, path: example/example_2.yaml } + - { path: $(find-pkg-share system_diagnostic_graph)/example/example_1.yaml } + - { path: $(find-pkg-share system_diagnostic_graph)/example/example_2.yaml } nodes: - path: /autoware/modes/stop diff --git a/system/system_diagnostic_graph/src/core/config.cpp b/system/system_diagnostic_graph/src/core/config.cpp index 439afc4a5a7a4..3e43cccd2d760 100644 --- a/system/system_diagnostic_graph/src/core/config.cpp +++ b/system/system_diagnostic_graph/src/core/config.cpp @@ -31,6 +31,11 @@ namespace system_diagnostic_graph { +ConfigData::ConfigData(const std::string & path) +{ + file = path; +} + ConfigData ConfigData::load(YAML::Node yaml) { if (!yaml.IsMap()) { @@ -44,16 +49,14 @@ ConfigData ConfigData::load(YAML::Node yaml) ConfigData ConfigData::type(const std::string & name) const { - ConfigData data; - data.file = file; + ConfigData data(file); data.mark = name; return data; } ConfigData ConfigData::node(const size_t index) const { - ConfigData data; - data.file = file; + ConfigData data(file); data.mark = mark + "-" + std::to_string(index); return data; } @@ -111,75 +114,6 @@ auto enumerate(const std::vector & v) return result; } -UnitConfig::SharedPtr parse_node_config(const ConfigData & data) -{ - const auto node = std::make_shared(); - node->data = data; - node->path = node->data.take_text("path", ""); - node->type = node->data.take_text("type"); - - for (const auto & [index, yaml] : enumerate(node->data.take_list("list"))) { - const auto child = data.node(index).load(yaml); - node->children.push_back(parse_node_config(child)); - } - return node; -} - -FileConfig::SharedPtr parse_file_config(const ConfigData & data) -{ - const auto file = std::make_shared(); - file->data = data; - file->package_name = file->data.take_text("package"); - file->package_path = file->data.take_text("path"); - return file; -} - -void dump_node(const UnitConfig & config, const std::string & indent = "") -{ - std::cout << indent << " - node: " << config.type << " (" << config.path << ")" << std::endl; - for (const auto & child : config.children) dump_node(*child, indent + " "); -} - -void dump_file(const FileConfig & config) -{ - std::cout << "=================================================================" << std::endl; - std::cout << config.path << std::endl; - for (const auto & file : config.files) { - std::cout << " - file: " << file->package_name << "/" << file->package_path << std::endl; - } - for (const auto & node : config.nodes) { - dump_node(*node); - } -} - -void load_config_file(FileConfig & config, const ConfigData & parent) -{ - if (config.path.empty()) { - const auto package_base = ament_index_cpp::get_package_share_directory(config.package_name); - config.path = package_base + "/" + config.package_path; - } - - if (!std::filesystem::exists(config.path)) { - (void)parent; - throw ConfigError("TODO"); - } - - ConfigData data; - data.file = config.path.empty() ? config.package_path + "/" + config.package_path : "root"; - data.load(YAML::LoadFile(config.path)); - const auto files = data.take_list("files"); - const auto nodes = data.take_list("nodes"); - - for (const auto & [index, yaml] : enumerate(files)) { - const auto file = data.type("file").node(index).load(yaml); - config.files.push_back(parse_file_config(file)); - } - for (const auto & [index, yaml] : enumerate(nodes)) { - const auto node = data.type("file").node(index).load(yaml); - config.nodes.push_back(parse_node_config(node)); - } -} - void check_config_nodes(const std::vector & nodes) { std::unordered_map path_count; @@ -195,18 +129,6 @@ void check_config_nodes(const std::vector & nodes) } } -std::string resolve_file_path(const std::string & path) -{ - static const std::regex pattern(R"(\$\([^()]*\))"); - std::smatch m; - std::regex_search(path, m, pattern); - - std::cout << path << std::endl; - std::cout << m.str() << std::endl; - - return path; -} - void resolve_link_nodes(std::vector & nodes) { std::vector filtered; @@ -234,33 +156,123 @@ void resolve_link_nodes(std::vector & nodes) } } -RootConfig load_config_root(const std::string & path) +std::string resolve_substitution(const std::string & substitution, const ConfigData & data) { - RootConfig config; - config.files.push_back(std::make_shared()); - config.files.back()->path = path; + std::stringstream ss(substitution); + std::string word; + std::vector words; + while (getline(ss, word, ' ')) { + words.push_back(word); + } - for (size_t i = 0; i < config.files.size(); ++i) { - const auto & file = config.files[i]; - load_config_file(*file, {}); - extend(config.files, file->files); + if (words.size() == 2 && words[0] == "find-pkg-share") { + return ament_index_cpp::get_package_share_directory(words[1]); } + if (words.size() == 1 && words[0] == "dirname") { + return std::filesystem::path(data.file).parent_path(); + } + throw ConfigError("unknown substitution: " + substitution); +} - for (const auto & file : config.files) { - extend(config.nodes, file->nodes); +std::string resolve_file_path(const std::string & path, const ConfigData & data) +{ + static const std::regex pattern(R"(\$\(([^()]*)\))"); + std::smatch m; + std::string result = path; + while (std::regex_search(result, m, pattern)) { + const std::string prefix = m.prefix(); + const std::string suffix = m.suffix(); + result = prefix + resolve_substitution(m.str(1), data) + suffix; } + return result; +} - for (size_t i = 0; i < config.nodes.size(); ++i) { - const auto & node = config.nodes[i]; - extend(config.nodes, node->children); +PathConfig::SharedPtr parse_path_config(const ConfigData & data) +{ + const auto path = std::make_shared(data); + path->original = path->data.take_text("path"); + path->resolved = resolve_file_path(path->original, path->data); + return path; +} + +UnitConfig::SharedPtr parse_node_config(const ConfigData & data) +{ + const auto node = std::make_shared(data); + node->path = node->data.take_text("path", ""); + node->type = node->data.take_text("type"); + + for (const auto & [index, yaml] : enumerate(node->data.take_list("list"))) { + const auto child = data.node(index).load(yaml); + node->children.push_back(parse_node_config(child)); } + return node; +} - resolve_file_path("$(find-pkg-share system_diagnostic_graph)/aaa/bbb/ccc.yaml"); - resolve_file_path("$(dirname)/$(find-pkg-share aaa)/bbb/ccc.yaml"); +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 paths = file->data.take_list("files"); + const auto nodes = file->data.take_list("nodes"); + + for (const auto & [index, yaml] : enumerate(paths)) { + const auto path = path_data.node(index).load(yaml); + file->paths.push_back(parse_path_config(path)); + } + for (const auto & [index, yaml] : enumerate(nodes)) { + const auto node = node_data.node(index).load(yaml); + file->nodes.push_back(parse_node_config(node)); + } + return file; +} + +FileConfig::SharedPtr load_config_file(PathConfig & config) +{ + const auto path = std::filesystem::path(config.resolved); + if (!std::filesystem::exists(path)) { + (void)config.data; + throw ConfigError("TODO: file does not exist"); + } + const auto file = ConfigData(path).load(YAML::LoadFile(path)); + return parse_file_config(file); +} - check_config_nodes(config.nodes); - resolve_link_nodes(config.nodes); +RootConfig load_config_root(const PathConfig::SharedPtr root) +{ + std::vector paths; + paths.push_back(root); + + std::vector files; + for (size_t i = 0; i < paths.size(); ++i) { + const auto path = paths[i]; + const auto file = load_config_file(*path); + files.push_back(file); + extend(paths, file->paths); + } + + std::vector nodes; + for (const auto & file : files) { + extend(nodes, file->nodes); + } + 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.nodes = nodes; return config; } +RootConfig load_config_root(const std::string & path) +{ + const auto root = std::make_shared(ConfigData("root")); + root->original = path; + root->resolved = path; + return load_config_root(root); +} + } // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/config.hpp b/system/system_diagnostic_graph/src/core/config.hpp index e9f9165a08c12..36e0174fe37aa 100644 --- a/system/system_diagnostic_graph/src/core/config.hpp +++ b/system/system_diagnostic_graph/src/core/config.hpp @@ -27,6 +27,7 @@ namespace system_diagnostic_graph struct ConfigData { + explicit ConfigData(const std::string & file); ConfigData load(YAML::Node yaml); ConfigData type(const std::string & name) const; ConfigData node(const size_t index) const; @@ -40,29 +41,39 @@ struct ConfigData std::unordered_map object; }; -struct UnitConfig +struct BaseConfig +{ + explicit BaseConfig(const ConfigData & data) : data(data) {} + ConfigData data; +}; + +struct PathConfig : public BaseConfig +{ + using SharedPtr = std::shared_ptr; + using BaseConfig::BaseConfig; + std::string original; + std::string resolved; +}; + +struct UnitConfig : public BaseConfig { using SharedPtr = std::shared_ptr; + using BaseConfig::BaseConfig; std::string type; std::string path; std::vector children; - ConfigData data; }; -struct FileConfig +struct FileConfig : public BaseConfig { using SharedPtr = std::shared_ptr; - std::string package_name; - std::string package_path; - std::string path; - std::vector files; + using BaseConfig::BaseConfig; + std::vector paths; std::vector nodes; - ConfigData data; }; struct RootConfig { - std::vector files; std::vector nodes; }; From 671e33f44cc360e1d29abb4216e7966666191762 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Fri, 27 Oct 2023 21:54:52 +0900 Subject: [PATCH 08/12] add tests Signed-off-by: Takagi, Isamu --- system/system_diagnostic_graph/CMakeLists.txt | 6 ++- .../src/core/config.cpp | 44 +++++++++++-------- .../src/core/error.hpp | 8 +++- .../test/files/file-not-found.yaml | 2 + .../system_diagnostic_graph/test/src/test.cpp | 16 ++++++- 5 files changed, 53 insertions(+), 23 deletions(-) create mode 100644 system/system_diagnostic_graph/test/files/file-not-found.yaml diff --git a/system/system_diagnostic_graph/CMakeLists.txt b/system/system_diagnostic_graph/CMakeLists.txt index 4b4f37891fa7b..142aa94eeef69 100644 --- a/system/system_diagnostic_graph/CMakeLists.txt +++ b/system/system_diagnostic_graph/CMakeLists.txt @@ -4,12 +4,15 @@ project(system_diagnostic_graph) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(aggregator +ament_auto_add_library(${PROJECT_NAME} SHARED src/core/config.cpp src/core/debug.cpp src/core/graph.cpp src/core/units.cpp src/core/modes.cpp +) + +ament_auto_add_executable(aggregator src/main.cpp ) @@ -21,6 +24,7 @@ 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) target_compile_definitions(gtest_${PROJECT_NAME} PRIVATE TEST_RESOURCE_PATH="${RESOURCE_PATH}") + target_include_directories(gtest_${PROJECT_NAME} PRIVATE src) endif() ament_auto_package(INSTALL_TO_SHARE config example launch) diff --git a/system/system_diagnostic_graph/src/core/config.cpp b/system/system_diagnostic_graph/src/core/config.cpp index 3e43cccd2d760..7ec4dc8332e7c 100644 --- a/system/system_diagnostic_graph/src/core/config.cpp +++ b/system/system_diagnostic_graph/src/core/config.cpp @@ -31,6 +31,29 @@ namespace system_diagnostic_graph { +template +T error(const std::string & text, const std::string & value, const ConfigData & data) +{ + const auto hint = data.mark.empty() ? data.file : data.mark + ":" + data.file; + return T(text + ": " + value + " (" + hint + ")"); +} + +template +void extend(std::vector & u, const std::vector & v) +{ + u.insert(u.end(), v.begin(), v.end()); +} + +template +auto enumerate(const std::vector & v) +{ + std::vector> result; + for (size_t i = 0; i < v.size(); ++i) { + result.push_back(std::make_pair(i, v[i])); + } + return result; +} + ConfigData::ConfigData(const std::string & path) { file = path; @@ -98,22 +121,6 @@ std::vector ConfigData::take_list(const std::string & name) return std::vector(yaml.begin(), yaml.end()); } -template -void extend(std::vector & u, const std::vector & v) -{ - u.insert(u.end(), v.begin(), v.end()); -} - -template -auto enumerate(const std::vector & v) -{ - std::vector> result; - for (size_t i = 0; i < v.size(); ++i) { - result.push_back(std::make_pair(i, v[i])); - } - return result; -} - void check_config_nodes(const std::vector & nodes) { std::unordered_map path_count; @@ -231,8 +238,7 @@ FileConfig::SharedPtr load_config_file(PathConfig & config) { const auto path = std::filesystem::path(config.resolved); if (!std::filesystem::exists(path)) { - (void)config.data; - throw ConfigError("TODO: file does not exist"); + throw error("file does not found", path, config.data); } const auto file = ConfigData(path).load(YAML::LoadFile(path)); return parse_file_config(file); @@ -269,7 +275,7 @@ RootConfig load_config_root(const PathConfig::SharedPtr root) RootConfig load_config_root(const std::string & path) { - const auto root = std::make_shared(ConfigData("root")); + const auto root = std::make_shared(ConfigData("root-file")); root->original = path; root->resolved = path; return load_config_root(root); diff --git a/system/system_diagnostic_graph/src/core/error.hpp b/system/system_diagnostic_graph/src/core/error.hpp index 1aa4b88eb91f8..d735dd9237b04 100644 --- a/system/system_diagnostic_graph/src/core/error.hpp +++ b/system/system_diagnostic_graph/src/core/error.hpp @@ -20,10 +20,16 @@ namespace system_diagnostic_graph { -struct ConfigError : public std::runtime_error +struct Exception : public std::runtime_error { using runtime_error::runtime_error; }; +using ConfigError = Exception; + +class FileNotFound : public Exception +{ + using Exception::Exception; +}; } // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/test/files/file-not-found.yaml b/system/system_diagnostic_graph/test/files/file-not-found.yaml new file mode 100644 index 0000000000000..ffc83b42096f0 --- /dev/null +++ b/system/system_diagnostic_graph/test/files/file-not-found.yaml @@ -0,0 +1,2 @@ +files: + - { path: $(dirname)/fake-file-name.yaml } diff --git a/system/system_diagnostic_graph/test/src/test.cpp b/system/system_diagnostic_graph/test/src/test.cpp index d5aa4cf5cacea..889001fcafd41 100644 --- a/system/system_diagnostic_graph/test/src/test.cpp +++ b/system/system_diagnostic_graph/test/src/test.cpp @@ -12,17 +12,29 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "core/error.hpp" +#include "core/graph.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(TestSuite, TestName) +TEST(TestSuite, RootNotFound) +{ + Graph graph; + EXPECT_THROW(graph.init(resource("fake-file-name.yaml"), ""), FileNotFound); +} + +TEST(TestSuite, FileNotFound) { - std::cout << resource("main.yaml") << std::endl; + Graph graph; + EXPECT_THROW(graph.init(resource("file-not-found.yaml"), ""), FileNotFound); } From 5aa2c7b9327f4e39105e54ede65de57780d4ad28 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Mon, 30 Oct 2023 21:03:13 +0900 Subject: [PATCH 09/12] update config errors Signed-off-by: Takagi, Isamu --- .../example/example_1.yaml | 6 ++-- .../example/example_2.yaml | 4 +-- .../src/core/config.cpp | 31 ++++++++----------- .../src/core/config.hpp | 28 ++++++++++++++++- .../src/core/error.hpp | 31 ++++++++++++++++++- .../src/core/graph.cpp | 6 ++-- .../src/core/modes.cpp | 2 +- .../src/core/units.cpp | 2 +- 8 files changed, 80 insertions(+), 30 deletions(-) diff --git a/system/system_diagnostic_graph/example/example_1.yaml b/system/system_diagnostic_graph/example/example_1.yaml index 8851bc15b93fd..07d4951b89446 100644 --- a/system/system_diagnostic_graph/example/example_1.yaml +++ b/system/system_diagnostic_graph/example/example_1.yaml @@ -12,12 +12,12 @@ nodes: - path: /sensing/lidars/top type: diag - name: "lidar_driver/top: status" + diag: "lidar_driver/top: status" - path: /sensing/lidars/front type: diag - name: "lidar_driver/front: status" + diag: "lidar_driver/front: status" - path: /sensing/radars/front type: diag - name: "radar_driver/front: status" + diag: "radar_driver/front: status" diff --git a/system/system_diagnostic_graph/example/example_2.yaml b/system/system_diagnostic_graph/example/example_2.yaml index f61f4accbfec8..a03701b661ba9 100644 --- a/system/system_diagnostic_graph/example/example_2.yaml +++ b/system/system_diagnostic_graph/example/example_2.yaml @@ -1,8 +1,8 @@ nodes: - path: /external/joystick_command type: diag - name: "external_command_checker: joystick_command" + diag: "external_command_checker: joystick_command" - path: /external/remote_command type: diag - name: "external_command_checker: remote_command" + diag: "external_command_checker: remote_command" diff --git a/system/system_diagnostic_graph/src/core/config.cpp b/system/system_diagnostic_graph/src/core/config.cpp index 7ec4dc8332e7c..7f089e359e152 100644 --- a/system/system_diagnostic_graph/src/core/config.cpp +++ b/system/system_diagnostic_graph/src/core/config.cpp @@ -31,13 +31,6 @@ namespace system_diagnostic_graph { -template -T error(const std::string & text, const std::string & value, const ConfigData & data) -{ - const auto hint = data.mark.empty() ? data.file : data.mark + ":" + data.file; - return T(text + ": " + value + " (" + hint + ")"); -} - template void extend(std::vector & u, const std::vector & v) { @@ -62,7 +55,7 @@ ConfigData::ConfigData(const std::string & path) ConfigData ConfigData::load(YAML::Node yaml) { if (!yaml.IsMap()) { - throw ConfigError(std::string("TODO") + " is not a dict type"); + throw error("object is not a dict type", *this); } for (const auto & kv : yaml) { object[kv.first.as()] = kv.second; @@ -87,7 +80,7 @@ ConfigData ConfigData::node(const size_t index) const std::string ConfigData::take_text(const std::string & name) { if (!object.count(name)) { - throw ConfigError("object has no '" + name + "' field"); + throw error("required field is not found", name, *this); } const auto yaml = object.at(name); @@ -116,7 +109,7 @@ std::vector ConfigData::take_list(const std::string & name) object.erase(name); if (!yaml.IsSequence()) { - throw ConfigError("the '" + name + "' field is not a list type"); + throw error("field is not a list type", name, *this); } return std::vector(yaml.begin(), yaml.end()); } @@ -131,7 +124,7 @@ void check_config_nodes(const std::vector & nodes) path_count.erase(""); for (const auto & [path, count] : path_count) { if (1 < count) { - throw ConfigError("TODO: path conflict"); + throw error("object path is not unique", path); } } } @@ -178,7 +171,7 @@ std::string resolve_substitution(const std::string & substitution, const ConfigD if (words.size() == 1 && words[0] == "dirname") { return std::filesystem::path(data.file).parent_path(); } - throw ConfigError("unknown substitution: " + substitution); + throw error("unknown substitution", substitution, data); } std::string resolve_file_path(const std::string & path, const ConfigData & data) @@ -234,17 +227,17 @@ FileConfig::SharedPtr parse_file_config(const ConfigData & data) return file; } -FileConfig::SharedPtr load_config_file(PathConfig & config) +FileConfig::SharedPtr load_file_config(PathConfig & config) { const auto path = std::filesystem::path(config.resolved); if (!std::filesystem::exists(path)) { - throw error("file does not found", path, config.data); + throw error("file is not found", path, config.data); } const auto file = ConfigData(path).load(YAML::LoadFile(path)); return parse_file_config(file); } -RootConfig load_config_root(const PathConfig::SharedPtr root) +RootConfig load_root_config(const PathConfig::SharedPtr root) { std::vector paths; paths.push_back(root); @@ -252,7 +245,7 @@ RootConfig load_config_root(const PathConfig::SharedPtr root) std::vector files; for (size_t i = 0; i < paths.size(); ++i) { const auto path = paths[i]; - const auto file = load_config_file(*path); + const auto file = load_file_config(*path); files.push_back(file); extend(paths, file->paths); } @@ -265,20 +258,22 @@ RootConfig load_config_root(const PathConfig::SharedPtr root) 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; return config; } -RootConfig load_config_root(const std::string & path) +RootConfig load_root_config(const std::string & path) { const auto root = std::make_shared(ConfigData("root-file")); root->original = path; root->resolved = path; - return load_config_root(root); + return load_root_config(root); } } // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/config.hpp b/system/system_diagnostic_graph/src/core/config.hpp index 36e0174fe37aa..d959f5b6be8aa 100644 --- a/system/system_diagnostic_graph/src/core/config.hpp +++ b/system/system_diagnostic_graph/src/core/config.hpp @@ -74,10 +74,36 @@ struct FileConfig : public BaseConfig struct RootConfig { + std::vector files; std::vector nodes; }; -RootConfig load_config_root(const std::string & path); +template +T error(const std::string & text, const ConfigData & data) +{ + const auto hint = data.mark.empty() ? data.file : data.mark + ":" + data.file; + return T(text + " (" + hint + ")"); +} + +template +T error(const std::string & text) +{ + return T(text); +} + +template +T error(const std::string & text, const std::string & value, const ConfigData & data) +{ + return error(text + ": " + value, data); +} + +template +T error(const std::string & text, const std::string & value) +{ + return error(text + ": " + value); +} + +RootConfig load_root_config(const std::string & path); } // 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 d735dd9237b04..e64ec4c5bd625 100644 --- a/system/system_diagnostic_graph/src/core/error.hpp +++ b/system/system_diagnostic_graph/src/core/error.hpp @@ -24,13 +24,42 @@ struct Exception : public std::runtime_error { using runtime_error::runtime_error; }; -using ConfigError = Exception; class FileNotFound : public Exception { using Exception::Exception; }; +class UnknownType : public Exception +{ + using Exception::Exception; +}; + +class InvalidType : public Exception +{ + using Exception::Exception; +}; + +class FieldNotFound : public Exception +{ + using Exception::Exception; +}; + +class PathNotFound : public Exception +{ + using Exception::Exception; +}; + +class PathConflict : public Exception +{ + using Exception::Exception; +}; + +class GraphStructure : public Exception +{ + using Exception::Exception; +}; + } // namespace system_diagnostic_graph #endif // CORE__ERROR_HPP_ diff --git a/system/system_diagnostic_graph/src/core/graph.cpp b/system/system_diagnostic_graph/src/core/graph.cpp index 5513a65003b2a..c87b3755bb654 100644 --- a/system/system_diagnostic_graph/src/core/graph.cpp +++ b/system/system_diagnostic_graph/src/core/graph.cpp @@ -71,7 +71,7 @@ BaseUnit::UniquePtrList topological_sort(BaseUnit::UniquePtrList && input) // Detect circulation because the result does not include the nodes on the loop. if (result.size() != nodes.size()) { - throw ConfigError("detect graph circulation"); + throw error("detect graph circulation"); } // Reverse the result to process from leaf node. @@ -115,7 +115,7 @@ BaseUnit::UniquePtr make_node(const UnitConfig::SharedPtr & config) if (config->type == "debug-stale") { return std::make_unique(config->path, DiagnosticStatus::STALE); } - throw ConfigError("unknown node type: " + config->type + " " + "TODO"); + throw error("unknown node type", config->type, config->data); } Graph::Graph() @@ -135,7 +135,7 @@ void Graph::init(const std::string & file, const std::string & mode) BaseUnit::UniquePtrList nodes; BaseUnit::NodeDict dict; - for (const auto & config : load_config_root(file).nodes) { + for (const auto & config : load_root_config(file).nodes) { const auto node = nodes.emplace_back(make_node(config)).get(); dict.configs[config] = node; dict.paths[config->path] = node; diff --git a/system/system_diagnostic_graph/src/core/modes.cpp b/system/system_diagnostic_graph/src/core/modes.cpp index a2e3d27791465..96944bd50f81a 100644 --- a/system/system_diagnostic_graph/src/core/modes.cpp +++ b/system/system_diagnostic_graph/src/core/modes.cpp @@ -40,7 +40,7 @@ OperationModes::OperationModes(rclcpp::Node & node, const std::vectorsecond; } - throw ConfigError("summary node '" + name + "' does node exist"); + throw error("summary node is not found", name); }; // clang-format off diff --git a/system/system_diagnostic_graph/src/core/units.cpp b/system/system_diagnostic_graph/src/core/units.cpp index 67556c94974a3..7cdd7c3e1266e 100644 --- a/system/system_diagnostic_graph/src/core/units.cpp +++ b/system/system_diagnostic_graph/src/core/units.cpp @@ -70,7 +70,7 @@ 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("name", path_); + name_ = config->data.take_text("diag"); } void DiagUnit::update(const rclcpp::Time & stamp) From e9923129e716b8894e9f4f865a2f450ddd1fe73d Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Tue, 31 Oct 2023 15:43:19 +0900 Subject: [PATCH 10/12] update tests Signed-off-by: Takagi, Isamu --- .../src/core/config.cpp | 6 +- .../src/core/error.hpp | 4 +- .../src/core/graph.hpp | 2 +- .../test/files/field-not-found.yaml | 2 + .../test/files/graph-circulation.yaml | 9 +++ .../test/files/invalid-dict-type.yaml | 1 + .../test/files/invalid-list-type.yaml | 3 + .../test/files/path-conflict.yaml | 5 ++ .../test/files/path-not-found.yaml | 5 ++ .../test/files/unknown-node-type.yaml | 2 + .../test/files/unknown-substitution.yaml | 2 + .../system_diagnostic_graph/test/src/test.cpp | 56 +++++++++++++++++-- 12 files changed, 89 insertions(+), 8 deletions(-) create mode 100644 system/system_diagnostic_graph/test/files/field-not-found.yaml create mode 100644 system/system_diagnostic_graph/test/files/graph-circulation.yaml create mode 100644 system/system_diagnostic_graph/test/files/invalid-dict-type.yaml create mode 100644 system/system_diagnostic_graph/test/files/invalid-list-type.yaml create mode 100644 system/system_diagnostic_graph/test/files/path-conflict.yaml create mode 100644 system/system_diagnostic_graph/test/files/path-not-found.yaml create mode 100644 system/system_diagnostic_graph/test/files/unknown-node-type.yaml create mode 100644 system/system_diagnostic_graph/test/files/unknown-substitution.yaml diff --git a/system/system_diagnostic_graph/src/core/config.cpp b/system/system_diagnostic_graph/src/core/config.cpp index 7f089e359e152..5b9346fa741f0 100644 --- a/system/system_diagnostic_graph/src/core/config.cpp +++ b/system/system_diagnostic_graph/src/core/config.cpp @@ -142,7 +142,11 @@ void resolve_link_nodes(std::vector & nodes) for (const auto & node : nodes) { if (node->type == "link" && node->path == "") { - links[node] = paths.at(node->data.take_text("link")); + const auto link = node->data.take_text("link"); + if (!paths.count(link)) { + throw error("link path is not found", link, node->data); + } + links[node] = paths.at(link); } else { filtered.push_back(node); } diff --git a/system/system_diagnostic_graph/src/core/error.hpp b/system/system_diagnostic_graph/src/core/error.hpp index e64ec4c5bd625..2c10d659f2df4 100644 --- a/system/system_diagnostic_graph/src/core/error.hpp +++ b/system/system_diagnostic_graph/src/core/error.hpp @@ -45,12 +45,12 @@ class FieldNotFound : public Exception using Exception::Exception; }; -class PathNotFound : public Exception +class PathConflict : public Exception { using Exception::Exception; }; -class PathConflict : public Exception +class PathNotFound : public Exception { using Exception::Exception; }; diff --git a/system/system_diagnostic_graph/src/core/graph.hpp b/system/system_diagnostic_graph/src/core/graph.hpp index 70c3e09696a83..366f6b457e272 100644 --- a/system/system_diagnostic_graph/src/core/graph.hpp +++ b/system/system_diagnostic_graph/src/core/graph.hpp @@ -34,7 +34,7 @@ class Graph final Graph(); ~Graph(); - void init(const std::string & file, const std::string & mode); + void init(const std::string & file, const std::string & mode = ""); void callback(const rclcpp::Time & stamp, const DiagnosticArray & array); DiagnosticGraph report(const rclcpp::Time & stamp); std::vector nodes() const; diff --git a/system/system_diagnostic_graph/test/files/field-not-found.yaml b/system/system_diagnostic_graph/test/files/field-not-found.yaml new file mode 100644 index 0000000000000..9e2b3708c049a --- /dev/null +++ b/system/system_diagnostic_graph/test/files/field-not-found.yaml @@ -0,0 +1,2 @@ +nodes: + - test-type: test-type diff --git a/system/system_diagnostic_graph/test/files/graph-circulation.yaml b/system/system_diagnostic_graph/test/files/graph-circulation.yaml new file mode 100644 index 0000000000000..4c014bdcec3f9 --- /dev/null +++ b/system/system_diagnostic_graph/test/files/graph-circulation.yaml @@ -0,0 +1,9 @@ +nodes: + - path: /foo/bar + type: and + list: + - { type: link, link: /foo/baz } + - path: /foo/baz + type: and + list: + - { type: link, link: /foo/bar } diff --git a/system/system_diagnostic_graph/test/files/invalid-dict-type.yaml b/system/system_diagnostic_graph/test/files/invalid-dict-type.yaml new file mode 100644 index 0000000000000..6499b05ab8b7d --- /dev/null +++ b/system/system_diagnostic_graph/test/files/invalid-dict-type.yaml @@ -0,0 +1 @@ +nodes: test-object diff --git a/system/system_diagnostic_graph/test/files/invalid-list-type.yaml b/system/system_diagnostic_graph/test/files/invalid-list-type.yaml new file mode 100644 index 0000000000000..4fc5d96c08c62 --- /dev/null +++ b/system/system_diagnostic_graph/test/files/invalid-list-type.yaml @@ -0,0 +1,3 @@ +nodes: + - type: and + list: test-list diff --git a/system/system_diagnostic_graph/test/files/path-conflict.yaml b/system/system_diagnostic_graph/test/files/path-conflict.yaml new file mode 100644 index 0000000000000..ea3e536b74216 --- /dev/null +++ b/system/system_diagnostic_graph/test/files/path-conflict.yaml @@ -0,0 +1,5 @@ +nodes: + - path: /foo/bar + type: and + - path: /foo/bar + type: and diff --git a/system/system_diagnostic_graph/test/files/path-not-found.yaml b/system/system_diagnostic_graph/test/files/path-not-found.yaml new file mode 100644 index 0000000000000..6b0b10dfa94f4 --- /dev/null +++ b/system/system_diagnostic_graph/test/files/path-not-found.yaml @@ -0,0 +1,5 @@ +nodes: + - path: /foo/bar + type: and + - link: /foo/baz + type: link diff --git a/system/system_diagnostic_graph/test/files/unknown-node-type.yaml b/system/system_diagnostic_graph/test/files/unknown-node-type.yaml new file mode 100644 index 0000000000000..a3d66fbfa43cb --- /dev/null +++ b/system/system_diagnostic_graph/test/files/unknown-node-type.yaml @@ -0,0 +1,2 @@ +nodes: + - type: test-type diff --git a/system/system_diagnostic_graph/test/files/unknown-substitution.yaml b/system/system_diagnostic_graph/test/files/unknown-substitution.yaml new file mode 100644 index 0000000000000..7286321c30439 --- /dev/null +++ b/system/system_diagnostic_graph/test/files/unknown-substitution.yaml @@ -0,0 +1,2 @@ +files: + - { path: $(dummy) } diff --git a/system/system_diagnostic_graph/test/src/test.cpp b/system/system_diagnostic_graph/test/src/test.cpp index 889001fcafd41..b763179be0791 100644 --- a/system/system_diagnostic_graph/test/src/test.cpp +++ b/system/system_diagnostic_graph/test/src/test.cpp @@ -27,14 +27,62 @@ std::filesystem::path resource(const std::string & path) return std::filesystem::path(TEST_RESOURCE_PATH) / path; } -TEST(TestSuite, RootNotFound) +TEST(ConfigFile, RootNotFound) { Graph graph; - EXPECT_THROW(graph.init(resource("fake-file-name.yaml"), ""), FileNotFound); + EXPECT_THROW(graph.init(resource("fake-file-name.yaml")), FileNotFound); } -TEST(TestSuite, FileNotFound) +TEST(ConfigFile, FileNotFound) { Graph graph; - EXPECT_THROW(graph.init(resource("file-not-found.yaml"), ""), FileNotFound); + EXPECT_THROW(graph.init(resource("file-not-found.yaml")), FileNotFound); +} + +TEST(ConfigFile, UnknownSubstitution) +{ + Graph graph; + EXPECT_THROW(graph.init(resource("unknown-substitution.yaml")), UnknownType); +} + +TEST(ConfigFile, UnknownNodeType) +{ + Graph graph; + EXPECT_THROW(graph.init(resource("unknown-node-type.yaml")), UnknownType); +} + +TEST(ConfigFile, InvalidDictType) +{ + Graph graph; + EXPECT_THROW(graph.init(resource("invalid-dict-type.yaml")), InvalidType); +} + +TEST(ConfigFile, InvalidListType) +{ + Graph graph; + EXPECT_THROW(graph.init(resource("invalid-list-type.yaml")), InvalidType); +} + +TEST(ConfigFile, FieldNotFound) +{ + Graph graph; + EXPECT_THROW(graph.init(resource("field-not-found.yaml")), FieldNotFound); +} + +TEST(ConfigFile, PathConflict) +{ + Graph graph; + EXPECT_THROW(graph.init(resource("path-conflict.yaml")), PathConflict); +} + +TEST(ConfigFile, PathNotFound) +{ + Graph graph; + EXPECT_THROW(graph.init(resource("path-not-found.yaml")), PathNotFound); +} + +TEST(ConfigFile, GraphCirculation) +{ + Graph graph; + EXPECT_THROW(graph.init(resource("graph-circulation.yaml")), GraphStructure); } From cc9bf26c9f754092a9a6c5f312fbff3560f49cfd Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Tue, 31 Oct 2023 20:18:02 +0900 Subject: [PATCH 11/12] add type estimation Signed-off-by: Takagi, Isamu --- system/system_diagnostic_graph/src/core/config.cpp | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/system/system_diagnostic_graph/src/core/config.cpp b/system/system_diagnostic_graph/src/core/config.cpp index 5b9346fa741f0..2339e96f3951f 100644 --- a/system/system_diagnostic_graph/src/core/config.cpp +++ b/system/system_diagnostic_graph/src/core/config.cpp @@ -160,6 +160,14 @@ void resolve_link_nodes(std::vector & nodes) } } +std::string complement_node_type(ConfigData & data) +{ + if (data.object.count("diag")) { + return "diag"; + } + return data.take_text("type"); +} + std::string resolve_substitution(const std::string & substitution, const ConfigData & data) { std::stringstream ss(substitution); @@ -203,7 +211,11 @@ UnitConfig::SharedPtr parse_node_config(const ConfigData & data) { const auto node = std::make_shared(data); node->path = node->data.take_text("path", ""); - node->type = node->data.take_text("type"); + node->type = node->data.take_text("type", ""); + + if (node->type.empty()) { + node->type = complement_node_type(node->data); + } for (const auto & [index, yaml] : enumerate(node->data.take_list("list"))) { const auto child = data.node(index).load(yaml); From d0e47c983ac0a5cf7793001d0929dbde09ffced2 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Wed, 1 Nov 2023 19:40:43 +0900 Subject: [PATCH 12/12] modify constant type name Signed-off-by: Takagi, Isamu --- system/system_diagnostic_graph/example/example_0.yaml | 4 ++-- system/system_diagnostic_graph/src/core/graph.cpp | 8 ++++---- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/system/system_diagnostic_graph/example/example_0.yaml b/system/system_diagnostic_graph/example/example_0.yaml index eb39868fd40de..fc4436bba1595 100644 --- a/system/system_diagnostic_graph/example/example_0.yaml +++ b/system/system_diagnostic_graph/example/example_0.yaml @@ -4,7 +4,7 @@ files: nodes: - path: /autoware/modes/stop - type: debug-ok + type: ok - path: /autoware/modes/autonomous type: and @@ -23,7 +23,7 @@ nodes: - { type: link, link: /external/remote_command } - path: /autoware/modes/emergency-stop - type: debug-ok + type: ok - path: /autoware/modes/comfortable-stop type: and diff --git a/system/system_diagnostic_graph/src/core/graph.cpp b/system/system_diagnostic_graph/src/core/graph.cpp index c87b3755bb654..96e9bcff5bfd9 100644 --- a/system/system_diagnostic_graph/src/core/graph.cpp +++ b/system/system_diagnostic_graph/src/core/graph.cpp @@ -103,16 +103,16 @@ BaseUnit::UniquePtr make_node(const UnitConfig::SharedPtr & config) if (config->type == "or") { return std::make_unique(config->path); } - if (config->type == "debug-ok") { + if (config->type == "ok") { return std::make_unique(config->path, DiagnosticStatus::OK); } - if (config->type == "debug-warn") { + if (config->type == "warn") { return std::make_unique(config->path, DiagnosticStatus::WARN); } - if (config->type == "debug-error") { + if (config->type == "error") { return std::make_unique(config->path, DiagnosticStatus::ERROR); } - if (config->type == "debug-stale") { + if (config->type == "stale") { return std::make_unique(config->path, DiagnosticStatus::STALE); } throw error("unknown node type", config->type, config->data);