From 62bc9e1f996c20aac1c8e72926ef20819bf32169 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Wed, 25 Oct 2023 05:42:15 +0900 Subject: [PATCH 01/26] 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/26] 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/26] 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/26] 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 544fb0a89ffd3f0ccf2c24439b07396c84deb758 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Thu, 26 Oct 2023 18:49:38 +0900 Subject: [PATCH 05/26] create package Signed-off-by: Takagi, Isamu --- .../system_diagnostic_monitor/CMakeLists.txt | 13 ++ system/system_diagnostic_monitor/package.xml | 23 ++++ .../script/hazard_status_converter.py | 116 ++++++++++++++++++ 3 files changed, 152 insertions(+) create mode 100644 system/system_diagnostic_monitor/CMakeLists.txt create mode 100644 system/system_diagnostic_monitor/package.xml create mode 100755 system/system_diagnostic_monitor/script/hazard_status_converter.py diff --git a/system/system_diagnostic_monitor/CMakeLists.txt b/system/system_diagnostic_monitor/CMakeLists.txt new file mode 100644 index 0000000000000..36c1bf301bd88 --- /dev/null +++ b/system/system_diagnostic_monitor/CMakeLists.txt @@ -0,0 +1,13 @@ +cmake_minimum_required(VERSION 3.14) +project(system_diagnostic_monitor) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +install(PROGRAMS + script/hazard_status_converter.py +# RENAME hazard_status_converter + DESTINATION lib/${PROJECT_NAME} +) + +ament_auto_package() diff --git a/system/system_diagnostic_monitor/package.xml b/system/system_diagnostic_monitor/package.xml new file mode 100644 index 0000000000000..22156c462690a --- /dev/null +++ b/system/system_diagnostic_monitor/package.xml @@ -0,0 +1,23 @@ + + + + system_diagnostic_monitor + 0.1.0 + The system_diagnostic_monitor package + Takagi, Isamu + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_adapi_v1_msgs + diagnostic_msgs + system_diagnostic_graph + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/system/system_diagnostic_monitor/script/hazard_status_converter.py b/system/system_diagnostic_monitor/script/hazard_status_converter.py new file mode 100755 index 0000000000000..90485a924b4a8 --- /dev/null +++ b/system/system_diagnostic_monitor/script/hazard_status_converter.py @@ -0,0 +1,116 @@ +#!/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. + +from autoware_adapi_v1_msgs.msg import OperationModeState +from diagnostic_msgs.msg import DiagnosticStatus +import rclpy +import rclpy.node +import rclpy.qos +import rclpy.time +from tier4_system_msgs.msg import DiagnosticGraph +from tier4_system_msgs.msg import HazardStatus + + +def level_text(level): + if level == DiagnosticStatus.OK: + return "OK " + if level == DiagnosticStatus.WARN: + return "WARN " + if level == DiagnosticStatus.ERROR: + return "ERROR" + if level == DiagnosticStatus.STALE: + return "STALE" + if level is None: + return "NONE " + return "-----" + + +class DiagNode: + def __init__(self, node): + self.node = node + self.view = DiagnosticStatus.OK + + @property + def level(self): + return self.node.status.level + + @property + def name(self): + return self.node.status.name + + @property + def links(self): + for link in self.node.links: + if link.used: + yield link.index + + @property + def category(self): + if self.level == DiagnosticStatus.OK: + return "NF " + if self.view == DiagnosticStatus.OK: + return "SF " + if self.view == DiagnosticStatus.WARN: + return "LF " + return "SPF" + + +class HazardStatusConverter(rclpy.node.Node): + def __init__(self): + super().__init__("hazard_status_converter") + self.create_interface() + self.mode_name = "/autoware/modes/autonomous" + + def create_interface(self): + durable_qos = rclpy.qos.QoSProfile( + depth=1, + durability=rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL, + reliability=rclpy.qos.ReliabilityPolicy.RELIABLE, + ) + self.sub_graph = self.create_subscription( + DiagnosticGraph, "/diagnostics_graph", self.on_graph, 1 + ) + self.sub_modes = self.create_subscription( + OperationModeState, "/api/operation_mode/state", self.on_modes, durable_qos + ) + self.pub_hazard = self.create_publisher(HazardStatus, "/hazard_status2", 1) + + def on_modes(self, msg): + pass + + def on_graph(self, msg): + nodes = [DiagNode(node) for node in msg.nodes] + for node in nodes: + for index in node.links: + link = nodes[index] + link.view = max(link.view, node.level) + if node.name == self.mode_name: + node.view = node.level + + print("=" * 100) + for node in nodes: + print( + level_text(node.level), level_text(node.view), node.category, node.node.status.name + ) + + hazards = HazardStatus() + self.pub_hazard.publish(hazards) + + +if __name__ == "__main__": + rclpy.init() + rclpy.spin(HazardStatusConverter()) + rclpy.shutdown() From ba93044ccf49f37d8248d80d09ce0c9614892e93 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Thu, 26 Oct 2023 20:02:27 +0900 Subject: [PATCH 06/26] 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 07/26] 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 08/26] 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 09/26] 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 10/26] 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 11/26] 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 e5ac8b10feb0987c58ece398a180853f26c89f73 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Tue, 31 Oct 2023 20:15:33 +0900 Subject: [PATCH 12/26] add config Signed-off-by: Takagi, Isamu --- .../system_diagnostic_monitor/CMakeLists.txt | 2 +- .../config/autoware.yaml | 63 +++++++++++++++++++ .../config/control.yaml | 12 ++++ .../config/localization.yaml | 8 +++ .../system_diagnostic_monitor/config/map.yaml | 16 +++++ .../config/perception.yaml | 8 +++ .../config/planning.yaml | 18 ++++++ .../config/system.yaml | 8 +++ .../config/vehicle.yaml | 12 ++++ system/system_diagnostic_monitor/package.xml | 2 +- 10 files changed, 147 insertions(+), 2 deletions(-) create mode 100644 system/system_diagnostic_monitor/config/autoware.yaml create mode 100644 system/system_diagnostic_monitor/config/control.yaml create mode 100644 system/system_diagnostic_monitor/config/localization.yaml create mode 100644 system/system_diagnostic_monitor/config/map.yaml create mode 100644 system/system_diagnostic_monitor/config/perception.yaml create mode 100644 system/system_diagnostic_monitor/config/planning.yaml create mode 100644 system/system_diagnostic_monitor/config/system.yaml create mode 100644 system/system_diagnostic_monitor/config/vehicle.yaml diff --git a/system/system_diagnostic_monitor/CMakeLists.txt b/system/system_diagnostic_monitor/CMakeLists.txt index 36c1bf301bd88..986133941741b 100644 --- a/system/system_diagnostic_monitor/CMakeLists.txt +++ b/system/system_diagnostic_monitor/CMakeLists.txt @@ -10,4 +10,4 @@ install(PROGRAMS DESTINATION lib/${PROJECT_NAME} ) -ament_auto_package() +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/system/system_diagnostic_monitor/config/autoware.yaml b/system/system_diagnostic_monitor/config/autoware.yaml new file mode 100644 index 0000000000000..f6959d52260f0 --- /dev/null +++ b/system/system_diagnostic_monitor/config/autoware.yaml @@ -0,0 +1,63 @@ +files: + - { path: $(find-pkg-share system_diagnostic_monitor)/config/map.yaml } + - { path: $(find-pkg-share system_diagnostic_monitor)/config/localization.yaml } + - { path: $(find-pkg-share system_diagnostic_monitor)/config/planning.yaml } + - { path: $(find-pkg-share system_diagnostic_monitor)/config/perception.yaml } + - { path: $(find-pkg-share system_diagnostic_monitor)/config/control.yaml } + - { path: $(find-pkg-share system_diagnostic_monitor)/config/vehicle.yaml } + - { path: $(find-pkg-share system_diagnostic_monitor)/config/system.yaml } + +nodes: + - path: /autoware/modes/stop + type: debug-ok + + - path: /autoware/modes/autonomous + type: and + list: + - { type: link, link: /autoware/map } + - { type: link, link: /autoware/localization } + - { type: link, link: /autoware/planning } + - { type: link, link: /autoware/perception } + - { type: link, link: /autoware/control } + - { type: link, link: /autoware/vehicle } + - { type: link, link: /autoware/system } + + - path: /autoware/modes/local + type: and + list: + - { type: link, link: /autoware/vehicle } + - { type: link, link: /autoware/system } + + - path: /autoware/modes/remote + type: and + list: + - { type: link, link: /autoware/vehicle } + - { type: link, link: /autoware/system } + + - path: /autoware/modes/emergency-stop + type: and + list: + - { type: link, link: /autoware/vehicle } + - { type: link, link: /autoware/system } + + - path: /autoware/modes/comfortable-stop + type: and + list: + - { type: link, link: /autoware/map } + - { type: link, link: /autoware/localization } + - { type: link, link: /autoware/planning } + - { type: link, link: /autoware/perception } + - { type: link, link: /autoware/control } + - { type: link, link: /autoware/vehicle } + - { type: link, link: /autoware/system } + + - path: /autoware/modes/pull-over + type: and + list: + - { type: link, link: /autoware/map } + - { type: link, link: /autoware/localization } + - { type: link, link: /autoware/planning } + - { type: link, link: /autoware/perception } + - { type: link, link: /autoware/control } + - { type: link, link: /autoware/vehicle } + - { type: link, link: /autoware/system } diff --git a/system/system_diagnostic_monitor/config/control.yaml b/system/system_diagnostic_monitor/config/control.yaml new file mode 100644 index 0000000000000..2f45ecba672f6 --- /dev/null +++ b/system/system_diagnostic_monitor/config/control.yaml @@ -0,0 +1,12 @@ +nodes: + - path: /autoware/control + type: and + list: + - { type: link, link: /autoware/control/topic_rate_check/trajectory_follower } + - { type: link, link: /autoware/control/topic_rate_check/control_command } + + - path: /autoware/control/topic_rate_check/trajectory_follower + diag: "topic_state_monitor_trajectory_follower_control_cmd: control_topic_status" + + - path: /autoware/control/topic_rate_check/control_command + diag: "topic_state_monitor_control_command_control_cmd: control_topic_status" diff --git a/system/system_diagnostic_monitor/config/localization.yaml b/system/system_diagnostic_monitor/config/localization.yaml new file mode 100644 index 0000000000000..03631560217bc --- /dev/null +++ b/system/system_diagnostic_monitor/config/localization.yaml @@ -0,0 +1,8 @@ +nodes: + - path: /autoware/localization + type: and + list: + - { type: link, link: /autoware/localization/state } + + - path: /autoware/localization/state + diag: "component_state_diagnostics: localization_state" diff --git a/system/system_diagnostic_monitor/config/map.yaml b/system/system_diagnostic_monitor/config/map.yaml new file mode 100644 index 0000000000000..920668a1f3251 --- /dev/null +++ b/system/system_diagnostic_monitor/config/map.yaml @@ -0,0 +1,16 @@ +nodes: + - path: /autoware/map + type: and + list: + - type: link + link: /autoware/map/topic_rate_check/vector_map + - type: link + link: /autoware/map/topic_rate_check/pointcloud_map + mode: { except: [psim] } + + - path: /autoware/map/topic_rate_check/vector_map + diag: "topic_state_monitor_vector_map: map_topic_status" + + - path: /autoware/map/topic_rate_check/pointcloud_map + mode: { except: [psim] } + diag: "topic_state_monitor_pointcloud_map: map_topic_status" diff --git a/system/system_diagnostic_monitor/config/perception.yaml b/system/system_diagnostic_monitor/config/perception.yaml new file mode 100644 index 0000000000000..4b48ede565565 --- /dev/null +++ b/system/system_diagnostic_monitor/config/perception.yaml @@ -0,0 +1,8 @@ +nodes: + - path: /autoware/perception + type: and + list: + - { type: link, link: /autoware/perception/topic_rate_check/objects } + + - path: /autoware/perception/topic_rate_check/objects + diag: "topic_state_monitor_object_recognition_objects: perception_topic_status" diff --git a/system/system_diagnostic_monitor/config/planning.yaml b/system/system_diagnostic_monitor/config/planning.yaml new file mode 100644 index 0000000000000..3503fed714cd0 --- /dev/null +++ b/system/system_diagnostic_monitor/config/planning.yaml @@ -0,0 +1,18 @@ +nodes: + - path: /autoware/planning + type: short-circuit-and + list: + - { type: link, link: /autoware/planning/routing/state } + - type: and + list: + - { type: link, link: /autoware/planning/routing/topic_rate_check/route } + - { type: link, link: /autoware/planning/routing/topic_rate_check/trajectory } + + - path: /autoware/planning/routing/state + diag: "component_state_diagnostics: route_state" + + - path: /autoware/planning/routing/topic_rate_check/route + diag: "topic_state_monitor_mission_planning_route: planning_topic_status" + + - path: /autoware/planning/routing/topic_rate_check/trajectory + diag: "topic_state_monitor_scenario_planning_trajectory: planning_topic_status" diff --git a/system/system_diagnostic_monitor/config/system.yaml b/system/system_diagnostic_monitor/config/system.yaml new file mode 100644 index 0000000000000..556e3d2605117 --- /dev/null +++ b/system/system_diagnostic_monitor/config/system.yaml @@ -0,0 +1,8 @@ +nodes: + - path: /autoware/system + type: and + list: + - { type: link, link: /autoware/system/topic_rate_check/emergency_control_command } + + - path: /autoware/system/topic_rate_check/emergency_control_command + diag: "topic_state_monitor_system_emergency_control_cmd: system_topic_status" diff --git a/system/system_diagnostic_monitor/config/vehicle.yaml b/system/system_diagnostic_monitor/config/vehicle.yaml new file mode 100644 index 0000000000000..120559e398154 --- /dev/null +++ b/system/system_diagnostic_monitor/config/vehicle.yaml @@ -0,0 +1,12 @@ +nodes: + - path: /autoware/vehicle + type: and + list: + - { type: link, link: /autoware/vehicle/topic_rate_check/velocity } + - { type: link, link: /autoware/vehicle/topic_rate_check/steering } + + - path: /autoware/vehicle/topic_rate_check/velocity + diag: "topic_state_monitor_vehicle_status_velocity_status: vehicle_topic_status" + + - path: /autoware/vehicle/topic_rate_check/steering + diag: "topic_state_monitor_vehicle_status_steering_status: vehicle_topic_status" diff --git a/system/system_diagnostic_monitor/package.xml b/system/system_diagnostic_monitor/package.xml index 22156c462690a..9d9151d5a029f 100644 --- a/system/system_diagnostic_monitor/package.xml +++ b/system/system_diagnostic_monitor/package.xml @@ -12,7 +12,7 @@ autoware_adapi_v1_msgs diagnostic_msgs - system_diagnostic_graph + tier4_system_msgs ament_lint_auto autoware_lint_common From cc9bf26c9f754092a9a6c5f312fbff3560f49cfd Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Tue, 31 Oct 2023 20:18:02 +0900 Subject: [PATCH 13/26] 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 763393bd2b9d45873186b7f363aecc4acc5fd43e Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Wed, 1 Nov 2023 10:31:20 +0900 Subject: [PATCH 14/26] apply mode Signed-off-by: Takagi, Isamu --- .../script/hazard_status_converter.py | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/system/system_diagnostic_monitor/script/hazard_status_converter.py b/system/system_diagnostic_monitor/script/hazard_status_converter.py index 90485a924b4a8..f425e04a5f422 100755 --- a/system/system_diagnostic_monitor/script/hazard_status_converter.py +++ b/system/system_diagnostic_monitor/script/hazard_status_converter.py @@ -24,6 +24,17 @@ from tier4_system_msgs.msg import HazardStatus +def get_mode_name(mode): + if mode == OperationModeState.STOP: + return "/autoware/modes/stop" + elif mode == OperationModeState.AUTONOMOUS: + return "/autoware/modes/autonomous" + if mode == OperationModeState.LOCAL: + return "/autoware/modes/local" + if mode == OperationModeState.REMOTE: + return "/autoware/modes/remote" + + def level_text(level): if level == DiagnosticStatus.OK: return "OK " @@ -72,7 +83,7 @@ class HazardStatusConverter(rclpy.node.Node): def __init__(self): super().__init__("hazard_status_converter") self.create_interface() - self.mode_name = "/autoware/modes/autonomous" + self.mode_name = None def create_interface(self): durable_qos = rclpy.qos.QoSProfile( @@ -89,7 +100,7 @@ def create_interface(self): self.pub_hazard = self.create_publisher(HazardStatus, "/hazard_status2", 1) def on_modes(self, msg): - pass + self.mode_name = get_mode_name(msg.mode) def on_graph(self, msg): nodes = [DiagNode(node) for node in msg.nodes] From a63520c07803f9c25033215b4275617fe2c28b02 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Wed, 1 Nov 2023 14:25:16 +0900 Subject: [PATCH 15/26] publish hazard status Signed-off-by: Takagi, Isamu --- .../script/hazard_status_converter.py | 63 ++++++++++--------- 1 file changed, 35 insertions(+), 28 deletions(-) diff --git a/system/system_diagnostic_monitor/script/hazard_status_converter.py b/system/system_diagnostic_monitor/script/hazard_status_converter.py index f425e04a5f422..931f5487d8ef0 100755 --- a/system/system_diagnostic_monitor/script/hazard_status_converter.py +++ b/system/system_diagnostic_monitor/script/hazard_status_converter.py @@ -14,6 +14,8 @@ # See the License for the specific language governing permissions and # limitations under the License. +import enum + from autoware_adapi_v1_msgs.msg import OperationModeState from diagnostic_msgs.msg import DiagnosticStatus import rclpy @@ -24,6 +26,13 @@ from tier4_system_msgs.msg import HazardStatus +class FaultLevel(enum.Enum): + NF = 1 + SF = 2 + LF = 3 + SPF = 4 + + def get_mode_name(mode): if mode == OperationModeState.STOP: return "/autoware/modes/stop" @@ -35,24 +44,10 @@ def get_mode_name(mode): return "/autoware/modes/remote" -def level_text(level): - if level == DiagnosticStatus.OK: - return "OK " - if level == DiagnosticStatus.WARN: - return "WARN " - if level == DiagnosticStatus.ERROR: - return "ERROR" - if level == DiagnosticStatus.STALE: - return "STALE" - if level is None: - return "NONE " - return "-----" - - class DiagNode: def __init__(self, node): self.node = node - self.view = DiagnosticStatus.OK + self.view = [] @property def level(self): @@ -71,12 +66,12 @@ def links(self): @property def category(self): if self.level == DiagnosticStatus.OK: - return "NF " + return FaultLevel.NF if self.view == DiagnosticStatus.OK: - return "SF " + return FaultLevel.SF if self.view == DiagnosticStatus.WARN: - return "LF " - return "SPF" + return FaultLevel.LF + return FaultLevel.SPF class HazardStatusConverter(rclpy.node.Node): @@ -84,6 +79,7 @@ def __init__(self): super().__init__("hazard_status_converter") self.create_interface() self.mode_name = None + self.mode_name = "/autoware/modes/autonomous" def create_interface(self): durable_qos = rclpy.qos.QoSProfile( @@ -104,20 +100,31 @@ def on_modes(self, msg): def on_graph(self, msg): nodes = [DiagNode(node) for node in msg.nodes] + level = DiagnosticStatus.OK for node in nodes: + if node.name.startswith("/autoware/modes/"): + if node.name == self.mode_name: + level = node.level + node.view = [node.level] + else: + node.view = [DiagnosticStatus.OK] + + for node in reversed(nodes): + node.view = min(node.level, max(node.view, default=DiagnosticStatus.OK)) for index in node.links: - link = nodes[index] - link.view = max(link.view, node.level) - if node.name == self.mode_name: - node.view = node.level + nodes[index].view.append(node.view) - print("=" * 100) - for node in nodes: - print( - level_text(node.level), level_text(node.view), node.category, node.node.status.name - ) + faults = {level: [] for level in FaultLevel} + for node in reversed(nodes): + faults[node.category].append(node.node.status) hazards = HazardStatus() + hazards.level = int.from_bytes(level, "little") + hazards.emergency = DiagnosticStatus.ERROR <= level + hazards.diagnostics_nf = faults[FaultLevel.NF] + hazards.diagnostics_sf = faults[FaultLevel.SF] + hazards.diagnostics_lf = faults[FaultLevel.LF] + hazards.diagnostics_spf = faults[FaultLevel.SPF] self.pub_hazard.publish(hazards) From 019d2fc74486ff9e8dfd5766db4f1a6fd03a4dd9 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Wed, 1 Nov 2023 14:34:19 +0900 Subject: [PATCH 16/26] fix debug code Signed-off-by: Takagi, Isamu --- .../system_diagnostic_monitor/script/hazard_status_converter.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/system/system_diagnostic_monitor/script/hazard_status_converter.py b/system/system_diagnostic_monitor/script/hazard_status_converter.py index 931f5487d8ef0..1fd735d740c7c 100755 --- a/system/system_diagnostic_monitor/script/hazard_status_converter.py +++ b/system/system_diagnostic_monitor/script/hazard_status_converter.py @@ -93,7 +93,7 @@ def create_interface(self): self.sub_modes = self.create_subscription( OperationModeState, "/api/operation_mode/state", self.on_modes, durable_qos ) - self.pub_hazard = self.create_publisher(HazardStatus, "/hazard_status2", 1) + self.pub_hazard = self.create_publisher(HazardStatus, "/hazard_status", 1) def on_modes(self, msg): self.mode_name = get_mode_name(msg.mode) From 5edf30a35725e950304c01ccc1c03dd541109ebe Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Wed, 1 Nov 2023 16:10:30 +0900 Subject: [PATCH 17/26] add component state check Signed-off-by: Takagi, Isamu --- .../system_diagnostic_monitor/CMakeLists.txt | 8 +- .../system_diagnostic_monitor.launch.xml | 7 ++ system/system_diagnostic_monitor/package.xml | 2 + .../script/component_state_diagnostics.py | 99 +++++++++++++++++++ 4 files changed, 115 insertions(+), 1 deletion(-) create mode 100644 system/system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml create mode 100755 system/system_diagnostic_monitor/script/component_state_diagnostics.py diff --git a/system/system_diagnostic_monitor/CMakeLists.txt b/system/system_diagnostic_monitor/CMakeLists.txt index 986133941741b..892bbb51acbf5 100644 --- a/system/system_diagnostic_monitor/CMakeLists.txt +++ b/system/system_diagnostic_monitor/CMakeLists.txt @@ -4,10 +4,16 @@ project(system_diagnostic_monitor) find_package(autoware_cmake REQUIRED) autoware_package() +install(PROGRAMS + script/component_state_diagnostics.py + RENAME component_state_diagnostics + DESTINATION lib/${PROJECT_NAME} +) + install(PROGRAMS script/hazard_status_converter.py # RENAME hazard_status_converter DESTINATION lib/${PROJECT_NAME} ) -ament_auto_package(INSTALL_TO_SHARE config) +ament_auto_package(INSTALL_TO_SHARE config launch) diff --git a/system/system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml b/system/system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml new file mode 100644 index 0000000000000..507ace9d08d78 --- /dev/null +++ b/system/system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml @@ -0,0 +1,7 @@ + + + + + + + diff --git a/system/system_diagnostic_monitor/package.xml b/system/system_diagnostic_monitor/package.xml index 9d9151d5a029f..9dda02c66d68f 100644 --- a/system/system_diagnostic_monitor/package.xml +++ b/system/system_diagnostic_monitor/package.xml @@ -12,6 +12,8 @@ autoware_adapi_v1_msgs diagnostic_msgs + rclpy + system_diagnostic_graph tier4_system_msgs ament_lint_auto diff --git a/system/system_diagnostic_monitor/script/component_state_diagnostics.py b/system/system_diagnostic_monitor/script/component_state_diagnostics.py new file mode 100755 index 0000000000000..60c7ebb597414 --- /dev/null +++ b/system/system_diagnostic_monitor/script/component_state_diagnostics.py @@ -0,0 +1,99 @@ +#!/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. + +from autoware_adapi_v1_msgs.msg import LocalizationInitializationState as LocalizationState +from autoware_adapi_v1_msgs.msg import RouteState +from diagnostic_msgs.msg import DiagnosticArray +from diagnostic_msgs.msg import DiagnosticStatus +import rclpy +import rclpy.node +import rclpy.qos +import rclpy.time + + +class ComponentStateDiagnostics: + def __init__(self, node, diag_name, topic_name, topic_type, topic_qos, diag_func): + self.node = node + self.name = diag_name + self.func = diag_func + self.sub = node.create_subscription(topic_type, topic_name, self.callback, topic_qos) + self.level = DiagnosticStatus.STALE + self.stamp = None + + def callback(self, msg): + self.level = DiagnosticStatus.OK if self.func(msg) else DiagnosticStatus.ERROR + self.stamp = self.node.get_clock().now() + + def update(self, stamp): + if self.stamp: + elapsed = (stamp - self.stamp).nanoseconds * 1e-9 + if 3.0 < elapsed: + self.level = DiagnosticStatus.STALE + self.stamp = None + + def message(self): + status = DiagnosticStatus() + status.name = self.name + status.level = self.level + return status + + +class ComponentStateDiagnosticsNode(rclpy.node.Node): + def __init__(self): + super().__init__("component_state_diagnostics") + durable_qos = rclpy.qos.QoSProfile( + depth=1, + durability=rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL, + reliability=rclpy.qos.ReliabilityPolicy.RELIABLE, + ) + + self.timer = self.create_timer(0.5, self.on_timer) + self.pub = self.create_publisher(DiagnosticArray, "/diagnostics", 1) + self.states = [] + self.states.append( + ComponentStateDiagnostics( + self, + "component_state_diagnostics: localization_state", + "/api/localization/initialization_state", + LocalizationState, + durable_qos, + lambda msg: msg.state == LocalizationState.INITIALIZED, + ) + ) + self.states.append( + ComponentStateDiagnostics( + self, + "component_state_diagnostics: route_state", + "/api/routing/state", + RouteState, + durable_qos, + lambda msg: msg.state != RouteState.UNSET, + ) + ) + + def on_timer(self): + stamp = self.get_clock().now() + array = DiagnosticArray() + array.header.stamp = stamp.to_msg() + for state in self.states: + array.status.append(state.message()) + self.pub.publish(array) + + +if __name__ == "__main__": + rclpy.init() + rclpy.spin(ComponentStateDiagnosticsNode()) + rclpy.shutdown() From d0e47c983ac0a5cf7793001d0929dbde09ffced2 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Wed, 1 Nov 2023 19:40:43 +0900 Subject: [PATCH 18/26] 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); From 075fb9359ced76be45201245f2de1bbc5d1bd69f Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Thu, 2 Nov 2023 10:28:18 +0900 Subject: [PATCH 19/26] fix cmake Signed-off-by: Takagi, Isamu --- system/system_diagnostic_monitor/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/system/system_diagnostic_monitor/CMakeLists.txt b/system/system_diagnostic_monitor/CMakeLists.txt index 892bbb51acbf5..1b8145b9b4c8a 100644 --- a/system/system_diagnostic_monitor/CMakeLists.txt +++ b/system/system_diagnostic_monitor/CMakeLists.txt @@ -12,7 +12,7 @@ install(PROGRAMS install(PROGRAMS script/hazard_status_converter.py -# RENAME hazard_status_converter + RENAME hazard_status_converter DESTINATION lib/${PROJECT_NAME} ) From 1e840417381ae9073bf5312fe75e79c413185a14 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Thu, 2 Nov 2023 11:09:30 +0900 Subject: [PATCH 20/26] update config type name Signed-off-by: Takagi, Isamu --- system/system_diagnostic_monitor/config/autoware.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/system/system_diagnostic_monitor/config/autoware.yaml b/system/system_diagnostic_monitor/config/autoware.yaml index f6959d52260f0..b46caa6c556ee 100644 --- a/system/system_diagnostic_monitor/config/autoware.yaml +++ b/system/system_diagnostic_monitor/config/autoware.yaml @@ -9,7 +9,7 @@ files: nodes: - path: /autoware/modes/stop - type: debug-ok + type: ok - path: /autoware/modes/autonomous type: and From 1d37cad3ac05b3f76d881aaae7d271ad63a8118e Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Wed, 8 Nov 2023 19:42:07 +0900 Subject: [PATCH 21/26] fix conversion table Signed-off-by: Takagi, Isamu --- .../system_diagnostic_monitor/script/hazard_status_converter.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/system/system_diagnostic_monitor/script/hazard_status_converter.py b/system/system_diagnostic_monitor/script/hazard_status_converter.py index 1fd735d740c7c..84a0a3611383f 100755 --- a/system/system_diagnostic_monitor/script/hazard_status_converter.py +++ b/system/system_diagnostic_monitor/script/hazard_status_converter.py @@ -69,6 +69,8 @@ def category(self): return FaultLevel.NF if self.view == DiagnosticStatus.OK: return FaultLevel.SF + if self.level == DiagnosticStatus.WARN: + return FaultLevel.LF if self.view == DiagnosticStatus.WARN: return FaultLevel.LF return FaultLevel.SPF From 11e3efc28d156bb93048d728abcb17035ece385a Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Wed, 20 Dec 2023 16:27:01 +0900 Subject: [PATCH 22/26] WIP Signed-off-by: Takagi, Isamu --- .../system_diagnostic_monitor/CMakeLists.txt | 12 --- .../config/autoware.yaml | 63 ------------ .../config/control.yaml | 12 --- .../config/localization.yaml | 8 -- .../system_diagnostic_monitor/config/map.yaml | 16 --- .../config/perception.yaml | 8 -- .../config/planning.yaml | 18 ---- .../config/system.yaml | 8 -- .../config/vehicle.yaml | 12 --- system/system_diagnostic_monitor/package.xml | 1 - .../script/component_state_diagnostics.py | 99 ------------------- 11 files changed, 257 deletions(-) delete mode 100644 system/system_diagnostic_monitor/config/autoware.yaml delete mode 100644 system/system_diagnostic_monitor/config/control.yaml delete mode 100644 system/system_diagnostic_monitor/config/localization.yaml delete mode 100644 system/system_diagnostic_monitor/config/map.yaml delete mode 100644 system/system_diagnostic_monitor/config/perception.yaml delete mode 100644 system/system_diagnostic_monitor/config/planning.yaml delete mode 100644 system/system_diagnostic_monitor/config/system.yaml delete mode 100644 system/system_diagnostic_monitor/config/vehicle.yaml delete mode 100755 system/system_diagnostic_monitor/script/component_state_diagnostics.py diff --git a/system/system_diagnostic_monitor/CMakeLists.txt b/system/system_diagnostic_monitor/CMakeLists.txt index 1b8145b9b4c8a..c07f39aef9793 100644 --- a/system/system_diagnostic_monitor/CMakeLists.txt +++ b/system/system_diagnostic_monitor/CMakeLists.txt @@ -4,16 +4,4 @@ project(system_diagnostic_monitor) find_package(autoware_cmake REQUIRED) autoware_package() -install(PROGRAMS - script/component_state_diagnostics.py - RENAME component_state_diagnostics - DESTINATION lib/${PROJECT_NAME} -) - -install(PROGRAMS - script/hazard_status_converter.py - RENAME hazard_status_converter - DESTINATION lib/${PROJECT_NAME} -) - ament_auto_package(INSTALL_TO_SHARE config launch) diff --git a/system/system_diagnostic_monitor/config/autoware.yaml b/system/system_diagnostic_monitor/config/autoware.yaml deleted file mode 100644 index b46caa6c556ee..0000000000000 --- a/system/system_diagnostic_monitor/config/autoware.yaml +++ /dev/null @@ -1,63 +0,0 @@ -files: - - { path: $(find-pkg-share system_diagnostic_monitor)/config/map.yaml } - - { path: $(find-pkg-share system_diagnostic_monitor)/config/localization.yaml } - - { path: $(find-pkg-share system_diagnostic_monitor)/config/planning.yaml } - - { path: $(find-pkg-share system_diagnostic_monitor)/config/perception.yaml } - - { path: $(find-pkg-share system_diagnostic_monitor)/config/control.yaml } - - { path: $(find-pkg-share system_diagnostic_monitor)/config/vehicle.yaml } - - { path: $(find-pkg-share system_diagnostic_monitor)/config/system.yaml } - -nodes: - - path: /autoware/modes/stop - type: ok - - - path: /autoware/modes/autonomous - type: and - list: - - { type: link, link: /autoware/map } - - { type: link, link: /autoware/localization } - - { type: link, link: /autoware/planning } - - { type: link, link: /autoware/perception } - - { type: link, link: /autoware/control } - - { type: link, link: /autoware/vehicle } - - { type: link, link: /autoware/system } - - - path: /autoware/modes/local - type: and - list: - - { type: link, link: /autoware/vehicle } - - { type: link, link: /autoware/system } - - - path: /autoware/modes/remote - type: and - list: - - { type: link, link: /autoware/vehicle } - - { type: link, link: /autoware/system } - - - path: /autoware/modes/emergency-stop - type: and - list: - - { type: link, link: /autoware/vehicle } - - { type: link, link: /autoware/system } - - - path: /autoware/modes/comfortable-stop - type: and - list: - - { type: link, link: /autoware/map } - - { type: link, link: /autoware/localization } - - { type: link, link: /autoware/planning } - - { type: link, link: /autoware/perception } - - { type: link, link: /autoware/control } - - { type: link, link: /autoware/vehicle } - - { type: link, link: /autoware/system } - - - path: /autoware/modes/pull-over - type: and - list: - - { type: link, link: /autoware/map } - - { type: link, link: /autoware/localization } - - { type: link, link: /autoware/planning } - - { type: link, link: /autoware/perception } - - { type: link, link: /autoware/control } - - { type: link, link: /autoware/vehicle } - - { type: link, link: /autoware/system } diff --git a/system/system_diagnostic_monitor/config/control.yaml b/system/system_diagnostic_monitor/config/control.yaml deleted file mode 100644 index 2f45ecba672f6..0000000000000 --- a/system/system_diagnostic_monitor/config/control.yaml +++ /dev/null @@ -1,12 +0,0 @@ -nodes: - - path: /autoware/control - type: and - list: - - { type: link, link: /autoware/control/topic_rate_check/trajectory_follower } - - { type: link, link: /autoware/control/topic_rate_check/control_command } - - - path: /autoware/control/topic_rate_check/trajectory_follower - diag: "topic_state_monitor_trajectory_follower_control_cmd: control_topic_status" - - - path: /autoware/control/topic_rate_check/control_command - diag: "topic_state_monitor_control_command_control_cmd: control_topic_status" diff --git a/system/system_diagnostic_monitor/config/localization.yaml b/system/system_diagnostic_monitor/config/localization.yaml deleted file mode 100644 index 03631560217bc..0000000000000 --- a/system/system_diagnostic_monitor/config/localization.yaml +++ /dev/null @@ -1,8 +0,0 @@ -nodes: - - path: /autoware/localization - type: and - list: - - { type: link, link: /autoware/localization/state } - - - path: /autoware/localization/state - diag: "component_state_diagnostics: localization_state" diff --git a/system/system_diagnostic_monitor/config/map.yaml b/system/system_diagnostic_monitor/config/map.yaml deleted file mode 100644 index 920668a1f3251..0000000000000 --- a/system/system_diagnostic_monitor/config/map.yaml +++ /dev/null @@ -1,16 +0,0 @@ -nodes: - - path: /autoware/map - type: and - list: - - type: link - link: /autoware/map/topic_rate_check/vector_map - - type: link - link: /autoware/map/topic_rate_check/pointcloud_map - mode: { except: [psim] } - - - path: /autoware/map/topic_rate_check/vector_map - diag: "topic_state_monitor_vector_map: map_topic_status" - - - path: /autoware/map/topic_rate_check/pointcloud_map - mode: { except: [psim] } - diag: "topic_state_monitor_pointcloud_map: map_topic_status" diff --git a/system/system_diagnostic_monitor/config/perception.yaml b/system/system_diagnostic_monitor/config/perception.yaml deleted file mode 100644 index 4b48ede565565..0000000000000 --- a/system/system_diagnostic_monitor/config/perception.yaml +++ /dev/null @@ -1,8 +0,0 @@ -nodes: - - path: /autoware/perception - type: and - list: - - { type: link, link: /autoware/perception/topic_rate_check/objects } - - - path: /autoware/perception/topic_rate_check/objects - diag: "topic_state_monitor_object_recognition_objects: perception_topic_status" diff --git a/system/system_diagnostic_monitor/config/planning.yaml b/system/system_diagnostic_monitor/config/planning.yaml deleted file mode 100644 index 3503fed714cd0..0000000000000 --- a/system/system_diagnostic_monitor/config/planning.yaml +++ /dev/null @@ -1,18 +0,0 @@ -nodes: - - path: /autoware/planning - type: short-circuit-and - list: - - { type: link, link: /autoware/planning/routing/state } - - type: and - list: - - { type: link, link: /autoware/planning/routing/topic_rate_check/route } - - { type: link, link: /autoware/planning/routing/topic_rate_check/trajectory } - - - path: /autoware/planning/routing/state - diag: "component_state_diagnostics: route_state" - - - path: /autoware/planning/routing/topic_rate_check/route - diag: "topic_state_monitor_mission_planning_route: planning_topic_status" - - - path: /autoware/planning/routing/topic_rate_check/trajectory - diag: "topic_state_monitor_scenario_planning_trajectory: planning_topic_status" diff --git a/system/system_diagnostic_monitor/config/system.yaml b/system/system_diagnostic_monitor/config/system.yaml deleted file mode 100644 index 556e3d2605117..0000000000000 --- a/system/system_diagnostic_monitor/config/system.yaml +++ /dev/null @@ -1,8 +0,0 @@ -nodes: - - path: /autoware/system - type: and - list: - - { type: link, link: /autoware/system/topic_rate_check/emergency_control_command } - - - path: /autoware/system/topic_rate_check/emergency_control_command - diag: "topic_state_monitor_system_emergency_control_cmd: system_topic_status" diff --git a/system/system_diagnostic_monitor/config/vehicle.yaml b/system/system_diagnostic_monitor/config/vehicle.yaml deleted file mode 100644 index 120559e398154..0000000000000 --- a/system/system_diagnostic_monitor/config/vehicle.yaml +++ /dev/null @@ -1,12 +0,0 @@ -nodes: - - path: /autoware/vehicle - type: and - list: - - { type: link, link: /autoware/vehicle/topic_rate_check/velocity } - - { type: link, link: /autoware/vehicle/topic_rate_check/steering } - - - path: /autoware/vehicle/topic_rate_check/velocity - diag: "topic_state_monitor_vehicle_status_velocity_status: vehicle_topic_status" - - - path: /autoware/vehicle/topic_rate_check/steering - diag: "topic_state_monitor_vehicle_status_steering_status: vehicle_topic_status" diff --git a/system/system_diagnostic_monitor/package.xml b/system/system_diagnostic_monitor/package.xml index 9dda02c66d68f..9d77c9708778a 100644 --- a/system/system_diagnostic_monitor/package.xml +++ b/system/system_diagnostic_monitor/package.xml @@ -12,7 +12,6 @@ autoware_adapi_v1_msgs diagnostic_msgs - rclpy system_diagnostic_graph tier4_system_msgs diff --git a/system/system_diagnostic_monitor/script/component_state_diagnostics.py b/system/system_diagnostic_monitor/script/component_state_diagnostics.py deleted file mode 100755 index 60c7ebb597414..0000000000000 --- a/system/system_diagnostic_monitor/script/component_state_diagnostics.py +++ /dev/null @@ -1,99 +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. - -from autoware_adapi_v1_msgs.msg import LocalizationInitializationState as LocalizationState -from autoware_adapi_v1_msgs.msg import RouteState -from diagnostic_msgs.msg import DiagnosticArray -from diagnostic_msgs.msg import DiagnosticStatus -import rclpy -import rclpy.node -import rclpy.qos -import rclpy.time - - -class ComponentStateDiagnostics: - def __init__(self, node, diag_name, topic_name, topic_type, topic_qos, diag_func): - self.node = node - self.name = diag_name - self.func = diag_func - self.sub = node.create_subscription(topic_type, topic_name, self.callback, topic_qos) - self.level = DiagnosticStatus.STALE - self.stamp = None - - def callback(self, msg): - self.level = DiagnosticStatus.OK if self.func(msg) else DiagnosticStatus.ERROR - self.stamp = self.node.get_clock().now() - - def update(self, stamp): - if self.stamp: - elapsed = (stamp - self.stamp).nanoseconds * 1e-9 - if 3.0 < elapsed: - self.level = DiagnosticStatus.STALE - self.stamp = None - - def message(self): - status = DiagnosticStatus() - status.name = self.name - status.level = self.level - return status - - -class ComponentStateDiagnosticsNode(rclpy.node.Node): - def __init__(self): - super().__init__("component_state_diagnostics") - durable_qos = rclpy.qos.QoSProfile( - depth=1, - durability=rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL, - reliability=rclpy.qos.ReliabilityPolicy.RELIABLE, - ) - - self.timer = self.create_timer(0.5, self.on_timer) - self.pub = self.create_publisher(DiagnosticArray, "/diagnostics", 1) - self.states = [] - self.states.append( - ComponentStateDiagnostics( - self, - "component_state_diagnostics: localization_state", - "/api/localization/initialization_state", - LocalizationState, - durable_qos, - lambda msg: msg.state == LocalizationState.INITIALIZED, - ) - ) - self.states.append( - ComponentStateDiagnostics( - self, - "component_state_diagnostics: route_state", - "/api/routing/state", - RouteState, - durable_qos, - lambda msg: msg.state != RouteState.UNSET, - ) - ) - - def on_timer(self): - stamp = self.get_clock().now() - array = DiagnosticArray() - array.header.stamp = stamp.to_msg() - for state in self.states: - array.status.append(state.message()) - self.pub.publish(array) - - -if __name__ == "__main__": - rclpy.init() - rclpy.spin(ComponentStateDiagnosticsNode()) - rclpy.shutdown() From d3d38ddd1c4accd60179339c091803f1566c73b9 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Wed, 20 Dec 2023 17:02:15 +0900 Subject: [PATCH 23/26] remake package Signed-off-by: Takagi, Isamu --- system/hazard_status_converter/CMakeLists.txt | 17 ++++++++ .../launch/hazard_status_converter.launch.xml | 7 +++ .../package.xml | 13 +++--- .../script/hazard_status_converter.py | 0 .../hazard_status_converter/src/converter.cpp | 32 ++++++++++++++ .../hazard_status_converter/src/converter.hpp | 43 +++++++++++++++++++ system/hazard_status_converter/src/merger.cpp | 37 ++++++++++++++++ system/hazard_status_converter/src/merger.hpp | 43 +++++++++++++++++++ .../system_diagnostic_monitor/CMakeLists.txt | 7 --- .../system_diagnostic_monitor.launch.xml | 7 --- 10 files changed, 186 insertions(+), 20 deletions(-) create mode 100644 system/hazard_status_converter/CMakeLists.txt create mode 100644 system/hazard_status_converter/launch/hazard_status_converter.launch.xml rename system/{system_diagnostic_monitor => hazard_status_converter}/package.xml (65%) rename system/{system_diagnostic_monitor => hazard_status_converter}/script/hazard_status_converter.py (100%) create mode 100644 system/hazard_status_converter/src/converter.cpp create mode 100644 system/hazard_status_converter/src/converter.hpp create mode 100644 system/hazard_status_converter/src/merger.cpp create mode 100644 system/hazard_status_converter/src/merger.hpp delete mode 100644 system/system_diagnostic_monitor/CMakeLists.txt delete mode 100644 system/system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml diff --git a/system/hazard_status_converter/CMakeLists.txt b/system/hazard_status_converter/CMakeLists.txt new file mode 100644 index 0000000000000..3943f3ad12a94 --- /dev/null +++ b/system/hazard_status_converter/CMakeLists.txt @@ -0,0 +1,17 @@ +cmake_minimum_required(VERSION 3.14) +project(hazard_status_converter) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/converter.cpp + src/merger.cpp +) + +rclcpp_components_register_nodes(${PROJECT_NAME} + "hazard_status_converter::Converter" + "hazard_status_converter::Merger" +) + +ament_auto_package(INSTALL_TO_SHARE config launch) diff --git a/system/hazard_status_converter/launch/hazard_status_converter.launch.xml b/system/hazard_status_converter/launch/hazard_status_converter.launch.xml new file mode 100644 index 0000000000000..f60ee90c3dbf0 --- /dev/null +++ b/system/hazard_status_converter/launch/hazard_status_converter.launch.xml @@ -0,0 +1,7 @@ + + + + + + + diff --git a/system/system_diagnostic_monitor/package.xml b/system/hazard_status_converter/package.xml similarity index 65% rename from system/system_diagnostic_monitor/package.xml rename to system/hazard_status_converter/package.xml index 9d77c9708778a..5788b94fc2ce5 100644 --- a/system/system_diagnostic_monitor/package.xml +++ b/system/hazard_status_converter/package.xml @@ -1,19 +1,20 @@ - system_diagnostic_monitor + hazard_status_converter 0.1.0 - The system_diagnostic_monitor package + The hazard_status_converter package Takagi, Isamu Apache License 2.0 ament_cmake_auto autoware_cmake - autoware_adapi_v1_msgs - diagnostic_msgs - system_diagnostic_graph - tier4_system_msgs + autoware_adapi_v1_msgs + diagnostic_msgs + rclcpp + rclcpp_components + tier4_system_msgs ament_lint_auto autoware_lint_common diff --git a/system/system_diagnostic_monitor/script/hazard_status_converter.py b/system/hazard_status_converter/script/hazard_status_converter.py similarity index 100% rename from system/system_diagnostic_monitor/script/hazard_status_converter.py rename to system/hazard_status_converter/script/hazard_status_converter.py diff --git a/system/hazard_status_converter/src/converter.cpp b/system/hazard_status_converter/src/converter.cpp new file mode 100644 index 0000000000000..bfb6a59f30bdd --- /dev/null +++ b/system/hazard_status_converter/src/converter.cpp @@ -0,0 +1,32 @@ +// 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 "converter.hpp" + +namespace hazard_status_converter +{ + +Converter::Converter(const rclcpp::NodeOptions & options) : Node("converter", options) +{ +} + +void Converter::on_graph(const DiagnosticGraph::ConstSharedPtr msg) +{ + (void)msg; +} + +} // namespace hazard_status_converter + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(hazard_status_converter::Converter) diff --git a/system/hazard_status_converter/src/converter.hpp b/system/hazard_status_converter/src/converter.hpp new file mode 100644 index 0000000000000..2723e081769c0 --- /dev/null +++ b/system/hazard_status_converter/src/converter.hpp @@ -0,0 +1,43 @@ +// 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 CONVERTER_HPP_ +#define CONVERTER_HPP_ + +#include + +#include +#include + +#include + +namespace hazard_status_converter +{ + +class Converter : public rclcpp::Node +{ +public: + explicit Converter(const rclcpp::NodeOptions & options); + +private: + using DiagnosticGraph = tier4_system_msgs::msg::DiagnosticGraph; + using HazardStatusStamped = tier4_system_msgs::msg::HazardStatusStamped; + rclcpp::Subscription::SharedPtr sub_; + rclcpp::Publisher::SharedPtr pub_; + void on_graph(const DiagnosticGraph::ConstSharedPtr msg); +}; + +} // namespace hazard_status_converter + +#endif // CONVERTER_HPP_ diff --git a/system/hazard_status_converter/src/merger.cpp b/system/hazard_status_converter/src/merger.cpp new file mode 100644 index 0000000000000..b5d83d41b7fa9 --- /dev/null +++ b/system/hazard_status_converter/src/merger.cpp @@ -0,0 +1,37 @@ +// 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 "merger.hpp" + +namespace hazard_status_converter +{ + +Merger::Merger(const rclcpp::NodeOptions & options) : Node("merger", options) +{ +} + +void Merger::on_timer() +{ +} + +void Merger::on_graph(const DiagnosticGraph::ConstSharedPtr msg, int index) +{ + (void)msg; + (void)index; +} + +} // namespace hazard_status_converter + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(hazard_status_converter::Merger) diff --git a/system/hazard_status_converter/src/merger.hpp b/system/hazard_status_converter/src/merger.hpp new file mode 100644 index 0000000000000..9d89191c86392 --- /dev/null +++ b/system/hazard_status_converter/src/merger.hpp @@ -0,0 +1,43 @@ +// 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 MERGER_HPP_ +#define MERGER_HPP_ + +#include + +#include + +#include + +namespace hazard_status_converter +{ + +class Merger : public rclcpp::Node +{ +public: + explicit Merger(const rclcpp::NodeOptions & options); + +private: + using DiagnosticGraph = tier4_system_msgs::msg::DiagnosticGraph; + std::vector::SharedPtr> sub_inputs_; + rclcpp::Publisher::SharedPtr pub_output_; + rclcpp::TimerBase::SharedPtr timer_; + void on_timer(); + void on_graph(const DiagnosticGraph::ConstSharedPtr msg, int index); +}; + +} // namespace hazard_status_converter + +#endif // MERGER_HPP_ diff --git a/system/system_diagnostic_monitor/CMakeLists.txt b/system/system_diagnostic_monitor/CMakeLists.txt deleted file mode 100644 index c07f39aef9793..0000000000000 --- a/system/system_diagnostic_monitor/CMakeLists.txt +++ /dev/null @@ -1,7 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(system_diagnostic_monitor) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_package(INSTALL_TO_SHARE config launch) diff --git a/system/system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml b/system/system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml deleted file mode 100644 index 507ace9d08d78..0000000000000 --- a/system/system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - From 5d1d3efb0236a7ddd98e58c1a06ace0fba99f262 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Fri, 22 Dec 2023 15:06:55 +0900 Subject: [PATCH 24/26] create converter Signed-off-by: Takagi, Isamu --- system/hazard_status_converter/CMakeLists.txt | 2 - system/hazard_status_converter/package.xml | 2 +- .../hazard_status_converter/src/converter.cpp | 154 +++++++++++++++++- .../hazard_status_converter/src/converter.hpp | 15 +- system/hazard_status_converter/src/merger.cpp | 37 ----- system/hazard_status_converter/src/merger.hpp | 43 ----- 6 files changed, 163 insertions(+), 90 deletions(-) delete mode 100644 system/hazard_status_converter/src/merger.cpp delete mode 100644 system/hazard_status_converter/src/merger.hpp diff --git a/system/hazard_status_converter/CMakeLists.txt b/system/hazard_status_converter/CMakeLists.txt index 3943f3ad12a94..faf444bc2dc46 100644 --- a/system/hazard_status_converter/CMakeLists.txt +++ b/system/hazard_status_converter/CMakeLists.txt @@ -6,12 +6,10 @@ autoware_package() ament_auto_add_library(${PROJECT_NAME} SHARED src/converter.cpp - src/merger.cpp ) rclcpp_components_register_nodes(${PROJECT_NAME} "hazard_status_converter::Converter" - "hazard_status_converter::Merger" ) ament_auto_package(INSTALL_TO_SHARE config launch) diff --git a/system/hazard_status_converter/package.xml b/system/hazard_status_converter/package.xml index 5788b94fc2ce5..9ae7a384927b9 100644 --- a/system/hazard_status_converter/package.xml +++ b/system/hazard_status_converter/package.xml @@ -10,7 +10,7 @@ ament_cmake_auto autoware_cmake - autoware_adapi_v1_msgs + autoware_auto_system_msgs diagnostic_msgs rclcpp rclcpp_components diff --git a/system/hazard_status_converter/src/converter.cpp b/system/hazard_status_converter/src/converter.cpp index bfb6a59f30bdd..18cc632ac59e1 100644 --- a/system/hazard_status_converter/src/converter.cpp +++ b/system/hazard_status_converter/src/converter.cpp @@ -14,16 +14,166 @@ #include "converter.hpp" +#include +#include + +namespace +{ + +using autoware_auto_system_msgs::msg::HazardStatus; +using autoware_auto_system_msgs::msg::HazardStatusStamped; +using diagnostic_msgs::msg::DiagnosticStatus; +using tier4_system_msgs::msg::DiagnosticGraph; +using tier4_system_msgs::msg::DiagnosticNode; +using DiagnosticLevel = DiagnosticStatus::_level_type; + +enum class HazardLevel { NF, SF, LF, SPF }; + +struct TempNode +{ + const DiagnosticNode & node; + bool is_auto_tree; +}; + +HazardLevel get_hazard_level(const TempNode & node, DiagnosticLevel auto_mode_level) +{ + // Convert the level according to the table below. + // The Level other than auto mode is considered OK. + // |-------|-------------------------------| + // | Level | Root level | + // |-------|-------------------------------| + // | | OK | WARN | ERROR | STALE | + // | OK | NF | NF | NF | NF | + // | WARN | SF | LF | LF | LF | + // | ERROR | SF | LF | SPF | SPF | + // | STALE | SF | LF | SPF | SPF | + // |-------|-------------------------------| + + const auto root_level = node.is_auto_tree ? auto_mode_level : DiagnosticStatus::OK; + const auto node_level = node.node.status.level; + + if (node_level == DiagnosticStatus::OK) { + return HazardLevel::NF; + } + if (root_level == DiagnosticStatus::OK) { + return HazardLevel::SF; + } + if (node_level == DiagnosticStatus::WARN) { + return HazardLevel::LF; + } + if (root_level == DiagnosticStatus::WARN) { + return HazardLevel::LF; + } + return HazardLevel::SPF; +} + +void set_auto_tree(std::vector & nodes, int index) +{ + TempNode & node = nodes[index]; + if (node.is_auto_tree) { + return; + } + + node.is_auto_tree = true; + for (const auto & link : node.node.links) { + set_auto_tree(nodes, link.index); + } +} + +void merge_hazard_status( + HazardStatusStamped & hazard, const std::string prefix, const DiagnosticGraph & graph) +{ + std::vector nodes; + nodes.reserve(graph.nodes.size()); + for (const auto & node : graph.nodes) { + nodes.push_back({node, false}); + } + + DiagnosticLevel auto_mode_level = DiagnosticStatus::STALE; + for (size_t index = 0; index < nodes.size(); ++index) { + const auto & status = nodes[index].node.status; + if (status.name == "/autoware/modes/autonomous") { + set_auto_tree(nodes, index); + auto_mode_level = status.level; + } + } + + for (const auto & node : nodes) { + switch (get_hazard_level(node, auto_mode_level)) { + case HazardLevel::NF: + hazard.status.diag_no_fault.push_back(node.node.status); + hazard.status.diag_no_fault.back().name = prefix + node.node.status.name; + break; + case HazardLevel::SF: + hazard.status.diag_safe_fault.push_back(node.node.status); + hazard.status.diag_safe_fault.back().name = prefix + node.node.status.name; + break; + case HazardLevel::LF: + hazard.status.diag_latent_fault.push_back(node.node.status); + hazard.status.diag_latent_fault.back().name = prefix + node.node.status.name; + break; + case HazardLevel::SPF: + hazard.status.diag_single_point_fault.push_back(node.node.status); + hazard.status.diag_single_point_fault.back().name = prefix + node.node.status.name; + break; + } + } +} + +} // namespace + namespace hazard_status_converter { Converter::Converter(const rclcpp::NodeOptions & options) : Node("converter", options) { + const auto inputs = std::vector>( + {{"/mm", "/main/diagnostics_graph/main"}, + {"/ss", "/sub/diagnostics_graph/sub"}, + {"/vm", "/supervisor/diagnostics_graph/main"}, + {"/vs", "/supervisor/diagnostics_graph/sub"}, + {"/vv", "/supervisor/diagnostics_graph/supervisor"}}); + + for (const auto & [prefix, topic] : inputs) { + const std::function callback = + std::bind(&Converter::on_graph, this, prefix, std::placeholders::_1); + sub_graphs_.push_back(create_subscription(topic, rclcpp::QoS(1), callback)); + } + pub_hazard_ = create_publisher("/hazard_status", rclcpp::QoS(1)); + + const auto period = rclcpp::Rate(10).period(); + timer_ = rclcpp::create_timer(this, get_clock(), period, [this] { on_timer(); }); } -void Converter::on_graph(const DiagnosticGraph::ConstSharedPtr msg) +void Converter::on_graph(const std::string & prefix, const DiagnosticGraph::ConstSharedPtr msg) { - (void)msg; + graphs_[prefix] = msg; +} + +void Converter::on_timer() +{ + const auto get_level = [](const HazardStatus & status) { + if (!status.diag_single_point_fault.empty()) { + return HazardStatus::SINGLE_POINT_FAULT; + } + if (!status.diag_latent_fault.empty()) { + return HazardStatus::LATENT_FAULT; + } + if (!status.diag_safe_fault.empty()) { + return HazardStatus::SAFE_FAULT; + } + return HazardStatus::NO_FAULT; + }; + + HazardStatusStamped hazard; + hazard.stamp = now(); + for (const auto & [prefix, graph] : graphs_) { + merge_hazard_status(hazard, prefix, *graph); + } + hazard.status.level = get_level(hazard.status); + hazard.status.emergency = hazard.status.level == HazardStatus::SINGLE_POINT_FAULT; + hazard.status.emergency_holding = false; + pub_hazard_->publish(hazard); } } // namespace hazard_status_converter diff --git a/system/hazard_status_converter/src/converter.hpp b/system/hazard_status_converter/src/converter.hpp index 2723e081769c0..cfdbf772ba852 100644 --- a/system/hazard_status_converter/src/converter.hpp +++ b/system/hazard_status_converter/src/converter.hpp @@ -17,9 +17,11 @@ #include +#include #include -#include +#include +#include #include namespace hazard_status_converter @@ -32,10 +34,13 @@ class Converter : public rclcpp::Node private: using DiagnosticGraph = tier4_system_msgs::msg::DiagnosticGraph; - using HazardStatusStamped = tier4_system_msgs::msg::HazardStatusStamped; - rclcpp::Subscription::SharedPtr sub_; - rclcpp::Publisher::SharedPtr pub_; - void on_graph(const DiagnosticGraph::ConstSharedPtr msg); + using HazardStatusStamped = autoware_auto_system_msgs::msg::HazardStatusStamped; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr pub_hazard_; + std::vector::SharedPtr> sub_graphs_; + std::unordered_map graphs_; + void on_timer(); + void on_graph(const std::string & prefix, const DiagnosticGraph::ConstSharedPtr msg); }; } // namespace hazard_status_converter diff --git a/system/hazard_status_converter/src/merger.cpp b/system/hazard_status_converter/src/merger.cpp deleted file mode 100644 index b5d83d41b7fa9..0000000000000 --- a/system/hazard_status_converter/src/merger.cpp +++ /dev/null @@ -1,37 +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 "merger.hpp" - -namespace hazard_status_converter -{ - -Merger::Merger(const rclcpp::NodeOptions & options) : Node("merger", options) -{ -} - -void Merger::on_timer() -{ -} - -void Merger::on_graph(const DiagnosticGraph::ConstSharedPtr msg, int index) -{ - (void)msg; - (void)index; -} - -} // namespace hazard_status_converter - -#include -RCLCPP_COMPONENTS_REGISTER_NODE(hazard_status_converter::Merger) diff --git a/system/hazard_status_converter/src/merger.hpp b/system/hazard_status_converter/src/merger.hpp deleted file mode 100644 index 9d89191c86392..0000000000000 --- a/system/hazard_status_converter/src/merger.hpp +++ /dev/null @@ -1,43 +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 MERGER_HPP_ -#define MERGER_HPP_ - -#include - -#include - -#include - -namespace hazard_status_converter -{ - -class Merger : public rclcpp::Node -{ -public: - explicit Merger(const rclcpp::NodeOptions & options); - -private: - using DiagnosticGraph = tier4_system_msgs::msg::DiagnosticGraph; - std::vector::SharedPtr> sub_inputs_; - rclcpp::Publisher::SharedPtr pub_output_; - rclcpp::TimerBase::SharedPtr timer_; - void on_timer(); - void on_graph(const DiagnosticGraph::ConstSharedPtr msg, int index); -}; - -} // namespace hazard_status_converter - -#endif // MERGER_HPP_ From 0cbe828b86166a94ac88507e125cf08b2b22e15e Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Mon, 25 Dec 2023 15:20:48 +0900 Subject: [PATCH 25/26] fix hazard status converter Signed-off-by: Takagi, Isamu --- system/hazard_status_converter/CMakeLists.txt | 5 +- .../config/default.param.yaml | 3 + .../launch/hazard_status_converter.launch.xml | 9 +- .../script/hazard_status_converter.py | 136 ------------------ .../hazard_status_converter/src/converter.cpp | 2 +- 5 files changed, 11 insertions(+), 144 deletions(-) create mode 100644 system/hazard_status_converter/config/default.param.yaml delete mode 100755 system/hazard_status_converter/script/hazard_status_converter.py diff --git a/system/hazard_status_converter/CMakeLists.txt b/system/hazard_status_converter/CMakeLists.txt index faf444bc2dc46..d5d0979e04848 100644 --- a/system/hazard_status_converter/CMakeLists.txt +++ b/system/hazard_status_converter/CMakeLists.txt @@ -8,8 +8,9 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/converter.cpp ) -rclcpp_components_register_nodes(${PROJECT_NAME} - "hazard_status_converter::Converter" +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "hazard_status_converter::Converter" + EXECUTABLE converter ) ament_auto_package(INSTALL_TO_SHARE config launch) diff --git a/system/hazard_status_converter/config/default.param.yaml b/system/hazard_status_converter/config/default.param.yaml new file mode 100644 index 0000000000000..55fa511b4b993 --- /dev/null +++ b/system/hazard_status_converter/config/default.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + rate: 10.0 diff --git a/system/hazard_status_converter/launch/hazard_status_converter.launch.xml b/system/hazard_status_converter/launch/hazard_status_converter.launch.xml index f60ee90c3dbf0..bac6d3540cd32 100644 --- a/system/hazard_status_converter/launch/hazard_status_converter.launch.xml +++ b/system/hazard_status_converter/launch/hazard_status_converter.launch.xml @@ -1,7 +1,6 @@ - - - - - + + + + diff --git a/system/hazard_status_converter/script/hazard_status_converter.py b/system/hazard_status_converter/script/hazard_status_converter.py deleted file mode 100755 index 84a0a3611383f..0000000000000 --- a/system/hazard_status_converter/script/hazard_status_converter.py +++ /dev/null @@ -1,136 +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 enum - -from autoware_adapi_v1_msgs.msg import OperationModeState -from diagnostic_msgs.msg import DiagnosticStatus -import rclpy -import rclpy.node -import rclpy.qos -import rclpy.time -from tier4_system_msgs.msg import DiagnosticGraph -from tier4_system_msgs.msg import HazardStatus - - -class FaultLevel(enum.Enum): - NF = 1 - SF = 2 - LF = 3 - SPF = 4 - - -def get_mode_name(mode): - if mode == OperationModeState.STOP: - return "/autoware/modes/stop" - elif mode == OperationModeState.AUTONOMOUS: - return "/autoware/modes/autonomous" - if mode == OperationModeState.LOCAL: - return "/autoware/modes/local" - if mode == OperationModeState.REMOTE: - return "/autoware/modes/remote" - - -class DiagNode: - def __init__(self, node): - self.node = node - self.view = [] - - @property - def level(self): - return self.node.status.level - - @property - def name(self): - return self.node.status.name - - @property - def links(self): - for link in self.node.links: - if link.used: - yield link.index - - @property - def category(self): - if self.level == DiagnosticStatus.OK: - return FaultLevel.NF - if self.view == DiagnosticStatus.OK: - return FaultLevel.SF - if self.level == DiagnosticStatus.WARN: - return FaultLevel.LF - if self.view == DiagnosticStatus.WARN: - return FaultLevel.LF - return FaultLevel.SPF - - -class HazardStatusConverter(rclpy.node.Node): - def __init__(self): - super().__init__("hazard_status_converter") - self.create_interface() - self.mode_name = None - self.mode_name = "/autoware/modes/autonomous" - - def create_interface(self): - durable_qos = rclpy.qos.QoSProfile( - depth=1, - durability=rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL, - reliability=rclpy.qos.ReliabilityPolicy.RELIABLE, - ) - self.sub_graph = self.create_subscription( - DiagnosticGraph, "/diagnostics_graph", self.on_graph, 1 - ) - self.sub_modes = self.create_subscription( - OperationModeState, "/api/operation_mode/state", self.on_modes, durable_qos - ) - self.pub_hazard = self.create_publisher(HazardStatus, "/hazard_status", 1) - - def on_modes(self, msg): - self.mode_name = get_mode_name(msg.mode) - - def on_graph(self, msg): - nodes = [DiagNode(node) for node in msg.nodes] - level = DiagnosticStatus.OK - for node in nodes: - if node.name.startswith("/autoware/modes/"): - if node.name == self.mode_name: - level = node.level - node.view = [node.level] - else: - node.view = [DiagnosticStatus.OK] - - for node in reversed(nodes): - node.view = min(node.level, max(node.view, default=DiagnosticStatus.OK)) - for index in node.links: - nodes[index].view.append(node.view) - - faults = {level: [] for level in FaultLevel} - for node in reversed(nodes): - faults[node.category].append(node.node.status) - - hazards = HazardStatus() - hazards.level = int.from_bytes(level, "little") - hazards.emergency = DiagnosticStatus.ERROR <= level - hazards.diagnostics_nf = faults[FaultLevel.NF] - hazards.diagnostics_sf = faults[FaultLevel.SF] - hazards.diagnostics_lf = faults[FaultLevel.LF] - hazards.diagnostics_spf = faults[FaultLevel.SPF] - self.pub_hazard.publish(hazards) - - -if __name__ == "__main__": - rclpy.init() - rclpy.spin(HazardStatusConverter()) - rclpy.shutdown() diff --git a/system/hazard_status_converter/src/converter.cpp b/system/hazard_status_converter/src/converter.cpp index 18cc632ac59e1..deaa940c95b47 100644 --- a/system/hazard_status_converter/src/converter.cpp +++ b/system/hazard_status_converter/src/converter.cpp @@ -141,7 +141,7 @@ Converter::Converter(const rclcpp::NodeOptions & options) : Node("converter", op } pub_hazard_ = create_publisher("/hazard_status", rclcpp::QoS(1)); - const auto period = rclcpp::Rate(10).period(); + const auto period = rclcpp::Rate(declare_parameter("rate")).period(); timer_ = rclcpp::create_timer(this, get_clock(), period, [this] { on_timer(); }); } From cadabaa5d1cac737597aa0e44e5605920bb1b0e2 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Thu, 15 Feb 2024 18:13:02 +0900 Subject: [PATCH 26/26] remove ecu prefix Signed-off-by: Takagi, Isamu --- system/hazard_status_converter/CMakeLists.txt | 2 +- .../config/default.param.yaml | 3 -- .../launch/hazard_status_converter.launch.xml | 5 +- .../hazard_status_converter/src/converter.cpp | 48 ++++++------------- .../hazard_status_converter/src/converter.hpp | 7 +-- 5 files changed, 18 insertions(+), 47 deletions(-) delete mode 100644 system/hazard_status_converter/config/default.param.yaml diff --git a/system/hazard_status_converter/CMakeLists.txt b/system/hazard_status_converter/CMakeLists.txt index d5d0979e04848..b253f9fc51f2b 100644 --- a/system/hazard_status_converter/CMakeLists.txt +++ b/system/hazard_status_converter/CMakeLists.txt @@ -13,4 +13,4 @@ rclcpp_components_register_node(${PROJECT_NAME} EXECUTABLE converter ) -ament_auto_package(INSTALL_TO_SHARE config launch) +ament_auto_package(INSTALL_TO_SHARE launch) diff --git a/system/hazard_status_converter/config/default.param.yaml b/system/hazard_status_converter/config/default.param.yaml deleted file mode 100644 index 55fa511b4b993..0000000000000 --- a/system/hazard_status_converter/config/default.param.yaml +++ /dev/null @@ -1,3 +0,0 @@ -/**: - ros__parameters: - rate: 10.0 diff --git a/system/hazard_status_converter/launch/hazard_status_converter.launch.xml b/system/hazard_status_converter/launch/hazard_status_converter.launch.xml index bac6d3540cd32..ff50dec3a342f 100644 --- a/system/hazard_status_converter/launch/hazard_status_converter.launch.xml +++ b/system/hazard_status_converter/launch/hazard_status_converter.launch.xml @@ -1,6 +1,3 @@ - - - - + diff --git a/system/hazard_status_converter/src/converter.cpp b/system/hazard_status_converter/src/converter.cpp index deaa940c95b47..b09f3de891b45 100644 --- a/system/hazard_status_converter/src/converter.cpp +++ b/system/hazard_status_converter/src/converter.cpp @@ -80,15 +80,16 @@ void set_auto_tree(std::vector & nodes, int index) } } -void merge_hazard_status( - HazardStatusStamped & hazard, const std::string prefix, const DiagnosticGraph & graph) +HazardStatusStamped convert_hazard_diagnostics(const DiagnosticGraph & graph) { + // Create temporary tree for conversion. std::vector nodes; nodes.reserve(graph.nodes.size()); for (const auto & node : graph.nodes) { nodes.push_back({node, false}); } + // Mark nodes included in the auto mode tree. DiagnosticLevel auto_mode_level = DiagnosticStatus::STALE; for (size_t index = 0; index < nodes.size(); ++index) { const auto & status = nodes[index].node.status; @@ -98,26 +99,25 @@ void merge_hazard_status( } } + // Calculate hazard level from node level and root level. + HazardStatusStamped hazard; for (const auto & node : nodes) { switch (get_hazard_level(node, auto_mode_level)) { case HazardLevel::NF: hazard.status.diag_no_fault.push_back(node.node.status); - hazard.status.diag_no_fault.back().name = prefix + node.node.status.name; break; case HazardLevel::SF: hazard.status.diag_safe_fault.push_back(node.node.status); - hazard.status.diag_safe_fault.back().name = prefix + node.node.status.name; break; case HazardLevel::LF: hazard.status.diag_latent_fault.push_back(node.node.status); - hazard.status.diag_latent_fault.back().name = prefix + node.node.status.name; break; case HazardLevel::SPF: hazard.status.diag_single_point_fault.push_back(node.node.status); - hazard.status.diag_single_point_fault.back().name = prefix + node.node.status.name; break; } } + return hazard; } } // namespace @@ -127,32 +127,15 @@ namespace hazard_status_converter Converter::Converter(const rclcpp::NodeOptions & options) : Node("converter", options) { - const auto inputs = std::vector>( - {{"/mm", "/main/diagnostics_graph/main"}, - {"/ss", "/sub/diagnostics_graph/sub"}, - {"/vm", "/supervisor/diagnostics_graph/main"}, - {"/vs", "/supervisor/diagnostics_graph/sub"}, - {"/vv", "/supervisor/diagnostics_graph/supervisor"}}); - - for (const auto & [prefix, topic] : inputs) { - const std::function callback = - std::bind(&Converter::on_graph, this, prefix, std::placeholders::_1); - sub_graphs_.push_back(create_subscription(topic, rclcpp::QoS(1), callback)); - } pub_hazard_ = create_publisher("/hazard_status", rclcpp::QoS(1)); - - const auto period = rclcpp::Rate(declare_parameter("rate")).period(); - timer_ = rclcpp::create_timer(this, get_clock(), period, [this] { on_timer(); }); -} - -void Converter::on_graph(const std::string & prefix, const DiagnosticGraph::ConstSharedPtr msg) -{ - graphs_[prefix] = msg; + sub_graph_ = create_subscription( + "/diagnostics_graph", rclcpp::QoS(3), + std::bind(&Converter::on_graph, this, std::placeholders::_1)); } -void Converter::on_timer() +void Converter::on_graph(const DiagnosticGraph::ConstSharedPtr msg) { - const auto get_level = [](const HazardStatus & status) { + const auto get_system_level = [](const HazardStatus & status) { if (!status.diag_single_point_fault.empty()) { return HazardStatus::SINGLE_POINT_FAULT; } @@ -165,12 +148,9 @@ void Converter::on_timer() return HazardStatus::NO_FAULT; }; - HazardStatusStamped hazard; - hazard.stamp = now(); - for (const auto & [prefix, graph] : graphs_) { - merge_hazard_status(hazard, prefix, *graph); - } - hazard.status.level = get_level(hazard.status); + HazardStatusStamped hazard = convert_hazard_diagnostics(*msg); + hazard.stamp = msg->stamp; + hazard.status.level = get_system_level(hazard.status); hazard.status.emergency = hazard.status.level == HazardStatus::SINGLE_POINT_FAULT; hazard.status.emergency_holding = false; pub_hazard_->publish(hazard); diff --git a/system/hazard_status_converter/src/converter.hpp b/system/hazard_status_converter/src/converter.hpp index cfdbf772ba852..90c6f6e42bf85 100644 --- a/system/hazard_status_converter/src/converter.hpp +++ b/system/hazard_status_converter/src/converter.hpp @@ -35,12 +35,9 @@ class Converter : public rclcpp::Node private: using DiagnosticGraph = tier4_system_msgs::msg::DiagnosticGraph; using HazardStatusStamped = autoware_auto_system_msgs::msg::HazardStatusStamped; - rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Subscription::SharedPtr sub_graph_; rclcpp::Publisher::SharedPtr pub_hazard_; - std::vector::SharedPtr> sub_graphs_; - std::unordered_map graphs_; - void on_timer(); - void on_graph(const std::string & prefix, const DiagnosticGraph::ConstSharedPtr msg); + void on_graph(const DiagnosticGraph::ConstSharedPtr msg); }; } // namespace hazard_status_converter