14
14
15
15
#include " converter.hpp"
16
16
17
- #include < memory>
18
- #include < string>
17
+ #include < algorithm>
19
18
20
19
namespace diagnostic_graph_aggregator
21
20
{
@@ -35,18 +34,46 @@ std::string level_to_string(DiagnosticLevel level)
35
34
return " UNKNOWN" ;
36
35
}
37
36
38
- ToolNode::ToolNode () : Node(" diagnostic_graph_aggregator_converter" )
37
+ std::string parent_path (const std::string & path)
38
+ {
39
+ return path.substr (0 , path.rfind (' /' ));
40
+ }
41
+
42
+ auto create_tree (const DiagnosticGraph & graph)
43
+ {
44
+ std::map<std::string, std::unique_ptr<TreeNode>, std::greater<std::string>> tree;
45
+ for (const auto & node : graph.nodes ) {
46
+ tree.emplace (node.status .name , std::make_unique<TreeNode>(true ));
47
+ }
48
+ for (const auto & node : graph.nodes ) {
49
+ std::string path = node.status .name ;
50
+ while (path = parent_path (path), !path.empty ()) {
51
+ if (tree.count (path)) break ;
52
+ tree.emplace (path, std::make_unique<TreeNode>(false ));
53
+ }
54
+ }
55
+ for (const auto & [path, node] : tree) {
56
+ const auto parent = parent_path (path);
57
+ node->parent = parent.empty () ? nullptr : tree[parent].get ();
58
+ }
59
+ return tree;
60
+ }
61
+
62
+ ConverterNode::ConverterNode () : Node(" converter" )
39
63
{
40
64
using std::placeholders::_1;
41
65
const auto qos_graph = rclcpp::QoS (1 );
42
66
const auto qos_array = rclcpp::QoS (1 );
43
67
44
- const auto callback = std::bind (&ToolNode ::on_graph, this , _1);
68
+ const auto callback = std::bind (&ConverterNode ::on_graph, this , _1);
45
69
sub_graph_ = create_subscription<DiagnosticGraph>(" /diagnostics_graph" , qos_graph, callback);
46
- pub_array_ = create_publisher<DiagnosticArray>(" /diagnostics_array" , qos_array);
70
+ pub_array_ = create_publisher<DiagnosticArray>(" /diagnostics_agg" , qos_array);
71
+
72
+ initialize_tree_ = false ;
73
+ complement_tree_ = declare_parameter<bool >(" complement_tree" );
47
74
}
48
75
49
- void ToolNode ::on_graph (const DiagnosticGraph::ConstSharedPtr msg)
76
+ void ConverterNode ::on_graph (const DiagnosticGraph::ConstSharedPtr msg)
50
77
{
51
78
DiagnosticArray message;
52
79
message.header .stamp = msg->stamp ;
@@ -63,17 +90,42 @@ void ToolNode::on_graph(const DiagnosticGraph::ConstSharedPtr msg)
63
90
}
64
91
}
65
92
}
93
+
94
+ if (complement_tree_ && !initialize_tree_) {
95
+ initialize_tree_ = true ;
96
+ tree_ = create_tree (*msg);
97
+ }
98
+
99
+ if (complement_tree_) {
100
+ for (const auto & [path, node] : tree_) {
101
+ node->level = DiagnosticStatus::OK;
102
+ }
103
+ for (const auto & node : msg->nodes ) {
104
+ tree_[node.status .name ]->level = node.status .level ;
105
+ }
106
+ for (const auto & [path, node] : tree_) {
107
+ if (!node->parent ) continue ;
108
+ node->parent ->level = std::max (node->parent ->level , node->level );
109
+ }
110
+ for (const auto & [path, node] : tree_) {
111
+ if (node->leaf ) continue ;
112
+ message.status .emplace_back ();
113
+ message.status .back ().name = path;
114
+ message.status .back ().level = node->level ;
115
+ }
116
+ }
117
+
66
118
pub_array_->publish (message);
67
119
}
68
120
69
121
} // namespace diagnostic_graph_aggregator
70
122
71
123
int main (int argc, char ** argv)
72
124
{
73
- using diagnostic_graph_aggregator::ToolNode ;
125
+ using diagnostic_graph_aggregator::ConverterNode ;
74
126
rclcpp::init (argc, argv);
75
127
rclcpp::executors::SingleThreadedExecutor executor;
76
- auto node = std::make_shared<ToolNode >();
128
+ auto node = std::make_shared<ConverterNode >();
77
129
executor.add_node (node);
78
130
executor.spin ();
79
131
executor.remove_node (node);
0 commit comments