From 0dd5555736f40a4a25070701f43ae5fd502cded2 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Fri, 3 Jan 2025 23:30:44 -0600 Subject: [PATCH] Leverage tesseract_common loadYamlFile and loadYamlString --- .../core/contact_managers_plugin_factory.h | 10 +- .../src/contact_managers_plugin_factory.cpp | 23 +- .../contact_managers_factory_static_unit.cpp | 7 +- .../test/contact_managers_factory_unit.cpp | 23 +- .../include/tesseract_common/yaml_utils.h | 19 +- tesseract_common/src/yaml_utils.cpp | 14 +- .../test/tesseract_common_unit.cpp | 72 ++-- .../core/kinematics_plugin_factory.h | 9 +- .../core/src/kinematics_plugin_factory.cpp | 22 +- .../test/kinematics_factory_unit.cpp | 349 +++++++++--------- tesseract_srdf/src/configs.cpp | 6 +- tesseract_srdf/test/tesseract_srdf_unit.cpp | 8 +- 12 files changed, 322 insertions(+), 240 deletions(-) diff --git a/tesseract_collision/core/include/tesseract_collision/core/contact_managers_plugin_factory.h b/tesseract_collision/core/include/tesseract_collision/core/contact_managers_plugin_factory.h index 82057e5acaf..9d7c97ec6d4 100644 --- a/tesseract_collision/core/include/tesseract_collision/core/contact_managers_plugin_factory.h +++ b/tesseract_collision/core/include/tesseract_collision/core/contact_managers_plugin_factory.h @@ -33,6 +33,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include #include #include #include @@ -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 diff --git a/tesseract_collision/core/src/contact_managers_plugin_factory.cpp b/tesseract_collision/core/src/contact_managers_plugin_factory.cpp index 28ec58ddb7e..fade4cebce5 100644 --- a/tesseract_collision/core/src/contact_managers_plugin_factory.cpp +++ b/tesseract_collision/core/src/contact_managers_plugin_factory.cpp @@ -31,6 +31,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include #include #include #include @@ -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. diff --git a/tesseract_collision/test/contact_managers_factory_static_unit.cpp b/tesseract_collision/test/contact_managers_factory_static_unit.cpp index e928bb54c33..3edd3169632 100644 --- a/tesseract_collision/test/contact_managers_factory_static_unit.cpp +++ b/tesseract_collision/test/contact_managers_factory_static_unit.cpp @@ -32,6 +32,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include +#include 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); diff --git a/tesseract_collision/test/contact_managers_factory_unit.cpp b/tesseract_collision/test/contact_managers_factory_unit.cpp index a6a3f50de1e..6eca35d85f6 100644 --- a/tesseract_collision/test/contact_managers_factory_unit.cpp +++ b/tesseract_collision/test/contact_managers_factory_unit.cpp @@ -32,13 +32,17 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include +#include 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"]; diff --git a/tesseract_common/include/tesseract_common/yaml_utils.h b/tesseract_common/include/tesseract_common/yaml_utils.h index 0b8756e92de..160d62eb345 100644 --- a/tesseract_common/include/tesseract_common/yaml_utils.h +++ b/tesseract_common/include/tesseract_common/yaml_utils.h @@ -40,6 +40,22 @@ 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. * @@ -47,8 +63,7 @@ class ResourceLocator; * 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. * diff --git a/tesseract_common/src/yaml_utils.cpp b/tesseract_common/src/yaml_utils.cpp index 6ead6da7092..861c121a9b9 100644 --- a/tesseract_common/src/yaml_utils.cpp +++ b/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,7 +63,7 @@ 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()) @@ -71,7 +71,7 @@ YAML::Node handleIncludeDirective(const YAML::Node& node, const ResourceLocator& // 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) diff --git a/tesseract_common/test/tesseract_common_unit.cpp b/tesseract_common/test/tesseract_common_unit.cpp index 55858756a4d..22b6fab9e48 100644 --- a/tesseract_common/test/tesseract_common_unit.cpp +++ b/tesseract_common/test/tesseract_common_unit.cpp @@ -1223,7 +1223,8 @@ TEST(TesseractPluginFactoryUnit, KinematicsPluginInfoYamlUnit) // NOLINT tip_link: tool0)"; { // Success - YAML::Node plugin_config = YAML::Load(yaml_string); + tesseract_common::GeneralResourceLocator locator; + YAML::Node plugin_config = tesseract_common::loadYamlString(yaml_string, locator); YAML::Node config = plugin_config[tesseract_common::KinematicsPluginInfo::CONFIG_KEY]; auto cmpi = config.as(); @@ -1292,7 +1293,8 @@ TEST(TesseractPluginFactoryUnit, KinematicsPluginInfoYamlUnit) // NOLINT base_link: base_link tip_link: tool0)"; - YAML::Node plugin_config = YAML::Load(yaml_string); + tesseract_common::GeneralResourceLocator locator; + YAML::Node plugin_config = tesseract_common::loadYamlString(yaml_string, locator); YAML::Node config = plugin_config[tesseract_common::KinematicsPluginInfo::CONFIG_KEY]; EXPECT_ANY_THROW(config.as()); // NOLINT } @@ -1327,7 +1329,8 @@ TEST(TesseractPluginFactoryUnit, KinematicsPluginInfoYamlUnit) // NOLINT base_link: base_link tip_link: tool0)"; - YAML::Node plugin_config = YAML::Load(yaml_string); + tesseract_common::GeneralResourceLocator locator; + YAML::Node plugin_config = tesseract_common::loadYamlString(yaml_string, locator); YAML::Node config = plugin_config[tesseract_common::KinematicsPluginInfo::CONFIG_KEY]; EXPECT_ANY_THROW(config.as()); // NOLINT } @@ -1356,7 +1359,8 @@ TEST(TesseractPluginFactoryUnit, KinematicsPluginInfoYamlUnit) // NOLINT base_link: base_link tip_link: tool0)"; - YAML::Node plugin_config = YAML::Load(yaml_string); + tesseract_common::GeneralResourceLocator locator; + YAML::Node plugin_config = tesseract_common::loadYamlString(yaml_string, locator); YAML::Node config = plugin_config[tesseract_common::KinematicsPluginInfo::CONFIG_KEY]; EXPECT_ANY_THROW(config.as()); // NOLINT } @@ -1387,7 +1391,8 @@ TEST(TesseractPluginFactoryUnit, KinematicsPluginInfoYamlUnit) // NOLINT base_link: base_link tip_link: tool0)"; - YAML::Node plugin_config = YAML::Load(yaml_string); + tesseract_common::GeneralResourceLocator locator; + YAML::Node plugin_config = tesseract_common::loadYamlString(yaml_string, locator); YAML::Node config = plugin_config[tesseract_common::KinematicsPluginInfo::CONFIG_KEY]; EXPECT_ANY_THROW(config.as()); // NOLINT } @@ -1411,7 +1416,8 @@ TEST(TesseractPluginFactoryUnit, KinematicsPluginInfoYamlUnit) // NOLINT iiwa_manipulator: default: KDLInvKinChainLMA)"; - YAML::Node plugin_config = YAML::Load(yaml_string); + tesseract_common::GeneralResourceLocator locator; + YAML::Node plugin_config = tesseract_common::loadYamlString(yaml_string, locator); YAML::Node config = plugin_config[tesseract_common::KinematicsPluginInfo::CONFIG_KEY]; EXPECT_ANY_THROW(config.as()); // NOLINT } @@ -1436,7 +1442,8 @@ TEST(TesseractPluginFactoryUnit, KinematicsPluginInfoYamlUnit) // NOLINT - tesseract_collision_bullet_factories - tesseract_collision_fcl_factories)"; - YAML::Node plugin_config = YAML::Load(yaml_string); + tesseract_common::GeneralResourceLocator locator; + YAML::Node plugin_config = tesseract_common::loadYamlString(yaml_string, locator); YAML::Node config = plugin_config[tesseract_common::KinematicsPluginInfo::CONFIG_KEY]; EXPECT_ANY_THROW(config.as()); // NOLINT } @@ -1468,7 +1475,8 @@ TEST(TesseractPluginFactoryUnit, ContactManagersPluginInfoYamlUnit) // NOLINT class: BulletCastSimpleManagerFactory)"; { // Success - YAML::Node plugin_config = YAML::Load(yaml_string); + tesseract_common::GeneralResourceLocator locator; + YAML::Node plugin_config = tesseract_common::loadYamlString(yaml_string, locator); YAML::Node config = plugin_config[tesseract_common::ContactManagersPluginInfo::CONFIG_KEY]; auto cmpi = config.as(); @@ -1530,7 +1538,8 @@ TEST(TesseractPluginFactoryUnit, ContactManagersPluginInfoYamlUnit) // NOLINT class: BulletCastBVHManagerFactory BulletCastSimpleManager: class: BulletCastSimpleManagerFactory)"; - YAML::Node plugin_config = YAML::Load(yaml_string); + tesseract_common::GeneralResourceLocator locator; + YAML::Node plugin_config = tesseract_common::loadYamlString(yaml_string, locator); YAML::Node config = plugin_config[tesseract_common::ContactManagersPluginInfo::CONFIG_KEY]; EXPECT_ANY_THROW(config.as()); // NOLINT } @@ -1557,7 +1566,8 @@ TEST(TesseractPluginFactoryUnit, ContactManagersPluginInfoYamlUnit) // NOLINT class: BulletCastBVHManagerFactory BulletCastSimpleManager: class: BulletCastSimpleManagerFactory)"; - YAML::Node plugin_config = YAML::Load(yaml_string); + tesseract_common::GeneralResourceLocator locator; + YAML::Node plugin_config = tesseract_common::loadYamlString(yaml_string, locator); YAML::Node config = plugin_config[tesseract_common::ContactManagersPluginInfo::CONFIG_KEY]; EXPECT_ANY_THROW(config.as()); // NOLINT } @@ -1578,7 +1588,8 @@ TEST(TesseractPluginFactoryUnit, ContactManagersPluginInfoYamlUnit) // NOLINT class: BulletCastBVHManagerFactory BulletCastSimpleManager: class: BulletCastSimpleManagerFactory)"; - YAML::Node plugin_config = YAML::Load(yaml_string); + tesseract_common::GeneralResourceLocator locator; + YAML::Node plugin_config = tesseract_common::loadYamlString(yaml_string, locator); YAML::Node config = plugin_config[tesseract_common::ContactManagersPluginInfo::CONFIG_KEY]; EXPECT_ANY_THROW(config.as()); // NOLINT } @@ -1600,7 +1611,8 @@ TEST(TesseractPluginFactoryUnit, ContactManagersPluginInfoYamlUnit) // NOLINT class: BulletCastBVHManagerFactory BulletCastSimpleManager: class: BulletCastSimpleManagerFactory)"; - YAML::Node plugin_config = YAML::Load(yaml_string); + tesseract_common::GeneralResourceLocator locator; + YAML::Node plugin_config = tesseract_common::loadYamlString(yaml_string, locator); YAML::Node config = plugin_config[tesseract_common::ContactManagersPluginInfo::CONFIG_KEY]; EXPECT_ANY_THROW(config.as()); // NOLINT } @@ -1623,7 +1635,8 @@ TEST(TesseractPluginFactoryUnit, ContactManagersPluginInfoYamlUnit) // NOLINT class: FCLDiscreteBVHManagerFactory continuous_plugins: default: BulletCastBVHManager)"; - YAML::Node plugin_config = YAML::Load(yaml_string); + tesseract_common::GeneralResourceLocator locator; + YAML::Node plugin_config = tesseract_common::loadYamlString(yaml_string, locator); YAML::Node config = plugin_config[tesseract_common::ContactManagersPluginInfo::CONFIG_KEY]; EXPECT_ANY_THROW(config.as()); // NOLINT } @@ -1647,7 +1660,8 @@ TEST(TesseractPluginFactoryUnit, ContactManagersPluginInfoYamlUnit) // NOLINT continuous_plugins: - tesseract_collision_bullet_factories - tesseract_collision_fcl_factories)"; - YAML::Node plugin_config = YAML::Load(yaml_string); + tesseract_common::GeneralResourceLocator locator; + YAML::Node plugin_config = tesseract_common::loadYamlString(yaml_string, locator); YAML::Node config = plugin_config[tesseract_common::ContactManagersPluginInfo::CONFIG_KEY]; EXPECT_ANY_THROW(config.as()); // NOLINT } @@ -1691,7 +1705,8 @@ TEST(TesseractPluginFactoryUnit, TaskComposerPluginInfoYamlUnit) // NOLINT output_key: "output")"; { // Success - YAML::Node plugin_config = YAML::Load(yaml_string); + tesseract_common::GeneralResourceLocator locator; + YAML::Node plugin_config = tesseract_common::loadYamlString(yaml_string, locator); YAML::Node config = plugin_config[tesseract_common::TaskComposerPluginInfo::CONFIG_KEY]; auto tcpi = config.as(); @@ -1765,7 +1780,8 @@ TEST(TesseractPluginFactoryUnit, TaskComposerPluginInfoYamlUnit) // NOLINT config: input_key: "input" output_key: "output")"; - YAML::Node plugin_config = YAML::Load(yaml_string); + tesseract_common::GeneralResourceLocator locator; + YAML::Node plugin_config = tesseract_common::loadYamlString(yaml_string, locator); YAML::Node config = plugin_config[tesseract_common::TaskComposerPluginInfo::CONFIG_KEY]; EXPECT_ANY_THROW(config.as()); // NOLINT } @@ -1804,7 +1820,8 @@ TEST(TesseractPluginFactoryUnit, TaskComposerPluginInfoYamlUnit) // NOLINT config: input_key: "input" output_key: "output")"; - YAML::Node plugin_config = YAML::Load(yaml_string); + tesseract_common::GeneralResourceLocator locator; + YAML::Node plugin_config = tesseract_common::loadYamlString(yaml_string, locator); YAML::Node config = plugin_config[tesseract_common::TaskComposerPluginInfo::CONFIG_KEY]; EXPECT_ANY_THROW(config.as()); // NOLINT } @@ -1831,7 +1848,8 @@ TEST(TesseractPluginFactoryUnit, TaskComposerPluginInfoYamlUnit) // NOLINT config: input_key: "input" output_key: "output")"; - YAML::Node plugin_config = YAML::Load(yaml_string); + tesseract_common::GeneralResourceLocator locator; + YAML::Node plugin_config = tesseract_common::loadYamlString(yaml_string, locator); YAML::Node config = plugin_config[tesseract_common::TaskComposerPluginInfo::CONFIG_KEY]; EXPECT_ANY_THROW(config.as()); // NOLINT } @@ -1869,7 +1887,8 @@ TEST(TesseractPluginFactoryUnit, TaskComposerPluginInfoYamlUnit) // NOLINT config: input_key: "input" output_key: "output")"; - YAML::Node plugin_config = YAML::Load(yaml_string); + tesseract_common::GeneralResourceLocator locator; + YAML::Node plugin_config = tesseract_common::loadYamlString(yaml_string, locator); YAML::Node config = plugin_config[tesseract_common::TaskComposerPluginInfo::CONFIG_KEY]; EXPECT_ANY_THROW(config.as()); // NOLINT } @@ -1898,7 +1917,8 @@ TEST(TesseractPluginFactoryUnit, TaskComposerPluginInfoYamlUnit) // NOLINT threads: 15 tasks: default: CartesianMotionPipeline)"; - YAML::Node plugin_config = YAML::Load(yaml_string); + tesseract_common::GeneralResourceLocator locator; + YAML::Node plugin_config = tesseract_common::loadYamlString(yaml_string, locator); YAML::Node config = plugin_config[tesseract_common::TaskComposerPluginInfo::CONFIG_KEY]; EXPECT_ANY_THROW(config.as()); // NOLINT } @@ -1936,7 +1956,8 @@ TEST(TesseractPluginFactoryUnit, TaskComposerPluginInfoYamlUnit) // NOLINT config: input_key: "input" output_key: "output")"; - YAML::Node plugin_config = YAML::Load(yaml_string); + tesseract_common::GeneralResourceLocator locator; + YAML::Node plugin_config = tesseract_common::loadYamlString(yaml_string, locator); YAML::Node config = plugin_config[tesseract_common::TaskComposerPluginInfo::CONFIG_KEY]; EXPECT_ANY_THROW(config.as()); // NOLINT } @@ -1968,7 +1989,8 @@ TEST(TesseractCommonUnit, TransformMapYamlUnit) // NOLINT w: 1)"; { // valid string - YAML::Node node = YAML::Load(yaml_string); + tesseract_common::GeneralResourceLocator locator; + YAML::Node node = tesseract_common::loadYamlString(yaml_string, locator); auto trans_map = node["joints"].as(); EXPECT_EQ(trans_map.size(), 2); EXPECT_FALSE(trans_map.empty()); @@ -1999,7 +2021,8 @@ TEST(TesseractCommonUnit, TransformMapYamlUnit) // NOLINT z: 0 w: 1)"; { // invalid string - YAML::Node node = YAML::Load(bad_yaml_string); + tesseract_common::GeneralResourceLocator locator; + YAML::Node node = tesseract_common::loadYamlString(bad_yaml_string, locator); EXPECT_ANY_THROW(node["joints"].as()); // NOLINT } } @@ -2030,7 +2053,8 @@ TEST(TesseractCommonUnit, CalibrationInfoYamlUnit) // NOLINT z: 0 w: 1)"; - YAML::Node node = YAML::Load(yaml_string); + tesseract_common::GeneralResourceLocator locator; + YAML::Node node = tesseract_common::loadYamlString(yaml_string, locator); auto cal_info = node[tesseract_common::CalibrationInfo::CONFIG_KEY].as(); EXPECT_FALSE(cal_info.empty()); EXPECT_TRUE(cal_info.joints.find("joint_1") != cal_info.joints.end()); diff --git a/tesseract_kinematics/core/include/tesseract_kinematics/core/kinematics_plugin_factory.h b/tesseract_kinematics/core/include/tesseract_kinematics/core/kinematics_plugin_factory.h index 3117fec6edb..c934183b936 100644 --- a/tesseract_kinematics/core/include/tesseract_kinematics/core/kinematics_plugin_factory.h +++ b/tesseract_kinematics/core/include/tesseract_kinematics/core/kinematics_plugin_factory.h @@ -36,6 +36,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include #include #include #include @@ -125,19 +126,19 @@ class KinematicsPluginFactory * @brief Load plugins from yaml node * @param config The config node */ - KinematicsPluginFactory(YAML::Node config); + KinematicsPluginFactory(YAML::Node config, const tesseract_common::ResourceLocator& locator); /** * @brief Load plugins from file path * @param config The config file path */ - KinematicsPluginFactory(const tesseract_common::fs::path& config); + KinematicsPluginFactory(const tesseract_common::fs::path& config, const tesseract_common::ResourceLocator& locator); /** * @brief Load plugins from string * @param config The config string */ - KinematicsPluginFactory(const std::string& config); + KinematicsPluginFactory(const std::string& config, const tesseract_common::ResourceLocator& locator); /** * @brief Add location for the plugin loader to search @@ -305,6 +306,8 @@ class KinematicsPluginFactory std::map fwd_plugin_info_; std::map inv_plugin_info_; tesseract_common::PluginLoader plugin_loader_; + + void loadConfig(const YAML::Node& config); }; } // namespace tesseract_kinematics diff --git a/tesseract_kinematics/core/src/kinematics_plugin_factory.cpp b/tesseract_kinematics/core/src/kinematics_plugin_factory.cpp index 1dbf272b765..7a26455fa41 100644 --- a/tesseract_kinematics/core/src/kinematics_plugin_factory.cpp +++ b/tesseract_kinematics/core/src/kinematics_plugin_factory.cpp @@ -32,6 +32,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include #include #include #include @@ -56,7 +57,7 @@ KinematicsPluginFactory::KinematicsPluginFactory() plugin_loader_.search_libraries, TESSERACT_KINEMATICS_PLUGINS, boost::is_any_of(":"), boost::token_compress_on); } -KinematicsPluginFactory::KinematicsPluginFactory(YAML::Node config) : KinematicsPluginFactory() +void KinematicsPluginFactory::loadConfig(const YAML::Node& config) { if (const YAML::Node& plugin_info = config[KinematicsPluginInfo::CONFIG_KEY]) { @@ -69,14 +70,25 @@ KinematicsPluginFactory::KinematicsPluginFactory(YAML::Node config) : Kinematics } } -KinematicsPluginFactory::KinematicsPluginFactory(const tesseract_common::fs::path& config) - : KinematicsPluginFactory(YAML::LoadFile(config.string())) +KinematicsPluginFactory::KinematicsPluginFactory(YAML::Node config, const tesseract_common::ResourceLocator& locator) + : KinematicsPluginFactory() { + config = tesseract_common::processYamlIncludeDirective(config, locator); + loadConfig(config); } -KinematicsPluginFactory::KinematicsPluginFactory(const std::string& config) - : KinematicsPluginFactory(YAML::Load(config)) +KinematicsPluginFactory::KinematicsPluginFactory(const tesseract_common::fs::path& config, + const tesseract_common::ResourceLocator& locator) + : KinematicsPluginFactory() { + loadConfig(tesseract_common::loadYamlFile(config.string(), locator)); +} + +KinematicsPluginFactory::KinematicsPluginFactory(const std::string& config, + const tesseract_common::ResourceLocator& locator) + : KinematicsPluginFactory() +{ + loadConfig(tesseract_common::loadYamlString(config, locator)); } // This prevents it from being defined inline. diff --git a/tesseract_kinematics/test/kinematics_factory_unit.cpp b/tesseract_kinematics/test/kinematics_factory_unit.cpp index d505f87de2d..0c7bbd9adf5 100644 --- a/tesseract_kinematics/test/kinematics_factory_unit.cpp +++ b/tesseract_kinematics/test/kinematics_factory_unit.cpp @@ -32,6 +32,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include "kinematics_test_utils.h" #include #include +#include +#include using namespace tesseract_kinematics::test_suite; using namespace tesseract_kinematics; @@ -59,8 +61,8 @@ void runKinematicsFactoryTest(const tesseract_common::fs::path& config_path) tesseract_scene_graph::KDLStateSolver rep_state_solver(*rep_scene_graph); tesseract_scene_graph::SceneState rep_scene_state = rep_state_solver.getState(); - KinematicsPluginFactory factory(config_path); - YAML::Node plugin_config = YAML::LoadFile(config_path.string()); + KinematicsPluginFactory factory(config_path, locator); + YAML::Node plugin_config = tesseract_common::loadYamlFile(config_path.string(), locator); const YAML::Node& plugin_info = plugin_config["kinematic_plugins"]; const YAML::Node& search_paths = plugin_info["search_paths"]; @@ -289,27 +291,27 @@ TEST(TesseractKinematicsFactoryUnit, LoadKinematicsPluginInfoUnit) // NOLINT sign_corrections: [1, 1, 1, -1, 1, 1])"; { // missing entry - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]; plugin.remove("OPWInvKin"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "OPWInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // missing class - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["OPWInvKin"]; plugin.remove("class"); - EXPECT_ANY_THROW(KinematicsPluginFactory factory(config)); // NOLINT + EXPECT_ANY_THROW(KinematicsPluginFactory factory(config, locator)); // NOLINT } { // missing default (which is allowed) - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["OPWInvKin"]; plugin.remove("default"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "OPWInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin != nullptr); } @@ -346,234 +348,234 @@ TEST(TesseractKinematicsFactoryUnit, LoadOPWKinematicsUnit) // NOLINT offsets: [0, 0, -1.57079632679, 0, 0, 0] sign_corrections: [1, 1, 1, -1, 1, 1])"; - KinematicsPluginFactory factory(YAML::Load(yaml_string)); + KinematicsPluginFactory factory(yaml_string, locator); auto inv_kin = factory.createInvKin("manipulator", "OPWInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin != nullptr); EXPECT_EQ(inv_kin->getSolverName(), "OPWInvKin"); { // missing config - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["OPWInvKin"]; plugin.remove("config"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "OPWInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // missing base_link - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["OPWInvKin"]; plugin["config"].remove("base_link"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "OPWInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // missing tip_link - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["OPWInvKin"]; plugin["config"].remove("tip_link"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "OPWInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // missing params - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["OPWInvKin"]; plugin["config"].remove("params"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "OPWInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // missing a1 - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["OPWInvKin"]; plugin["config"]["params"].remove("a1"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "OPWInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // missing a2 - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["OPWInvKin"]; plugin["config"]["params"].remove("a2"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "OPWInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // missing b - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["OPWInvKin"]; plugin["config"]["params"].remove("b"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "OPWInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // missing c1 - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["OPWInvKin"]; plugin["config"]["params"].remove("c1"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "OPWInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // missing c2 - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["OPWInvKin"]; plugin["config"]["params"].remove("c2"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "OPWInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // missing c3 - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["OPWInvKin"]; plugin["config"]["params"].remove("c3"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "OPWInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // missing c4 - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["OPWInvKin"]; plugin["config"]["params"].remove("c4"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "OPWInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // missing offset is allowed - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["OPWInvKin"]; plugin["config"]["params"].remove("offset"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "OPWInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin != nullptr); } { // missing sign_corrections is allowed - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["OPWInvKin"]; plugin["config"]["params"].remove("sign_corrections"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "OPWInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin != nullptr); } { // invalid a1 - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["OPWInvKin"]; plugin["config"]["params"]["a1"] = "abcd"; - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "OPWInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // invalid a2 - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["OPWInvKin"]; plugin["config"]["params"]["a2"] = "abcd"; - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "OPWInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // invalid b - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["OPWInvKin"]; plugin["config"]["params"]["b"] = "abcd"; - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "OPWInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // invalid c1 - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["OPWInvKin"]; plugin["config"]["params"]["c1"] = "abcd"; - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "OPWInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // invalid c2 - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["OPWInvKin"]; plugin["config"]["params"]["c2"] = "abcd"; - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "OPWInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // invalid c3 - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["OPWInvKin"]; plugin["config"]["params"]["c3"] = "abcd"; - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "OPWInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // invalid c4 - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["OPWInvKin"]; plugin["config"]["params"]["c4"] = "abcd"; - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "OPWInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // invalid offset - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["OPWInvKin"]; plugin["config"]["params"]["offsets"][0] = "abcd"; - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "OPWInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // invalid offset size - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["OPWInvKin"]; plugin["config"]["params"]["offsets"].push_back(0); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "OPWInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // invalid sign_corrections - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["OPWInvKin"]; plugin["config"]["params"]["sign_corrections"][0] = "a"; - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "OPWInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // invalid sign_corrections - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["OPWInvKin"]; plugin["config"]["params"]["sign_corrections"][0] = 5; - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "OPWInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // invalid sign_corrections size - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["OPWInvKin"]; plugin["config"]["params"]["sign_corrections"].push_back(0); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "OPWInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } @@ -583,6 +585,7 @@ TEST(TesseractKinematicsFactoryUnit, LoadURKinematicsUnit) // NOLINT { using namespace tesseract_scene_graph; + tesseract_common::GeneralResourceLocator locator; tesseract_scene_graph::SceneGraph::UPtr scene_graph = getSceneGraphUR(UR10Parameters, 0.220941, -0.1719); tesseract_scene_graph::KDLStateSolver state_solver(*scene_graph); tesseract_scene_graph::SceneState scene_state = state_solver.getState(); @@ -620,221 +623,221 @@ TEST(TesseractKinematicsFactoryUnit, LoadURKinematicsUnit) // NOLINT d6: 0.0922)"; { // Test loading UR10 - KinematicsPluginFactory factory(YAML::Load(yaml_model_string)); + KinematicsPluginFactory factory(yaml_model_string, locator); auto inv_kin = factory.createInvKin("manipulator", "URInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin != nullptr); EXPECT_EQ(inv_kin->getSolverName(), "URInvKin"); } { // Test loading UR10e - YAML::Node config = YAML::Load(yaml_model_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_model_string, locator); config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["URInvKin"]["config"]["model"] = "UR10e"; - KinematicsPluginFactory factory(YAML::Load(yaml_model_string)); + KinematicsPluginFactory factory(yaml_model_string, locator); auto inv_kin = factory.createInvKin("manipulator", "URInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin != nullptr); EXPECT_EQ(inv_kin->getSolverName(), "URInvKin"); } { // Test loading UR5 - YAML::Node config = YAML::Load(yaml_model_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_model_string, locator); config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["URInvKin"]["config"]["model"] = "UR5"; - KinematicsPluginFactory factory(YAML::Load(yaml_model_string)); + KinematicsPluginFactory factory(yaml_model_string, locator); auto inv_kin = factory.createInvKin("manipulator", "URInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin != nullptr); EXPECT_EQ(inv_kin->getSolverName(), "URInvKin"); } { // Test loading UR5e - YAML::Node config = YAML::Load(yaml_model_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_model_string, locator); config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["URInvKin"]["config"]["model"] = "UR5e"; - KinematicsPluginFactory factory(YAML::Load(yaml_model_string)); + KinematicsPluginFactory factory(yaml_model_string, locator); auto inv_kin = factory.createInvKin("manipulator", "URInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin != nullptr); EXPECT_EQ(inv_kin->getSolverName(), "URInvKin"); } { // Test loading UR3 - YAML::Node config = YAML::Load(yaml_model_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_model_string, locator); config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["URInvKin"]["config"]["model"] = "UR3"; - KinematicsPluginFactory factory(YAML::Load(yaml_model_string)); + KinematicsPluginFactory factory(yaml_model_string, locator); auto inv_kin = factory.createInvKin("manipulator", "URInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin != nullptr); EXPECT_EQ(inv_kin->getSolverName(), "URInvKin"); } { // Test loading UR3e - YAML::Node config = YAML::Load(yaml_model_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_model_string, locator); config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["URInvKin"]["config"]["model"] = "UR3e"; - KinematicsPluginFactory factory(YAML::Load(yaml_model_string)); + KinematicsPluginFactory factory(yaml_model_string, locator); auto inv_kin = factory.createInvKin("manipulator", "URInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin != nullptr); EXPECT_EQ(inv_kin->getSolverName(), "URInvKin"); } { // Test loading custom UR parameters - KinematicsPluginFactory factory(YAML::Load(yaml_params_string)); + KinematicsPluginFactory factory(yaml_model_string, locator); auto inv_kin = factory.createInvKin("manipulator", "URInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin != nullptr); EXPECT_EQ(inv_kin->getSolverName(), "URInvKin"); } { // invalid ur model - YAML::Node config = YAML::Load(yaml_model_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_model_string, locator); config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["URInvKin"]["config"]["model"] = "DoesNotE" "xist"; - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "URInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // missing config - YAML::Node config = YAML::Load(yaml_model_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_model_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["URInvKin"]; plugin.remove("config"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "URInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // missing base_link - YAML::Node config = YAML::Load(yaml_model_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_model_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["URInvKin"]; plugin["config"].remove("base_link"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "URInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // missing tip_link - YAML::Node config = YAML::Load(yaml_model_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_model_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["URInvKin"]; plugin["config"].remove("tip_link"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "URInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // missing model and params - YAML::Node config = YAML::Load(yaml_model_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_model_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["URInvKin"]; plugin["config"].remove("model"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "URInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // missing model and params - YAML::Node config = YAML::Load(yaml_params_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_params_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["URInvKin"]; plugin["config"].remove("params"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "URInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // missing d1 - YAML::Node config = YAML::Load(yaml_params_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_params_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["URInvKin"]; plugin["config"]["params"].remove("d1"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "URInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // missing a2 - YAML::Node config = YAML::Load(yaml_params_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_params_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["URInvKin"]; plugin["config"]["params"].remove("a2"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "URInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // missing a3 - YAML::Node config = YAML::Load(yaml_params_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_params_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["URInvKin"]; plugin["config"]["params"].remove("a3"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "URInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // missing d4 - YAML::Node config = YAML::Load(yaml_params_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_params_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["URInvKin"]; plugin["config"]["params"].remove("d4"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "URInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // missing d5 - YAML::Node config = YAML::Load(yaml_params_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_params_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["URInvKin"]; plugin["config"]["params"].remove("d5"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "URInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // missing d6 - YAML::Node config = YAML::Load(yaml_params_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_params_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["URInvKin"]; plugin["config"]["params"].remove("d6"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "URInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // invalid d1 - YAML::Node config = YAML::Load(yaml_params_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_params_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["URInvKin"]; plugin["config"]["params"]["d1"] = "abcd"; - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "URInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // invalid a2 - YAML::Node config = YAML::Load(yaml_params_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_params_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["URInvKin"]; plugin["config"]["params"]["a2"] = "abcd"; - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "URInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // invalid a3 - YAML::Node config = YAML::Load(yaml_params_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_params_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["URInvKin"]; plugin["config"]["params"]["a3"] = "abcd"; - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "URInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // invalid d4 - YAML::Node config = YAML::Load(yaml_params_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_params_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["URInvKin"]; plugin["config"]["params"]["d4"] = "abcd"; - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "URInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // invalid d5 - YAML::Node config = YAML::Load(yaml_params_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_params_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["URInvKin"]; plugin["config"]["params"]["d5"] = "abcd"; - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "URInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // invalid d6 - YAML::Node config = YAML::Load(yaml_params_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_params_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["URInvKin"]; plugin["config"]["params"]["d6"] = "abcd"; - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "URInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } @@ -887,89 +890,89 @@ TEST(TesseractKinematicsFactoryUnit, LoadREPKinematicsUnit) // NOLINT offsets: [0, 0, -1.57079632679, 0, 0, 0] sign_corrections: [1, 1, 1, 1, 1, 1])"; - KinematicsPluginFactory factory(YAML::Load(yaml_string)); + KinematicsPluginFactory factory(yaml_string, locator); auto inv_kin = factory.createInvKin("manipulator", "REPInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin != nullptr); EXPECT_EQ(inv_kin->getSolverName(), "REPInvKin"); { // missing config - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["REPInvKin"]; plugin.remove("config"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "REPInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // missing manipulator_reach - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["REPInvKin"]; plugin["config"].remove("manipulator_reach"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "REPInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // missing positioner_sample_resolution - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["REPInvKin"]; plugin["config"].remove("positioner_sample_resolution"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "REPInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // missing positioner_sample_resolution entry name - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["REPInvKin"]; plugin["config"]["positioner_sample_resolution"][0].remove("name"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "REPInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // missing positioner_sample_resolution entry value - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["REPInvKin"]; plugin["config"]["positioner_sample_resolution"][0].remove("value"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "REPInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // missing positioner - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["REPInvKin"]; plugin["config"].remove("positioner"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "REPInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // missing positioner entry class - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["REPInvKin"]; plugin["config"]["positioner"].remove("class"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "REPInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // missing manipulator - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["REPInvKin"]; plugin["config"].remove("manipulator"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "REPInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // missing manipulator entry class - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["REPInvKin"]; plugin["config"]["manipulator"].remove("class"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "REPInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } @@ -1020,89 +1023,89 @@ TEST(TesseractKinematicsFactoryUnit, LoadROPKinematicsUnit) // NOLINT offsets: [0, 0, -1.57079632679, 0, 0, 0] sign_corrections: [1, 1, 1, 1, 1, 1])"; - KinematicsPluginFactory factory(YAML::Load(yaml_string)); + KinematicsPluginFactory factory(yaml_string, locator); auto inv_kin = factory.createInvKin("manipulator", "ROPInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin != nullptr); EXPECT_EQ(inv_kin->getSolverName(), "ROPInvKin"); { // missing config - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["ROPInvKin"]; plugin.remove("config"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "ROPInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // missing manipulator_reach - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["ROPInvKin"]; plugin["config"].remove("manipulator_reach"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "ROPInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // missing positioner_sample_resolution - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["ROPInvKin"]; plugin["config"].remove("positioner_sample_resolution"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "ROPInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // missing positioner_sample_resolution entry name - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["ROPInvKin"]; plugin["config"]["positioner_sample_resolution"][0].remove("name"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "REPInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // missing positioner_sample_resolution entry value - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["ROPInvKin"]; plugin["config"]["positioner_sample_resolution"][0].remove("value"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "REPInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // missing positioner - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["ROPInvKin"]; plugin["config"].remove("positioner"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "ROPInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // missing positioner entry class - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["ROPInvKin"]; plugin["config"]["positioner"].remove("class"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "ROPInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // missing manipulator - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["ROPInvKin"]; plugin["config"].remove("manipulator"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "ROPInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } { // missing manipulator entry class - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["ROPInvKin"]; plugin["config"]["manipulator"].remove("class"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto inv_kin = factory.createInvKin("manipulator", "ROPInvKin", *scene_graph, scene_state); EXPECT_TRUE(inv_kin == nullptr); } @@ -1176,138 +1179,138 @@ TEST(TesseractKinematicsFactoryUnit, LoadKDLKinematicsUnit) // NOLINT position_iterations: 100)"; { - KinematicsPluginFactory factory(YAML::Load(yaml_string)); + KinematicsPluginFactory factory(yaml_string, locator); auto fwd_kin = factory.createFwdKin("manipulator", "KDLFwdKinChain", *scene_graph, scene_state); EXPECT_TRUE(fwd_kin != nullptr); EXPECT_EQ(fwd_kin->getSolverName(), "KDLFwdKinChain"); } { - KinematicsPluginFactory factory(YAML::Load(yaml_string)); + KinematicsPluginFactory factory(yaml_string, locator); auto inv_kin = factory.createInvKin("manipulator", "KDLInvKinChainLMA", *scene_graph, scene_state); EXPECT_TRUE(inv_kin != nullptr); EXPECT_EQ(inv_kin->getSolverName(), "KDLInvKinChainLMA"); } { - KinematicsPluginFactory factory(YAML::Load(yaml_string)); + KinematicsPluginFactory factory(yaml_string, locator); auto inv_kin = factory.createInvKin("manipulator", "KDLInvKinChainNR", *scene_graph, scene_state); EXPECT_TRUE(inv_kin != nullptr); EXPECT_EQ(inv_kin->getSolverName(), "KDLInvKinChainNR"); } { - KinematicsPluginFactory factory(YAML::Load(yaml_string)); + KinematicsPluginFactory factory(yaml_string, locator); auto inv_kin = factory.createInvKin("manipulator", "KDLInvKinChainNR_JL", *scene_graph, scene_state); EXPECT_TRUE(inv_kin != nullptr); EXPECT_EQ(inv_kin->getSolverName(), "KDLInvKinChainNR_JL"); } { // KDLFwdKinChain missing config - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["fwd_kin_plugins"]["manipulator"]["plugins"]["KDLFwdKinChain"]; plugin.remove("config"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto kin = factory.createFwdKin("manipulator", "KDLFwdKinChain", *scene_graph, scene_state); EXPECT_TRUE(kin == nullptr); } { // KDLInvKinChainLMA missing config - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["KDLInvKinChainLMA"]; plugin.remove("config"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto kin = factory.createInvKin("manipulator", "KDLInvKinChainLMA", *scene_graph, scene_state); EXPECT_TRUE(kin == nullptr); } { // KDLInvKinChainNR missing config - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["KDLInvKinChainNR"]; plugin.remove("config"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto kin = factory.createInvKin("manipulator", "KDLInvKinChainNR", *scene_graph, scene_state); EXPECT_TRUE(kin == nullptr); } { // KDLInvKinChainNR_JL missing config - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["KDLInvKinChainNR_JL"]; plugin.remove("config"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto kin = factory.createInvKin("manipulator", "KDLInvKinChainNR_JL", *scene_graph, scene_state); EXPECT_TRUE(kin == nullptr); } { // KDLFwdKinChain missing base_link - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["fwd_kin_plugins"]["manipulator"]["plugins"]["KDLFwdKinChain"]; plugin["config"].remove("base_link"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto kin = factory.createFwdKin("manipulator", "KDLFwdKinChain", *scene_graph, scene_state); EXPECT_TRUE(kin == nullptr); } { // KDLInvKinChainLMA missing base_link - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["KDLInvKinChainLMA"]; plugin["config"].remove("base_link"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto kin = factory.createInvKin("manipulator", "KDLInvKinChainLMA", *scene_graph, scene_state); EXPECT_TRUE(kin == nullptr); } { // KDLInvKinChainNR missing base_link - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["KDLInvKinChainNR"]; plugin["config"].remove("base_link"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto kin = factory.createInvKin("manipulator", "KDLInvKinChainNR", *scene_graph, scene_state); EXPECT_TRUE(kin == nullptr); } { // KDLInvKinChainNR_JL missing base_link - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["KDLInvKinChainNR_JL"]; plugin["config"].remove("base_link"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto kin = factory.createInvKin("manipulator", "KDLInvKinChainNR_JL", *scene_graph, scene_state); EXPECT_TRUE(kin == nullptr); } { // KDLFwdKinChain missing tip_link - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["fwd_kin_plugins"]["manipulator"]["plugins"]["KDLFwdKinChain"]; plugin["config"].remove("tip_link"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto kin = factory.createFwdKin("manipulator", "KDLFwdKinChain", *scene_graph, scene_state); EXPECT_TRUE(kin == nullptr); } { // KDLInvKinChainLMA missing tip_link - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["KDLInvKinChainLMA"]; plugin["config"].remove("tip_link"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto kin = factory.createInvKin("manipulator", "KDLInvKinChainLMA", *scene_graph, scene_state); EXPECT_TRUE(kin == nullptr); } { // KDLInvKinChainNR missing tip_link - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["KDLInvKinChainNR"]; plugin["config"].remove("tip_link"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto kin = factory.createInvKin("manipulator", "KDLInvKinChainNR", *scene_graph, scene_state); EXPECT_TRUE(kin == nullptr); } { // KDLInvKinChainNR_JL missing tip_link - YAML::Node config = YAML::Load(yaml_string); + YAML::Node config = tesseract_common::loadYamlString(yaml_string, locator); auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["KDLInvKinChainNR_JL"]; plugin["config"].remove("tip_link"); - KinematicsPluginFactory factory(config); + KinematicsPluginFactory factory(config, locator); auto kin = factory.createInvKin("manipulator", "KDLInvKinChainNR_JL", *scene_graph, scene_state); EXPECT_TRUE(kin == nullptr); } diff --git a/tesseract_srdf/src/configs.cpp b/tesseract_srdf/src/configs.cpp index bd5d727a7d8..b3ae3c07898 100644 --- a/tesseract_srdf/src/configs.cpp +++ b/tesseract_srdf/src/configs.cpp @@ -71,7 +71,7 @@ tesseract_common::CalibrationInfo parseCalibrationConfig(const tesseract_scene_g YAML::Node config; try { - config = YAML::LoadFile(cal_config_file_path.string()); + config = tesseract_common::processYamlIncludeDirective(YAML::LoadFile(cal_config_file_path.string()), locator); } // LCOV_EXCL_START catch (...) @@ -103,7 +103,7 @@ tesseract_common::KinematicsPluginInfo parseKinematicsPluginConfig(const tessera YAML::Node config; try { - config = YAML::LoadFile(kin_plugin_file_path.string()); + config = tesseract_common::processYamlIncludeDirective(YAML::LoadFile(kin_plugin_file_path.string()), locator); } // LCOV_EXCL_START catch (...) @@ -128,7 +128,7 @@ parseContactManagersPluginConfig(const tesseract_common::ResourceLocator& locato YAML::Node config; try { - config = YAML::LoadFile(cm_plugin_file_path.string()); + config = tesseract_common::processYamlIncludeDirective(YAML::LoadFile(cm_plugin_file_path.string()), locator); } // LCOV_EXCL_START catch (...) diff --git a/tesseract_srdf/test/tesseract_srdf_unit.cpp b/tesseract_srdf/test/tesseract_srdf_unit.cpp index 6b14ad2118b..c2d66d96bdb 100644 --- a/tesseract_srdf/test/tesseract_srdf_unit.cpp +++ b/tesseract_srdf/test/tesseract_srdf_unit.cpp @@ -697,15 +697,15 @@ TEST(TesseractSRDFUnit, LoadSRDFSaveUnit) // NOLINT SRDFModel srdf_save; srdf_save.initString(*g, xml_string, locator); - YAML::Node kinematics_plugin_config = YAML::Load(yaml_kin_plugins_string); + YAML::Node kinematics_plugin_config = tesseract_common::loadYamlString(yaml_kin_plugins_string, locator); srdf_save.kinematics_information.kinematics_plugin_info = kinematics_plugin_config[KinematicsPluginInfo::CONFIG_KEY].as(); - YAML::Node contact_managers_plugin_config = YAML::Load(yaml_cm_plugins_string); + YAML::Node contact_managers_plugin_config = tesseract_common::loadYamlString(yaml_cm_plugins_string, locator); srdf_save.contact_managers_plugin_info = contact_managers_plugin_config[ContactManagersPluginInfo::CONFIG_KEY].as(); - YAML::Node calibration_config = YAML::Load(yaml_calibration_string); + YAML::Node calibration_config = tesseract_common::loadYamlString(yaml_calibration_string, locator); srdf_save.calibration_info = calibration_config[CalibrationInfo::CONFIG_KEY].as(); std::string save_path = tesseract_common::getTempPath() + "unit_test_save_srdf.srdf"; @@ -791,7 +791,7 @@ TEST(TesseractSRDFUnit, LoadSRDFSaveUnit) // NOLINT y: 0 z: 0 w: 1)"; - YAML::Node bad_calibration_config = YAML::Load(yaml_calibration_string); + YAML::Node bad_calibration_config = tesseract_common::loadYamlString(yaml_calibration_string, locator); srdf_save.calibration_info = bad_calibration_config[CalibrationInfo::CONFIG_KEY].as(); save_path = tesseract_common::getTempPath() + "unit_test_save_bad_srdf.srdf";