feat(diagnostic_graph_aggregator): update tools (autowarefoundation#6614

)

Signed-off-by: Takagi, Isamu <[email protected]>

system/diagnostic_graph_aggregator/CMakeLists.txt

@@ -22,10 +22,17 @@ ament_auto_add_executable(converter
 )
 target_include_directories(converter PRIVATE src/common)
 
-ament_auto_add_executable(tool
-  src/tool/tool.cpp
+ament_auto_add_executable(tree
+  src/tool/tree.cpp
+  src/tool/utils/loader.cpp
 )
-target_include_directories(tool PRIVATE src/common)
+target_include_directories(tree PRIVATE src/common)
+
+ament_auto_add_executable(plantuml
+  src/tool/plantuml.cpp
+  src/tool/utils/loader.cpp
+)
+target_include_directories(plantuml PRIVATE src/common)
 
 if(BUILD_TESTING)
   get_filename_component(RESOURCE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/test/files ABSOLUTE)

system/diagnostic_graph_aggregator/README.md

@@ -71,6 +71,12 @@ You can reuse the graph by making partial edits. For example, disable hardware c
 ros2 launch diagnostic_graph_aggregator example-edit.launch.xml
 ```
 
+## Debug tools
+
+- [dump](./doc/tool/dump.md)
+- [converter](./doc/tool/converter.md)
+- [tree](./doc/tool/tree.md)
+
 ## Graph file format
 
 - [graph](./doc/format/graph.md)

system/diagnostic_graph_aggregator/doc/tool/converter.md

@@ -0,0 +1,29 @@
+# Converter tool
+
+This tool converts `/diagnostics_graph` to `/diagnostics_agg` so it can be read by tools such as `rqt_runtime_monitor` and `rqt_robot_monitor`.
+
+## Usage
+
+```bash
+ros2 launch diagnostic_graph_aggregator converter.launch.xml complement:=false
+```
+
+The `complement` argument specifies whether to add an intermediate path that does not exist.
+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.
+This is useful for tree view in `rqt_robot_monitor`. The completed node has an error level of `STALE`.
+
+## Examples
+
+```bash
+ros2 launch diagnostic_graph_aggregator example-main.launch.xml complement:=false
+ros2 run rqt_runtime_monitor rqt_runtime_monitor --ros-args -r diagnostics:=diagnostics_agg
+```
+
+![rqt_runtime_monitor](./images/rqt_runtime_monitor.png)
+
+```bash
+ros2 launch diagnostic_graph_aggregator example-main.launch.xml complement:=true
+ros2 run rqt_robot_monitor rqt_robot_monitor
+```
+
+![rqt_robot_monitor](./images/rqt_robot_monitor.png)

system/diagnostic_graph_aggregator/doc/tool/dump.md

@@ -0,0 +1,36 @@
+# Dump tool
+
+This tool displays `/diagnostics_graph` in table format.
+
+## Usage
+
+```bash
+ros2 run diagnostic_graph_aggregator dump
+```
+
+## Examples
+
+```bash
+ros2 launch diagnostic_graph_aggregator example-main.launch.xml
+ros2 run diagnostic_graph_aggregator dump
+```
+
+```txt
+| ----- | ----- | -------------------------------- | ----- |
+| index | level | name                             | links |
+| ----- | ----- | -------------------------------- | ----- |
+| 0     | OK    | /sensing/radars/front            |       |
+| 1     | OK    | /sensing/lidars/front            |       |
+| 2     | ERROR | /sensing/lidars/top              |       |
+| 3     | OK    | /functions/obstacle_detection    | 1 0   |
+| 4     | ERROR | /functions/pose_estimation       | 2     |
+| 5     | OK    | /external/remote_command         |       |
+| 6     | OK    | /external/joystick_command       |       |
+| 7     | ERROR | /autoware/modes/pull_over        | 4 3   |
+| 8     | OK    | /autoware/modes/comfortable_stop | 3     |
+| 9     | OK    | /autoware/modes/emergency_stop   |       |
+| 10    | OK    | /autoware/modes/remote           | 5     |
+| 11    | OK    | /autoware/modes/local            | 6     |
+| 12    | ERROR | /autoware/modes/autonomous       | 4 3   |
+| 13    | OK    | /autoware/modes/stop             |       |
+```

system/diagnostic_graph_aggregator/doc/tool/tree.md

@@ -0,0 +1,40 @@
+# Tree tool
+
+This tool displays the graph structure of the configuration file in tree format.
+
+## Usage
+
+```bash
+ros2 run diagnostic_graph_aggregator tree <graph-config-path>
+```
+
+## Examples
+
+```bash
+ros2 run diagnostic_graph_aggregator tree system/diagnostic_graph_aggregator/example/graph/main.yaml
+```
+
+```txt
+===== root nodes =================================
+- /autoware/modes/local (and)
+    - /external/joystick_command (diag)
+- /autoware/modes/comfortable_stop (and)
+    - /functions/obstacle_detection (or)
+- /autoware/modes/pull_over (and)
+    - /functions/pose_estimation (and)
+    - /functions/obstacle_detection (or)
+- /autoware/modes/autonomous (and)
+    - /functions/pose_estimation (and)
+    - /functions/obstacle_detection (or)
+- /autoware/modes/remote (and)
+    - /external/remote_command (diag)
+===== intermediate nodes =========================
+- /functions/obstacle_detection (or)
+    - /sensing/lidars/front (diag)
+    - /sensing/radars/front (diag)
+- /functions/pose_estimation (and)
+    - /sensing/lidars/top (diag)
+===== isolated nodes =============================
+- /autoware/modes/stop (const)
+- /autoware/modes/emergency_stop (const)
+```

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

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

@@ -1,7 +1,10 @@
 <launch>
+  <arg name="complement" default="false"/>
   <include file="$(find-pkg-share diagnostic_graph_aggregator)/launch/diagnostic_graph_aggregator.launch.xml">
     <arg name="graph_file" value="$(find-pkg-share diagnostic_graph_aggregator)/example/graph/edit.yaml"/>
   </include>
-  <node pkg="diagnostic_graph_aggregator" exec="converter" name="converter"/>
+  <include file="$(find-pkg-share diagnostic_graph_aggregator)/launch/converter.launch.xml">
+    <arg name="complement" value="$(var complement)"/>
+  </include>
   <executable cmd="python3 $(find-pkg-share diagnostic_graph_aggregator)/example/dummy-diags.py"/>
 </launch>

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

@@ -1,7 +1,10 @@
 <launch>
-  <include file="$(find-pkg-share diagnostic_graph_aggregator)/launch/diagnostic_graph_aggregator.launch.xml">
+  <arg name="complement" default="false"/>
+  <include file="$(find-pkg-share diagnostic_graph_aggregator)/launch/aggregator.launch.xml">
     <arg name="graph_file" value="$(find-pkg-share diagnostic_graph_aggregator)/example/graph/main.yaml"/>
   </include>
-  <node pkg="diagnostic_graph_aggregator" exec="converter" name="converter"/>
+  <include file="$(find-pkg-share diagnostic_graph_aggregator)/launch/converter.launch.xml">
+    <arg name="complement" value="$(var complement)"/>
+  </include>
   <executable cmd="python3 $(find-pkg-share diagnostic_graph_aggregator)/example/dummy-diags.py"/>
 </launch>

system/diagnostic_graph_aggregator/launch/converter.launch.xml

@@ -0,0 +1,6 @@
+<launch>
+  <arg name="complement"/>
+  <node pkg="diagnostic_graph_aggregator" exec="converter" name="converter">
+    <param name="complement_tree" value="$(var complement)"/>
+  </node>
+</launch>

system/diagnostic_graph_aggregator/script/dump.py

@@ -84,7 +84,6 @@ def callback(self, msg):
 if __name__ == "__main__":
     parser = argparse.ArgumentParser()
     parser.add_argument("--topic", default="/diagnostics_graph")
-    parser.add_argument("--order", default="index")
     args, unparsed = parser.parse_known_args()
 
     try:

system/diagnostic_graph_aggregator/src/node/aggregator.cpp

@@ -20,7 +20,7 @@
 namespace diagnostic_graph_aggregator
 {
 
-MainNode::MainNode() : Node("diagnostic_graph_aggregator_aggregator")
+AggregatorNode::AggregatorNode() : Node("aggregator")
 {
   // Init diagnostics graph.
   {
@@ -39,7 +39,7 @@ MainNode::MainNode() : Node("diagnostic_graph_aggregator_aggregator")
     const auto qos_input = rclcpp::QoS(declare_parameter<int64_t>("input_qos_depth"));
     const auto qos_graph = rclcpp::QoS(declare_parameter<int64_t>("graph_qos_depth"));
 
-    const auto callback = std::bind(&MainNode::on_diag, this, _1);
+    const auto callback = std::bind(&AggregatorNode::on_diag, this, _1);
     sub_input_ = create_subscription<DiagnosticArray>("/diagnostics", qos_input, callback);
     pub_graph_ = create_publisher<DiagnosticGraph>("/diagnostics_graph", qos_graph);
 
@@ -51,20 +51,20 @@ MainNode::MainNode() : Node("diagnostic_graph_aggregator_aggregator")
   debug_ = declare_parameter<bool>("use_debug_mode");
 }
 
-MainNode::~MainNode()
+AggregatorNode::~AggregatorNode()
 {
   // for unique_ptr
 }
 
-void MainNode::on_timer()
+void AggregatorNode::on_timer()
 {
   const auto stamp = now();
   pub_graph_->publish(graph_.report(stamp));
   if (debug_) graph_.debug();
   if (modes_) modes_->update(stamp);
 }
 
-void MainNode::on_diag(const DiagnosticArray::ConstSharedPtr msg)
+void AggregatorNode::on_diag(const DiagnosticArray::ConstSharedPtr msg)
 {
   graph_.callback(now(), *msg);
 }
@@ -73,10 +73,10 @@ void MainNode::on_diag(const DiagnosticArray::ConstSharedPtr msg)
 
 int main(int argc, char ** argv)
 {
-  using diagnostic_graph_aggregator::MainNode;
+  using diagnostic_graph_aggregator::AggregatorNode;
   rclcpp::init(argc, argv);
   rclcpp::executors::SingleThreadedExecutor executor;
-  auto node = std::make_shared<MainNode>();
+  auto node = std::make_shared<AggregatorNode>();
   executor.add_node(node);
   executor.spin();
   executor.remove_node(node);

system/diagnostic_graph_aggregator/src/node/aggregator.hpp

@@ -26,11 +26,11 @@
 namespace diagnostic_graph_aggregator
 {
 
-class MainNode : public rclcpp::Node
+class AggregatorNode : public rclcpp::Node
 {
 public:
-  MainNode();
-  ~MainNode();
+  AggregatorNode();
+  ~AggregatorNode();
 
 private:
   Graph graph_;

system/diagnostic_graph_aggregator/src/node/converter.cpp

@@ -14,8 +14,7 @@
 
 #include "converter.hpp"
 
-#include <memory>
-#include <string>
+#include <algorithm>
 
 namespace diagnostic_graph_aggregator
 {
@@ -35,18 +34,46 @@ std::string level_to_string(DiagnosticLevel level)
   return "UNKNOWN";
 }
 
-ToolNode::ToolNode() : Node("diagnostic_graph_aggregator_converter")
+std::string parent_path(const std::string & path)
+{
+  return path.substr(0, path.rfind('/'));
+}
+
+auto create_tree(const DiagnosticGraph & graph)
+{
+  std::map<std::string, std::unique_ptr<TreeNode>, std::greater<std::string>> tree;
+  for (const auto & node : graph.nodes) {
+    tree.emplace(node.status.name, std::make_unique<TreeNode>(true));
+  }
+  for (const auto & node : graph.nodes) {
+    std::string path = node.status.name;
+    while (path = parent_path(path), !path.empty()) {
+      if (tree.count(path)) break;
+      tree.emplace(path, std::make_unique<TreeNode>(false));
+    }
+  }
+  for (const auto & [path, node] : tree) {
+    const auto parent = parent_path(path);
+    node->parent = parent.empty() ? nullptr : tree[parent].get();
+  }
+  return tree;
+}
+
+ConverterNode::ConverterNode() : Node("converter")
 {
   using std::placeholders::_1;
   const auto qos_graph = rclcpp::QoS(1);
   const auto qos_array = rclcpp::QoS(1);
 
-  const auto callback = std::bind(&ToolNode::on_graph, this, _1);
+  const auto callback = std::bind(&ConverterNode::on_graph, this, _1);
   sub_graph_ = create_subscription<DiagnosticGraph>("/diagnostics_graph", qos_graph, callback);
-  pub_array_ = create_publisher<DiagnosticArray>("/diagnostics_array", qos_array);
+  pub_array_ = create_publisher<DiagnosticArray>("/diagnostics_agg", qos_array);
+
+  initialize_tree_ = false;
+  complement_tree_ = declare_parameter<bool>("complement_tree");
 }
 
-void ToolNode::on_graph(const DiagnosticGraph::ConstSharedPtr msg)
+void ConverterNode::on_graph(const DiagnosticGraph::ConstSharedPtr msg)
 {
   DiagnosticArray message;
   message.header.stamp = msg->stamp;
@@ -63,17 +90,42 @@ void ToolNode::on_graph(const DiagnosticGraph::ConstSharedPtr 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 & [path, node] : tree_) {
+      if (node->leaf) continue;
+      message.status.emplace_back();
+      message.status.back().name = path;
+      message.status.back().level = node->level;
+    }
+  }
+
   pub_array_->publish(message);
 }
 
 }  // namespace diagnostic_graph_aggregator
 
 int main(int argc, char ** argv)
 {
-  using diagnostic_graph_aggregator::ToolNode;
+  using diagnostic_graph_aggregator::ConverterNode;
   rclcpp::init(argc, argv);
   rclcpp::executors::SingleThreadedExecutor executor;
-  auto node = std::make_shared<ToolNode>();
+  auto node = std::make_shared<ConverterNode>();
   executor.add_node(node);
   executor.spin();
   executor.remove_node(node);