fixup

Levi-Armstrong · Levi-Armstrong · commit cb719c266f0e · 2025-07-05T20:50:44.000-05:00
diff --git a/tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_factories.h b/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)
diff --git a/tesseract_collision/bullet/src/bullet_factories.cpp b/tesseract_collision/bullet/src/bullet_factories.cpp
@@ -39,24 +39,60 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #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 @@ BulletDiscreteBVHManagerFactory::create(const std::string& name, const YAML::Nod
   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
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
@@ -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
diff --git a/tesseract_collision/core/src/contact_managers_plugin_factory.cpp b/tesseract_collision/core/src/contact_managers_plugin_factory.cpp
@@ -34,6 +34,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #include <tesseract_common/resource_locator.h>
 #include <tesseract_common/yaml_utils.h>
 #include <tesseract_common/yaml_extenstions.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 @@ ContactManagersPluginFactory::ContactManagersPluginFactory()
                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])
diff --git a/tesseract_collision/core/src/types.cpp b/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
diff --git a/tesseract_collision/fcl/include/tesseract_collision/fcl/fcl_factories.h b/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)
diff --git a/tesseract_collision/fcl/src/fcl_factories.cpp b/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 @@ FCLDiscreteBVHManagerFactory::create(const std::string& name, const YAML::Node&
   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
diff --git a/tesseract_collision/test/collision_core_unit.cpp b/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;
diff --git a/tesseract_collision/test/contact_manager_plugins.yaml b/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
 
diff --git a/tesseract_common/include/tesseract_common/plugin_info.h b/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" };
 
diff --git a/tesseract_common/include/tesseract_common/property_tree.h b/tesseract_common/include/tesseract_common/property_tree.h
@@ -40,6 +40,7 @@ constexpr std::string_view DOUBLE{ "double" };
 
 // Container of properties
 constexpr std::string_view CONTAINER{ "YAML::NodeType::Map" };
+constexpr std::string_view ONEOF{ "oneOf" };
 
 // Eigen Types
 constexpr std::string_view EIGEN_ISOMETRY_3D{ "Eigen::Isometry3d" };
@@ -93,6 +94,15 @@ class PropertyTree
    */
   PropertyTree() = default;
 
+  /** @brief Deep-copy constructor — clones all YAML::Nodes and subtrees */
+  PropertyTree(const PropertyTree& other);
+  /** @brief Deep-copy assignment operator */
+  PropertyTree& operator=(const PropertyTree& other);
+
+  /** @brief Default move constructor/assignment are fine */
+  PropertyTree(PropertyTree&&) noexcept = default;
+  PropertyTree& operator=(PropertyTree&&) noexcept = default;
+
   /**
    * @brief Given that *this* is purely a schema tree, merge in the
    *        user’s YAML config to populate all values (and apply defaults).
@@ -159,6 +169,16 @@ class PropertyTree
    */
   const YAML::Node& getValue() const;
 
+  /**
+   * @brief Retrieve the YAML value casted to the provided type
+   * @return The casted value
+   */
+  template <typename T>
+  inline T as() const
+  {
+    return value_.as<T>();
+  }
+
   /**
    * @brief Check if property value is null
    * @return True if required, otherwise false
@@ -193,6 +213,8 @@ class PropertyTree
   void setAttribute(std::string_view name, int attr);
   /** @brief Set a double attribute. */
   void setAttribute(std::string_view name, double attr);
+  /** @brief Set a vector of strings attribute. */
+  void setAttribute(std::string_view name, const std::vector<std::string>& attr);
 
   /**
    * @brief Check if an attribute exists and is not null.
@@ -234,13 +256,17 @@ class PropertyTree
    */
   YAML::Node toYAML(bool exclude_attributes = true) const;
 
+  /** @brief Indicate if defined, meaning it has children or a value */
+  explicit operator bool() const noexcept;
+
 private:
   YAML::Node value_;                             /**< Value stored at this node */
   YAML::Node follow_;                            /**< Follow stored at this node */
   std::map<std::string, YAML::Node> attributes_; /**< Metadata attributes */
   std::map<std::string, PropertyTree> children_; /**< Nested child nodes */
   std::vector<std::string> keys_;                /**< Nested child keys */
   std::vector<ValidatorFn> validators_;          /**< Validators to invoke */
+  std::unique_ptr<PropertyTree> oneof_;          /**< Store the property content on merge */
 };
 
 /**
@@ -293,6 +319,11 @@ void validateSequence(const PropertyTree& node);
  */
 void validateContainer(const PropertyTree& node);
 
+/**
+ * @brief Validator: Retrieve schema for the custom type and runs it validtor
+ * @param node Node to validate
+ * @throws runtime_error if not correct.
+ */
 void validateCustomType(const PropertyTree& node);
 
 /**
diff --git a/tesseract_common/include/tesseract_common/yaml_extenstions.h b/tesseract_common/include/tesseract_common/yaml_extenstions.h
diff --git a/tesseract_common/src/plugin_info.cpp b/tesseract_common/src/plugin_info.cpp
diff --git a/tesseract_common/src/property_tree.cpp b/tesseract_common/src/property_tree.cpp
diff --git a/tesseract_support/CMakeLists.txt b/tesseract_support/CMakeLists.txt

Original file line number	Diff line number	Diff line change
`@@ -27,6 +27,7 @@`
`27`	`27`	`#include <tesseract_collision/fcl/fcl_factories.h>`
`28`	`28`	`#include <tesseract_collision/fcl/fcl_discrete_managers.h>`
`29`	`29`	`#include <tesseract_collision/core/discrete_contact_manager.h>`
	`30`	`+#include <tesseract_common/property_tree.h>`
`30`	`31`
`31`	`32`	`namespace tesseract_collision::tesseract_collision_fcl`
`32`	`33`	`{`
`@@ -36,6 +37,8 @@ FCLDiscreteBVHManagerFactory::create(const std::string& name, const YAML::Node&`
`36`	`37`	`return std::make_unique<FCLDiscreteBVHManager>(name);`
`37`	`38`	`}`
`38`	`39`
	`40`	`+tesseract_common::PropertyTree FCLDiscreteBVHManagerFactory::schema() const { return {}; }`
	`41`	`+`
`39`	`42`	`PLUGIN_ANCHOR_IMPL(FCLFactoriesAnchor) // LCOV_EXCL_LINE`
`40`	`43`
`41`	`44`	`} // namespace tesseract_collision::tesseract_collision_fcl`
Original file line number	Diff line number	Diff line change
`@@ -125,6 +125,12 @@ TEST(TesseractCoreUnit, ContactManagerConfigTest) // NOLINT`
`125`	`125`	`EXPECT_ANY_THROW(config.validate()); // NOLINT`
`126`	`126`	`}`
`127`	`127`
	`128`	`+ {`
	`129`	`+ tesseract_collision::ContactManagerConfig config;`
	`130`	`+ config.pair_margin_override_type = tesseract_collision::CollisionMarginPairOverrideType::MODIFY;`
	`131`	`+ EXPECT_ANY_THROW(config.validate()); // NOLINT`
	`132`	`+ }`
	`133`	`+`
`128`	`134`	`{`
`129`	`135`	`tesseract_collision::ContactManagerConfig config;`
`130`	`136`	`config.pair_margin_override_type = tesseract_collision::CollisionMarginPairOverrideType::MODIFY;`
`@@ -138,6 +144,12 @@ TEST(TesseractCoreUnit, ContactManagerConfigTest) // NOLINT`
`138`	`144`	`EXPECT_ANY_THROW(config.validate()); // NOLINT`
`139`	`145`	`}`
`140`	`146`
	`147`	`+ {`
	`148`	`+ tesseract_collision::ContactManagerConfig config;`
	`149`	`+ config.acm_override_type = tesseract_collision::ACMOverrideType::OR;`
	`150`	`+ EXPECT_ANY_THROW(config.validate()); // NOLINT`
	`151`	`+ }`
	`152`	`+`
`141`	`153`	`{`
`142`	`154`	`tesseract_collision::ContactManagerConfig config;`
`143`	`155`	`config.acm_override_type = tesseract_collision::ACMOverrideType::OR;`