Skip to content

Commit 5a6c82e

Browse files
isamu-takagiisamu-takagi
authored
feat(diagnostic_graph_aggregator): update tools (#6614)
Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>
1 parent 6ee9627 commit 5a6c82e

22 files changed

+454
-170
lines changed

‎system/diagnostic_graph_aggregator/CMakeLists.txt

+10-3
Original file line numberDiff line numberDiff line change
@@ -22,10 +22,17 @@ ament_auto_add_executable(converter
2222
)
2323
target_include_directories(converter PRIVATE src/common)
2424

25-
ament_auto_add_executable(tool
26-
src/tool/tool.cpp
25+
ament_auto_add_executable(tree
26+
src/tool/tree.cpp
27+
src/tool/utils/loader.cpp
2728
)
28-
target_include_directories(tool PRIVATE src/common)
29+
target_include_directories(tree PRIVATE src/common)
30+
31+
ament_auto_add_executable(plantuml
32+
src/tool/plantuml.cpp
33+
src/tool/utils/loader.cpp
34+
)
35+
target_include_directories(plantuml PRIVATE src/common)
2936

3037
if(BUILD_TESTING)
3138
get_filename_component(RESOURCE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/test/files ABSOLUTE)

‎system/diagnostic_graph_aggregator/README.md

+6
Original file line numberDiff line numberDiff line change
@@ -71,6 +71,12 @@ You can reuse the graph by making partial edits. For example, disable hardware c
7171
ros2 launch diagnostic_graph_aggregator example-edit.launch.xml
7272
```
7373

74+
## Debug tools
75+
76+
- [dump](./doc/tool/dump.md)
77+
- [converter](./doc/tool/converter.md)
78+
- [tree](./doc/tool/tree.md)
79+
7480
## Graph file format
7581

7682
- [graph](./doc/format/graph.md)

‎system/diagnostic_graph_aggregator/doc/tool/converter.md

+29
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
# Converter tool
2+
3+
This tool converts `/diagnostics_graph` to `/diagnostics_agg` so it can be read by tools such as `rqt_runtime_monitor` and `rqt_robot_monitor`.
4+
5+
## Usage
6+
7+
```bash
8+
ros2 launch diagnostic_graph_aggregator converter.launch.xml complement:=false
9+
```
10+
11+
The `complement` argument specifies whether to add an intermediate path that does not exist.
12+
This means that if the graph contains paths `/A/B` and `/A/B/C/D/E`, the intermediate paths `/A`, `/A/B/C` and `/A/B/C/D` will be added.
13+
This is useful for tree view in `rqt_robot_monitor`. The completed node has an error level of `STALE`.
14+
15+
## Examples
16+
17+
```bash
18+
ros2 launch diagnostic_graph_aggregator example-main.launch.xml complement:=false
19+
ros2 run rqt_runtime_monitor rqt_runtime_monitor --ros-args -r diagnostics:=diagnostics_agg
20+
```
21+
22+
![rqt_runtime_monitor](./images/rqt_runtime_monitor.png)
23+
24+
```bash
25+
ros2 launch diagnostic_graph_aggregator example-main.launch.xml complement:=true
26+
ros2 run rqt_robot_monitor rqt_robot_monitor
27+
```
28+
29+
![rqt_robot_monitor](./images/rqt_robot_monitor.png)

‎system/diagnostic_graph_aggregator/doc/tool/dump.md

+36
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,36 @@
1+
# Dump tool
2+
3+
This tool displays `/diagnostics_graph` in table format.
4+
5+
## Usage
6+
7+
```bash
8+
ros2 run diagnostic_graph_aggregator dump
9+
```
10+
11+
## Examples
12+
13+
```bash
14+
ros2 launch diagnostic_graph_aggregator example-main.launch.xml
15+
ros2 run diagnostic_graph_aggregator dump
16+
```
17+
18+
```txt
19+
| ----- | ----- | -------------------------------- | ----- |
20+
| index | level | name | links |
21+
| ----- | ----- | -------------------------------- | ----- |
22+
| 0 | OK | /sensing/radars/front | |
23+
| 1 | OK | /sensing/lidars/front | |
24+
| 2 | ERROR | /sensing/lidars/top | |
25+
| 3 | OK | /functions/obstacle_detection | 1 0 |
26+
| 4 | ERROR | /functions/pose_estimation | 2 |
27+
| 5 | OK | /external/remote_command | |
28+
| 6 | OK | /external/joystick_command | |
29+
| 7 | ERROR | /autoware/modes/pull_over | 4 3 |
30+
| 8 | OK | /autoware/modes/comfortable_stop | 3 |
31+
| 9 | OK | /autoware/modes/emergency_stop | |
32+
| 10 | OK | /autoware/modes/remote | 5 |
33+
| 11 | OK | /autoware/modes/local | 6 |
34+
| 12 | ERROR | /autoware/modes/autonomous | 4 3 |
35+
| 13 | OK | /autoware/modes/stop | |
36+
```

‎system/diagnostic_graph_aggregator/doc/tool/images/rqt_robot_monitor.png

85.3 KB
Loading

‎system/diagnostic_graph_aggregator/doc/tool/images/rqt_runtime_monitor.png

80.6 KB
Loading

‎system/diagnostic_graph_aggregator/doc/tool/tree.md

+40
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,40 @@
1+
# Tree tool
2+
3+
This tool displays the graph structure of the configuration file in tree format.
4+
5+
## Usage
6+
7+
```bash
8+
ros2 run diagnostic_graph_aggregator tree <graph-config-path>
9+
```
10+
11+
## Examples
12+
13+
```bash
14+
ros2 run diagnostic_graph_aggregator tree system/diagnostic_graph_aggregator/example/graph/main.yaml
15+
```
16+
17+
```txt
18+
===== root nodes =================================
19+
- /autoware/modes/local (and)
20+
- /external/joystick_command (diag)
21+
- /autoware/modes/comfortable_stop (and)
22+
- /functions/obstacle_detection (or)
23+
- /autoware/modes/pull_over (and)
24+
- /functions/pose_estimation (and)
25+
- /functions/obstacle_detection (or)
26+
- /autoware/modes/autonomous (and)
27+
- /functions/pose_estimation (and)
28+
- /functions/obstacle_detection (or)
29+
- /autoware/modes/remote (and)
30+
- /external/remote_command (diag)
31+
===== intermediate nodes =========================
32+
- /functions/obstacle_detection (or)
33+
- /sensing/lidars/front (diag)
34+
- /sensing/radars/front (diag)
35+
- /functions/pose_estimation (and)
36+
- /sensing/lidars/top (diag)
37+
===== isolated nodes =============================
38+
- /autoware/modes/stop (const)
39+
- /autoware/modes/emergency_stop (const)
40+
```

‎system/diagnostic_graph_aggregator/example/dummy-diags.py

+6-3
Original file line numberDiff line numberDiff line change
@@ -59,6 +59,9 @@ def create_status(name: str):
5959
"external_command_checker: joystick_command",
6060
"external_command_checker: remote_command",
6161
]
62-
rclpy.init()
63-
rclpy.spin(DummyDiagnostics(diags))
64-
rclpy.shutdown()
62+
try:
63+
rclpy.init()
64+
rclpy.spin(DummyDiagnostics(diags))
65+
rclpy.shutdown()
66+
except KeyboardInterrupt:
67+
pass

‎system/diagnostic_graph_aggregator/example/example-edit.launch.xml

+4-1
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,10 @@
11
<launch>
2+
<arg name="complement" default="false"/>
23
<include file="$(find-pkg-share diagnostic_graph_aggregator)/launch/diagnostic_graph_aggregator.launch.xml">
34
<arg name="graph_file" value="$(find-pkg-share diagnostic_graph_aggregator)/example/graph/edit.yaml"/>
45
</include>
5-
<node pkg="diagnostic_graph_aggregator" exec="converter" name="converter"/>
6+
<include file="$(find-pkg-share diagnostic_graph_aggregator)/launch/converter.launch.xml">
7+
<arg name="complement" value="$(var complement)"/>
8+
</include>
69
<executable cmd="python3 $(find-pkg-share diagnostic_graph_aggregator)/example/dummy-diags.py"/>
710
</launch>

‎system/diagnostic_graph_aggregator/example/example-main.launch.xml

+5-2
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,10 @@
11
<launch>
2-
<include file="$(find-pkg-share diagnostic_graph_aggregator)/launch/diagnostic_graph_aggregator.launch.xml">
2+
<arg name="complement" default="false"/>
3+
<include file="$(find-pkg-share diagnostic_graph_aggregator)/launch/aggregator.launch.xml">
34
<arg name="graph_file" value="$(find-pkg-share diagnostic_graph_aggregator)/example/graph/main.yaml"/>
45
</include>
5-
<node pkg="diagnostic_graph_aggregator" exec="converter" name="converter"/>
6+
<include file="$(find-pkg-share diagnostic_graph_aggregator)/launch/converter.launch.xml">
7+
<arg name="complement" value="$(var complement)"/>
8+
</include>
69
<executable cmd="python3 $(find-pkg-share diagnostic_graph_aggregator)/example/dummy-diags.py"/>
710
</launch>

‎system/diagnostic_graph_aggregator/launch/diagnostic_graph_aggregator.launch.xml ‎system/diagnostic_graph_aggregator/launch/aggregator.launch.xml

File renamed without changes.

‎system/diagnostic_graph_aggregator/launch/converter.launch.xml

+6
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,6 @@
1+
<launch>
2+
<arg name="complement"/>
3+
<node pkg="diagnostic_graph_aggregator" exec="converter" name="converter">
4+
<param name="complement_tree" value="$(var complement)"/>
5+
</node>
6+
</launch>

‎system/diagnostic_graph_aggregator/script/dump.py

-1
Original file line numberDiff line numberDiff line change
@@ -84,7 +84,6 @@ def callback(self, msg):
8484
if __name__ == "__main__":
8585
parser = argparse.ArgumentParser()
8686
parser.add_argument("--topic", default="/diagnostics_graph")
87-
parser.add_argument("--order", default="index")
8887
args, unparsed = parser.parse_known_args()
8988

9089
try:

‎system/diagnostic_graph_aggregator/src/node/aggregator.cpp

+7-7
Original file line numberDiff line numberDiff line change
@@ -20,7 +20,7 @@
2020
namespace diagnostic_graph_aggregator
2121
{
2222

23-
MainNode::MainNode() : Node("diagnostic_graph_aggregator_aggregator")
23+
AggregatorNode::AggregatorNode() : Node("aggregator")
2424
{
2525
// Init diagnostics graph.
2626
{
@@ -39,7 +39,7 @@ MainNode::MainNode() : Node("diagnostic_graph_aggregator_aggregator")
3939
const auto qos_input = rclcpp::QoS(declare_parameter<int64_t>("input_qos_depth"));
4040
const auto qos_graph = rclcpp::QoS(declare_parameter<int64_t>("graph_qos_depth"));
4141

42-
const auto callback = std::bind(&MainNode::on_diag, this, _1);
42+
const auto callback = std::bind(&AggregatorNode::on_diag, this, _1);
4343
sub_input_ = create_subscription<DiagnosticArray>("/diagnostics", qos_input, callback);
4444
pub_graph_ = create_publisher<DiagnosticGraph>("/diagnostics_graph", qos_graph);
4545

@@ -51,20 +51,20 @@ MainNode::MainNode() : Node("diagnostic_graph_aggregator_aggregator")
5151
debug_ = declare_parameter<bool>("use_debug_mode");
5252
}
5353

54-
MainNode::~MainNode()
54+
AggregatorNode::~AggregatorNode()
5555
{
5656
// for unique_ptr
5757
}
5858

59-
void MainNode::on_timer()
59+
void AggregatorNode::on_timer()
6060
{
6161
const auto stamp = now();
6262
pub_graph_->publish(graph_.report(stamp));
6363
if (debug_) graph_.debug();
6464
if (modes_) modes_->update(stamp);
6565
}
6666

67-
void MainNode::on_diag(const DiagnosticArray::ConstSharedPtr msg)
67+
void AggregatorNode::on_diag(const DiagnosticArray::ConstSharedPtr msg)
6868
{
6969
graph_.callback(now(), *msg);
7070
}
@@ -73,10 +73,10 @@ void MainNode::on_diag(const DiagnosticArray::ConstSharedPtr msg)
7373

7474
int main(int argc, char ** argv)
7575
{
76-
using diagnostic_graph_aggregator::MainNode;
76+
using diagnostic_graph_aggregator::AggregatorNode;
7777
rclcpp::init(argc, argv);
7878
rclcpp::executors::SingleThreadedExecutor executor;
79-
auto node = std::make_shared<MainNode>();
79+
auto node = std::make_shared<AggregatorNode>();
8080
executor.add_node(node);
8181
executor.spin();
8282
executor.remove_node(node);

‎system/diagnostic_graph_aggregator/src/node/aggregator.hpp

+3-3
Original file line numberDiff line numberDiff line change
@@ -26,11 +26,11 @@
2626
namespace diagnostic_graph_aggregator
2727
{
2828

29-
class MainNode : public rclcpp::Node
29+
class AggregatorNode : public rclcpp::Node
3030
{
3131
public:
32-
MainNode();
33-
~MainNode();
32+
AggregatorNode();
33+
~AggregatorNode();
3434

3535
private:
3636
Graph graph_;

‎system/diagnostic_graph_aggregator/src/node/converter.cpp

+60-8
Original file line numberDiff line numberDiff line change
@@ -14,8 +14,7 @@
1414

1515
#include "converter.hpp"
1616

17-
#include <memory>
18-
#include <string>
17+
#include <algorithm>
1918

2019
namespace diagnostic_graph_aggregator
2120
{
@@ -35,18 +34,46 @@ std::string level_to_string(DiagnosticLevel level)
3534
return "UNKNOWN";
3635
}
3736

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")
3963
{
4064
using std::placeholders::_1;
4165
const auto qos_graph = rclcpp::QoS(1);
4266
const auto qos_array = rclcpp::QoS(1);
4367

44-
const auto callback = std::bind(&ToolNode::on_graph, this, _1);
68+
const auto callback = std::bind(&ConverterNode::on_graph, this, _1);
4569
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");
4774
}
4875

49-
void ToolNode::on_graph(const DiagnosticGraph::ConstSharedPtr msg)
76+
void ConverterNode::on_graph(const DiagnosticGraph::ConstSharedPtr msg)
5077
{
5178
DiagnosticArray message;
5279
message.header.stamp = msg->stamp;
@@ -63,17 +90,42 @@ void ToolNode::on_graph(const DiagnosticGraph::ConstSharedPtr msg)
6390
}
6491
}
6592
}
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+
66118
pub_array_->publish(message);
67119
}
68120

69121
} // namespace diagnostic_graph_aggregator
70122

71123
int main(int argc, char ** argv)
72124
{
73-
using diagnostic_graph_aggregator::ToolNode;
125+
using diagnostic_graph_aggregator::ConverterNode;
74126
rclcpp::init(argc, argv);
75127
rclcpp::executors::SingleThreadedExecutor executor;
76-
auto node = std::make_shared<ToolNode>();
128+
auto node = std::make_shared<ConverterNode>();
77129
executor.add_node(node);
78130
executor.spin();
79131
executor.remove_node(node);

0 commit comments

Comments
 (0)