Leverage tesseract_common loadYamlFile and loadYamlString

tesseract_collision/core/include/tesseract_collision/core/contact_managers_plugin_factory.h

@@ -33,6 +33,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <map>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
+#include <tesseract_common/fwd.h>
 #include <tesseract_common/filesystem.h>
 #include <tesseract_common/plugin_loader.h>
 #include <tesseract_common/plugin_info.h>
@@ -109,19 +110,20 @@ class ContactManagersPluginFactory
    * @brief Load plugins from yaml node
    * @param config The config node
    */
-  ContactManagersPluginFactory(YAML::Node config);
+  ContactManagersPluginFactory(YAML::Node config, const tesseract_common::ResourceLocator& locator);
 
   /**
    * @brief Load plugins from file path
    * @param config The config file path
    */
-  ContactManagersPluginFactory(const tesseract_common::fs::path& config);
+  ContactManagersPluginFactory(const tesseract_common::fs::path& config,
+                               const tesseract_common::ResourceLocator& locator);
 
   /**
    * @brief Load plugins from string
    * @param config The config string
    */
-  ContactManagersPluginFactory(const std::string& config);
+  ContactManagersPluginFactory(const std::string& config, const tesseract_common::ResourceLocator& locator);
 
   /**
    * @brief Add location for the plugin loader to search
@@ -281,6 +283,8 @@ class ContactManagersPluginFactory
   tesseract_common::PluginInfoContainer discrete_plugin_info_;
   tesseract_common::PluginInfoContainer continuous_plugin_info_;
   tesseract_common::PluginLoader plugin_loader_;
+
+  void loadConfig(const YAML::Node& config);
 };
 }  // namespace tesseract_collision
 #endif  // TESSERACT_COLLISION_CONTACT_MANAGERS_PLUGIN_FACTORY_H

tesseract_collision/core/src/contact_managers_plugin_factory.cpp

@@ -31,6 +31,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_collision/core/discrete_contact_manager.h>
 #include <tesseract_collision/core/continuous_contact_manager.h>
+#include <tesseract_common/resource_locator.h>
 #include <tesseract_common/plugin_loader.hpp>
 #include <tesseract_common/yaml_utils.h>
 #include <tesseract_common/yaml_extenstions.h>
@@ -58,7 +59,7 @@ ContactManagersPluginFactory::ContactManagersPluginFactory()
                boost::token_compress_on);
 }
 
-ContactManagersPluginFactory::ContactManagersPluginFactory(YAML::Node config) : ContactManagersPluginFactory()
+void ContactManagersPluginFactory::loadConfig(const YAML::Node& config)
 {
   if (const YAML::Node& plugin_info = config[ContactManagersPluginInfo::CONFIG_KEY])
   {
@@ -71,14 +72,26 @@ ContactManagersPluginFactory::ContactManagersPluginFactory(YAML::Node config) :
   }
 }
 
-ContactManagersPluginFactory::ContactManagersPluginFactory(const tesseract_common::fs::path& config)
-  : ContactManagersPluginFactory(YAML::LoadFile(config.string()))
+ContactManagersPluginFactory::ContactManagersPluginFactory(YAML::Node config,
+                                                           const tesseract_common::ResourceLocator& locator)
+  : ContactManagersPluginFactory()
 {
+  config = tesseract_common::processYamlIncludeDirective(config, locator);
+  loadConfig(config);
 }
 
-ContactManagersPluginFactory::ContactManagersPluginFactory(const std::string& config)
-  : ContactManagersPluginFactory(YAML::Load(config))
+ContactManagersPluginFactory::ContactManagersPluginFactory(const tesseract_common::fs::path& config,
+                                                           const tesseract_common::ResourceLocator& locator)
+  : ContactManagersPluginFactory()
 {
+  loadConfig(tesseract_common::loadYamlFile(config.string(), locator));
+}
+
+ContactManagersPluginFactory::ContactManagersPluginFactory(const std::string& config,
+                                                           const tesseract_common::ResourceLocator& locator)
+  : ContactManagersPluginFactory()
+{
+  loadConfig(tesseract_common::loadYamlString(config, locator));
 }
 
 // This prevents it from being defined inline.

tesseract_collision/test/contact_managers_factory_static_unit.cpp

@@ -32,6 +32,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #include <tesseract_collision/core/contact_managers_plugin_factory.h>
 #include <tesseract_collision/core/discrete_contact_manager.h>
 #include <tesseract_collision/bullet/bullet_factories.h>
+#include <tesseract_common/yaml_utils.h>
+#include <tesseract_common/resource_locator.h>
 
 using namespace tesseract_collision;
 
@@ -63,10 +65,11 @@ TEST(TesseractContactManagersFactoryUnit, StaticLoadPlugin)  // NOLINT
                                 BulletCastSimpleManager:
                                   class: BulletCastSimpleManagerFactory)";
 
-  ContactManagersPluginFactory factory(config);
+  tesseract_common::GeneralResourceLocator locator;
+  ContactManagersPluginFactory factory(config, locator);
   factory.clearSearchLibraries();
   factory.clearSearchPaths();
-  YAML::Node plugin_config = YAML::Load(config);
+  YAML::Node plugin_config = tesseract_common::loadYamlString(config, locator);
 
   DiscreteContactManager::UPtr cm = factory.createDiscreteContactManager("BulletDiscreteBVHManager");
   EXPECT_TRUE(cm != nullptr);

tesseract_collision/test/contact_managers_factory_unit.cpp

@@ -32,13 +32,17 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #include <tesseract_collision/core/contact_managers_plugin_factory.h>
 #include <tesseract_collision/core/discrete_contact_manager.h>
 #include <tesseract_collision/core/continuous_contact_manager.h>
+#include <tesseract_common/yaml_utils.h>
+#include <tesseract_common/resource_locator.h>
 
 using namespace tesseract_collision;
 
 void runContactManagersFactoryTest(const tesseract_common::fs::path& config_path)
 {
-  ContactManagersPluginFactory factory(config_path);
-  YAML::Node plugin_config = YAML::LoadFile(config_path.string());
+  tesseract_common::GeneralResourceLocator locator;
+  ContactManagersPluginFactory factory(config_path, locator);
+  YAML::Node plugin_config =
+      tesseract_common::processYamlIncludeDirective(YAML::LoadFile(config_path.string()), locator);
 
   const YAML::Node& plugin_info = plugin_config["contact_manager_plugins"];
   const YAML::Node& search_paths = plugin_info["search_paths"];
@@ -152,8 +156,9 @@ TEST(TesseractContactManagersFactoryUnit, LoadStringPluginTest)  // NOLINT
                                 BulletCastSimpleManager:
                                   class: BulletCastSimpleManagerFactory)";
 
-  ContactManagersPluginFactory factory(config);
-  YAML::Node plugin_config = YAML::Load(config);
+  tesseract_common::GeneralResourceLocator locator;
+  ContactManagersPluginFactory factory(config, locator);
+  YAML::Node plugin_config = tesseract_common::loadYamlString(config, locator);
 
   const YAML::Node& plugin_info = plugin_config["contact_manager_plugins"];
   const YAML::Node& search_paths = plugin_info["search_paths"];
@@ -318,8 +323,9 @@ TEST(TesseractContactManagersFactoryUnit, LoadOnlyDiscretePluginTest)  // NOLINT
                                 FCLDiscreteBVHManager:
                                   class: FCLDiscreteBVHManagerFactory)";
 
-  ContactManagersPluginFactory factory(config);
-  YAML::Node plugin_config = YAML::Load(config);
+  tesseract_common::GeneralResourceLocator locator;
+  ContactManagersPluginFactory factory(config, locator);
+  YAML::Node plugin_config = tesseract_common::loadYamlString(config, locator);
 
   const YAML::Node& plugin_info = plugin_config["contact_manager_plugins"];
   const YAML::Node& search_paths = plugin_info["search_paths"];
@@ -372,8 +378,9 @@ TEST(TesseractContactManagersFactoryUnit, LoadOnlyContinuousPluginTest)  // NOLI
                                 BulletCastSimpleManager:
                                   class: BulletCastSimpleManagerFactory)";
 
-  ContactManagersPluginFactory factory(config);
-  YAML::Node plugin_config = YAML::Load(config);
+  tesseract_common::GeneralResourceLocator locator;
+  ContactManagersPluginFactory factory(config, locator);
+  YAML::Node plugin_config = tesseract_common::loadYamlString(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

@@ -40,15 +40,30 @@ namespace tesseract_common
 {
 class ResourceLocator;
 
+/**
+ * @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);
+
 /**
  * @brief Loads a YAML file and processes `!include` directives recursively.
  *
  * This function loads a YAML file and replaces any node tagged with `!include`
  * with the content of the specified file. It handles nested `!include` directives
  * and works with both maps and sequences.
  *
- * @param file_path The path to the YAML file to be loaded. Relative paths should be
- *                 resolved based on the caller's context.
+ * @param file_path The path to the YAML file/url to be loaded.
  * @param locator The locator used to resolve urls and relative file paths.
  * @return A YAML::Node object containing the fully processed YAML structure.
  *

tesseract_common/src/yaml_utils.cpp

@@ -49,7 +49,7 @@ namespace tesseract_common
  * @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 handleIncludeDirective(const YAML::Node& node, const ResourceLocator& locator)
+YAML::Node processYamlIncludeDirective(const YAML::Node& node, const ResourceLocator& locator)
 {
   if (node.Tag() == "!include")
   {
@@ -63,15 +63,15 @@ YAML::Node handleIncludeDirective(const YAML::Node& node, const ResourceLocator&
     if (resource == nullptr)
       throw std::runtime_error("Unable to locate resource: " + included_file);
 
-    return handleIncludeDirective(YAML::LoadFile(resource->getFilePath()), *resource);
+    return processYamlIncludeDirective(YAML::LoadFile(resource->getFilePath()), *resource);
   }
 
   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] = handleIncludeDirective(it->second, locator);
+      processed_map[it->first] = processYamlIncludeDirective(it->second, locator);
 
     return processed_map;
   }
@@ -81,7 +81,7 @@ YAML::Node handleIncludeDirective(const YAML::Node& node, const ResourceLocator&
     // 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(handleIncludeDirective(child, locator));
+      processed_sequence.push_back(processYamlIncludeDirective(child, locator));
 
     return processed_sequence;
   }
@@ -92,17 +92,15 @@ YAML::Node handleIncludeDirective(const YAML::Node& node, const ResourceLocator&
 
 YAML::Node loadYamlFile(const std::string& file_path, const ResourceLocator& locator)
 {
-  // Load the base YAML file
   auto resource = locator.locateResource(file_path);
   YAML::Node root = YAML::LoadFile(resource->getFilePath());
-  return handleIncludeDirective(root, *resource);
+  return processYamlIncludeDirective(root, *resource);
 }
 
 YAML::Node loadYamlString(const std::string& yaml_string, const ResourceLocator& locator)
 {
-  // Load the base YAML file
   YAML::Node root = YAML::Load(yaml_string);
-  return handleIncludeDirective(root, locator);
+  return processYamlIncludeDirective(root, locator);
 }
 
 void writeYamlToFile(const YAML::Node& node, const std::string& file_path)