Add property tree class to support yaml schema and validation

tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_factories.h

@@ -55,27 +55,35 @@ class BulletDiscreteBVHManagerFactory : public DiscreteContactManagerFactory
 public:
   std::unique_ptr<DiscreteContactManager> create(const std::string& name,
                                                  const YAML::Node& config) const override final;
+
+  tesseract_common::PropertyTree schema() const override final;
 };
 
 class BulletDiscreteSimpleManagerFactory : public DiscreteContactManagerFactory
 {
 public:
   std::unique_ptr<DiscreteContactManager> create(const std::string& name,
                                                  const YAML::Node& config) const override final;
+
+  tesseract_common::PropertyTree schema() const override final;
 };
 
 class BulletCastBVHManagerFactory : public ContinuousContactManagerFactory
 {
 public:
   std::unique_ptr<ContinuousContactManager> create(const std::string& name,
                                                    const YAML::Node& config) const override final;
+
+  tesseract_common::PropertyTree schema() const override final;
 };
 
 class BulletCastSimpleManagerFactory : public ContinuousContactManagerFactory
 {
 public:
   std::unique_ptr<ContinuousContactManager> create(const std::string& name,
                                                    const YAML::Node& config) const override final;
+
+  tesseract_common::PropertyTree schema() const override final;
 };
 
 PLUGIN_ANCHOR_DECL(BulletFactoriesAnchor)

tesseract_collision/bullet/src/bullet_factories.cpp

@@ -39,24 +39,60 @@
 #include <tesseract_collision/core/discrete_contact_manager.h>
 #include <tesseract_collision/core/continuous_contact_manager.h>
 
+#include <tesseract_common/property_tree.h>
+
 namespace tesseract_collision::tesseract_collision_bullet
 {
+tesseract_common::PropertyTree getConfigSchema()
+{
+  using namespace tesseract_common;
+  PropertyTree schema;
+  {
+    auto& prop = schema["share_pool_allocators"];
+    prop.setAttribute(property_attribute::TYPE, property_type::BOOL);
+    prop.setAttribute(property_attribute::DOC, "Indicate if shared pool allocators should be leverage.");
+    prop.setAttribute(property_attribute::REQUIRED, false);
+    prop.setAttribute(property_attribute::DEFAULT, false);
+  }
+
+  {
+    auto& prop = schema["max_persistent_manifold_pool_size"];
+    prop.setAttribute(property_attribute::TYPE, property_type::INT);
+    prop.setAttribute(property_attribute::DOC, "The max size for the persistent manifold pool size.");
+    prop.setAttribute(property_attribute::MINIMUM, 0);
+  }
+
+  {
+    auto& prop = schema["max_collision_algorithm_pool_size"];
+    prop.setAttribute(property_attribute::TYPE, property_type::INT);
+    prop.setAttribute(property_attribute::DOC, "The max size for the persistent algorithm pool size.");
+    prop.setAttribute(property_attribute::MINIMUM, 0);
+  }
+
+  return schema;
+}
+
 TesseractCollisionConfigurationInfo getConfigInfo(const YAML::Node& config)
 {
   if (config.IsNull())
     return {};
 
+  // Validate config
+  tesseract_common::PropertyTree schema = getConfigSchema();
+  schema.mergeConfig(config);
+  schema.validate();
+
   bool share_pool_allocators{ false };
-  if (YAML::Node n = config["share_pool_allocators"])
-    share_pool_allocators = n.as<bool>();
+  if (const tesseract_common::PropertyTree& prop = schema.at("share_pool_allocators"))
+    share_pool_allocators = prop.as<bool>();
 
   TesseractCollisionConfigurationInfo config_info(false, share_pool_allocators);
 
-  if (YAML::Node n = config["max_persistent_manifold_pool_size"])
-    config_info.m_defaultMaxPersistentManifoldPoolSize = n.as<int>();
+  if (const tesseract_common::PropertyTree& prop = schema.at("max_persistent_manifold_pool_size"))
+    config_info.m_defaultMaxPersistentManifoldPoolSize = prop.as<int>();
 
-  if (YAML::Node n = config["max_collision_algorithm_pool_size"])
-    config_info.m_defaultMaxCollisionAlgorithmPoolSize = n.as<int>();
+  if (const tesseract_common::PropertyTree& prop = schema.at("max_collision_algorithm_pool_size"))
+    config_info.m_defaultMaxCollisionAlgorithmPoolSize = prop.as<int>();
 
   config_info.createPoolAllocators();
   return config_info;
@@ -68,24 +104,32 @@
   return std::make_unique<BulletDiscreteBVHManager>(name, getConfigInfo(config));
 }
 
+tesseract_common::PropertyTree BulletDiscreteBVHManagerFactory::schema() const { return getConfigSchema(); }
+
 std::unique_ptr<DiscreteContactManager> BulletDiscreteSimpleManagerFactory::create(const std::string& name,
                                                                                    const YAML::Node& config) const
 {
   return std::make_unique<BulletDiscreteSimpleManager>(name, getConfigInfo(config));
 }
 
+tesseract_common::PropertyTree BulletDiscreteSimpleManagerFactory::schema() const { return getConfigSchema(); }
+
 std::unique_ptr<ContinuousContactManager> BulletCastBVHManagerFactory::create(const std::string& name,
                                                                               const YAML::Node& config) const
 {
   return std::make_unique<BulletCastBVHManager>(name, getConfigInfo(config));
 }
 
+tesseract_common::PropertyTree BulletCastBVHManagerFactory::schema() const { return getConfigSchema(); }
+
 std::unique_ptr<ContinuousContactManager> BulletCastSimpleManagerFactory::create(const std::string& name,
                                                                                  const YAML::Node& config) const
 {
   return std::make_unique<BulletCastSimpleManager>(name, getConfigInfo(config));
 }
 
+tesseract_common::PropertyTree BulletCastSimpleManagerFactory::schema() const { return getConfigSchema(); }
+
 PLUGIN_ANCHOR_IMPL(BulletFactoriesAnchor)
 
 }  // namespace tesseract_collision::tesseract_collision_bullet

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

@@ -70,6 +70,12 @@ class DiscreteContactManagerFactory
    */
   virtual std::unique_ptr<DiscreteContactManager> create(const std::string& name, const YAML::Node& config) const = 0;
 
+  /**
+   * @brief Get the config yaml schema
+   * @return The schema
+   */
+  virtual tesseract_common::PropertyTree schema() const = 0;
+
 protected:
   static std::string getSection();
   friend class boost_plugin_loader::PluginLoader;
@@ -92,6 +98,12 @@ class ContinuousContactManagerFactory
   virtual std::unique_ptr<ContinuousContactManager> create(const std::string& solver_name,
                                                            const YAML::Node& config) const = 0;
 
+  /**
+   * @brief Get the config yaml schema
+   * @return The schema
+   */
+  virtual tesseract_common::PropertyTree schema() const = 0;
+
 protected:
   static std::string getSection();
   friend class boost_plugin_loader::PluginLoader;
@@ -125,6 +137,12 @@ class ContactManagersPluginFactory
    */
   ContactManagersPluginFactory(const std::string& config, const tesseract_common::ResourceLocator& locator);
 
+  /**
+   * @brief Get the schema
+   * @return The schema
+   */
+  tesseract_common::PropertyTree getSchema() const;
+
   /**
    * @brief Add location for the plugin loader to search
    * @param path The full path to the directory

tesseract_collision/core/src/contact_managers_plugin_factory.cpp

@@ -33,7 +33,8 @@
 #include <tesseract_collision/core/continuous_contact_manager.h>
 #include <tesseract_common/resource_locator.h>
 #include <tesseract_common/yaml_utils.h>
-#include <tesseract_common/yaml_extenstions.h>
+#include <tesseract_common/yaml_extensions.h>
+#include <tesseract_common/property_tree.h>
 #include <tesseract_collision/core/contact_managers_plugin_factory.h>
 #include <boost_plugin_loader/plugin_loader.hpp>
 #include <console_bridge/console.h>
@@ -61,6 +62,13 @@
                boost::token_compress_on);
 }
 
+tesseract_common::PropertyTree ContactManagersPluginFactory::getSchema() const
+{
+  tesseract_common::PropertyTree schema;
+
+  return schema;
+}
+
 void ContactManagersPluginFactory::loadConfig(const YAML::Node& config)
 {
   if (const YAML::Node& plugin_info = config[ContactManagersPluginInfo::CONFIG_KEY])

tesseract_collision/core/src/types.cpp

@@ -399,8 +399,17 @@ void ContactManagerConfig::validate() const
     throw std::runtime_error("ContactManagerConfig, pair margin override type is NONE but pair collision margins "
                              "exist!");
 
+  if (pair_margin_override_type != CollisionMarginPairOverrideType::NONE &&
+      pair_margin_data.getCollisionMargins().empty())
+    throw std::runtime_error("ContactManagerConfig, pair collision margins are empty, but pair margin override type is "
+                             "not NONE!");
+
   if (acm_override_type == ACMOverrideType::NONE && !acm.getAllAllowedCollisions().empty())
     throw std::runtime_error("ContactManagerConfig, acm override type is NONE but allowed collision entries exist!");
+
+  if (acm_override_type != ACMOverrideType::NONE && acm.getAllAllowedCollisions().empty())
+    throw std::runtime_error("ContactManagerConfig, allowed collision entries are empty, but acm override type is not "
+                             "NONE!");
 }
 
 bool ContactManagerConfig::operator==(const ContactManagerConfig& rhs) const

tesseract_collision/fcl/include/tesseract_collision/fcl/fcl_factories.h

@@ -36,6 +36,8 @@ class FCLDiscreteBVHManagerFactory : public DiscreteContactManagerFactory
 public:
   std::unique_ptr<DiscreteContactManager> create(const std::string& name,
                                                  const YAML::Node& config) const override final;
+
+  tesseract_common::PropertyTree schema() const override final;
 };
 
 PLUGIN_ANCHOR_DECL(FCLFactoriesAnchor)

tesseract_collision/fcl/src/fcl_factories.cpp

@@ -27,6 +27,7 @@
 #include <tesseract_collision/fcl/fcl_factories.h>
 #include <tesseract_collision/fcl/fcl_discrete_managers.h>
 #include <tesseract_collision/core/discrete_contact_manager.h>
+#include <tesseract_common/property_tree.h>
 
 namespace tesseract_collision::tesseract_collision_fcl
 {
@@ -36,6 +37,8 @@
   return std::make_unique<FCLDiscreteBVHManager>(name);
 }
 
+tesseract_common::PropertyTree FCLDiscreteBVHManagerFactory::schema() const { return {}; }
+
 PLUGIN_ANCHOR_IMPL(FCLFactoriesAnchor)  // LCOV_EXCL_LINE
 
 }  // namespace tesseract_collision::tesseract_collision_fcl

tesseract_collision/test/collision_core_unit.cpp

@@ -125,6 +125,12 @@ TEST(TesseractCoreUnit, ContactManagerConfigTest)  // NOLINT
     EXPECT_ANY_THROW(config.validate());  // NOLINT
   }
 
+  {
+    tesseract_collision::ContactManagerConfig config;
+    config.pair_margin_override_type = tesseract_collision::CollisionMarginPairOverrideType::MODIFY;
+    EXPECT_ANY_THROW(config.validate());  // NOLINT
+  }
+
   {
     tesseract_collision::ContactManagerConfig config;
     config.pair_margin_override_type = tesseract_collision::CollisionMarginPairOverrideType::MODIFY;
@@ -138,6 +144,12 @@ TEST(TesseractCoreUnit, ContactManagerConfigTest)  // NOLINT
     EXPECT_ANY_THROW(config.validate());  // NOLINT
   }
 
+  {
+    tesseract_collision::ContactManagerConfig config;
+    config.acm_override_type = tesseract_collision::ACMOverrideType::OR;
+    EXPECT_ANY_THROW(config.validate());  // NOLINT
+  }
+
   {
     tesseract_collision::ContactManagerConfig config;
     config.acm_override_type = tesseract_collision::ACMOverrideType::OR;

tesseract_collision/test/contact_manager_plugins.yaml

@@ -11,6 +11,8 @@ contact_manager_plugins:
         class: BulletDiscreteBVHManagerFactory
       BulletDiscreteSimpleManager:
         class: BulletDiscreteSimpleManagerFactory
+        config:
+          share_pool_allocators: false
       FCLDiscreteBVHManager:
         class: FCLDiscreteBVHManagerFactory
   continuous_plugins:
@@ -20,4 +22,6 @@ contact_manager_plugins:
         class: BulletCastBVHManagerFactory
       BulletCastSimpleManager:
         class: BulletCastSimpleManagerFactory
+        config:
+          share_pool_allocators: false
 

tesseract_common/CMakeLists.txt

@@ -94,10 +94,14 @@ add_library(
   src/plugin_info.cpp
   src/profile_dictionary.cpp
   src/profile.cpp
+  src/property_tree.cpp
   src/resource_locator.cpp
-  src/types.cpp
+  src/schema_registration.cpp
+  src/schema_registry.cpp
   src/stopwatch.cpp
+  src/types.cpp
   src/timer.cpp
+  src/yaml_extensions.cpp
   src/yaml_utils.cpp)
 target_link_libraries(
   ${PROJECT_NAME}
@@ -124,7 +128,14 @@ target_code_coverage(
 target_include_directories(${PROJECT_NAME} PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
                                                   "$<INSTALL_INTERFACE:include>")
 
-configure_package(NAMESPACE tesseract TARGETS ${PROJECT_NAME})
+add_executable(${PROJECT_NAME}_property_tree_demo src/property_tree_demo.cpp)
+target_link_libraries(${PROJECT_NAME}_property_tree_demo PRIVATE ${PROJECT_NAME})
+target_compile_options(${PROJECT_NAME}_property_tree_demo PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE}
+                                                                  ${TESSERACT_COMPILE_OPTIONS_PUBLIC})
+target_compile_definitions(${PROJECT_NAME}_property_tree_demo PRIVATE ${TESSERACT_COMPILE_DEFINITIONS})
+target_cxx_version(${PROJECT_NAME}_property_tree_demo PUBLIC VERSION ${TESSERACT_CXX_VERSION})
+
+configure_package(NAMESPACE tesseract TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_property_tree_demo)
 if(WIN32)
   target_link_libraries(${PROJECT_NAME} PUBLIC Bcrypt)
 endif()

tesseract_common/include/tesseract_common/fwd.h

@@ -80,6 +80,12 @@ class Timer;
 // Profile Dictionary
 class Profile;
 class ProfileDictionary;
+
+// property_tree.h
+class PropertyTree;
+
+// schema_registry.h
+class SchemaRegistry;
 }  // namespace tesseract_common
 
 #endif  // TESSERACT_COMMON_TESSERACT_COMMON_FWD_H

tesseract_common/include/tesseract_common/plugin_info.h

@@ -45,6 +45,7 @@ class access;
 namespace tesseract_common
 {
 struct Serialization;
+class PropertyTree;
 
 /** @brief The Plugin Information struct */
 // NOLINTNEXTLINE
@@ -155,6 +156,9 @@ struct ContactManagersPluginInfo
   /** @brief Check if structure is empty */
   bool empty() const;
 
+  /** @brief Get the schema */
+  static PropertyTree schema();
+
   // Yaml Config key
   static inline const std::string CONFIG_KEY{ "contact_manager_plugins" };