feat(system_diagnostic_graph): support config override and add tests (#…

…5816)

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

system/system_diagnostic_graph/CMakeLists.txt

@@ -20,9 +20,18 @@ ament_auto_add_executable(converter
   src/tool.cpp
 )
 
+ament_auto_add_executable(tool
+  src/tool/main.cpp
+)
+target_include_directories(tool PRIVATE src/core)
+
 if(BUILD_TESTING)
   get_filename_component(RESOURCE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/test/files ABSOLUTE)
-  ament_auto_add_gtest(gtest_${PROJECT_NAME} test/src/test.cpp)
+  ament_auto_add_gtest(gtest_${PROJECT_NAME}
+    test/src/test1.cpp
+    test/src/test2.cpp
+    test/src/utils.cpp
+  )
   target_compile_definitions(gtest_${PROJECT_NAME} PRIVATE TEST_RESOURCE_PATH="${RESOURCE_PATH}")
   target_include_directories(gtest_${PROJECT_NAME} PRIVATE src)
 endif()

system/system_diagnostic_graph/config/default.param.yaml

@@ -1,7 +1,7 @@
 /**:
   ros__parameters:
-    mode_availability: true
-    mode: psim
+    use_operation_mode_availability: true
+    use_debug_mode: false
     rate: 1.0
     input_qos_depth: 1000
     graph_qos_depth: 1

system/system_diagnostic_graph/example/example_0.yaml

@@ -22,15 +22,15 @@ nodes:
     list:
       - { type: link, link: /external/remote_command }
 
-  - path: /autoware/modes/emergency-stop
+  - path: /autoware/modes/emergency_stop
     type: ok
 
-  - path: /autoware/modes/comfortable-stop
+  - path: /autoware/modes/comfortable_stop
     type: and
     list:
       - { type: link, link: /functions/obstacle_detection }
 
-  - path: /autoware/modes/pull-over
+  - path: /autoware/modes/pull_over
     type: and
     list:
       - { type: link, link: /functions/pose_estimation }

system/system_diagnostic_graph/src/core/config.cpp

@@ -22,6 +22,8 @@
 #include <memory>
 #include <regex>
 #include <string>
+#include <unordered_map>
+#include <unordered_set>
 #include <utility>
 #include <vector>
 
@@ -66,15 +68,24 @@ ConfigData ConfigData::load(YAML::Node yaml)
 ConfigData ConfigData::type(const std::string & name) const
 {
   ConfigData data(file);
-  data.mark = name;
+  data.mark = mark.empty() ? name : mark + "-" + name;
   return data;
 }
 
 ConfigData ConfigData::node(const size_t index) const
 {
-  ConfigData data(file);
-  data.mark = mark + "-" + std::to_string(index);
-  return data;
+  return type(std::to_string(index));
+}
+
+std::optional<YAML::Node> ConfigData::take_yaml(const std::string & name)
+{
+  if (!object.count(name)) {
+    return std::nullopt;
+  }
+
+  const auto yaml = object.at(name);
+  object.erase(name);
+  return yaml;
 }
 
 std::string ConfigData::take_text(const std::string & name)
@@ -114,50 +125,82 @@ std::vector<YAML::Node> ConfigData::take_list(const std::string & name)
   return std::vector<YAML::Node>(yaml.begin(), yaml.end());
 }
 
-void check_config_nodes(const std::vector<UnitConfig::SharedPtr> & nodes)
+void resolve_link_nodes(RootConfig & root)
 {
-  std::unordered_map<std::string, size_t> path_count;
-  for (const auto & node : nodes) {
-    path_count[node->path] += 1;
+  std::unordered_map<std::string, UnitConfig::SharedPtr> paths;
+  for (const auto & node : root.nodes) {
+    if (node->path.empty()) {
+      continue;
+    }
+    if (paths.count(node->path)) {
+      throw error<PathConflict>("object path is not unique", node->path);
+    }
+    paths[node->path] = node;
   }
 
-  path_count.erase("");
-  for (const auto & [path, count] : path_count) {
-    if (1 < count) {
-      throw error<PathConflict>("object path is not unique", path);
+  std::vector<UnitConfig::SharedPtr> nodes;
+  std::vector<UnitConfig::SharedPtr> links;
+  for (const auto & node : root.nodes) {
+    if (node->type == "link") {
+      links.push_back(node);
+    } else {
+      nodes.push_back(node);
     }
   }
+
+  std::unordered_map<UnitConfig::SharedPtr, UnitConfig::SharedPtr> targets;
+  for (const auto & node : nodes) {
+    targets[node] = node;
+  }
+  for (const auto & node : links) {
+    const auto path = node->data.take_text("link");
+    if (!paths.count(path)) {
+      throw error<PathNotFound>("link path is not found", path, node->data);
+    }
+    const auto link = paths.at(path);
+    if (link->type == "link") {
+      throw error<GraphStructure>("link target is link type", path, node->data);
+    }
+    targets[node] = link;
+  }
+  for (const auto & node : nodes) {
+    for (auto & child : node->children) {
+      child = targets.at(child);
+    }
+  }
+  root.nodes = nodes;
 }
 
-void resolve_link_nodes(std::vector<UnitConfig::SharedPtr> & nodes)
+void resolve_remove_edits(RootConfig & root)
 {
-  std::vector<UnitConfig::SharedPtr> filtered;
-  std::unordered_map<UnitConfig::SharedPtr, UnitConfig::SharedPtr> links;
   std::unordered_map<std::string, UnitConfig::SharedPtr> paths;
-
-  for (const auto & node : nodes) {
-    links[node] = node;
+  for (const auto & node : root.nodes) {
     paths[node->path] = node;
   }
 
-  for (const auto & node : nodes) {
-    if (node->type == "link" && node->path == "") {
-      const auto link = node->data.take_text("link");
-      if (!paths.count(link)) {
-        throw error<PathNotFound>("link path is not found", link, node->data);
+  std::unordered_set<UnitConfig::SharedPtr> removes;
+  for (const auto & edit : root.edits) {
+    if (edit->type == "remove") {
+      if (!paths.count(edit->path)) {
+        throw error<PathNotFound>("remove path is not found", edit->path, edit->data);
       }
-      links[node] = paths.at(link);
-    } else {
-      filtered.push_back(node);
+      removes.insert(paths.at(edit->path));
     }
   }
-  nodes = filtered;
 
-  for (const auto & node : nodes) {
-    for (auto & child : node->children) {
-      child = links.at(child);
+  const auto filter = [removes](const std::vector<UnitConfig::SharedPtr> & nodes) {
+    std::vector<UnitConfig::SharedPtr> result;
+    for (const auto & node : nodes) {
+      if (!removes.count(node)) {
+        result.push_back(node);
+      }
     }
+    return result;
+  };
+  for (const auto & node : root.nodes) {
+    node->children = filter(node->children);
   }
+  root.nodes = filter(root.nodes);
 }
 
 std::string complement_node_type(ConfigData & data)
@@ -224,13 +267,23 @@ UnitConfig::SharedPtr parse_node_config(const ConfigData & data)
   return node;
 }
 
+EditConfig::SharedPtr parse_edit_config(const ConfigData & data)
+{
+  const auto edit = std::make_shared<EditConfig>(data);
+  edit->path = edit->data.take_text("path", "");
+  edit->type = edit->data.take_text("type", "");
+  return edit;
+}
+
 FileConfig::SharedPtr parse_file_config(const ConfigData & data)
 {
   const auto file = std::make_shared<FileConfig>(data);
   const auto path_data = data.type("file");
   const auto node_data = data.type("node");
+  const auto edit_data = data.type("edit");
   const auto paths = file->data.take_list("files");
   const auto nodes = file->data.take_list("nodes");
+  const auto edits = file->data.take_list("edits");
 
   for (const auto & [index, yaml] : enumerate(paths)) {
     const auto path = path_data.node(index).load(yaml);
@@ -240,6 +293,10 @@ FileConfig::SharedPtr parse_file_config(const ConfigData & data)
     const auto node = node_data.node(index).load(yaml);
     file->nodes.push_back(parse_node_config(node));
   }
+  for (const auto & [index, yaml] : enumerate(edits)) {
+    const auto edit = edit_data.node(index).load(yaml);
+    file->edits.push_back(parse_edit_config(edit));
+  }
   return file;
 }
 
@@ -267,20 +324,22 @@ RootConfig load_root_config(const PathConfig::SharedPtr root)
   }
 
   std::vector<UnitConfig::SharedPtr> nodes;
+  std::vector<EditConfig::SharedPtr> edits;
   for (const auto & file : files) {
     extend(nodes, file->nodes);
+    extend(edits, file->edits);
   }
   for (size_t i = 0; i < nodes.size(); ++i) {
     const auto node = nodes[i];
     extend(nodes, node->children);
   }
 
-  check_config_nodes(nodes);
-  resolve_link_nodes(nodes);
-
   RootConfig config;
   config.files = files;
   config.nodes = nodes;
+  config.edits = edits;
+  resolve_link_nodes(config);
+  resolve_remove_edits(config);
   return config;
 }
 

system/system_diagnostic_graph/src/core/config.hpp

@@ -18,6 +18,7 @@
 #include <yaml-cpp/yaml.h>
 
 #include <memory>
+#include <optional>
 #include <string>
 #include <unordered_map>
 #include <vector>
@@ -32,6 +33,14 @@ struct ConfigData
   ConfigData type(const std::string & name) const;
   ConfigData node(const size_t index) const;
 
+  template <class T>
+  T take(const std::string & name, const T & fail)
+  {
+    const auto yaml = take_yaml(name);
+    return yaml ? yaml.value().as<T>() : fail;
+  }
+
+  std::optional<YAML::Node> take_yaml(const std::string & name);
   std::string take_text(const std::string & name);
   std::string take_text(const std::string & name, const std::string & fail);
   std::vector<YAML::Node> take_list(const std::string & name);
@@ -64,18 +73,28 @@ struct UnitConfig : public BaseConfig
   std::vector<UnitConfig::SharedPtr> children;
 };
 
+struct EditConfig : public BaseConfig
+{
+  using SharedPtr = std::shared_ptr<EditConfig>;
+  using BaseConfig::BaseConfig;
+  std::string type;
+  std::string path;
+};
+
 struct FileConfig : public BaseConfig
 {
   using SharedPtr = std::shared_ptr<FileConfig>;
   using BaseConfig::BaseConfig;
   std::vector<PathConfig::SharedPtr> paths;
   std::vector<UnitConfig::SharedPtr> nodes;
+  std::vector<EditConfig::SharedPtr> edits;
 };
 
 struct RootConfig
 {
   std::vector<FileConfig::SharedPtr> files;
   std::vector<UnitConfig::SharedPtr> nodes;
+  std::vector<EditConfig::SharedPtr> edits;
 };
 
 template <class T>

system/system_diagnostic_graph/src/core/debug.cpp

@@ -26,19 +26,32 @@
 namespace system_diagnostic_graph
 {
 
-const std::unordered_map<DiagnosticLevel, std::string> level_names = {
-  {DiagnosticStatus::OK, "OK"},
-  {DiagnosticStatus::WARN, "WARN"},
-  {DiagnosticStatus::ERROR, "ERROR"},
-  {DiagnosticStatus::STALE, "STALE"}};
+std::string get_level_text(DiagnosticLevel level)
+{
+  switch (level) {
+    case DiagnosticStatus::OK:
+      return "OK";
+    case DiagnosticStatus::WARN:
+      return "WARN";
+    case DiagnosticStatus::ERROR:
+      return "ERROR";
+    case DiagnosticStatus::STALE:
+      return "STALE";
+  }
+  return "UNKNOWN";
+}
 
 void Graph::debug()
 {
   std::vector<DiagDebugData> lines;
   for (const auto & node : nodes_) {
-    const auto level_name = level_names.at(node->level());
+    const auto level_name = get_level_text(node->level());
     const auto index_name = std::to_string(node->index());
-    lines.push_back({"unit", index_name, level_name, node->path(), "-----"});
+    lines.push_back({index_name, level_name, node->path(), node->type()});
+  }
+  for (const auto & [name, level] : unknowns_) {
+    const auto level_name = get_level_text(level);
+    lines.push_back({"*", level_name, name, "unknown"});
   }
 
   std::array<size_t, diag_debug_size> widths = {};

system/system_diagnostic_graph/src/core/debug.hpp

@@ -21,7 +21,7 @@
 namespace system_diagnostic_graph
 {
 
-constexpr size_t diag_debug_size = 5;
+constexpr size_t diag_debug_size = 4;
 using DiagDebugData = std::array<std::string, diag_debug_size>;
 
 }  // namespace system_diagnostic_graph

system/system_diagnostic_graph/src/core/error.hpp

@@ -40,6 +40,11 @@ class InvalidType : public Exception
   using Exception::Exception;
 };
 
+class InvalidValue : public Exception
+{
+  using Exception::Exception;
+};
+
 class FieldNotFound : public Exception
 {
   using Exception::Exception;