Improve processYamlIncludeDirective peformance

Author: Levi-Armstrong

tesseract_collision/core/src/contact_managers_plugin_factory.cpp

@@ -78,7 +78,7 @@ ContactManagersPluginFactory::ContactManagersPluginFactory(YAML::Node config,
                                                            const tesseract_common::ResourceLocator& locator)
   : ContactManagersPluginFactory()
 {
-  config = tesseract_common::processYamlIncludeDirective(config, locator);
+  tesseract_common::processYamlIncludeDirective(config, locator);
   loadConfig(config);
 }
 

tesseract_collision/test/contact_managers_factory_unit.cpp

@@ -41,8 +41,8 @@ void runContactManagersFactoryTest(const std::filesystem::path& config_path)
 {
   tesseract_common::GeneralResourceLocator locator;
   ContactManagersPluginFactory factory(config_path, locator);
-  YAML::Node plugin_config =
-      tesseract_common::processYamlIncludeDirective(YAML::LoadFile(config_path.string()), locator);
+  YAML::Node plugin_config = YAML::LoadFile(config_path.string());
+  tesseract_common::processYamlIncludeDirective(plugin_config, locator);
 
   const YAML::Node& plugin_info = plugin_config["contact_manager_plugins"];
   const YAML::Node& search_paths = plugin_info["search_paths"];

tesseract_common/include/tesseract_common/yaml_utils.h

@@ -41,20 +41,30 @@ namespace tesseract_common
 class ResourceLocator;
 
 /**
- * @brief Recursively processes a YAML node to resolve `!include` directives.
+ * @brief Recursively expands `!include` directives in a YAML node, in-place.
  *
- * This function replaces any node tagged with `!include` with the content of the
- * specified file. It also recursively processes maps and sequences to handle nested
- * `!include` directives.
+ * This function walks the given node and:
+ *  - When it encounters a node tagged `!include`, it loads the referenced file
+ *    (via the provided ResourceLocator), replaces the tagged node with the parsed
+ *    content of that file, clears the `!include` tag, and then continues processing
+ *    the newly loaded subtree.
+ *  - When it encounters a mapping, it recurses into each value in-place.
+ *  - When it encounters a sequence, it recurses into each element in-place.
  *
- * @param node The YAML node to process.
- * @param locator The locator used to resolve urls and relative file paths.
- * @return A YAML::Node with all `!include` directives resolved.
+ * After this call returns, `node` and its entire subtree will have had all
+ * `!include` directives resolved and removed. Subsequent calls on the same
+ * expanded tree are no-ops.
  *
- * @throws std::runtime_error if an `!include` tag is used improperly (e.g., not scalar),
- *         or if a file specified in an `!include` directive cannot be loaded.
+ * @param node    The YAML node to process. Must be non-const so it can
+ *                        be mutated in-place as includes are expanded.
+ * @param locator The locator used to resolve file paths or URLs for
+ *                        `!include` directives. May maintain internal state
+ *                        (e.g. cache) between calls.
+ *
+ * @throws std::runtime_error if an `!include` tag is not a scalar string,
+ *         or if the specified file cannot be located or loaded.
  */
-YAML::Node processYamlIncludeDirective(const YAML::Node& node, const ResourceLocator& locator);
+void processYamlIncludeDirective(YAML::Node& node, const ResourceLocator& locator);
 
 /**
  * @brief Loads a YAML file and processes `!include` directives recursively.

tesseract_common/src/yaml_utils.cpp

@@ -35,22 +35,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 namespace tesseract_common
 {
-/**
- * @brief Recursively processes a YAML node to resolve `!include` directives.
- *
- * This function replaces any node tagged with `!include` with the content of the
- * specified file. It also recursively processes maps and sequences to handle nested
- * `!include` directives.
- *
- * @param node The YAML node to process.
- * @param locator The locator used to resolve urls and relative file paths.
- * @return A YAML::Node with all `!include` directives resolved.
- *
- * @throws std::runtime_error if an `!include` tag is used improperly (e.g., not scalar),
- *         or if a file specified in an `!include` directive cannot be loaded.
- */
-YAML::Node processYamlIncludeDirective(const YAML::Node& node, const ResourceLocator& locator)
+void processYamlIncludeDirective(YAML::Node& node, const ResourceLocator& locator)
 {
+  // Case 1: this node *is* an include → replace it with the loaded file
   if (node.Tag() == "!include")
   {
     // Ensure the node is scalar and contains a file path
@@ -63,44 +50,76 @@ YAML::Node processYamlIncludeDirective(const YAML::Node& node, const ResourceLoc
     if (resource == nullptr)
       throw std::runtime_error("Unable to locate resource: " + included_file);
 
-    return processYamlIncludeDirective(YAML::LoadFile(resource->getFilePath()), *resource);
+    // Parse once
+    YAML::Node loaded = YAML::LoadFile(resource->getFilePath());
+
+    // Take over this node
+    node = loaded;
+
+    // Clear the old tag so we don't re‐process it
+    node.SetTag("");
+
+    // Recurse into what we just loaded
+    processYamlIncludeDirective(node, *resource);
+    return;
   }
 
+  // Case 2: map → mutate each value in‐place
   if (node.IsMap())
   {
-    // Create a new map and process each key-value pair
-    YAML::Node processed_map = YAML::Node(YAML::NodeType::Map);
     for (auto it = node.begin(); it != node.end(); ++it)
-      processed_map[it->first] = processYamlIncludeDirective(it->second, locator);
+    {
+      // 1) pull out the key
+      const std::string key = it->first.Scalar();
 
-    return processed_map;
+      // 2) copy the child handle (this is cheap)
+      YAML::Node child = it->second;
+
+      // 3) recurse & mutate the child
+      processYamlIncludeDirective(child, locator);
+
+      // 4) write it back into the map
+      node[key] = child;
+    }
+
+    return;
   }
 
+  // Case 3: sequence → mutate each element in‐place
   if (node.IsSequence())
   {
-    // Create a new sequence and process each element
-    YAML::Node processed_sequence = YAML::Node(YAML::NodeType::Sequence);
-    for (const auto& child : node)
-      processed_sequence.push_back(processYamlIncludeDirective(child, locator));
+    // NOLINTNEXTLINE(modernize-loop-convert)
+    for (std::size_t i = 0; i < node.size(); ++i)
+    {
+      // 1) pull out the element (this is cheap—just a handle copy)
+      YAML::Node child = node[i];
+
+      // 2) recurse & mutate that subtree
+      processYamlIncludeDirective(child, locator);
+
+      // 3) write it back into the sequence
+      node[i] = child;
+    }
 
-    return processed_sequence;
+    return;
   }
 
-  // Return the node as-is for scalars and unsupported types
-  return node;
+  // Case 4: scalar or anything else → nothing to do
 }
 
 YAML::Node loadYamlFile(const std::string& file_path, const ResourceLocator& locator)
 {
   auto resource = locator.locateResource(file_path);
   YAML::Node root = YAML::LoadFile(resource->getFilePath());
-  return processYamlIncludeDirective(root, *resource);
+  processYamlIncludeDirective(root, *resource);
+  return root;
 }
 
 YAML::Node loadYamlString(const std::string& yaml_string, const ResourceLocator& locator)
 {
   YAML::Node root = YAML::Load(yaml_string);
-  return processYamlIncludeDirective(root, locator);
+  processYamlIncludeDirective(root, locator);
+  return root;
 }
 
 void writeYamlToFile(const YAML::Node& node, const std::string& file_path)

tesseract_kinematics/core/src/kinematics_plugin_factory.cpp

@@ -76,7 +76,7 @@ void KinematicsPluginFactory::loadConfig(const YAML::Node& config)
 KinematicsPluginFactory::KinematicsPluginFactory(YAML::Node config, const tesseract_common::ResourceLocator& locator)
   : KinematicsPluginFactory()
 {
-  config = tesseract_common::processYamlIncludeDirective(config, locator);
+  tesseract_common::processYamlIncludeDirective(config, locator);
   loadConfig(config);
 }
 

tesseract_srdf/src/configs.cpp

@@ -71,7 +71,8 @@ tesseract_common::CalibrationInfo parseCalibrationConfig(const tesseract_scene_g
   YAML::Node config;
   try
   {
-    config = tesseract_common::processYamlIncludeDirective(YAML::LoadFile(cal_config_file_path.string()), locator);
+    config = YAML::LoadFile(cal_config_file_path.string());
+    tesseract_common::processYamlIncludeDirective(config, locator);
   }
   // LCOV_EXCL_START
   catch (...)
@@ -103,7 +104,8 @@ tesseract_common::KinematicsPluginInfo parseKinematicsPluginConfig(const tessera
   YAML::Node config;
   try
   {
-    config = tesseract_common::processYamlIncludeDirective(YAML::LoadFile(kin_plugin_file_path.string()), locator);
+    config = YAML::LoadFile(kin_plugin_file_path.string());
+    tesseract_common::processYamlIncludeDirective(config, locator);
   }
   // LCOV_EXCL_START
   catch (...)
@@ -128,7 +130,8 @@ parseContactManagersPluginConfig(const tesseract_common::ResourceLocator& locato
   YAML::Node config;
   try
   {
-    config = tesseract_common::processYamlIncludeDirective(YAML::LoadFile(cm_plugin_file_path.string()), locator);
+    config = YAML::LoadFile(cm_plugin_file_path.string());
+    tesseract_common::processYamlIncludeDirective(config, locator);
   }
   // LCOV_EXCL_START
   catch (...)