update tools

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

Author: isamu-takagi

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 `SATLE`.
+
+## 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/images/rqt_robot_monitor.png

@@ -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/doc/tool/images/rqt_runtime_monitor.png

@@ -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/doc/tool/tree.md

@@ -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/example/example-edit.launch.xml

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

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

@@ -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/launch/diagnostic_graph_aggregator.launch.xml system/diagnostic_graph_aggregator/launch/aggregator.launch.xml

@@ -16,6 +16,8 @@
 
 #include <memory>
 #include <string>
+#include <unordered_map>
+#include <vector>
 
 namespace diagnostic_graph_aggregator
 {
@@ -35,6 +37,29 @@ std::string level_to_string(DiagnosticLevel level)
   return "UNKNOWN";
 }
 
+std::string parent_path(const std::string & path)
+{
+  return path.substr(0, path.rfind('/'));
+}
+
+std::vector<std::string> complement_paths(const DiagnosticGraph & graph)
+{
+  std::unordered_map<std::string, bool> paths;
+  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;
+    }
+  }
+  std::vector<std::string> result;
+  for (const auto & [path, flag] : paths) {
+    if (flag) result.push_back(path);
+  }
+  return result;
+}
+
 ToolNode::ToolNode() : Node("diagnostic_graph_aggregator_converter")
 {
   using std::placeholders::_1;
@@ -43,7 +68,8 @@ ToolNode::ToolNode() : Node("diagnostic_graph_aggregator_converter")
 
   const auto callback = std::bind(&ToolNode::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);
+  complement_inner_nodes_ = declare_parameter<bool>("complement_inner_nodes");
 }
 
 void ToolNode::on_graph(const DiagnosticGraph::ConstSharedPtr msg)
@@ -63,6 +89,18 @@ void ToolNode::on_graph(const DiagnosticGraph::ConstSharedPtr msg)
       }
     }
   }
+
+  if (complement_inner_nodes_) {
+    if (!inner_node_names_) {
+      inner_node_names_ = complement_paths(*msg);
+    }
+    for (const auto & name : inner_node_names_.value()) {
+      message.status.emplace_back();
+      message.status.back().name = name;
+      message.status.back().level = DiagnosticStatus::STALE;
+    }
+  }
+
   pub_array_->publish(message);
 }
 

system/diagnostic_graph_aggregator/launch/converter.launch.xml

@@ -19,6 +19,9 @@
 
 #include <rclcpp/rclcpp.hpp>
 
+#include <string>
+#include <vector>
+
 namespace diagnostic_graph_aggregator
 {
 
@@ -28,6 +31,8 @@ class ToolNode : public rclcpp::Node
   ToolNode();
 
 private:
+  bool complement_inner_nodes_;
+  std::optional<std::vector<std::string>> inner_node_names_;
   rclcpp::Subscription<DiagnosticGraph>::SharedPtr sub_graph_;
   rclcpp::Publisher<DiagnosticArray>::SharedPtr pub_array_;
   void on_graph(const DiagnosticGraph::ConstSharedPtr msg);

system/diagnostic_graph_aggregator/script/dump.py

@@ -0,0 +1,49 @@
+// 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 "utils/loader.hpp"
+
+#include <iostream>
+
+namespace diagnostic_graph_aggregator
+{
+
+void dump_root(const std::string & path)
+{
+  const auto graph = load_graph_nodes(path);
+  const auto color = "#FFFFFF";
+
+  for (const auto & node : graph.nodes) {
+    std::cout << "card " << node << " " << color << " [" << std::endl;
+    std::cout << node->path << std::endl;
+    std::cout << "]" << std::endl;
+  }
+
+  for (const auto & node : graph.nodes) {
+    for (const auto & child : node->children) {
+      std::cout << node << " --> " << child << std::endl;
+    }
+  }
+}
+
+}  // namespace diagnostic_graph_aggregator
+
+int main(int argc, char ** argv)
+{
+  if (argc != 2) {
+    std::cerr << "usage: plantuml <path>" << std::endl;
+    return 1;
+  }
+  diagnostic_graph_aggregator::dump_root(argv[1]);
+}

system/diagnostic_graph_aggregator/src/node/converter.cpp

@@ -0,0 +1,69 @@
+// 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 "utils/loader.hpp"
+
+#include <iostream>
+
+namespace diagnostic_graph_aggregator
+{
+
+void dump_node(const GraphNode * node, const std::string & indent = "", bool root = true)
+{
+  const auto path = node->path.empty() ? "" : node->path + " ";
+  const auto type = "(" + node->type + ")";
+  std::cout << indent << "- " << path << type << std::endl;
+
+  if (root || node->parents.size() == 1) {
+    for (const auto child : node->children) {
+      dump_node(child, indent + "    ", false);
+    }
+  }
+}
+
+void dump_root(const std::string & path)
+{
+  const auto graph = load_graph_nodes(path);
+
+  std::cout << "===== root nodes =================================" << std::endl;
+  for (const auto & node : graph.nodes) {
+    if (node->parents.size() == 0 && node->children.size() != 0) {
+      dump_node(node);
+    }
+  }
+  std::cout << "===== intermediate nodes =========================" << std::endl;
+  for (const auto & node : graph.nodes) {
+    if (node->parents.size() >= 2) {
+      dump_node(node);
+    }
+  }
+
+  std::cout << "===== isolated nodes =============================" << std::endl;
+  for (const auto & node : graph.nodes) {
+    if (node->parents.size() == 0 && node->children.size() == 0) {
+      dump_node(node);
+    }
+  }
+}
+
+}  // namespace diagnostic_graph_aggregator
+
+int main(int argc, char ** argv)
+{
+  if (argc != 2) {
+    std::cerr << "usage: tree <path>" << std::endl;
+    return 1;
+  }
+  diagnostic_graph_aggregator::dump_root(argv[1]);
+}

system/diagnostic_graph_aggregator/src/node/converter.hpp

@@ -0,0 +1,57 @@
+// 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 "loader.hpp"
+
+#include <unordered_map>
+#include <utility>
+
+namespace diagnostic_graph_aggregator
+{
+
+GraphRoot load_graph_nodes(const std::string & path)
+{
+  GraphRoot result;
+  {
+    std::unordered_map<BaseUnit *, GraphNode::UniquePtr> mapping;
+    Graph graph;
+    graph.init(path);
+
+    for (const auto & node : graph.nodes()) {
+      auto data = std::make_unique<GraphNode>();
+      data->path = node->path();
+      data->type = node->type();
+      mapping[node] = std::move(data);
+    }
+
+    for (const auto & [node, data] : mapping) {
+      for (const auto & link : node->children()) {
+        const auto parent = data.get();
+        const auto child = mapping.at(link).get();
+        child->parents.push_back(parent);
+        parent->children.push_back(child);
+      }
+    }
+
+    for (auto & [node, data] : mapping) {
+      result.owner.push_back(std::move(data));
+    }
+    for (const auto & node : result.owner) {
+      result.nodes.push_back(node.get());
+    }
+  }
+  return result;
+}
+
+}  // namespace diagnostic_graph_aggregator

system/diagnostic_graph_aggregator/src/tool/plantuml.cpp

@@ -0,0 +1,48 @@
+// 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 TOOL__UTILS__LOADER_HPP_
+#define TOOL__UTILS__LOADER_HPP_
+
+#include "graph/graph.hpp"
+#include "graph/types.hpp"
+#include "graph/units.hpp"
+
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace diagnostic_graph_aggregator
+{
+
+struct GraphNode
+{
+  using UniquePtr = std::unique_ptr<GraphNode>;
+  std::string type;
+  std::string path;
+  std::vector<GraphNode *> children;
+  std::vector<GraphNode *> parents;
+};
+
+struct GraphRoot
+{
+  std::vector<GraphNode::UniquePtr> owner;
+  std::vector<GraphNode *> nodes;
+};
+
+GraphRoot load_graph_nodes(const std::string & path);
+
+}  // namespace diagnostic_graph_aggregator
+
+#endif  // TOOL__UTILS__LOADER_HPP_