diff --git a/tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_factories.h b/tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_factories.h index c29272fa40c..4087771bdd0 100644 --- a/tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_factories.h +++ b/tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_factories.h @@ -55,6 +55,8 @@ class BulletDiscreteBVHManagerFactory : public DiscreteContactManagerFactory public: std::unique_ptr create(const std::string& name, const YAML::Node& config) const override final; + + tesseract_common::PropertyTree schema() const override final; }; class BulletDiscreteSimpleManagerFactory : public DiscreteContactManagerFactory @@ -62,6 +64,8 @@ class BulletDiscreteSimpleManagerFactory : public DiscreteContactManagerFactory public: std::unique_ptr create(const std::string& name, const YAML::Node& config) const override final; + + tesseract_common::PropertyTree schema() const override final; }; class BulletCastBVHManagerFactory : public ContinuousContactManagerFactory @@ -69,6 +73,8 @@ class BulletCastBVHManagerFactory : public ContinuousContactManagerFactory public: std::unique_ptr create(const std::string& name, const YAML::Node& config) const override final; + + tesseract_common::PropertyTree schema() const override final; }; class BulletCastSimpleManagerFactory : public ContinuousContactManagerFactory @@ -76,6 +82,8 @@ class BulletCastSimpleManagerFactory : public ContinuousContactManagerFactory public: std::unique_ptr 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 index 2d992251fad..9f182f90c0c 100644 --- 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 #include +#include + 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(); + if (const tesseract_common::PropertyTree& prop = schema.at("share_pool_allocators")) + share_pool_allocators = prop.as(); TesseractCollisionConfigurationInfo config_info(false, share_pool_allocators); - if (YAML::Node n = config["max_persistent_manifold_pool_size"]) - config_info.m_defaultMaxPersistentManifoldPoolSize = n.as(); + if (const tesseract_common::PropertyTree& prop = schema.at("max_persistent_manifold_pool_size")) + config_info.m_defaultMaxPersistentManifoldPoolSize = prop.as(); - if (YAML::Node n = config["max_collision_algorithm_pool_size"]) - config_info.m_defaultMaxCollisionAlgorithmPoolSize = n.as(); + if (const tesseract_common::PropertyTree& prop = schema.at("max_collision_algorithm_pool_size")) + config_info.m_defaultMaxCollisionAlgorithmPoolSize = prop.as(); config_info.createPoolAllocators(); return config_info; @@ -68,24 +104,32 @@ BulletDiscreteBVHManagerFactory::create(const std::string& name, const YAML::Nod return std::make_unique(name, getConfigInfo(config)); } +tesseract_common::PropertyTree BulletDiscreteBVHManagerFactory::schema() const { return getConfigSchema(); } + std::unique_ptr BulletDiscreteSimpleManagerFactory::create(const std::string& name, const YAML::Node& config) const { return std::make_unique(name, getConfigInfo(config)); } +tesseract_common::PropertyTree BulletDiscreteSimpleManagerFactory::schema() const { return getConfigSchema(); } + std::unique_ptr BulletCastBVHManagerFactory::create(const std::string& name, const YAML::Node& config) const { return std::make_unique(name, getConfigInfo(config)); } +tesseract_common::PropertyTree BulletCastBVHManagerFactory::schema() const { return getConfigSchema(); } + std::unique_ptr BulletCastSimpleManagerFactory::create(const std::string& name, const YAML::Node& config) const { return std::make_unique(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 index 1b01033b8c9..a22e30ea869 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 @@ -70,6 +70,12 @@ class DiscreteContactManagerFactory */ virtual std::unique_ptr 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 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 index bc27e8c28d8..87d7c5c6f22 100644 --- a/tesseract_collision/core/src/contact_managers_plugin_factory.cpp +++ b/tesseract_collision/core/src/contact_managers_plugin_factory.cpp @@ -33,7 +33,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include +#include #include #include #include @@ -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 index ac264ad2686..00d805acb97 100644 --- 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 index d2806227600..0f49c1ca71d 100644 --- 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 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 index 51c764a8989..5e7a643426e 100644 --- a/tesseract_collision/fcl/src/fcl_factories.cpp +++ b/tesseract_collision/fcl/src/fcl_factories.cpp @@ -27,6 +27,7 @@ #include #include #include +#include namespace tesseract_collision::tesseract_collision_fcl { @@ -36,6 +37,8 @@ FCLDiscreteBVHManagerFactory::create(const std::string& name, const YAML::Node& return std::make_unique(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 index 3c27fcf4cf7..d641563a3cc 100644 --- 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 index 4209235a988..cd829f85c92 100644 --- 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/CMakeLists.txt b/tesseract_common/CMakeLists.txt index e6a2a05d8a3..063e5ab3a6b 100644 --- a/tesseract_common/CMakeLists.txt +++ b/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 "$" "$") -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() diff --git a/tesseract_common/include/tesseract_common/fwd.h b/tesseract_common/include/tesseract_common/fwd.h index c5f6bfd4f53..b5681e62adb 100644 --- a/tesseract_common/include/tesseract_common/fwd.h +++ b/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 diff --git a/tesseract_common/include/tesseract_common/plugin_info.h b/tesseract_common/include/tesseract_common/plugin_info.h index cef50daceda..e9d1bdfed19 100644 --- 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 new file mode 100644 index 00000000000..decc1c10eeb --- /dev/null +++ b/tesseract_common/include/tesseract_common/property_tree.h @@ -0,0 +1,399 @@ +#ifndef TESSERACT_COMMON_PROPERTY_TREE_H +#define TESSERACT_COMMON_PROPERTY_TREE_H + +#include +#include +#include +#include +#include +#include +#include + +namespace tesseract_common +{ +namespace property_type +{ +/** + * @brief A utility for constructing the vector + * @param type The type assoicated with the list + * @param length The length if fixed size + * @return The string representation of the vector, aka. type[] and type[length] for fixed size + */ +std::string createList(std::string_view type, std::size_t length = 0); + +/** + * @brief A utility for constructing the map + * @param type The value type assoicated with the map + * @return The string representation of the map, aka. {string,string} + */ +std::string createMap(std::string_view type); + +/** + * @brief A utility for constructing the map + * @param type The value type assoicated with the map + * @return The string representation of the map, aka. {string, string} or {string[2], string} + */ +std::string createMap(std::string_view key, std::string_view type); + +// Integral Types +constexpr std::string_view BOOL{ "bool" }; +constexpr std::string_view CHAR{ "char" }; +constexpr std::string_view STRING{ "std::string" }; +constexpr std::string_view INT{ "int" }; +constexpr std::string_view UNSIGNED_INT{ "unsigned int" }; +constexpr std::string_view LONG_INT{ "long int" }; +constexpr std::string_view LONG_UNSIGNED_INT{ "long unsigned int" }; +constexpr std::string_view FLOAT{ "float" }; +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" }; +// constexpr std::string_view EIGEN_MATRIX_XD{ "Eigen::MatrixXd" }; +constexpr std::string_view EIGEN_VECTOR_XD{ "Eigen::VectorXd" }; +// constexpr std::string_view EIGEN_MATRIX_2D{ "Eigen::Matrix2d" }; +constexpr std::string_view EIGEN_VECTOR_2D{ "Eigen::Vector2d" }; +// constexpr std::string_view EIGEN_MATRIX_3D{ "Eigen::Matrix3d" }; +constexpr std::string_view EIGEN_VECTOR_3D{ "Eigen::Vector3d" }; +} // namespace property_type + +namespace property_attribute +{ +constexpr std::string_view TYPE{ "type" }; +constexpr std::string_view DOC{ "doc" }; +constexpr std::string_view REQUIRED{ "required" }; +constexpr std::string_view DEFAULT{ "default" }; +constexpr std::string_view ENUM{ "enum" }; +constexpr std::string_view MINIMUM{ "minimum" }; +constexpr std::string_view MAXIMUM{ "maximum" }; +} // namespace property_attribute + +/** + * @file property_tree.h + * @brief Defines PropertyTree, a hierarchical structure for YAML-based + * configuration with metadata and validation support. + */ + +/** + * @class PropertyTree + * @brief Represents a node in a hierarchical property tree. + * + * Each PropertyTree node may contain: + * - A YAML::Node value (scalar, sequence, or map) + * - A map of metadata attributes (YAML::Node) keyed by string + * - Nested child PropertyTree nodes + * - A list of custom validator functions + */ +class PropertyTree +{ +public: + /** + * @brief Signature for custom validator callbacks. + * @param node The PropertyTree node being validated. + * @throws std::runtime_error on validation failure. + */ + using ValidatorFn = std::function; + + /** + * @brief Default constructor. Creates an empty tree node. + */ + 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). + * @param config The user-supplied YAML::Node (possibly null). + * @param allow_extra_properties If false, “extra” keys in config will be flagged. + */ + void mergeConfig(const YAML::Node& config, bool allow_extra_properties = false); + + /** + * @brief Validate the tree using registered validators. + * + * Recursively invokes all validators on this node and its children. + * Throws on the first error encountered. + * + * @param allow_extra_properties Indicate if extra properties are allowed + */ + void validate(bool allow_extra_properties = false) const; + + /** + * @brief Register a custom validator for this node. + * @param fn Callback to invoke during validation. + */ + void addValidator(ValidatorFn fn); + + /** + * @brief Access or create a child node by key. + * @param key Child identifier (string key). + * @return Reference to the child PropertyTree. + */ + PropertyTree& operator[](std::string_view key); + + /** + * @brief Access a child node by key. + * @param key Child identifier. + * @return Reference to the child. + * @throws std::out_of_range if key not found. + */ + PropertyTree& at(std::string_view key); + + /** + * @brief Access a child node by key (const). + * @param key Child identifier. + * @return Const reference to the child. + * @throws std::out_of_range if key not found. + */ + const PropertyTree& at(std::string_view key) const; + + /** + * @brief Find a child node without creating it. + * @param key Child identifier. + * @return Pointer to child or nullptr if not present. + */ + const PropertyTree* find(std::string_view key) const; + + /** + * @brief Set the YAML value of this node. + * @param v YAML::Node representing the new value. + */ + void setValue(const YAML::Node& v); + + /** + * @brief Retrieve the YAML value stored at this node. + * @return Const reference to a YAML::Node. + */ + const YAML::Node& getValue() const; + + /** + * @brief Retrieve the YAML value casted to the provided type + * @return The casted value + */ + template + inline T as() const + { + return value_.as(); + } + + /** + * @brief Check if property value is null + * @return True if required, otherwise false + */ + bool isNull() const; + + /** + * @brief Check if property is a container of child properties + * @return True if container, otherwise false + */ + bool isContainer() const; + + /** + * @brief List all immediate child keys. + * @return Vector of child key strings. + */ + std::vector keys() const; + + /** + * @brief Set a metadata attribute (YAML node form). + * @param name Attribute name. + * @param attr YAML::Node value. + */ + void setAttribute(std::string_view name, const YAML::Node& attr); + + /** @brief Convenience overload to set a string attribute. */ + void setAttribute(std::string_view name, std::string_view attr); + void setAttribute(std::string_view name, const char* attr); + /** @brief Set a boolean attribute. */ + void setAttribute(std::string_view name, bool attr); + /** @brief Set an integer attribute. */ + 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& attr); + + /** + * @brief Check if an attribute exists and is not null. + * @param name Attribute name. + * @return True if present and non-null, false otherwise. + */ + bool hasAttribute(std::string_view name) const; + + /** + * @brief Retrieve an attribute value by name. + * @param name Attribute name. + * @return Optional containing YAML::Node if found. + */ + std::optional getAttribute(std::string_view name) const; + + /** + * @brief List all metadata attribute keys. + * @return Vector of attribute names. + */ + std::vector getAttributeKeys() const; + + /** + * @brief Check if property is requried by checking for attribute and its value + * @return True if required, otherwise false + */ + bool isRequired() const; + + /** + * @brief Create a PropertyTree from a YAML::Node. + * @param node Root YAML::Node to convert. + * @return Populated PropertyTree hierarchy. + */ + static PropertyTree fromYAML(const YAML::Node& node); + + /** + * @brief Serialize this tree back into a YAML::Node. + * @param exclude_attributes If true, omit the attributes map. + * @return YAML::Node representation of the tree. + */ + 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 attributes_; /**< Metadata attributes */ + std::map children_; /**< Nested child nodes */ + std::vector keys_; /**< Nested child keys */ + std::vector validators_; /**< Validators to invoke */ + std::unique_ptr oneof_; /**< Store the property content on merge */ +}; + +/** + * @brief Check if type is a sequence + * @param type The type to check + * @return If it is a sequence, the underlying type is returned and size + */ +std::optional> isSequenceType(std::string_view type); + +/** + * @brief Check if type is a map + * @param type The type to check + * @return If it is a map, the underlying pair is returned + */ +std::optional> isMapType(std::string_view type); + +/** + * @brief Validator: ensure 'required' attribute is present and non-null. + * @param node Node to validate. + * @throws runtime_error if missing. + */ +void validateRequired(const PropertyTree& node); + +/** + * @brief Validator: enforce that node's value is in 'enum' list. + * @param node Node to validate. + * @throws runtime_error if not found. + */ +void validateEnum(const PropertyTree& node); + +/** + * @brief Validtor: ensure node value is of type YAML::NodeType::Map + * @param node Node to validate. + * @throws runtime_error if not correct type. + */ +void validateMap(const PropertyTree& node); + +/** + * @brief Validtor: ensure node value is of type YAML::NodeType::Sequence + * @param node Node to validate. + * @param length The length if fixed size. If zero, it is considered dynamic size sequence + * @throws runtime_error if not correct type. + */ +void validateSequence(const PropertyTree& node, std::size_t length = 0); + +/** + * @brief Validator: ensure property is a container of child properties + * The property should have children and the value should be null + * @param node Node to validate. + * @throws runtime_error if not correct. + */ +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); + +/** + * @brief Validate that the node’s value can be interpreted as the provided type. + * @param node PropertyTree node whose value to validate. + * @throws std::runtime_error or YAML::BadConversion if the value is not a valid type. + */ +template +void validateTypeCast(const PropertyTree& node) +{ + try + { + node.getValue().as(); + } + catch (const std::exception& e) + { + std::throw_with_nested(e); + } +} + +/** + * @brief Validator: type cast and enforce 'minimum'/'maximum' range constraints. + * @param node Node to validate. + * @throws runtime_error if out of range. + */ +template +void validateTypeCastWithRange(const PropertyTree& node) +{ + // Get value + const T val = [&]() { + try + { + return node.getValue().as(); + } + catch (const std::exception& e) + { + std::throw_with_nested(e); + } + }(); + + // If minimum attribute exist, validate + auto min_attr = node.getAttribute(property_attribute::MINIMUM); + if (min_attr) + { + const auto minv = min_attr->as(); + if (val < minv) + std::throw_with_nested(std::runtime_error("Property value " + std::to_string(val) + " is less than minimum " + + std::to_string(minv))); + } + + // If maximum attribute exist, validate + auto max_attr = node.getAttribute(property_attribute::MAXIMUM); + if (max_attr) + { + const auto maxv = max_attr->as(); + if (val > maxv) + std::throw_with_nested(std::runtime_error("Property value " + std::to_string(val) + " is greater than maximum " + + std::to_string(maxv))); + } +} + +} // namespace tesseract_common + +#endif // TESSERACT_COMMON_PROPERTY_TREE_H diff --git a/tesseract_common/include/tesseract_common/schema_registration.h b/tesseract_common/include/tesseract_common/schema_registration.h new file mode 100644 index 00000000000..95a32e555ba --- /dev/null +++ b/tesseract_common/include/tesseract_common/schema_registration.h @@ -0,0 +1,39 @@ +#ifndef TESSERACT_COMMON_SCHEMA_REGISTRATION_H +#define TESSERACT_COMMON_SCHEMA_REGISTRATION_H + +#include +#include + +namespace tesseract_common +{ +class PropertyTree; + +struct SchemaRegistrar +{ + SchemaRegistrar(const std::string& key, const std::string& path); + + SchemaRegistrar(const std::string& key, const std::function& fn); +}; + +} // namespace tesseract_common + +// first level: does the actual pasting +#define _SCHEMA_REG_PASTE(a, b) a##b + +// second level: expands its arguments before passing to the first +#define _SCHEMA_REG_MAKE_NAME(a, b) _SCHEMA_REG_PASTE(a, b) + +/// Macro to register either a file‐based schema or a function‐built schema +#define TESSERACT_REGISTER_SCHEMA(KEY, SCHEMA_SOURCE) \ + namespace \ + { \ + /* now a const POD, linter is happy */ \ + static const int _SCHEMA_REG_MAKE_NAME(_schema_reg_, __COUNTER__) = []() -> int { \ + using namespace tesseract_common; \ + /* calls the appropriate SchemaRegistrar constructor */ \ + SchemaRegistrar(#KEY, SCHEMA_SOURCE); \ + return 0; \ + }(); \ + } + +#endif // TESSERACT_COMMON_SCHEMA_REGISTRATION_H diff --git a/tesseract_common/include/tesseract_common/schema_registry.h b/tesseract_common/include/tesseract_common/schema_registry.h new file mode 100644 index 00000000000..67cc2ff674f --- /dev/null +++ b/tesseract_common/include/tesseract_common/schema_registry.h @@ -0,0 +1,77 @@ +#ifndef TESSERACT_COMMON_SCHEMA_REGISTRY_H +#define TESSERACT_COMMON_SCHEMA_REGISTRY_H + +#include +#include +#include +#include + +namespace tesseract_common +{ +class PropertyTree; + +/** + * @brief A global registry of named schemas for PropertyTree. + * + * Use SchemaRegistry::instance() to access the singleton. + */ +class SchemaRegistry +{ +public: + ~SchemaRegistry() = default; + SchemaRegistry(const SchemaRegistry&) = delete; + SchemaRegistry& operator=(const SchemaRegistry&) = delete; + SchemaRegistry(const SchemaRegistry&&) = delete; + SchemaRegistry& operator=(const SchemaRegistry&&) = delete; + + /** @brief Access the singleton instance. */ + static std::shared_ptr instance(); + + /** + * @brief Register an already-parsed schema under a logical key. + * @param key Unique identifier for this schema. + * @param schema Parsed PropertyTree schema. + */ + void registerSchema(const std::string& key, const PropertyTree& schema); + + /** + * @brief Register a schema from a YAML file path. + * @param key Unique identifier for this schema. + * @param path Path to a .yaml schema file. + */ + void registerSchemaFromFile(const std::string& key, const std::string& path); + + /** + * @brief Check whether a schema is registered under this key. + * @param key Logical identifier. + * @return True if present. + */ + bool contains(const std::string& key) const; + + /** + * @brief Retrieve a registered schema by key. + * @param key Logical identifier. + * @return Const reference to the schema PropertyTree. + * @throws std::out_of_range if the key is not found. + */ + const PropertyTree& get(const std::string& key) const; + + /** + * @brief Load and parse an arbitrary YAML file into a PropertyTree. + * @param path Path to a .yaml file. + * @return Parsed PropertyTree. + * @throws YAML::Exception on parse errors. + */ + static PropertyTree loadFile(const std::string& path); + +private: + SchemaRegistry() = default; + + mutable std::mutex mutex_; + mutable std::map schemas_; + mutable std::map paths_; +}; + +} // namespace tesseract_common + +#endif // TESSERACT_COMMON_SCHEMA_REGISTRY_H diff --git a/tesseract_common/include/tesseract_common/yaml_extenstions.h b/tesseract_common/include/tesseract_common/yaml_extensions.h similarity index 69% rename from tesseract_common/include/tesseract_common/yaml_extenstions.h rename to tesseract_common/include/tesseract_common/yaml_extensions.h index cdfc652df08..3bb6fe39b1a 100644 --- a/tesseract_common/include/tesseract_common/yaml_extenstions.h +++ b/tesseract_common/include/tesseract_common/yaml_extensions.h @@ -1,5 +1,5 @@ /** - * @file yaml_extenstions.h + * @file yaml_extensions.h * @brief YAML Type conversions * * @author Levi Armstrong @@ -35,6 +35,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include namespace YAML { @@ -199,6 +200,55 @@ struct convert rhs = out; return true; } + + static tesseract_common::PropertyTree schema() + { + using namespace tesseract_common; + using namespace property_attribute; + using namespace property_type; + + // Top-level must be a map container + PropertyTree sch; + sch.setAttribute(TYPE, CONTAINER); + + // position: map{x:double,y:double,z:double} + { + auto& prop = sch["position"]; + prop.setAttribute(TYPE, CONTAINER); + prop.setAttribute(REQUIRED, true); + + // x, y, z all required doubles + for (auto key : { "x", "y", "z" }) + { + auto& c = prop[key]; + c.setAttribute(TYPE, DOUBLE); + c.setAttribute(REQUIRED, true); + } + } + + // orientation: either quaternion (x,y,z,w) or rpy (r,p,y) + { + auto& prop = sch["orientation"]; + prop.setAttribute(TYPE, ONEOF); + prop.setAttribute(REQUIRED, true); + + auto& option0 = prop["xyzw"]; + for (auto key : { "x", "y", "z", "w" }) + { + auto& c = option0[key]; + c.setAttribute(TYPE, DOUBLE); + } + + auto& option1 = prop["rpy"]; + for (auto key : { "r", "p", "y" }) + { + auto& c = option1[key]; + c.setAttribute(TYPE, DOUBLE); + } + } + + return sch; + } }; template <> @@ -224,6 +274,14 @@ struct convert return true; } + + static tesseract_common::PropertyTree schema() + { + using namespace tesseract_common; + PropertyTree sch; + sch.setAttribute(property_attribute::TYPE, property_type::EIGEN_VECTOR_XD); + return sch; + } }; template <> @@ -248,6 +306,14 @@ struct convert return true; } + + static tesseract_common::PropertyTree schema() + { + using namespace tesseract_common; + PropertyTree sch; + sch.setAttribute(property_attribute::TYPE, property_type::EIGEN_VECTOR_2D); + return sch; + } }; template <> @@ -272,18 +338,25 @@ struct convert return true; } + + static tesseract_common::PropertyTree schema() + { + using namespace tesseract_common; + PropertyTree sch; + sch.setAttribute(property_attribute::TYPE, property_type::EIGEN_VECTOR_3D); + return sch; + } }; template <> struct convert { + inline static const std::string SEARCH_PATHS_KEY{ "search_paths" }; + inline static const std::string SEARCH_LIBRARIES_KEY{ "search_libraries" }; + inline static const std::string FWD_KIN_PLUGINS_KEY{ "fwd_kin_plugins" }; + inline static const std::string INV_KIN_PLUGINS_KEY{ "inv_kin_plugins" }; static Node encode(const tesseract_common::KinematicsPluginInfo& rhs) { - const std::string SEARCH_PATHS_KEY{ "search_paths" }; - const std::string SEARCH_LIBRARIES_KEY{ "search_libraries" }; - const std::string FWD_KIN_PLUGINS_KEY{ "fwd_kin_plugins" }; - const std::string INV_KIN_PLUGINS_KEY{ "inv_kin_plugins" }; - YAML::Node kinematic_plugins; if (!rhs.search_paths.empty()) kinematic_plugins[SEARCH_PATHS_KEY] = rhs.search_paths; @@ -302,11 +375,6 @@ struct convert static bool decode(const Node& node, tesseract_common::KinematicsPluginInfo& rhs) { - const std::string SEARCH_PATHS_KEY{ "search_paths" }; - const std::string SEARCH_LIBRARIES_KEY{ "search_libraries" }; - const std::string FWD_KIN_PLUGINS_KEY{ "fwd_kin_plugins" }; - const std::string INV_KIN_PLUGINS_KEY{ "inv_kin_plugins" }; - if (const YAML::Node& search_paths = node[SEARCH_PATHS_KEY]) { std::set sp; @@ -379,18 +447,54 @@ struct convert return true; } + + static tesseract_common::PropertyTree schema() + { + using namespace tesseract_common; + using namespace property_attribute; + using namespace property_type; + + PropertyTree sch; + + // This is a map of named fields + sch.setAttribute(TYPE, CONTAINER); + + { // search_paths: optional list + auto& prop = sch[SEARCH_PATHS_KEY]; + prop.setAttribute(TYPE, createList(STRING)); + } + + { // search_libraries: optional list + auto& prop = sch[SEARCH_LIBRARIES_KEY]; + prop.setAttribute(TYPE, createList(STRING)); + } + + { // fwd_kin_plugins: optional map + auto& prop = sch[FWD_KIN_PLUGINS_KEY]; + prop.setAttribute(TYPE, createMap("tesseract_common::PluginInfoContainer")); + // contents are custom type → use validateCustomType + prop.addValidator(validateCustomType); + } + + { // inv_kin_plugins: optional map + auto& prop = sch[INV_KIN_PLUGINS_KEY]; + prop.setAttribute(TYPE, createMap("tesseract_common::PluginInfoContainer")); + prop.addValidator(validateCustomType); + } + + return sch; + } }; template <> struct convert { + inline static const std::string SEARCH_PATHS_KEY{ "search_paths" }; + inline static const std::string SEARCH_LIBRARIES_KEY{ "search_libraries" }; + inline static const std::string DISCRETE_PLUGINS_KEY{ "discrete_plugins" }; + inline static const std::string CONTINUOUS_PLUGINS_KEY{ "continuous_plugins" }; static Node encode(const tesseract_common::ContactManagersPluginInfo& rhs) { - const std::string SEARCH_PATHS_KEY{ "search_paths" }; - const std::string SEARCH_LIBRARIES_KEY{ "search_libraries" }; - const std::string DISCRETE_PLUGINS_KEY{ "discrete_plugins" }; - const std::string CONTINUOUS_PLUGINS_KEY{ "continuous_plugins" }; - YAML::Node contact_manager_plugins; if (!rhs.search_paths.empty()) contact_manager_plugins[SEARCH_PATHS_KEY] = rhs.search_paths; @@ -409,11 +513,6 @@ struct convert static bool decode(const Node& node, tesseract_common::ContactManagersPluginInfo& rhs) { - const std::string SEARCH_PATHS_KEY{ "search_paths" }; - const std::string SEARCH_LIBRARIES_KEY{ "search_libraries" }; - const std::string DISCRETE_PLUGINS_KEY{ "discrete_plugins" }; - const std::string CONTINUOUS_PLUGINS_KEY{ "continuous_plugins" }; - if (const YAML::Node& search_paths = node[SEARCH_PATHS_KEY]) { std::set sp; @@ -482,18 +581,57 @@ struct convert return true; } + + static tesseract_common::PropertyTree schema() + { + using namespace tesseract_common; + using namespace property_attribute; + using namespace property_type; + + PropertyTree sch; + // top-level is a map container + sch.setAttribute(TYPE, CONTAINER); + + // search_paths: optional list + { + auto& prop = sch[SEARCH_PATHS_KEY]; + prop.setAttribute(TYPE, createList(STRING)); + } + + // search_libraries: optional list + { + auto& prop = sch[SEARCH_LIBRARIES_KEY]; + prop.setAttribute(TYPE, createList(STRING)); + } + + // discrete_plugins: optional map + { + auto& prop = sch[DISCRETE_PLUGINS_KEY]; + prop.setAttribute(TYPE, createMap("tesseract_common::PluginInfoContainer")); + prop.addValidator(validateCustomType); + } + + // continuous_plugins: optional map + { + auto& prop = sch[CONTINUOUS_PLUGINS_KEY]; + prop.setAttribute(TYPE, createMap("tesseract_common::PluginInfoContainer")); + prop.addValidator(validateCustomType); + } + + return sch; + } }; template <> struct convert { + inline static const std::string SEARCH_PATHS_KEY{ "search_paths" }; + inline static const std::string SEARCH_LIBRARIES_KEY{ "search_libraries" }; + inline static const std::string EXECUTOR_PLUGINS_KEY{ "executors" }; + inline static const std::string NODE_PLUGINS_KEY{ "tasks" }; + static Node encode(const tesseract_common::TaskComposerPluginInfo& rhs) { - const std::string SEARCH_PATHS_KEY{ "search_paths" }; - const std::string SEARCH_LIBRARIES_KEY{ "search_libraries" }; - const std::string EXECUTOR_PLUGINS_KEY{ "executors" }; - const std::string NODE_PLUGINS_KEY{ "tasks" }; - YAML::Node task_composer_plugins; if (!rhs.search_paths.empty()) task_composer_plugins[SEARCH_PATHS_KEY] = rhs.search_paths; @@ -512,11 +650,6 @@ struct convert static bool decode(const Node& node, tesseract_common::TaskComposerPluginInfo& rhs) { - const std::string SEARCH_PATHS_KEY{ "search_paths" }; - const std::string SEARCH_LIBRARIES_KEY{ "search_libraries" }; - const std::string EXECUTOR_PLUGINS_KEY{ "executors" }; - const std::string NODE_PLUGINS_KEY{ "tasks" }; - if (const YAML::Node& search_paths = node[SEARCH_PATHS_KEY]) { std::set sp; @@ -586,6 +719,46 @@ struct convert return true; } + + static tesseract_common::PropertyTree schema() + { + using namespace tesseract_common; + using namespace property_attribute; + using namespace property_type; + + PropertyTree sch; + + // top‐level must be a map container + sch.setAttribute(TYPE, CONTAINER); + + // search_paths: optional list + { + auto& node = sch[SEARCH_PATHS_KEY]; + node.setAttribute(TYPE, createList(STRING)); + } + + // search_libraries: optional list + { + auto& node = sch[SEARCH_LIBRARIES_KEY]; + node.setAttribute(TYPE, createList(STRING)); + } + + // executors: optional map + { + auto& node = sch[EXECUTOR_PLUGINS_KEY]; + node.setAttribute(TYPE, createMap("tesseract_common::PluginInfoContainer")); + node.addValidator(validateCustomType); + } + + // tasks: optional map + { + auto& node = sch[NODE_PLUGINS_KEY]; + node.setAttribute(TYPE, createMap("tesseract_common::PluginInfoContainer")); + node.addValidator(validateCustomType); + } + + return sch; + } }; template <> @@ -610,27 +783,58 @@ struct convert return true; } + + static tesseract_common::PropertyTree schema() + { + using namespace tesseract_common; + using namespace property_attribute; + using namespace property_type; + + // top‐level must be a map container + PropertyTree sch; + sch.setAttribute(TYPE, createMap(EIGEN_ISOMETRY_3D)); + + return sch; + } }; template <> struct convert { + inline static const std::string JOINTS_KEY{ "joints" }; static Node encode(const tesseract_common::CalibrationInfo& rhs) { Node node; - node["joints"] = rhs.joints; + node[JOINTS_KEY] = rhs.joints; return node; } static bool decode(const Node& node, tesseract_common::CalibrationInfo& rhs) { - const YAML::Node& joints_node = node["joints"]; + const YAML::Node& joints_node = node[JOINTS_KEY]; rhs.joints = joints_node.as(); return true; } + + static tesseract_common::PropertyTree schema() + { + using namespace tesseract_common; + using namespace property_attribute; + using namespace property_type; + + // top‐level must be a map container + PropertyTree sch; + sch.setAttribute(TYPE, CONTAINER); + + auto& prop = sch[JOINTS_KEY]; + prop.setAttribute(TYPE, "tesseract_common::TransformMap"); + prop.setAttribute(REQUIRED, true); + + return sch; + } }; template <> @@ -669,6 +873,19 @@ struct convert return true; } + + static tesseract_common::PropertyTree schema() + { + using namespace tesseract_common; + using namespace property_attribute; + using namespace property_type; + + // top‐level must be a map container + PropertyTree sch; + sch.setAttribute(TYPE, createList(EIGEN_ISOMETRY_3D)); + + return sch; + } }; //=========================== CollisionMarginPairOverrideType Enum =========================== @@ -707,6 +924,20 @@ struct convert rhs = it->second; return true; } + + static tesseract_common::PropertyTree schema() + { + using namespace tesseract_common; + using namespace property_attribute; + using namespace property_type; + + // top‐level must be a map container + PropertyTree sch; + sch.setAttribute(TYPE, STRING); + sch.setAttribute(ENUM, { "NONE", "MODIFY", "REPLACE" }); + + return sch; + } }; //============================ PairsCollisionMarginData ============================ @@ -751,6 +982,40 @@ struct convert } return true; } + + static tesseract_common::PropertyTree schema() + { + using namespace tesseract_common; + using namespace property_attribute; + using namespace property_type; + + // top‐level must be a map container + PropertyTree sch; + sch.setAttribute(TYPE, "tesseract_common::PairsCollisionMarginData"); + sch.addValidator([](const PropertyTree& node) { + const YAML::Node& yn = node.getValue(); + if (!yn.IsMap()) + std::throw_with_nested(std::runtime_error("PairsCollisionMarginData, must be a map")); + + for (auto it = yn.begin(); it != yn.end(); ++it) + { + Node key_node = it->first; + if (!key_node.IsSequence() || key_node.size() != 2) + std::throw_with_nested(std::runtime_error("PairsCollisionMarginData, key must be a sequenc of size 2")); + + try + { + it->second.as(); + } + catch (const std::exception& e) + { + std::throw_with_nested(e); + } + } + }); + + return sch; + } }; //================================== CollisionMarginPairData ================================= @@ -769,6 +1034,19 @@ struct convert rhs = tesseract_common::CollisionMarginPairData(data); return true; } + + static tesseract_common::PropertyTree schema() + { + using namespace tesseract_common; + using namespace property_attribute; + using namespace property_type; + + // top‐level must be a map container + PropertyTree sch; + sch.setAttribute(TYPE, "tesseract_common::PairsCollisionMarginData"); + + return sch; + } }; //============================ AllowedCollisionEntries ============================ @@ -813,6 +1091,40 @@ struct convert } return true; } + + static tesseract_common::PropertyTree schema() + { + using namespace tesseract_common; + using namespace property_attribute; + using namespace property_type; + + // top‐level must be a map container + PropertyTree sch; + sch.setAttribute(TYPE, "tesseract_common::AllowedCollisionEntries"); + sch.addValidator([](const PropertyTree& node) { + const YAML::Node& yn = node.getValue(); + if (!yn.IsMap()) + std::throw_with_nested(std::runtime_error("AllowedCollisionEntries, must be a map")); + + for (auto it = yn.begin(); it != yn.end(); ++it) + { + Node key_node = it->first; + if (!key_node.IsSequence() || key_node.size() != 2) + std::throw_with_nested(std::runtime_error("AllowedCollisionEntries, key must be a sequenc of size 2")); + + try + { + it->second.as(); + } + catch (const std::exception& e) + { + std::throw_with_nested(e); + } + } + }); + + return sch; + } }; //================================== AllowedCollisionMatrix ================================= @@ -831,6 +1143,19 @@ struct convert rhs = tesseract_common::AllowedCollisionMatrix(data); return true; } + + static tesseract_common::PropertyTree schema() + { + using namespace tesseract_common; + using namespace property_attribute; + using namespace property_type; + + // top‐level must be a map container + PropertyTree sch; + sch.setAttribute(TYPE, "tesseract_common::AllowedCollisionEntries"); + + return sch; + } }; //============================== std::unordered_map ============================= diff --git a/tesseract_common/src/plugin_info.cpp b/tesseract_common/src/plugin_info.cpp index ce4337757ea..170e1d57063 100644 --- a/tesseract_common/src/plugin_info.cpp +++ b/tesseract_common/src/plugin_info.cpp @@ -40,7 +40,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include +#include namespace tesseract_common { @@ -215,6 +216,13 @@ bool ContactManagersPluginInfo::empty() const continuous_plugin_infos.plugins.empty()); } +PropertyTree ContactManagersPluginInfo::schema() +{ + PropertyTree schema; + + return schema; +} + bool ContactManagersPluginInfo::operator==(const ContactManagersPluginInfo& rhs) const { bool equal = true; diff --git a/tesseract_common/src/property_tree.cpp b/tesseract_common/src/property_tree.cpp new file mode 100644 index 00000000000..a06c3feb2b2 --- /dev/null +++ b/tesseract_common/src/property_tree.cpp @@ -0,0 +1,602 @@ +#include +#include +#include +#include +#include + +const static std::string ATTRIBUTES_KEY{ "_attributes" }; +const static std::string VALUE_KEY{ "_value" }; +const static std::string EXTRA_KEY{ "_extra" }; +const static std::string ONEOF_KEY{ "_oneof" }; +const static std::string FOLLOW_KEY{ "follow" }; + +namespace tesseract_common +{ +namespace property_type +{ +std::string createList(std::string_view type, std::size_t length) +{ + if (length == 0) + return std::string(type) + "[]"; + + return std::string(type) + "[" + std::to_string(length) + "]"; +} + +std::string createMap(std::string_view key, std::string_view type) +{ + return "{" + std::string(key) + "," + std::string(type) + "}"; +} +std::string createMap(std::string_view type) { return createMap(STRING, type); } +} // namespace property_type + +PropertyTree::PropertyTree(const PropertyTree& other) + : value_(YAML::Clone(other.value_)) + , follow_(YAML::Clone(other.follow_)) + , children_(other.children_) + , keys_(other.keys_) + , validators_(other.validators_) +{ + if (other.oneof_ != nullptr) + oneof_ = std::make_unique(*other.oneof_); + + // Deep-clone all attributes + for (auto const& [k, node] : other.attributes_) + attributes_[k] = YAML::Clone(node); +} + +PropertyTree& PropertyTree::operator=(const PropertyTree& other) +{ + if (this == &other) + return *this; + + // Clone the YAML values + value_ = YAML::Clone(other.value_); + follow_ = YAML::Clone(other.follow_); + + // Copy and clone attributes + attributes_.clear(); + for (const auto& [k, node] : other.attributes_) + attributes_[k] = YAML::Clone(node); + + // Copy children, keys and validators + children_ = other.children_; + keys_ = other.keys_; + validators_ = other.validators_; + + // Copy oneOf + if (other.oneof_ != nullptr) + oneof_ = std::make_unique(*other.oneof_); + + return *this; +} + +void PropertyTree::mergeConfig(const YAML::Node& config, bool allow_extra_properties) +{ + // Handle oneOf nodes up front + auto t = getAttribute(property_attribute::TYPE); + if (t.has_value() && t->as() == property_type::ONEOF) + { + if (!config || !config.IsMap()) + std::throw_with_nested(std::runtime_error("oneOf schema expects a YAML map")); + + // Find exactly one branch whose schema-keys all appear in config + std::string chosen; + for (const auto& [branch_name, branch_schema] : children_) + { + bool matches = true; + for (const auto& key : branch_schema.keys()) + { + if (branch_schema.at(key).isRequired() && !config[key]) + { + matches = false; + break; + } + } + + if (matches) + { + if (!chosen.empty()) + std::throw_with_nested(std::runtime_error("oneOf: multiple branches match")); + + chosen = branch_name; + } + } + + if (chosen.empty()) + std::throw_with_nested(std::runtime_error("oneOf: no branch matches the provided keys")); + + // Store schema + oneof_ = std::make_unique(*this); + + // Flatten: replace this node's children with the chosen branch's children + PropertyTree schema_copy = children_.at(chosen); + *this = schema_copy; + + // Now call merge again + mergeConfig(config, allow_extra_properties); + return; + } + + // Leaf-schema override for both maps and sequences: + // If this schema node has no children, but the user provided + // either a map or a sequence, just store it wholesale. + if (children_.empty() && config && (config.IsMap() || config.IsSequence())) + { + value_ = config; + return; + } + + // Apply default if no config & not required + auto def_it = attributes_.find(std::string(property_attribute::DEFAULT)); + bool required = isRequired(); + if ((!config || config.IsNull()) && def_it != attributes_.end() && !required) + value_ = YAML::Clone(def_it->second); + + // Scalar or (now only non‐leaf) sequence override + if (config && config.IsScalar()) + value_ = config; + + // Map: recurse into declared children + if (config && config.IsMap()) + { + // Fill declared children + for (auto& [key, child_schema] : children_) + { + auto sub = config[key]; + child_schema.mergeConfig(sub, allow_extra_properties); + } + + // Handle extras + if (!allow_extra_properties) + { + for (auto it = config.begin(); it != config.end(); ++it) + { + const auto& key = it->first.as(); + if (children_.count(key) == 0) + { + auto& extra_node = (*this)[key]; + extra_node.setAttribute(EXTRA_KEY, YAML::Node(true)); + extra_node.mergeConfig(it->second, allow_extra_properties); + } + } + } + } + + // Sequence (non-leaf): apply wildcard or numeric schema + else if (config && config.IsSequence()) + { + PropertyTree elem_schema; + auto w = children_.find("*"); + if (w != children_.end()) + elem_schema = w->second; + + children_.clear(); + keys_.clear(); + int idx = 0; + for (const auto& elt : config) + { + std::string key = std::to_string(idx++); + keys_.push_back(key); + children_[key] = elem_schema; + children_[key].mergeConfig(elt, allow_extra_properties); + } + } + // Otherwise leave value_ (possibly set by default) as-is. +} + +void PropertyTree::validate(bool allow_extra_properties) const +{ + // check if it is an extra property not found in schema + if (!allow_extra_properties && hasAttribute(EXTRA_KEY)) + std::throw_with_nested(std::runtime_error("Property does not exist in schema")); + + // recurse children + for (const auto& kv : children_) + { + try + { + kv.second.validate(); + } + catch (...) + { + std::throw_with_nested(std::runtime_error("Validation failed for property: " + kv.first)); + } + } + + // if not required and null skip validators + if (!isRequired() && isNull()) + return; + + // run custom validators + for (const auto& vfn : validators_) + vfn(*this); +} + +void PropertyTree::addValidator(ValidatorFn fn) { validators_.push_back(std::move(fn)); } + +PropertyTree& PropertyTree::operator[](std::string_view key) +{ + auto k = std::string(key); + auto it = children_.find(k); + if (it == children_.end()) + { + // first time insertion + keys_.push_back(k); + // default-construct in map + it = children_.emplace(std::move(k), PropertyTree{}).first; + } + return it->second; +} + +PropertyTree& PropertyTree::at(std::string_view key) { return children_.at(std::string(key)); } +const PropertyTree& PropertyTree::at(std::string_view key) const { return children_.at(std::string(key)); } + +const PropertyTree* PropertyTree::find(std::string_view key) const +{ + auto it = children_.find(std::string(key)); + return it != children_.end() ? &it->second : nullptr; +} + +void PropertyTree::setValue(const YAML::Node& v) { value_ = v; } +const YAML::Node& PropertyTree::getValue() const { return value_; } + +bool PropertyTree::isNull() const { return value_.IsNull(); } + +bool PropertyTree::isContainer() const { return !children_.empty(); } + +std::vector PropertyTree::keys() const { return keys_; } + +void PropertyTree::setAttribute(std::string_view name, const YAML::Node& attr) +{ + attributes_[std::string(name)] = attr; + + if (name == property_attribute::REQUIRED) + validators_.emplace_back(validateRequired); + + if (name == property_attribute::ENUM) + validators_.emplace_back(validateEnum); + + if (name == property_attribute::TYPE) + { + const auto str_type = attr.as(); + + std::optional> is_sequence = isSequenceType(str_type); + if (is_sequence.has_value()) + validators_.emplace_back( + [length = is_sequence.value().second](const PropertyTree& node) { validateSequence(node, length); }); + + std::optional> is_map = isMapType(str_type); + if (is_map.has_value()) + validators_.emplace_back(validateMap); + + if (str_type == property_type::CONTAINER) + validators_.emplace_back(validateContainer); + else if (str_type == property_type::STRING) + validators_.emplace_back(validateTypeCast); + else if (str_type == property_type::BOOL) + validators_.emplace_back(validateTypeCast); + else if (str_type == property_type::CHAR) + validators_.emplace_back(validateTypeCast); + else if (str_type == property_type::FLOAT) + validators_.emplace_back(validateTypeCastWithRange); + else if (str_type == property_type::DOUBLE) + validators_.emplace_back(validateTypeCastWithRange); + else if (str_type == property_type::INT) + validators_.emplace_back(validateTypeCastWithRange); + else if (str_type == property_type::UNSIGNED_INT) + validators_.emplace_back(validateTypeCastWithRange); + else if (str_type == property_type::LONG_INT) + validators_.emplace_back(validateTypeCastWithRange); + else if (str_type == property_type::LONG_UNSIGNED_INT) + validators_.emplace_back(validateTypeCastWithRange); + else if (str_type == property_type::EIGEN_ISOMETRY_3D) + validators_.emplace_back(validateTypeCast); + // else if (str_type == property_type::EIGEN_MATRIX_2D) + // validators_.emplace_back(validateTypeCast); + else if (str_type == property_type::EIGEN_VECTOR_XD) + { + validators_.emplace_back([](const PropertyTree& node) { validateSequence(node); }); + validators_.emplace_back(validateTypeCast); + } + // else if (str_type == property_type::EIGEN_MATRIX_2D) + // validators_.emplace_back(validateTypeCast); + else if (str_type == property_type::EIGEN_VECTOR_2D) + { + validators_.emplace_back([](const PropertyTree& node) { validateSequence(node, 2); }); + validators_.emplace_back(validateTypeCast); + } + // else if (str_type == property_type::EIGEN_MATRIX_3D) + // validators_.emplace_back(validateTypeCast); + else if (str_type == property_type::EIGEN_VECTOR_3D) + { + validators_.emplace_back([](const PropertyTree& node) { validateSequence(node, 3); }); + validators_.emplace_back(validateTypeCast); + } + } +} + +void PropertyTree::setAttribute(std::string_view name, std::string_view attr) +{ + setAttribute(name, YAML::Node(std::string(attr))); +} + +void PropertyTree::setAttribute(std::string_view name, const char* attr) +{ + setAttribute(name, YAML::Node(std::string(attr))); +} + +void PropertyTree::setAttribute(std::string_view name, bool attr) { setAttribute(name, YAML::Node(attr)); } + +void PropertyTree::setAttribute(std::string_view name, int attr) { setAttribute(name, YAML::Node(attr)); } + +void PropertyTree::setAttribute(std::string_view name, double attr) { setAttribute(name, YAML::Node(attr)); } + +void PropertyTree::setAttribute(std::string_view name, const std::vector& attr) +{ + setAttribute(name, YAML::Node(attr)); +} + +bool PropertyTree::hasAttribute(std::string_view name) const +{ + auto it = attributes_.find(std::string(name)); + return (it != attributes_.end() && it->second && !it->second.IsNull()); +} + +std::optional PropertyTree::getAttribute(std::string_view name) const +{ + auto it = attributes_.find(std::string(name)); + if (it != attributes_.end()) + return it->second; + return std::nullopt; +} + +std::vector PropertyTree::getAttributeKeys() const +{ + std::vector res; + res.reserve(attributes_.size()); + for (const auto& pair : attributes_) + res.push_back(pair.first); + return res; +} + +bool PropertyTree::isRequired() const +{ + std::optional required = getAttribute(property_attribute::REQUIRED); + if (!required.has_value()) + return false; + + return required.value().as(); +} + +PropertyTree PropertyTree::fromYAML(const YAML::Node& node) +{ + // Handle 'follow' directive: load external YAML or schema file + if (node.IsMap() && node[FOLLOW_KEY] && node[FOLLOW_KEY].IsScalar()) + { + if (node.size() > 1) + std::throw_with_nested(std::runtime_error("'follow' cannot be mixed with other entries")); + + try + { + auto key = node[FOLLOW_KEY].as(); + auto registry = SchemaRegistry::instance(); + return registry->contains(key) ? registry->get(key) : SchemaRegistry::loadFile(key); + } + catch (const std::exception& e) + { + std::throw_with_nested(e); + } + } + + PropertyTree tree; + tree.value_ = node; + if (node.IsMap()) + { + // extract attributes if it exists + if (node[ATTRIBUTES_KEY] && node[ATTRIBUTES_KEY].IsMap()) + { + for (const auto& it : node[ATTRIBUTES_KEY]) + { + const auto key = it.first.as(); + tree.attributes_[key] = it.second; + } + } + + // extract oneof if it exist + if (node[ONEOF_KEY] && node[ONEOF_KEY].IsMap()) + tree.oneof_ = std::make_unique(fromYAML(node[ONEOF_KEY])); + + // extract children + for (const auto& it : node) + { + const auto key = it.first.as(); + if (key == ATTRIBUTES_KEY || key == ONEOF_KEY) + continue; + + tree.children_[key] = fromYAML(it.second); + } + } + else if (node.IsSequence()) + { + int idx = 0; + for (const auto& it : node) + tree.children_[std::to_string(idx++)] = fromYAML(it); + } + return tree; +} + +YAML::Node PropertyTree::toYAML(bool exclude_attributes) const +{ + // pure leaf without attributes or children: emit scalar/sequence directly + if (attributes_.empty() && children_.empty()) + return value_; + + // pure leaf with attributes excluded and no children: emit scalar/sequence directly + if (exclude_attributes && children_.empty()) + return value_; + + // Always emit a mapping if attributes exist or children exist + YAML::Node node(YAML::NodeType::Map); + // emit attributes first + if (!exclude_attributes && !attributes_.empty()) + { + YAML::Node attr_node(YAML::NodeType::Map); + for (const auto& pair : attributes_) + attr_node[pair.first] = pair.second; + + node[ATTRIBUTES_KEY] = attr_node; + } + + if (keys_.size() != children_.size()) + std::throw_with_nested(std::runtime_error("PropertyTree, keys_ and children_ are not the same size")); + + // emit children + for (const auto& key : keys_) + { + const PropertyTree& child = children_.at(key); + // If the property is not required and is null then skip when excluding attributes + if (exclude_attributes && !child.isRequired() && child.isNull()) + continue; + + node[key] = child.toYAML(exclude_attributes); + } + + // emit oneof + if (!exclude_attributes && oneof_ != nullptr) + node[ONEOF_KEY] = oneof_->toYAML(exclude_attributes); + + // if leaf (no children) but value present, emit under 'value' + if (children_.empty() && value_) + node[VALUE_KEY] = value_; + + return node; +} + +PropertyTree::operator bool() const noexcept { return (!children_.empty() || !value_.IsNull()); } + +std::optional> isSequenceType(std::string_view type) +{ + static const std::regex re(R"(^([^\[\]]+)\[(\d*)\]$)"); + std::string s{ type }; + + std::smatch m; + if (std::regex_match(s, m, re)) + { + std::string base_type = m[1].str(); + std::string length_str = m[2].str(); + std::size_t length{ 0 }; + if (!length_str.empty()) + { + if (!toNumeric(length_str, length)) + std::throw_with_nested(std::runtime_error("Invalid fixed size sequence definition")); + } + + return std::make_pair(base_type, length); + } + + return std::nullopt; +} + +std::optional> isMapType(std::string_view type) +{ + static const std::regex re(R"(^\{([^,]+),([^}]+)\}$)"); + std::string s{ type }; + std::smatch m; + + // m[0] is the full match, m[1] is the first capture, m[2] the second + if (std::regex_match(s, m, re)) + return std::make_pair(m[1].str(), m[2].str()); + + return std::nullopt; +} + +void validateRequired(const PropertyTree& node) +{ + auto req_attr = node.getAttribute(property_attribute::REQUIRED); + if (req_attr && req_attr->as()) + { + // if leaf node with no value or null + if (!node.isContainer() && node.isNull()) + std::throw_with_nested(std::runtime_error("Required property missing or null")); + } +} + +void validateEnum(const PropertyTree& node) +{ + auto enum_attr = node.getAttribute(property_attribute::ENUM); + if (enum_attr.has_value() && enum_attr->IsSequence()) + { + const auto val = node.getValue().as(); + for (const auto& v : enum_attr.value()) + { + if (v.as() == val) + return; + } + std::throw_with_nested(std::runtime_error("Property value '" + val + "' not in enum list")); + } +} + +void validateMap(const PropertyTree& node) +{ + if (!node.getValue().IsMap()) + std::throw_with_nested(std::runtime_error("Property value is not of type YAML::NodeType::Map")); +} + +void validateSequence(const PropertyTree& node, std::size_t length) +{ + if (!node.getValue().IsSequence()) + std::throw_with_nested(std::runtime_error("Property value is not of type YAML::NodeType::Sequence")); + + if (length == 0) + return; + + if (node.getValue().size() != length) + std::throw_with_nested(std::runtime_error("Property value YAML::NodeType::Sequence is not correct length")); +} + +void validateContainer(const PropertyTree& node) +{ + if (!node.isContainer()) + std::throw_with_nested(std::runtime_error("Property is not a container")); + + if (!node.isNull()) + std::throw_with_nested(std::runtime_error("Property is a container but value is not null")); +} + +void validateCustomType(const PropertyTree& node) +{ + const auto type_attr = node.getAttribute(property_attribute::TYPE); + if (!type_attr.has_value()) + std::throw_with_nested(std::runtime_error("Custom type validator was added buy type attribute does not exist")); + + const auto type_str = type_attr.value().as(); + std::optional> is_sequence = isSequenceType(type_str); + + auto registry = SchemaRegistry::instance(); + if (!is_sequence.has_value()) + { + if (!registry->contains(type_str)) + std::throw_with_nested(std::runtime_error("No scheme registry entry found for key: " + type_str)); + + PropertyTree schema = registry->get(type_str); + schema.mergeConfig(node.getValue()); + schema.validate(); + } + else + { + if (!registry->contains(is_sequence.value().first)) + std::throw_with_nested( + std::runtime_error("No scheme registry entry found for key: " + is_sequence.value().first)); + + const YAML::Node& sequence = node.getValue(); + PropertyTree schema = registry->get(is_sequence.value().first); + for (auto it = sequence.begin(); it != sequence.end(); ++it) + { + PropertyTree copy_schema(schema); + copy_schema.mergeConfig(*it); + copy_schema.validate(); + } + } +} + +} // namespace tesseract_common diff --git a/tesseract_common/src/property_tree_demo.cpp b/tesseract_common/src/property_tree_demo.cpp new file mode 100644 index 00000000000..32a6b614a0e --- /dev/null +++ b/tesseract_common/src/property_tree_demo.cpp @@ -0,0 +1,459 @@ +#include +#include +#include +#include +#include +#include + +using namespace tesseract_common; + +/// Build a detailed schema for 'config', attaching free-function validators. +PropertyTree buildConfigSchema() +{ + PropertyTree schema; + auto& cfg = schema["config"]; + cfg.setAttribute(property_attribute::TYPE, property_type::CONTAINER); + cfg.setAttribute(property_attribute::DOC, "Main config for plugin"); + cfg.setAttribute(property_attribute::REQUIRED, true); + std::map return_options; + return_options[0] = "Error"; + return_options[1] = "Successful"; + cfg.setAttribute("return_options", YAML::Node(return_options)); + + // conditional + { + auto& prop = cfg["conditional"]; + prop.setAttribute(property_attribute::TYPE, property_type::BOOL); + prop.setAttribute(property_attribute::DEFAULT, true); + prop.setAttribute(property_attribute::DOC, "Enable conditional execution"); + prop.setAttribute(property_attribute::REQUIRED, true); + } + // inputs + { + auto& inputs = cfg["inputs"]; + inputs.setAttribute(property_attribute::TYPE, property_type::CONTAINER); + inputs.setAttribute(property_attribute::DOC, "Input sources"); + inputs.setAttribute(property_attribute::REQUIRED, true); + // program + { + auto& prop = inputs["program"]; + prop.setAttribute(property_attribute::TYPE, property_type::STRING); + prop.setAttribute(property_attribute::DOC, "The composite instruction"); + prop.setAttribute(property_attribute::REQUIRED, true); + } + // environment + { + auto& prop = inputs["environment"]; + // env.setAttribute("enum", YAML::Load(R"(["dev","stag","prod"])")); + prop.setAttribute(property_attribute::TYPE, property_type::STRING); + prop.setAttribute(property_attribute::DOC, "The tesseract environment"); + prop.setAttribute(property_attribute::REQUIRED, true); + } + // profiles + { + auto& prop = inputs["profiles"]; + prop.setAttribute(property_attribute::TYPE, property_type::STRING); + prop.setAttribute(property_attribute::DOC, "The tesseract profiles"); + prop.setAttribute(property_attribute::REQUIRED, true); + // prof.setAttribute("enum", YAML::Load(R"(["A","B"])")); + // prof.addValidator(validateEnum); + // prof.addValidator([](auto const& node, auto& errs, const auto& path){ + // auto s = node[Value().template as(); + // if (s.find(',') == std::string::npos) { + // errs.push_back(path + ": profiles should be comma-separated"); + // return false; + // } + // return true; + // }); + } + } + // outputs + { + auto& outputs = cfg["outputs"]; + outputs.setAttribute(property_attribute::TYPE, property_type::CONTAINER); + outputs.setAttribute(property_attribute::DOC, "Output sources"); + outputs.setAttribute(property_attribute::REQUIRED, true); + { + auto& prop = outputs["program"]; + prop.setAttribute(property_attribute::TYPE, property_type::STRING); + prop.setAttribute(property_attribute::REQUIRED, true); + } + } + // format_result_as_input + { + auto& prop = cfg["format_result_as_input"]; + prop.setAttribute(property_attribute::TYPE, property_type::BOOL); + prop.setAttribute(property_attribute::DEFAULT, false); + } + return schema; +} + +tesseract_common::PropertyTree getTaskComposerGraphSchema() +{ + using namespace tesseract_common; + PropertyTree schema; + schema.setAttribute(property_attribute::TYPE, property_type::CONTAINER); + schema.setAttribute(property_attribute::DOC, "TaskComposerGraph"); + { + auto& prop = schema["class"]; + prop.setAttribute(property_attribute::TYPE, property_type::STRING); + prop.setAttribute(property_attribute::DOC, "The class factory name"); + prop.setAttribute(property_attribute::REQUIRED, true); + } + + auto& config_schema = schema["config"]; + config_schema.setAttribute(property_attribute::TYPE, property_type::CONTAINER); + config_schema.setAttribute(property_attribute::REQUIRED, true); + { + auto& prop = config_schema["conditional"]; + prop.setAttribute(property_attribute::TYPE, property_type::BOOL); + prop.setAttribute(property_attribute::DEFAULT, true); + prop.setAttribute(property_attribute::DOC, "Enable conditional execution"); + prop.setAttribute(property_attribute::REQUIRED, true); + } + + { + auto& inputs = config_schema["inputs"]; + inputs.setAttribute(property_attribute::TYPE, property_type::CONTAINER); + inputs.setAttribute(property_attribute::DOC, "Input sources"); + } + + { + auto& outputs = config_schema["outputs"]; + outputs.setAttribute(property_attribute::TYPE, property_type::CONTAINER); + outputs.setAttribute(property_attribute::DOC, "Output sources"); + } + + { + auto& prop = config_schema["nodes"]; + prop.setAttribute(property_attribute::TYPE, "TaskComposerGraphNode{}"); + prop.setAttribute(property_attribute::DOC, "Map of all task nodes"); + prop.setAttribute(property_attribute::REQUIRED, true); + } + + { + auto& prop = config_schema["edges"]; + prop.setAttribute(property_attribute::TYPE, "TaskComposerGraphEdge[]"); + prop.setAttribute(property_attribute::DOC, "List of graph edges"); + prop.setAttribute(property_attribute::REQUIRED, true); + prop.addValidator(validateCustomType); + } + + { + auto& prop = config_schema["terminals"]; + prop.setAttribute(property_attribute::TYPE, property_type::createList(property_type::STRING)); + prop.setAttribute(property_attribute::DOC, "List of terminal tasks"); + prop.setAttribute(property_attribute::REQUIRED, true); + prop.addValidator(validateTypeCast>); + } + + return schema; +} + +tesseract_common::PropertyTree getTaskComposerGraphEdgeSchema() +{ + using namespace tesseract_common; + PropertyTree schema; + schema.setAttribute(property_attribute::TYPE, property_type::CONTAINER); + schema.setAttribute(property_attribute::DOC, "TaskComposerGraphEdge"); + { + auto& prop = schema["source"]; + prop.setAttribute(property_attribute::TYPE, property_type::STRING); + prop.setAttribute(property_attribute::DOC, "The source task name"); + prop.setAttribute(property_attribute::REQUIRED, true); + } + + { + auto& prop = schema["destinations"]; + prop.setAttribute(property_attribute::TYPE, property_type::createList(property_type::STRING)); + prop.setAttribute(property_attribute::DOC, "The list of destination task name"); + prop.setAttribute(property_attribute::REQUIRED, true); + prop.addValidator(validateTypeCast>); + } + + return schema; +} + +tesseract_common::PropertyTree getTaskComposerRasterOnlySchema() +{ + using namespace tesseract_common; + PropertyTree schema; + schema.setAttribute(property_attribute::TYPE, property_type::CONTAINER); + schema.setAttribute(property_attribute::DOC, "TaskComposerGraph"); + std::map return_options; + return_options[0] = "Error"; + return_options[1] = "Successful"; + schema.setAttribute("return_options", YAML::Node(return_options)); + { + auto& prop = schema["class"]; + prop.setAttribute(property_attribute::TYPE, property_type::STRING); + prop.setAttribute(property_attribute::DOC, "The class factory name"); + prop.setAttribute(property_attribute::REQUIRED, true); + } + + auto& config_schema = schema["config"]; + config_schema.setAttribute(property_attribute::TYPE, property_type::CONTAINER); + config_schema.setAttribute(property_attribute::REQUIRED, true); + { + auto& prop = config_schema["conditional"]; + prop.setAttribute(property_attribute::TYPE, property_type::BOOL); + prop.setAttribute(property_attribute::DEFAULT, true); + prop.setAttribute(property_attribute::DOC, "Enable conditional execution"); + prop.setAttribute(property_attribute::REQUIRED, true); + } + + { + auto& inputs = config_schema["inputs"]; + inputs.setAttribute(property_attribute::TYPE, property_type::CONTAINER); + inputs.setAttribute(property_attribute::DOC, "Input sources"); + + // program + { + auto& prop = inputs["program"]; + prop.setAttribute(property_attribute::TYPE, property_type::STRING); + prop.setAttribute(property_attribute::DOC, "The composite instruction"); + prop.setAttribute(property_attribute::REQUIRED, true); + } + // environment + { + auto& prop = inputs["environment"]; + prop.setAttribute(property_attribute::TYPE, property_type::STRING); + prop.setAttribute(property_attribute::DOC, "The tesseract environment"); + prop.setAttribute(property_attribute::REQUIRED, true); + } + } + + { + auto& outputs = config_schema["outputs"]; + outputs.setAttribute(property_attribute::TYPE, property_type::CONTAINER); + outputs.setAttribute(property_attribute::DOC, "Output sources"); + // program + { + auto& prop = outputs["program"]; + prop.setAttribute(property_attribute::TYPE, property_type::STRING); + prop.setAttribute(property_attribute::DOC, "The composite instruction"); + prop.setAttribute(property_attribute::REQUIRED, true); + } + } + + { + auto& raster = config_schema["raster"]; + raster.setAttribute(property_attribute::TYPE, property_type::CONTAINER); + raster.setAttribute(property_attribute::DOC, "The raster task"); + raster.setAttribute(property_attribute::REQUIRED, true); + { + auto& prop = raster["task"]; + prop.setAttribute(property_attribute::TYPE, property_type::STRING); + prop.setAttribute(property_attribute::DOC, "The task name"); + prop.setAttribute(property_attribute::REQUIRED, true); + } + auto& raster_config = raster["config"]; + raster_config.setAttribute(property_attribute::TYPE, property_type::CONTAINER); + raster_config.setAttribute(property_attribute::REQUIRED, true); + { + auto& prop = raster_config["abort_terminal"]; + prop.setAttribute(property_attribute::TYPE, property_type::INT); + prop.setAttribute(property_attribute::MINIMUM, 0); + prop.setAttribute(property_attribute::DOC, "The abort terminal"); + } + { + auto& prop = raster_config["remapping"]; + prop.setAttribute(property_attribute::TYPE, property_type::createMap(property_type::STRING)); + prop.setAttribute(property_attribute::DOC, "The remapping of input and output keys"); + } + { + auto& prop = raster_config["indexing"]; + prop.setAttribute(property_attribute::TYPE, property_type::createList(property_type::STRING)); + prop.setAttribute(property_attribute::DOC, "The input and output keys to index"); + } + } + + { + auto& transition = config_schema["transition"]; + transition.setAttribute(property_attribute::TYPE, property_type::CONTAINER); + transition.setAttribute(property_attribute::DOC, "The transition task"); + transition.setAttribute(property_attribute::REQUIRED, true); + { + auto& prop = transition["task"]; + prop.setAttribute(property_attribute::TYPE, property_type::STRING); + prop.setAttribute(property_attribute::DOC, "The task name"); + prop.setAttribute(property_attribute::REQUIRED, true); + } + auto& transition_config = transition["config"]; + transition_config.setAttribute(property_attribute::TYPE, property_type::CONTAINER); + transition_config.setAttribute(property_attribute::REQUIRED, true); + { + auto& prop = transition_config["abort_terminal"]; + prop.setAttribute(property_attribute::TYPE, property_type::INT); + prop.setAttribute(property_attribute::MINIMUM, 0); + prop.setAttribute(property_attribute::DOC, "The abort terminal"); + } + { + auto& prop = transition_config["remapping"]; + prop.setAttribute(property_attribute::TYPE, property_type::createMap(property_type::STRING)); + prop.setAttribute(property_attribute::DOC, "The remapping of input and output keys"); + } + { + auto& prop = transition_config["indexing"]; + prop.setAttribute(property_attribute::TYPE, property_type::createList(property_type::STRING)); + prop.setAttribute(property_attribute::DOC, "The input and output keys to index"); + } + } + + return schema; +} + +TESSERACT_REGISTER_SCHEMA(TaskComposerGraphEdge, getTaskComposerGraphEdgeSchema) + +const std::string str = R"(config: + conditional: true + inputs: + program: input_data + environment: environment + profiles: profiles + outputs: + program: output_data + format_result_as_input: false)"; + +const std::string str2 = R"(config: + conditional: true + inputs: + program: input_data + environment: environment + profiles: profiles + outputs: + program: output_data)"; + +const std::string str3 = R"(config: + conditional: true + inputs: + program: input_data + environment: environment + profiles: profiles + outputs: + programs: output_data)"; + +const std::string str4 = R"(config: + conditional: true + should_not_exist: true + inputs: + program: input_data + environment: environment + profiles: profiles + outputs: + program: output_data)"; + +const std::string graph_str = R"( +class: PipelineTaskFactory +config: + conditional: true + nodes: + StartTask: + class: StartTaskFactory + config: + conditional: false + DoneTask: + class: DoneTaskFactory + config: + conditional: false + edges: + - source: StartTask + destinations: [DoneTask] + terminals: [DoneTask])"; + +std::string raster_only_str = R"( +class: RasterOnlyTaskFactory +config: + conditional: true + inputs: + program: input_data + environment: environment + outputs: + program: output_data + raster: + task: CartesianPipeline + config: + indexing: [input_data, output_data] + transition: + task: CartesianPipeline + config: + indexing: [input_data, output_data])"; + +int testBasic() +{ + // Parse schema from external file (config_schema.yaml) + PropertyTree schema = buildConfigSchema(); + + // Merge schema metadata into config tree + schema.mergeConfig(YAML::Load(str4)); + std::cout << "Exclude attrubutes:\n" << schema.toYAML() << "\n\n"; + std::cout << "Include attrubutes:\n" << schema.toYAML(false) << "\n\n"; + + try + { + schema.validate(); + } + catch (const std::exception& e) + { + tesseract_common::printNestedException(e); + return 1; + } + + bool cond = schema["config"]["conditional"].getValue().as(); + std::cout << "conditional = " << std::boolalpha << cond << "\n"; + return 0; +} + +int testCustomType() +{ + // Parse schema from external file (config_schema.yaml) + PropertyTree schema = getTaskComposerGraphSchema(); + std::cout << "Schema:\n" << schema.toYAML(false) << "\n\n"; + + // Merge schema metadata into config tree + schema.mergeConfig(YAML::Load(graph_str)); + std::cout << "Exclude attrubutes:\n" << schema.toYAML() << "\n\n"; + std::cout << "Include attrubutes:\n" << schema.toYAML(false) << "\n\n"; + + try + { + schema.validate(); + } + catch (const std::exception& e) + { + tesseract_common::printNestedException(e); + return 1; + } + + return 0; +} + +int testRasterOnlyType() +{ + // Parse schema from external file (config_schema.yaml) + PropertyTree schema = getTaskComposerRasterOnlySchema(); + std::cout << "Schema:\n" << schema.toYAML(false) << "\n\n"; + + // Merge schema metadata into config tree + schema.mergeConfig(YAML::Load(raster_only_str)); + std::cout << "Exclude attrubutes:\n" << schema.toYAML() << "\n\n"; + std::cout << "Include attrubutes:\n" << schema.toYAML(false) << "\n\n"; + + try + { + schema.validate(); + } + catch (const std::exception& e) + { + tesseract_common::printNestedException(e); + return 1; + } + + return 0; +} + +int main() +{ + // return testCustomType(); + return testRasterOnlyType(); +} diff --git a/tesseract_common/src/schema_registration.cpp b/tesseract_common/src/schema_registration.cpp new file mode 100644 index 00000000000..9f3950dbe57 --- /dev/null +++ b/tesseract_common/src/schema_registration.cpp @@ -0,0 +1,19 @@ +#include +#include +#include + +namespace tesseract_common +{ +SchemaRegistrar::SchemaRegistrar(const std::string& key, const std::string& path) +{ + auto reg = SchemaRegistry::instance(); + reg->registerSchemaFromFile(key, path); +} + +SchemaRegistrar::SchemaRegistrar(const std::string& key, const std::function& fn) +{ + auto reg = SchemaRegistry::instance(); + PropertyTree tree = fn(); + reg->registerSchema(key, tree); +} +} // namespace tesseract_common diff --git a/tesseract_common/src/schema_registry.cpp b/tesseract_common/src/schema_registry.cpp new file mode 100644 index 00000000000..1b13934f4ad --- /dev/null +++ b/tesseract_common/src/schema_registry.cpp @@ -0,0 +1,72 @@ +#include +#include + +#include + +#include + +namespace tesseract_common +{ +std::shared_ptr SchemaRegistry::instance() +{ + // local statics, not global variables + static std::once_flag flag; + static std::shared_ptr singleton; + + // NOLINTNEXTLINE(cppcoreguidelines-owning-memory) + std::call_once(flag, []() { singleton.reset(new SchemaRegistry()); }); + return singleton; +} + +void SchemaRegistry::registerSchema(const std::string& key, const PropertyTree& schema) +{ + std::lock_guard lock(mutex_); + schemas_[key] = schema; +} + +void SchemaRegistry::registerSchemaFromFile(const std::string& key, const std::string& path) +{ + std::lock_guard lock(mutex_); + paths_[key] = path; + // Parse immediately and store + YAML::Node node = YAML::LoadFile(path); + schemas_[key] = PropertyTree::fromYAML(node); +} + +bool SchemaRegistry::contains(const std::string& key) const +{ + std::lock_guard lock(mutex_); + return schemas_.count(key) > 0 || paths_.count(key) > 0; +} + +const PropertyTree& SchemaRegistry::get(const std::string& key) const +{ + std::lock_guard lock(mutex_); + auto sit = schemas_.find(key); + if (sit != schemas_.end()) + return sit->second; + auto pit = paths_.find(key); + if (pit != paths_.end()) + { + // Lazy-load if path only was registered + YAML::Node node = YAML::LoadFile(pit->second); + schemas_[key] = PropertyTree::fromYAML(node); + return schemas_.at(key); + } + throw std::out_of_range("SchemaRegistry: key not found -> " + key); +} + +PropertyTree SchemaRegistry::loadFile(const std::string& path) +{ + // Allow relative or absolute paths + std::filesystem::path p(path); + if (!p.is_absolute()) + { + // Optionally resolve against current working directory + p = std::filesystem::current_path() / p; + } + YAML::Node node = YAML::LoadFile(p.string()); + return PropertyTree::fromYAML(node); +} + +} // namespace tesseract_common diff --git a/tesseract_common/src/yaml_extensions.cpp b/tesseract_common/src/yaml_extensions.cpp new file mode 100644 index 00000000000..f87e99454df --- /dev/null +++ b/tesseract_common/src/yaml_extensions.cpp @@ -0,0 +1,55 @@ +/** + * @file yaml_extensions.h + * @brief YAML Type conversions + * + * @author Levi Armstrong + * @date September 5, 2021 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2021, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include + +TESSERACT_REGISTER_SCHEMA(Eigen::Isometry3d, YAML::convert::schema) +TESSERACT_REGISTER_SCHEMA(Eigen::VectorXd, YAML::convert::schema) +TESSERACT_REGISTER_SCHEMA(Eigen::Vector2d, YAML::convert::schema) +TESSERACT_REGISTER_SCHEMA(Eigen::Vector3d, YAML::convert::schema) +// TESSERACT_REGISTER_SCHEMA(tesseract_common::PluginInfo, YAML::convert::schema) +// TESSERACT_REGISTER_SCHEMA(tesseract_common::PluginInfoContainer, +// YAML::convert::schema) +TESSERACT_REGISTER_SCHEMA(tesseract_common::KinematicsPluginInfo, + YAML::convert::schema) +TESSERACT_REGISTER_SCHEMA(tesseract_common::ContactManagersPluginInfo, + YAML::convert::schema) +TESSERACT_REGISTER_SCHEMA(tesseract_common::TaskComposerPluginInfo, + YAML::convert::schema) +TESSERACT_REGISTER_SCHEMA(tesseract_common::CalibrationInfo, YAML::convert::schema) +TESSERACT_REGISTER_SCHEMA(tesseract_common::TransformMap, YAML::convert::schema) +TESSERACT_REGISTER_SCHEMA(tesseract_common::Toolpath, YAML::convert::schema) +TESSERACT_REGISTER_SCHEMA(tesseract_common::CollisionMarginPairOverrideType, + YAML::convert::schema) +TESSERACT_REGISTER_SCHEMA(tesseract_common::PairsCollisionMarginData, + YAML::convert::schema) +TESSERACT_REGISTER_SCHEMA(tesseract_common::CollisionMarginPairData, + YAML::convert::schema) +TESSERACT_REGISTER_SCHEMA(tesseract_common::AllowedCollisionEntries, + YAML::convert::schema) +TESSERACT_REGISTER_SCHEMA(tesseract_common::AllowedCollisionMatrix, + YAML::convert::schema) diff --git a/tesseract_common/test/tesseract_common_unit.cpp b/tesseract_common/test/tesseract_common_unit.cpp index ab7d8f78d48..0ae0f9179c6 100644 --- a/tesseract_common/test/tesseract_common_unit.cpp +++ b/tesseract_common/test/tesseract_common_unit.cpp @@ -18,7 +18,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include #include #include #include diff --git a/tesseract_kinematics/core/src/kinematics_plugin_factory.cpp b/tesseract_kinematics/core/src/kinematics_plugin_factory.cpp index a160e14d4c1..4c8bca60e1b 100644 --- a/tesseract_kinematics/core/src/kinematics_plugin_factory.cpp +++ b/tesseract_kinematics/core/src/kinematics_plugin_factory.cpp @@ -34,7 +34,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include #include #include #include diff --git a/tesseract_srdf/src/configs.cpp b/tesseract_srdf/src/configs.cpp index 1d366afccc1..6bcc3168925 100644 --- a/tesseract_srdf/src/configs.cpp +++ b/tesseract_srdf/src/configs.cpp @@ -33,7 +33,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include #include namespace tesseract_srdf diff --git a/tesseract_srdf/src/srdf_model.cpp b/tesseract_srdf/src/srdf_model.cpp index 6944e8ec7c1..548e8428564 100644 --- a/tesseract_srdf/src/srdf_model.cpp +++ b/tesseract_srdf/src/srdf_model.cpp @@ -51,7 +51,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include #include #include #include diff --git a/tesseract_srdf/test/tesseract_srdf_unit.cpp b/tesseract_srdf/test/tesseract_srdf_unit.cpp index f8018d036da..59c72c0bebc 100644 --- a/tesseract_srdf/test/tesseract_srdf_unit.cpp +++ b/tesseract_srdf/test/tesseract_srdf_unit.cpp @@ -11,7 +11,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include #include #include #include diff --git a/tesseract_support/CMakeLists.txt b/tesseract_support/CMakeLists.txt index 50f0aa7b73b..0bfbed2e230 100644 --- a/tesseract_support/CMakeLists.txt +++ b/tesseract_support/CMakeLists.txt @@ -12,7 +12,8 @@ tesseract_variables() # Configure Package configure_package() -foreach(dir urdf meshes) +# Install resources +foreach(dir meshes urdf) install(DIRECTORY ${dir}/ DESTINATION share/${PROJECT_NAME}/${dir}) endforeach()