diff --git a/system/diagnostic_graph_aggregator/example/dummy-diags.py b/system/diagnostic_graph_aggregator/example/dummy-diags.py
index 52d6189a63f30..08f81bcd738ed 100755
--- a/system/diagnostic_graph_aggregator/example/dummy-diags.py
+++ b/system/diagnostic_graph_aggregator/example/dummy-diags.py
@@ -59,6 +59,9 @@ def create_status(name: str):
"external_command_checker: joystick_command",
"external_command_checker: remote_command",
]
- rclpy.init()
- rclpy.spin(DummyDiagnostics(diags))
- rclpy.shutdown()
+ try:
+ rclpy.init()
+ rclpy.spin(DummyDiagnostics(diags))
+ rclpy.shutdown()
+ except KeyboardInterrupt:
+ pass
diff --git a/system/diagnostic_graph_aggregator/launch/converter.launch.xml b/system/diagnostic_graph_aggregator/launch/converter.launch.xml
index effebf8381ee1..9111338bf622a 100644
--- a/system/diagnostic_graph_aggregator/launch/converter.launch.xml
+++ b/system/diagnostic_graph_aggregator/launch/converter.launch.xml
@@ -1,6 +1,6 @@
-
+
diff --git a/system/diagnostic_graph_aggregator/src/node/converter.cpp b/system/diagnostic_graph_aggregator/src/node/converter.cpp
index e93a7c6800201..f0b2108374017 100644
--- a/system/diagnostic_graph_aggregator/src/node/converter.cpp
+++ b/system/diagnostic_graph_aggregator/src/node/converter.cpp
@@ -14,10 +14,7 @@
#include "converter.hpp"
-#include
-#include
-#include
-#include
+#include
namespace diagnostic_graph_aggregator
{
@@ -42,22 +39,24 @@ std::string parent_path(const std::string & path)
return path.substr(0, path.rfind('/'));
}
-std::vector complement_paths(const DiagnosticGraph & graph)
+auto create_tree(const DiagnosticGraph & graph)
{
- std::unordered_map paths;
+ std::map, std::greater> tree;
+ for (const auto & node : graph.nodes) {
+ tree.emplace(node.status.name, std::make_unique(true));
+ }
for (const auto & node : graph.nodes) {
std::string path = node.status.name;
- paths[path] = false;
while (path = parent_path(path), !path.empty()) {
- if (paths.count(path)) break;
- paths[path] = true;
+ if (tree.count(path)) break;
+ tree.emplace(path, std::make_unique(false));
}
}
- std::vector result;
- for (const auto & [path, flag] : paths) {
- if (flag) result.push_back(path);
+ for (const auto & [path, node] : tree) {
+ const auto parent = parent_path(path);
+ node->parent = parent.empty() ? nullptr : tree[parent].get();
}
- return result;
+ return tree;
}
ConverterNode::ConverterNode() : Node("converter")
@@ -69,7 +68,9 @@ ConverterNode::ConverterNode() : Node("converter")
const auto callback = std::bind(&ConverterNode::on_graph, this, _1);
sub_graph_ = create_subscription("/diagnostics_graph", qos_graph, callback);
pub_array_ = create_publisher("/diagnostics_agg", qos_array);
- complement_inner_nodes_ = declare_parameter("complement_inner_nodes");
+
+ initialize_tree_ = false;
+ complement_tree_ = declare_parameter("complement_tree");
}
void ConverterNode::on_graph(const DiagnosticGraph::ConstSharedPtr msg)
@@ -90,14 +91,27 @@ void ConverterNode::on_graph(const DiagnosticGraph::ConstSharedPtr msg)
}
}
- if (complement_inner_nodes_) {
- if (!inner_node_names_) {
- inner_node_names_ = complement_paths(*msg);
+ if (complement_tree_ && !initialize_tree_) {
+ initialize_tree_ = true;
+ tree_ = create_tree(*msg);
+ }
+
+ if (complement_tree_) {
+ for (const auto & [path, node] : tree_) {
+ node->level = DiagnosticStatus::OK;
+ }
+ for (const auto & node : msg->nodes) {
+ tree_[node.status.name]->level = node.status.level;
+ }
+ for (const auto & [path, node] : tree_) {
+ if (!node->parent) continue;
+ node->parent->level = std::max(node->parent->level, node->level);
}
- for (const auto & name : inner_node_names_.value()) {
+ for (const auto & [path, node] : tree_) {
+ if (node->leaf) continue;
message.status.emplace_back();
- message.status.back().name = name;
- message.status.back().level = DiagnosticStatus::STALE;
+ message.status.back().name = path;
+ message.status.back().level = node->level;
}
}
diff --git a/system/diagnostic_graph_aggregator/src/node/converter.hpp b/system/diagnostic_graph_aggregator/src/node/converter.hpp
index 6a70cbc2df4c6..1f98abed4d619 100644
--- a/system/diagnostic_graph_aggregator/src/node/converter.hpp
+++ b/system/diagnostic_graph_aggregator/src/node/converter.hpp
@@ -19,20 +19,32 @@
#include
+#include
+#include