Leverage tesseract_common loadYamlFile and loadYamlString

tesseract-robotics · Jan 5, 2025 · fcd73ab · fcd73ab
1 parent f6c8709
commit fcd73ab
Show file tree

Hide file tree

Showing 19 changed files with 92 additions and 56 deletions.
diff --git a/tesseract_examples/src/basic_cartesian_example.cpp b/tesseract_examples/src/basic_cartesian_example.cpp
@@ -178,7 +178,7 @@ bool BasicCartesianExample::run()
   // Create Task Composer Plugin Factory
   const std::string share_dir(TESSERACT_TASK_COMPOSER_DIR);
   tesseract_common::fs::path config_path(share_dir + "/config/task_composer_plugins.yaml");
-  TaskComposerPluginFactory factory(config_path);
+  TaskComposerPluginFactory factory(config_path, *env_->getResourceLocator());
 
   // Create Program
   CompositeInstruction program("cartesian_program", ManipulatorInfo("manipulator", "base_link", "tool0"));

diff --git a/tesseract_examples/src/car_seat_example.cpp b/tesseract_examples/src/car_seat_example.cpp
@@ -250,7 +250,7 @@ bool CarSeatExample::run()
   // Create Task Composer Plugin Factory
   const std::string share_dir(TESSERACT_TASK_COMPOSER_DIR);
   tesseract_common::fs::path config_path(share_dir + "/config/task_composer_plugins.yaml");
-  TaskComposerPluginFactory factory(config_path);
+  TaskComposerPluginFactory factory(config_path, *env_->getResourceLocator());
 
   // Get manipulator
   JointGroup::ConstPtr joint_group = env_->getJointGroup("manipulator");

diff --git a/tesseract_examples/src/freespace_hybrid_example.cpp b/tesseract_examples/src/freespace_hybrid_example.cpp
@@ -164,7 +164,7 @@ bool FreespaceHybridExample::run()
   // Create Task Composer Plugin Factory
   const std::string share_dir(TESSERACT_TASK_COMPOSER_DIR);
   tesseract_common::fs::path config_path(share_dir + "/config/task_composer_plugins.yaml");
-  TaskComposerPluginFactory factory(config_path);
+  TaskComposerPluginFactory factory(config_path, *env_->getResourceLocator());
 
   // Create Program
   CompositeInstruction program("FREESPACE", ManipulatorInfo("manipulator", "base_link", "tool0"));

diff --git a/tesseract_examples/src/freespace_ompl_example.cpp b/tesseract_examples/src/freespace_ompl_example.cpp
@@ -150,7 +150,7 @@ bool FreespaceOMPLExample::run()
   // Create Task Composer Plugin Factory
   const std::string share_dir(TESSERACT_TASK_COMPOSER_DIR);
   tesseract_common::fs::path config_path(share_dir + "/config/task_composer_plugins.yaml");
-  TaskComposerPluginFactory factory(config_path);
+  TaskComposerPluginFactory factory(config_path, *env_->getResourceLocator());
 
   // Create Program
   CompositeInstruction program("FREESPACE", ManipulatorInfo("manipulator", "base_link", "tool0"));

diff --git a/tesseract_examples/src/glass_upright_example.cpp b/tesseract_examples/src/glass_upright_example.cpp
@@ -166,7 +166,7 @@ bool GlassUprightExample::run()
   // Create Task Composer Plugin Factory
   const std::string share_dir(TESSERACT_TASK_COMPOSER_DIR);
   tesseract_common::fs::path config_path(share_dir + "/config/task_composer_plugins.yaml");
-  TaskComposerPluginFactory factory(config_path);
+  TaskComposerPluginFactory factory(config_path, *env_->getResourceLocator());
 
   // Create Program
   CompositeInstruction program("UPRIGHT", ManipulatorInfo("manipulator", "base_link", "tool0"));

diff --git a/tesseract_examples/src/pick_and_place_example.cpp b/tesseract_examples/src/pick_and_place_example.cpp
@@ -181,7 +181,7 @@ bool PickAndPlaceExample::run()
   // Create Task Composer Plugin Factory
   const std::string share_dir(TESSERACT_TASK_COMPOSER_DIR);
   tesseract_common::fs::path config_path(share_dir + "/config/task_composer_plugins.yaml");
-  TaskComposerPluginFactory factory(config_path);
+  TaskComposerPluginFactory factory(config_path, *env_->getResourceLocator());
 
   // Create Program
   CompositeInstruction pick_program("DEFAULT", ManipulatorInfo("manipulator", LINK_BASE_NAME, LINK_END_EFFECTOR_NAME));

diff --git a/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp b/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp
@@ -200,7 +200,7 @@ bool PuzzlePieceAuxillaryAxesExample::run()
   // Create Task Composer Plugin Factory
   const std::string share_dir(TESSERACT_TASK_COMPOSER_DIR);
   tesseract_common::fs::path config_path(share_dir + "/config/task_composer_plugins.yaml");
-  TaskComposerPluginFactory factory(config_path);
+  TaskComposerPluginFactory factory(config_path, *env_->getResourceLocator());
 
   // Create Program
   CompositeInstruction program("DEFAULT", mi);

diff --git a/tesseract_examples/src/puzzle_piece_example.cpp b/tesseract_examples/src/puzzle_piece_example.cpp
@@ -194,7 +194,7 @@ bool PuzzlePieceExample::run()
   // Create Task Composer Plugin Factory
   const std::string share_dir(TESSERACT_TASK_COMPOSER_DIR);
   tesseract_common::fs::path config_path(share_dir + "/config/task_composer_plugins.yaml");
-  TaskComposerPluginFactory factory(config_path);
+  TaskComposerPluginFactory factory(config_path, *env_->getResourceLocator());
 
   // Create Program
   CompositeInstruction program("DEFAULT", mi);

diff --git a/...ct_task_composer/core/include/tesseract_task_composer/core/task_composer_plugin_factory.h b/...ct_task_composer/core/include/tesseract_task_composer/core/task_composer_plugin_factory.h
@@ -113,19 +113,19 @@ class TaskComposerPluginFactory
    * @brief Load plugins from yaml node
    * @param config The config node
    */
-  TaskComposerPluginFactory(const YAML::Node& config);
+  TaskComposerPluginFactory(const YAML::Node& config, const tesseract_common::ResourceLocator& locator);
 
   /**
    * @brief Load plugins from file path
    * @param config The config file path
    */
-  TaskComposerPluginFactory(const tesseract_common::fs::path& config);
+  TaskComposerPluginFactory(const tesseract_common::fs::path& config, const tesseract_common::ResourceLocator& locator);
 
   /**
    * @brief Load plugins from string
    * @param config The config string
    */
-  TaskComposerPluginFactory(const std::string& config);
+  TaskComposerPluginFactory(const std::string& config, const tesseract_common::ResourceLocator& locator);
 
   /**
    * @brief Loads plugins from a configuration object
@@ -137,19 +137,19 @@ class TaskComposerPluginFactory
    * @brief Load plugins from yaml node
    * @param config The config node
    */
-  void loadConfig(const YAML::Node& config);
+  void loadConfig(const YAML::Node& config, const tesseract_common::ResourceLocator& locator);
 
   /**
    * @brief Load plugins from file path
    * @param config The config file path
    */
-  void loadConfig(const tesseract_common::fs::path& config);
+  void loadConfig(const tesseract_common::fs::path& config, const tesseract_common::ResourceLocator& locator);
 
   /**
    * @brief Load plugins from string
    * @param config The config string
    */
-  void loadConfig(const std::string& config);
+  void loadConfig(const std::string& config, const tesseract_common::ResourceLocator& locator);
 
   /**
    * @brief Add location for the plugin loader to search
@@ -307,6 +307,8 @@ class TaskComposerPluginFactory
 private:
   struct Implementation;
   std::unique_ptr<Implementation> impl_;
+
+  void loadConfig(const YAML::Node& config);
 };
 }  // namespace tesseract_planning
 #endif  // TESSERACT_TASK_COMPOSER_TASK_COMPOSER_FACTORY_H
diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_server.h b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_server.h
@@ -32,6 +32,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <map>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
+#include <tesseract_common/fwd.h>
 #include <tesseract_common/filesystem.h>
 
 namespace YAML
@@ -62,19 +63,19 @@ class TaskComposerServer
    * @brief Load plugins from yaml node
    * @param config The config node
    */
-  void loadConfig(const YAML::Node& config);
+  void loadConfig(const YAML::Node& config, const tesseract_common::ResourceLocator& locator);
 
   /**
    * @brief Load plugins from file path
    * @param config The config file path
    */
-  void loadConfig(const tesseract_common::fs::path& config);
+  void loadConfig(const tesseract_common::fs::path& config, const tesseract_common::ResourceLocator& locator);
 
   /**
    * @brief Load plugins from string
    * @param config The config string
    */
-  void loadConfig(const std::string& config);
+  void loadConfig(const std::string& config, const tesseract_common::ResourceLocator& locator);
 
   /**
    * @brief Add a executors (thread pool)

diff --git a/...oser/core/include/tesseract_task_composer/core/test_suite/task_composer_executor_unit.hpp b/...oser/core/include/tesseract_task_composer/core/test_suite/task_composer_executor_unit.hpp
@@ -40,6 +40,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #include <tesseract_task_composer/core/task_composer_plugin_factory.h>
 #include <tesseract_task_composer/core/nodes/done_task.h>
 
+#include <tesseract_common/resource_locator.h>
+#include <tesseract_common/yaml_utils.h>
+
 namespace tesseract_planning::test_suite
 {
 template <typename T>
@@ -78,6 +81,7 @@ void runTaskComposerExecutorTest()
     test_suite::runSerializationPointerTest(executor, "TaskComposerExecutorTests");
   }
 
+  tesseract_common::GeneralResourceLocator locator;
   std::string str = R"(task_composer_plugins:
                          search_paths:
                            - /usr/local/lib
@@ -154,7 +158,7 @@ void runTaskComposerExecutorTest()
                                      destinations: [AbortTask, DoneTask]
                                  terminals: [AbortTask, DoneTask])";
 
-  TaskComposerPluginFactory factory(str);
+  TaskComposerPluginFactory factory(str, locator);
 
   {  // Pipeline
     std::string str2 = R"(config:

diff --git a/tesseract_task_composer/core/src/task_composer_plugin_factory.cpp b/tesseract_task_composer/core/src/task_composer_plugin_factory.cpp
@@ -30,6 +30,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <utility>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
+#include <tesseract_common/resource_locator.h>
 #include <tesseract_common/plugin_loader.hpp>
 #include <tesseract_common/yaml_utils.h>
 #include <tesseract_common/yaml_extenstions.h>
@@ -73,20 +74,25 @@ TaskComposerPluginFactory::TaskComposerPluginFactory(const tesseract_common::Tas
   loadConfig(config);
 }
 
-TaskComposerPluginFactory::TaskComposerPluginFactory(const YAML::Node& config) : TaskComposerPluginFactory()
+TaskComposerPluginFactory::TaskComposerPluginFactory(const YAML::Node& config,
+                                                     const tesseract_common::ResourceLocator& locator)
+  : TaskComposerPluginFactory()
 {
-  loadConfig(config);
+  loadConfig(config, locator);
 }
 
-TaskComposerPluginFactory::TaskComposerPluginFactory(const tesseract_common::fs::path& config)
+TaskComposerPluginFactory::TaskComposerPluginFactory(const tesseract_common::fs::path& config,
+                                                     const tesseract_common::ResourceLocator& locator)
   : TaskComposerPluginFactory()
 {
-  loadConfig(config);
+  loadConfig(config, locator);
 }
 
-TaskComposerPluginFactory::TaskComposerPluginFactory(const std::string& config) : TaskComposerPluginFactory()
+TaskComposerPluginFactory::TaskComposerPluginFactory(const std::string& config,
+                                                     const tesseract_common::ResourceLocator& locator)
+  : TaskComposerPluginFactory()
 {
-  loadConfig(config);
+  loadConfig(config, locator);
 }
 
 // This prevents it from being defined inline.
@@ -120,12 +126,21 @@ void TaskComposerPluginFactory::loadConfig(const YAML::Node& config)
   }
 }
 
-void TaskComposerPluginFactory::loadConfig(const tesseract_common::fs::path& config)
+void TaskComposerPluginFactory::loadConfig(const YAML::Node& config, const tesseract_common::ResourceLocator& locator)
 {
-  loadConfig(YAML::LoadFile(config.string()));
+  loadConfig(tesseract_common::processYamlIncludeDirective(config, locator));
 }
 
-void TaskComposerPluginFactory::loadConfig(const std::string& config) { loadConfig(YAML::Load(config)); }
+void TaskComposerPluginFactory::loadConfig(const tesseract_common::fs::path& config,
+                                           const tesseract_common::ResourceLocator& locator)
+{
+  loadConfig(tesseract_common::loadYamlFile(config.string(), locator));
+}
+
+void TaskComposerPluginFactory::loadConfig(const std::string& config, const tesseract_common::ResourceLocator& locator)
+{
+  loadConfig(tesseract_common::loadYamlString(config, locator));
+}
 
 void TaskComposerPluginFactory::addSearchPath(const std::string& path)
 {

diff --git a/tesseract_task_composer/core/src/task_composer_server.cpp b/tesseract_task_composer/core/src/task_composer_server.cpp
@@ -36,26 +36,28 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #include <tesseract_task_composer/core/task_composer_node.h>
 #include <tesseract_task_composer/core/task_composer_data_storage.h>
 #include <tesseract_task_composer/core/task_composer_plugin_factory.h>
+#include <tesseract_common/resource_locator.h>
 
 namespace tesseract_planning
 {
 TaskComposerServer::TaskComposerServer() : plugin_factory_(std::make_shared<TaskComposerPluginFactory>()) {}
 
-void TaskComposerServer::loadConfig(const YAML::Node& config)
+void TaskComposerServer::loadConfig(const YAML::Node& config, const tesseract_common::ResourceLocator& locator)
 {
-  plugin_factory_->loadConfig(config);
+  plugin_factory_->loadConfig(config, locator);
   loadPlugins();
 }
 
-void TaskComposerServer::loadConfig(const tesseract_common::fs::path& config)
+void TaskComposerServer::loadConfig(const tesseract_common::fs::path& config,
+                                    const tesseract_common::ResourceLocator& locator)
 {
-  plugin_factory_->loadConfig(config);
+  plugin_factory_->loadConfig(config, locator);
   loadPlugins();
 }
 
-void TaskComposerServer::loadConfig(const std::string& config)
+void TaskComposerServer::loadConfig(const std::string& config, const tesseract_common::ResourceLocator& locator)
 {
-  plugin_factory_->loadConfig(config);
+  plugin_factory_->loadConfig(config, locator);
   loadPlugins();
 }
 

diff --git a/tesseract_task_composer/examples/task_composer_example.cpp b/tesseract_task_composer/examples/task_composer_example.cpp
@@ -1,6 +1,7 @@
 
 #include <iostream>
 #include <tesseract_common/utils.h>
+#include <tesseract_common/resource_locator.h>
 #include <tesseract_task_composer/core/task_composer_context.h>
 #include <tesseract_task_composer/core/task_composer_future.h>
 #include <tesseract_task_composer/core/task_composer_node_info.h>
@@ -124,8 +125,9 @@ int main()
   task_composer.addEdges(task2_id, { task3_id });
 
   const std::string share_dir(TESSERACT_TASK_COMPOSER_DIR);
+  tesseract_common::GeneralResourceLocator locator;
   tesseract_common::fs::path config_path(share_dir + "/config/task_composer_plugins.yaml");
-  TaskComposerPluginFactory factory(config_path);
+  TaskComposerPluginFactory factory(config_path, locator);
 
   auto task_executor = factory.createTaskComposerExecutor("TaskflowExecutor");
   TaskComposerFuture::UPtr future = task_executor->run(task_composer, std::move(task_data));

diff --git a/tesseract_task_composer/examples/task_composer_raster_example.cpp b/tesseract_task_composer/examples/task_composer_raster_example.cpp
@@ -52,7 +52,7 @@ int main()
   // Get plugin factory
   const std::string share_dir(TESSERACT_TASK_COMPOSER_DIR);
   tesseract_common::fs::path config_path(share_dir + "/config/task_composer_plugins.yaml");
-  TaskComposerPluginFactory factory(config_path);
+  TaskComposerPluginFactory factory(config_path, *locator);
 
   // Create raster task
   TaskComposerNode::UPtr task = factory.createTaskComposerNode("RasterFtPipeline");

diff --git a/tesseract_task_composer/examples/task_composer_trajopt_example.cpp b/tesseract_task_composer/examples/task_composer_trajopt_example.cpp
@@ -16,14 +16,14 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_common/types.h>
 #include <tesseract_common/utils.h>
+#include <tesseract_common/resource_locator.h>
 #include <tesseract_state_solver/state_solver.h>
 #include <tesseract_environment/environment.h>
 #include <tesseract_command_language/composite_instruction.h>
 #include <tesseract_command_language/profile_dictionary.h>
 #include <tesseract_command_language/utils.h>
 #include <tesseract_visualization/visualization.h>
 #include <tesseract_visualization/visualization_loader.h>
-#include <tesseract_common/resource_locator.h>
 
 using namespace tesseract_planning;
 
@@ -53,7 +53,7 @@ int main()
   // Get plugin factory
   const std::string share_dir(TESSERACT_TASK_COMPOSER_DIR);
   tesseract_common::fs::path config_path(share_dir + "/config/task_composer_plugins.yaml");
-  TaskComposerPluginFactory factory(config_path);
+  TaskComposerPluginFactory factory(config_path, *locator);
 
   // Create trajopt pipeline
   TaskComposerNode::UPtr task = factory.createTaskComposerNode("TrajOptPipeline");