diff --git a/tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_cast_bvh_manager.h b/tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_cast_bvh_manager.h index 01e5f18f296..35ee466830a 100644 --- a/tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_cast_bvh_manager.h +++ b/tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_cast_bvh_manager.h @@ -125,9 +125,10 @@ class BulletCastBVHManager : public ContinuousContactManager const CollisionMarginData& getCollisionMarginData() const override final; - void setIsContactAllowedFn(IsContactAllowedFn fn) override final; + void + setContactAllowedValidator(std::shared_ptr validator) override final; - IsContactAllowedFn getIsContactAllowedFn() const override final; + std::shared_ptr getContactAllowedValidator() const override final; void contactTest(ContactResultMap& collisions, const ContactRequest& request) override final; diff --git a/tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_cast_simple_manager.h b/tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_cast_simple_manager.h index 1c8a508fa24..e4439debb9f 100644 --- a/tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_cast_simple_manager.h +++ b/tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_cast_simple_manager.h @@ -125,9 +125,10 @@ class BulletCastSimpleManager : public ContinuousContactManager const std::string& name2, double collision_margin) override final; - void setIsContactAllowedFn(IsContactAllowedFn fn) override final; + void + setContactAllowedValidator(std::shared_ptr validator) override final; - IsContactAllowedFn getIsContactAllowedFn() const override final; + std::shared_ptr getContactAllowedValidator() const override final; void contactTest(ContactResultMap& collisions, const ContactRequest& request) override final; diff --git a/tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_discrete_bvh_manager.h b/tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_discrete_bvh_manager.h index a052247c562..8a0507add2c 100644 --- a/tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_discrete_bvh_manager.h +++ b/tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_discrete_bvh_manager.h @@ -114,9 +114,10 @@ class BulletDiscreteBVHManager : public DiscreteContactManager const CollisionMarginData& getCollisionMarginData() const override final; - void setIsContactAllowedFn(IsContactAllowedFn fn) override final; + void + setContactAllowedValidator(std::shared_ptr validator) override final; - IsContactAllowedFn getIsContactAllowedFn() const override final; + std::shared_ptr getContactAllowedValidator() const override final; void contactTest(ContactResultMap& collisions, const ContactRequest& request) override final; diff --git a/tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_discrete_simple_manager.h b/tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_discrete_simple_manager.h index 8650ba1d5ad..9e4bff98ff5 100644 --- a/tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_discrete_simple_manager.h +++ b/tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_discrete_simple_manager.h @@ -114,9 +114,10 @@ class BulletDiscreteSimpleManager : public DiscreteContactManager const CollisionMarginData& getCollisionMarginData() const override final; - void setIsContactAllowedFn(IsContactAllowedFn fn) override final; + void + setContactAllowedValidator(std::shared_ptr validator) override final; - IsContactAllowedFn getIsContactAllowedFn() const override final; + std::shared_ptr getContactAllowedValidator() const override final; void contactTest(ContactResultMap& collisions, const ContactRequest& request) override final; diff --git a/tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_utils.h b/tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_utils.h index f66ebdce970..4b5615581f0 100644 --- a/tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_utils.h +++ b/tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_utils.h @@ -209,11 +209,14 @@ btTransform getLinkTransformFromCOW(const btCollisionObjectWrapper* cow); * @brief This is used to check if a collision check is required between the provided two collision objects * @param cow1 The first collision object * @param cow2 The second collision object - * @param acm The contact allowed function pointer + * @param validator The contact allowed validator * @param verbose Indicate if verbose information should be printed to the terminal * @return True if the two collision objects should be checked for collision, otherwise false */ -bool needsCollisionCheck(const COW& cow1, const COW& cow2, const IsContactAllowedFn& acm, bool verbose = false); +bool needsCollisionCheck(const COW& cow1, + const COW& cow2, + const std::shared_ptr& validator, + bool verbose = false); btScalar addDiscreteSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, diff --git a/tesseract_collision/bullet/src/bullet_cast_bvh_manager.cpp b/tesseract_collision/bullet/src/bullet_cast_bvh_manager.cpp index a69a52e1209..14733be7bfa 100644 --- a/tesseract_collision/bullet/src/bullet_cast_bvh_manager.cpp +++ b/tesseract_collision/bullet/src/bullet_cast_bvh_manager.cpp @@ -39,7 +39,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "tesseract_collision/bullet/bullet_cast_bvh_manager.h" +#include +#include extern btScalar gDbvtMargin; // NOLINT @@ -104,7 +105,7 @@ ContinuousContactManager::UPtr BulletCastBVHManager::clone() const manager->setActiveCollisionObjects(active_); manager->setCollisionMarginData(contact_test_data_.collision_margin_data); - manager->setIsContactAllowedFn(contact_test_data_.fn); + manager->setContactAllowedValidator(contact_test_data_.validator); return manager; } @@ -451,8 +452,16 @@ const CollisionMarginData& BulletCastBVHManager::getCollisionMarginData() const { return contact_test_data_.collision_margin_data; } -void BulletCastBVHManager::setIsContactAllowedFn(IsContactAllowedFn fn) { contact_test_data_.fn = fn; } -IsContactAllowedFn BulletCastBVHManager::getIsContactAllowedFn() const { return contact_test_data_.fn; } +void BulletCastBVHManager::setContactAllowedValidator( + std::shared_ptr validator) +{ + contact_test_data_.validator = std::move(validator); +} +std::shared_ptr +BulletCastBVHManager::getContactAllowedValidator() const +{ + return contact_test_data_.validator; +} void BulletCastBVHManager::contactTest(ContactResultMap& collisions, const ContactRequest& request) { contact_test_data_.res = &collisions; diff --git a/tesseract_collision/bullet/src/bullet_cast_simple_manager.cpp b/tesseract_collision/bullet/src/bullet_cast_simple_manager.cpp index fc2f8e74de9..027efabc36a 100644 --- a/tesseract_collision/bullet/src/bullet_cast_simple_manager.cpp +++ b/tesseract_collision/bullet/src/bullet_cast_simple_manager.cpp @@ -39,7 +39,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "tesseract_collision/bullet/bullet_cast_simple_manager.h" +#include +#include namespace tesseract_collision::tesseract_collision_bullet { @@ -84,7 +85,7 @@ ContinuousContactManager::UPtr BulletCastSimpleManager::clone() const manager->setActiveCollisionObjects(active_); manager->setCollisionMarginData(contact_test_data_.collision_margin_data); - manager->setIsContactAllowedFn(contact_test_data_.fn); + manager->setContactAllowedValidator(contact_test_data_.validator); return manager; } @@ -364,8 +365,16 @@ const CollisionMarginData& BulletCastSimpleManager::getCollisionMarginData() con { return contact_test_data_.collision_margin_data; } -void BulletCastSimpleManager::setIsContactAllowedFn(IsContactAllowedFn fn) { contact_test_data_.fn = fn; } -IsContactAllowedFn BulletCastSimpleManager::getIsContactAllowedFn() const { return contact_test_data_.fn; } +void BulletCastSimpleManager::setContactAllowedValidator( + std::shared_ptr validator) +{ + contact_test_data_.validator = std::move(validator); +} +std::shared_ptr +BulletCastSimpleManager::getContactAllowedValidator() const +{ + return contact_test_data_.validator; +} void BulletCastSimpleManager::contactTest(ContactResultMap& collisions, const ContactRequest& request) { contact_test_data_.res = &collisions; @@ -401,7 +410,7 @@ void BulletCastSimpleManager::contactTest(ContactResultMap& collisions, const Co if (aabb_check) { - bool needs_collision = needsCollisionCheck(*cow1, *cow2, contact_test_data_.fn, false); + bool needs_collision = needsCollisionCheck(*cow1, *cow2, contact_test_data_.validator, false); if (needs_collision) { diff --git a/tesseract_collision/bullet/src/bullet_discrete_bvh_manager.cpp b/tesseract_collision/bullet/src/bullet_discrete_bvh_manager.cpp index 00dd1845136..fe506166348 100644 --- a/tesseract_collision/bullet/src/bullet_discrete_bvh_manager.cpp +++ b/tesseract_collision/bullet/src/bullet_discrete_bvh_manager.cpp @@ -40,6 +40,7 @@ */ #include +#include extern btScalar gDbvtMargin; // NOLINT @@ -100,7 +101,7 @@ DiscreteContactManager::UPtr BulletDiscreteBVHManager::clone() const manager->setActiveCollisionObjects(active_); manager->setCollisionMarginData(contact_test_data_.collision_margin_data); - manager->setIsContactAllowedFn(contact_test_data_.fn); + manager->setContactAllowedValidator(contact_test_data_.validator); return manager; } @@ -272,8 +273,16 @@ const CollisionMarginData& BulletDiscreteBVHManager::getCollisionMarginData() co { return contact_test_data_.collision_margin_data; } -void BulletDiscreteBVHManager::setIsContactAllowedFn(IsContactAllowedFn fn) { contact_test_data_.fn = fn; } -IsContactAllowedFn BulletDiscreteBVHManager::getIsContactAllowedFn() const { return contact_test_data_.fn; } +void BulletDiscreteBVHManager::setContactAllowedValidator( + std::shared_ptr validator) +{ + contact_test_data_.validator = std::move(validator); +} +std::shared_ptr +BulletDiscreteBVHManager::getContactAllowedValidator() const +{ + return contact_test_data_.validator; +} void BulletDiscreteBVHManager::contactTest(ContactResultMap& collisions, const ContactRequest& request) { contact_test_data_.res = &collisions; diff --git a/tesseract_collision/bullet/src/bullet_discrete_simple_manager.cpp b/tesseract_collision/bullet/src/bullet_discrete_simple_manager.cpp index fb3523393d9..7555994ad64 100644 --- a/tesseract_collision/bullet/src/bullet_discrete_simple_manager.cpp +++ b/tesseract_collision/bullet/src/bullet_discrete_simple_manager.cpp @@ -39,7 +39,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "tesseract_collision/bullet/bullet_discrete_simple_manager.h" +#include +#include namespace tesseract_collision::tesseract_collision_bullet { @@ -86,7 +87,7 @@ DiscreteContactManager::UPtr BulletDiscreteSimpleManager::clone() const manager->setActiveCollisionObjects(active_); manager->setCollisionMarginData(contact_test_data_.collision_margin_data); - manager->setIsContactAllowedFn(contact_test_data_.fn); + manager->setContactAllowedValidator(contact_test_data_.validator); return manager; } @@ -250,8 +251,16 @@ const CollisionMarginData& BulletDiscreteSimpleManager::getCollisionMarginData() { return contact_test_data_.collision_margin_data; } -void BulletDiscreteSimpleManager::setIsContactAllowedFn(IsContactAllowedFn fn) { contact_test_data_.fn = fn; } -IsContactAllowedFn BulletDiscreteSimpleManager::getIsContactAllowedFn() const { return contact_test_data_.fn; } +void BulletDiscreteSimpleManager::setContactAllowedValidator( + std::shared_ptr validator) +{ + contact_test_data_.validator = std::move(validator); +} +std::shared_ptr +BulletDiscreteSimpleManager::getContactAllowedValidator() const +{ + return contact_test_data_.validator; +} void BulletDiscreteSimpleManager::contactTest(ContactResultMap& collisions, const ContactRequest& request) { contact_test_data_.res = &collisions; @@ -287,7 +296,7 @@ void BulletDiscreteSimpleManager::contactTest(ContactResultMap& collisions, cons if (aabb_check) { - bool needs_collision = needsCollisionCheck(*cow1, *cow2, contact_test_data_.fn, false); + bool needs_collision = needsCollisionCheck(*cow1, *cow2, contact_test_data_.validator, false); if (needs_collision) { diff --git a/tesseract_collision/bullet/src/bullet_utils.cpp b/tesseract_collision/bullet/src/bullet_utils.cpp index 04dc3ac9d49..4ca68f3da88 100644 --- a/tesseract_collision/bullet/src/bullet_utils.cpp +++ b/tesseract_collision/bullet/src/bullet_utils.cpp @@ -694,11 +694,14 @@ btTransform getLinkTransformFromCOW(const btCollisionObjectWrapper* cow) return cow->getWorldTransform(); } -bool needsCollisionCheck(const COW& cow1, const COW& cow2, const IsContactAllowedFn& acm, bool verbose) +bool needsCollisionCheck(const COW& cow1, + const COW& cow2, + const std::shared_ptr& validator, + bool verbose) { return cow1.m_enabled && cow2.m_enabled && (cow2.m_collisionFilterGroup & cow1.m_collisionFilterMask) && // NOLINT (cow1.m_collisionFilterGroup & cow2.m_collisionFilterMask) && // NOLINT - !isContactAllowed(cow1.getName(), cow2.getName(), acm, verbose); + !isContactAllowed(cow1.getName(), cow2.getName(), validator, verbose); } btScalar addDiscreteSingleResult(btManifoldPoint& cp, @@ -979,7 +982,7 @@ BroadphaseContactResultCallback::BroadphaseContactResultCallback(ContactTestData bool BroadphaseContactResultCallback::needsCollision(const CollisionObjectWrapper* cow0, const CollisionObjectWrapper* cow1) const { - return !collisions_.done && needsCollisionCheck(*cow0, *cow1, collisions_.fn, verbose_); + return !collisions_.done && needsCollisionCheck(*cow0, *cow1, collisions_.validator, verbose_); } DiscreteBroadphaseContactResultCallback::DiscreteBroadphaseContactResultCallback(ContactTestData& collisions, @@ -1182,7 +1185,7 @@ bool DiscreteCollisionCollector::needsCollision(btBroadphaseProxy* proxy0) const { return !collisions_.done && needsCollisionCheck( - *cow_, *(static_cast(proxy0->m_clientObject)), collisions_.fn, verbose_); + *cow_, *(static_cast(proxy0->m_clientObject)), collisions_.validator, verbose_); } CastCollisionCollector::CastCollisionCollector(ContactTestData& collisions, @@ -1215,7 +1218,7 @@ bool CastCollisionCollector::needsCollision(btBroadphaseProxy* proxy0) const { return !collisions_.done && needsCollisionCheck( - *cow_, *(static_cast(proxy0->m_clientObject)), collisions_.fn, verbose_); + *cow_, *(static_cast(proxy0->m_clientObject)), collisions_.validator, verbose_); } COW::Ptr makeCastCollisionObject(const COW::Ptr& cow) diff --git a/tesseract_collision/core/CMakeLists.txt b/tesseract_collision/core/CMakeLists.txt index d7c31651d1e..d3f6e11d00a 100644 --- a/tesseract_collision/core/CMakeLists.txt +++ b/tesseract_collision/core/CMakeLists.txt @@ -7,7 +7,8 @@ add_library( src/discrete_contact_manager.cpp src/serialization.cpp src/types.cpp - src/utils.cpp) + src/utils.cpp + src/contact_result_validator.cpp) target_link_libraries( ${PROJECT_NAME}_core PUBLIC Eigen3::Eigen diff --git a/tesseract_collision/core/include/tesseract_collision/core/common.h b/tesseract_collision/core/include/tesseract_collision/core/common.h index 9db6ea922a1..37920c58886 100644 --- a/tesseract_collision/core/include/tesseract_collision/core/common.h +++ b/tesseract_collision/core/include/tesseract_collision/core/common.h @@ -33,6 +33,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include namespace tesseract_collision { @@ -43,12 +44,13 @@ using ObjectPairKey = std::pair; * @todo Should this also filter out links without geometry? * @param active_links The active link names * @param static_links The static link names - * @param acm The is contact allowed function + * @param validator The is contact allowed validator * @return A vector of collision object pairs */ -std::vector getCollisionObjectPairs(const std::vector& active_links, - const std::vector& static_links, - const IsContactAllowedFn& acm = nullptr); +std::vector +getCollisionObjectPairs(const std::vector& active_links, + const std::vector& static_links, + const std::shared_ptr& validator = nullptr); /** * @brief This will check if a link is active provided a list. If the list is empty the link is considered active. @@ -61,13 +63,13 @@ bool isLinkActive(const std::vector& active, const std::string& nam * @brief Determine if contact is allowed between two objects. * @param name1 The name of the first object * @param name2 The name of the second object - * @param acm The contact allowed function + * @param validator The contact allowed validator * @param verbose If true print debug information * @return True if contact is allowed between the two object, otherwise false. */ bool isContactAllowed(const std::string& name1, const std::string& name2, - const IsContactAllowedFn& acm, + const std::shared_ptr& validator, bool verbose = false); /** diff --git a/tesseract_collision/core/include/tesseract_collision/core/contact_result_validator.h b/tesseract_collision/core/include/tesseract_collision/core/contact_result_validator.h new file mode 100644 index 00000000000..2f2c3f562c5 --- /dev/null +++ b/tesseract_collision/core/include/tesseract_collision/core/contact_result_validator.h @@ -0,0 +1,64 @@ +/** + * @file contact_result_validator.h + * @brief Contact result validator + * + * @author Levi Armstrong + * @date Jan 2, 2025 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2025, Levi Armstrong + * + * @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. + */ +#ifndef TESSERACT_COLLISION_CORE_CONTACT_RESULT_VALIDATORS_H +#define TESSERACT_COLLISION_CORE_CONTACT_RESULT_VALIDATORS_H + +#include +#include +#include + +namespace tesseract_collision +{ +struct ContactResult; + +/** + * @brief Should return true if contact results are valid, otherwise false. + * + * This is used so users may provide a callback to reject/approve collision results in various algorithms. + */ +class ContactResultValidator +{ +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + using UPtr = std::unique_ptr; + using ConstUPtr = std::unique_ptr; + + virtual ~ContactResultValidator() = default; + + virtual bool operator()(const ContactResult&) const = 0; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int version); // NOLINT +}; + +} // namespace tesseract_collision + +BOOST_CLASS_EXPORT_KEY(tesseract_collision::ContactResultValidator) + +#endif // TESSERACT_COLLISION_CORE_CONTACT_RESULT_VALIDATORS_H diff --git a/tesseract_collision/core/include/tesseract_collision/core/continuous_contact_manager.h b/tesseract_collision/core/include/tesseract_collision/core/continuous_contact_manager.h index 0ce61057d93..cc2a9e08979 100644 --- a/tesseract_collision/core/include/tesseract_collision/core/continuous_contact_manager.h +++ b/tesseract_collision/core/include/tesseract_collision/core/continuous_contact_manager.h @@ -34,6 +34,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include namespace tesseract_collision { @@ -254,10 +255,11 @@ class ContinuousContactManager virtual const CollisionMarginData& getCollisionMarginData() const = 0; /** @brief Set the active function for determining if two links are allowed to be in collision */ - virtual void setIsContactAllowedFn(IsContactAllowedFn fn) = 0; + virtual void + setContactAllowedValidator(std::shared_ptr validator) = 0; /** @brief Get the active function for determining if two links are allowed to be in collision */ - virtual IsContactAllowedFn getIsContactAllowedFn() const = 0; + virtual std::shared_ptr getContactAllowedValidator() const = 0; /** * @brief Perform a contact test for all objects based diff --git a/tesseract_collision/core/include/tesseract_collision/core/discrete_contact_manager.h b/tesseract_collision/core/include/tesseract_collision/core/discrete_contact_manager.h index 38d59ae37e7..2701e5b182a 100644 --- a/tesseract_collision/core/include/tesseract_collision/core/discrete_contact_manager.h +++ b/tesseract_collision/core/include/tesseract_collision/core/discrete_contact_manager.h @@ -34,6 +34,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include namespace tesseract_collision { @@ -207,10 +208,11 @@ class DiscreteContactManager virtual const CollisionMarginData& getCollisionMarginData() const = 0; /** @brief Set the active function for determining if two links are allowed to be in collision */ - virtual void setIsContactAllowedFn(IsContactAllowedFn fn) = 0; + virtual void + setContactAllowedValidator(std::shared_ptr validator) = 0; /** @brief Get the active function for determining if two links are allowed to be in collision */ - virtual IsContactAllowedFn getIsContactAllowedFn() const = 0; + virtual std::shared_ptr getContactAllowedValidator() const = 0; /** * @brief Perform a contact test for all objects based diff --git a/tesseract_collision/core/include/tesseract_collision/core/fwd.h b/tesseract_collision/core/include/tesseract_collision/core/fwd.h index b5cd0717850..253193a6cf8 100644 --- a/tesseract_collision/core/include/tesseract_collision/core/fwd.h +++ b/tesseract_collision/core/include/tesseract_collision/core/fwd.h @@ -43,6 +43,7 @@ struct CollisionCheckConfig; struct ContactTrajectorySubstepResults; struct ContactTrajectoryStepResults; struct ContactTrajectoryResults; +class ContactResultValidator; // contact_managers_plugin_factory.h class DiscreteContactManagerFactory; diff --git a/tesseract_collision/core/include/tesseract_collision/core/types.h b/tesseract_collision/core/include/tesseract_collision/core/types.h index e70b026a838..a51dcb94ff8 100644 --- a/tesseract_collision/core/include/tesseract_collision/core/types.h +++ b/tesseract_collision/core/include/tesseract_collision/core/types.h @@ -53,12 +53,7 @@ using CollisionMarginOverrideType = tesseract_common::CollisionMarginOverrideTyp using PairsCollisionMarginData = std::unordered_map, double, tesseract_common::PairHash>; -/** - * @brief Should return true if contact allowed, otherwise false. - * - * Also the order of strings should not matter, the function should handled by the function. - */ -using IsContactAllowedFn = std::function; +class ContactResultValidator; enum class ContinuousCollisionType { @@ -301,13 +296,6 @@ class ContactResultMap long count_{ 0 }; }; -/** - * @brief Should return true if contact results are valid, otherwise false. - * - * This is used so users may provide a callback to reject/approve collision results in various algorithms. - */ -using IsContactResultValidFn = std::function; - /** @brief The ContactRequest struct */ struct ContactRequest { @@ -325,7 +313,7 @@ struct ContactRequest long contact_limit = 0; /** @brief This provides a user defined function approve/reject contact results */ - IsContactResultValidFn is_valid = nullptr; + std::shared_ptr is_valid; ContactRequest(ContactTestType type = ContactTestType::ALL); }; @@ -341,7 +329,7 @@ struct ContactTestData ContactTestData() = default; ContactTestData(const std::vector& active, CollisionMarginData collision_margin_data, - IsContactAllowedFn fn, + std::shared_ptr validator, ContactRequest req, ContactResultMap& res); @@ -352,7 +340,7 @@ struct ContactTestData CollisionMarginData collision_margin_data{ 0 }; /** @brief The allowed collision function used to check if two links should be excluded from collision checking */ - IsContactAllowedFn fn = nullptr; + std::shared_ptr validator; /** @brief The type of contact request data */ ContactRequest req; @@ -409,11 +397,11 @@ enum class ACMOverrideType { /** @brief Do not apply AllowedCollisionMatrix */ NONE, - /** @brief Replace the current IsContactAllowedFn with one generated from the ACM provided */ + /** @brief Replace the current ContactAllowedValidator with one generated from the ACM provided */ ASSIGN, - /** @brief New IsContactAllowedFn combines the contact manager fn and the ACM generated fn with and AND */ + /** @brief New ContactAllowedValidator combines the contact manager fn and the ACM generated fn with and AND */ AND, - /** @brief New IsContactAllowedFn combines the contact manager fn and the ACM generated fn with and OR */ + /** @brief New ContactAllowedValidator combines the contact manager fn and the ACM generated fn with and OR */ OR, }; @@ -438,7 +426,7 @@ struct ContactManagerConfig /** @brief Additional AllowedCollisionMatrix to consider for this collision check. */ tesseract_common::AllowedCollisionMatrix acm; - /** @brief Specifies how to combine the IsContactAllowedFn from acm with the one preset in the contact manager */ + /** @brief Specifies how to combine the ContactAllowedValidator from acm with the one preset in the contact manager */ ACMOverrideType acm_override_type{ ACMOverrideType::OR }; /** @brief Each key is an object name. Objects will be enabled/disabled based on the value. Objects that aren't in the diff --git a/tesseract_collision/core/include/tesseract_collision/core/utils.h b/tesseract_collision/core/include/tesseract_collision/core/utils.h index ae009305b65..ff7049a65bb 100644 --- a/tesseract_collision/core/include/tesseract_collision/core/utils.h +++ b/tesseract_collision/core/include/tesseract_collision/core/utils.h @@ -25,40 +25,38 @@ #ifndef TESSERACT_COLLISION_CORE_UTILS_H #define TESSERACT_COLLISION_CORE_UTILS_H -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include -TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include +#include namespace tesseract_collision { /** - * @brief Combines two IsContactAllowedFns using the override type - * @param original Original IsContactAllowedFns. This will be returned if ACMOverrideType is None - * @param override Overriding IsContactAllowedFns. This will be returned if ACMOverrideType is ASSIGN - * @param type Override type used to combine the IsContactAllowedFns - * @return One IsContactAllowedFn that combines the two + * @brief Combines two ContactAllowedValidator using the override type + * @param original Original ContactAllowedValidator. This will be returned if ACMOverrideType is None + * @param override Overriding ContactAllowedValidator. This will be returned if ACMOverrideType is ASSIGN + * @param type Override type used to combine the ContactAllowedValidator + * @return One ContactAllowedValidator that combines the two */ -IsContactAllowedFn combineContactAllowedFn(const IsContactAllowedFn& original, - const IsContactAllowedFn& override, - ACMOverrideType type = ACMOverrideType::OR); +tesseract_common::ContactAllowedValidator::ConstPtr +combineContactAllowedValidators(tesseract_common::ContactAllowedValidator::ConstPtr original, + tesseract_common::ContactAllowedValidator::ConstPtr override, + ACMOverrideType type = ACMOverrideType::OR); /** * @brief Applies ACM to contact manager using override type - * @param manager Manager whose IsContactAllowedFn will be overwritten - * @param acm ACM used to create IsContactAllowedFn - * @param type Determines how the two IsContactAllowedFns are combined + * @param manager Manager whose ContactAllowedValidator will be overwritten + * @param acm ACM used to create ContactAllowedValidator + * @param type Determines how the two ContactAllowedValidator are combined */ template -inline void applyIsContactAllowedFnOverride(ManagerType& manager, - const tesseract_common::AllowedCollisionMatrix& acm, - ACMOverrideType type) +inline void applyContactAllowedValidatorOverride(ManagerType& manager, + const tesseract_common::AllowedCollisionMatrix& acm, + ACMOverrideType type) { - IsContactAllowedFn original = manager.getIsContactAllowedFn(); - IsContactAllowedFn override = [acm](const std::string& str1, const std::string& str2) { - return acm.isCollisionAllowed(str1, str2); - }; - manager.setIsContactAllowedFn(combineContactAllowedFn(original, override, type)); + tesseract_common::ContactAllowedValidator::ConstPtr original = manager.getContactAllowedValidator(); + auto override = std::make_shared(acm); + manager.setContactAllowedValidator(combineContactAllowedValidators(original, override, type)); } /** diff --git a/tesseract_collision/core/src/common.cpp b/tesseract_collision/core/src/common.cpp index 6045a30a4ae..812a25763ff 100644 --- a/tesseract_collision/core/src/common.cpp +++ b/tesseract_collision/core/src/common.cpp @@ -37,12 +37,14 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include namespace tesseract_collision { -std::vector getCollisionObjectPairs(const std::vector& active_links, - const std::vector& static_links, - const IsContactAllowedFn& acm) +std::vector +getCollisionObjectPairs(const std::vector& active_links, + const std::vector& static_links, + const std::shared_ptr& validator) { std::size_t num_pairs = active_links.size() * (active_links.size() - 1) / 2; num_pairs += (active_links.size() * static_links.size()); @@ -57,7 +59,7 @@ std::vector getCollisionObjectPairs(const std::vector getCollisionObjectPairs(const std::vector& active, const std::string& nam return active.empty() || (std::find(active.begin(), active.end(), name) != active.end()); } -bool isContactAllowed(const std::string& name1, const std::string& name2, const IsContactAllowedFn& acm, bool verbose) +bool isContactAllowed(const std::string& name1, + const std::string& name2, + const std::shared_ptr& validator, + bool verbose) { // do not distance check geoms part of the same object / link / attached body if (name1 == name2) return true; - if (acm != nullptr && acm(name1, name2)) + if (validator != nullptr && (*validator)(name1, name2)) { if (verbose) { @@ -109,7 +114,7 @@ ContactResult* processResult(ContactTestData& cdata, const std::pair& key, bool found) { - if (cdata.req.is_valid && !cdata.req.is_valid(contact)) + if (cdata.req.is_valid && !(*cdata.req.is_valid)(contact)) return nullptr; if ((cdata.req.calculate_distance || cdata.req.calculate_penetration) && diff --git a/tesseract_collision/core/src/contact_result_validator.cpp b/tesseract_collision/core/src/contact_result_validator.cpp new file mode 100644 index 00000000000..c12df21c276 --- /dev/null +++ b/tesseract_collision/core/src/contact_result_validator.cpp @@ -0,0 +1,40 @@ +/** + * @file contact_result_validator.h + * @brief Contact result validator + * + * @author Levi Armstrong + * @date Jan 2, 2025 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2025, Levi Armstrong + * + * @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 + +namespace tesseract_collision +{ +template +void ContactResultValidator::serialize(Archive& /*ar*/, const unsigned int /*version*/) +{ +} + +} // namespace tesseract_collision + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_collision::ContactResultValidator) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_collision::ContactResultValidator) diff --git a/tesseract_collision/core/src/continuous_contact_manager.cpp b/tesseract_collision/core/src/continuous_contact_manager.cpp index 3a1c1eca79b..99f82d28f91 100644 --- a/tesseract_collision/core/src/continuous_contact_manager.cpp +++ b/tesseract_collision/core/src/continuous_contact_manager.cpp @@ -32,7 +32,7 @@ namespace tesseract_collision void ContinuousContactManager::applyContactManagerConfig(const ContactManagerConfig& config) { setCollisionMarginData(config.margin_data, config.margin_data_override_type); - applyIsContactAllowedFnOverride(*this, config.acm, config.acm_override_type); + applyContactAllowedValidatorOverride(*this, config.acm, config.acm_override_type); applyModifyObjectEnabled(*this, config.modify_object_enabled); } } // namespace tesseract_collision diff --git a/tesseract_collision/core/src/discrete_contact_manager.cpp b/tesseract_collision/core/src/discrete_contact_manager.cpp index ebb69522c43..2e75db8c16a 100644 --- a/tesseract_collision/core/src/discrete_contact_manager.cpp +++ b/tesseract_collision/core/src/discrete_contact_manager.cpp @@ -32,7 +32,7 @@ namespace tesseract_collision void DiscreteContactManager::applyContactManagerConfig(const ContactManagerConfig& config) { setCollisionMarginData(config.margin_data, config.margin_data_override_type); - applyIsContactAllowedFnOverride(*this, config.acm, config.acm_override_type); + applyContactAllowedValidatorOverride(*this, config.acm, config.acm_override_type); applyModifyObjectEnabled(*this, config.modify_object_enabled); } } // namespace tesseract_collision diff --git a/tesseract_collision/core/src/serialization.cpp b/tesseract_collision/core/src/serialization.cpp index a4366cadbb4..f17fb427704 100644 --- a/tesseract_collision/core/src/serialization.cpp +++ b/tesseract_collision/core/src/serialization.cpp @@ -40,11 +40,13 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include #include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include namespace boost::serialization { @@ -102,7 +104,7 @@ void serialize(Archive& ar, tesseract_collision::ContactRequest& g, const unsign ar& boost::serialization::make_nvp("calculate_penetration", g.calculate_penetration); ar& boost::serialization::make_nvp("calculate_distance", g.calculate_distance); ar& boost::serialization::make_nvp("contact_limit", g.contact_limit); - // ar& boost::serialization::make_nvp("is_valid", g.is_valid); /** @todo FIX */ + ar& boost::serialization::make_nvp("is_valid", g.is_valid); } template diff --git a/tesseract_collision/core/src/types.cpp b/tesseract_collision/core/src/types.cpp index 21c396c02cb..eed2a650cb6 100644 --- a/tesseract_collision/core/src/types.cpp +++ b/tesseract_collision/core/src/types.cpp @@ -29,6 +29,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include + namespace tesseract_collision { void ContactResult::clear() @@ -347,12 +349,12 @@ std::string ContactResultMap::getSummary() const ContactTestData::ContactTestData(const std::vector& active, CollisionMarginData collision_margin_data, - IsContactAllowedFn fn, + std::shared_ptr validator, ContactRequest req, ContactResultMap& res) : active(&active) , collision_margin_data(std::move(collision_margin_data)) - , fn(std::move(fn)) + , validator(std::move(validator)) , req(std::move(req)) , res(&res) { diff --git a/tesseract_collision/core/src/utils.cpp b/tesseract_collision/core/src/utils.cpp index 1819c8bee09..0ba0b93deaa 100644 --- a/tesseract_collision/core/src/utils.cpp +++ b/tesseract_collision/core/src/utils.cpp @@ -27,9 +27,10 @@ namespace tesseract_collision { -IsContactAllowedFn combineContactAllowedFn(const IsContactAllowedFn& original, - const IsContactAllowedFn& override, - ACMOverrideType type) +tesseract_common::ContactAllowedValidator::ConstPtr +combineContactAllowedValidators(tesseract_common::ContactAllowedValidator::ConstPtr original, + tesseract_common::ContactAllowedValidator::ConstPtr override, + ACMOverrideType type) { switch (type) { @@ -38,13 +39,23 @@ IsContactAllowedFn combineContactAllowedFn(const IsContactAllowedFn& original, case ACMOverrideType::ASSIGN: return override; case ACMOverrideType::AND: - return [original, override](const std::string& str1, const std::string& str2) { - return (original == nullptr) ? false : original(str1, str2) && override(str1, str2); - }; + { + if (original == nullptr) + return nullptr; + + std::vector> validators = { original, override }; + return std::make_shared( + validators, tesseract_common::CombinedContactAllowedValidatorType::AND); + } case ACMOverrideType::OR: - return [original, override](const std::string& str1, const std::string& str2) { - return (original == nullptr) ? override(str1, str2) : original(str1, str2) || override(str1, str2); - }; + { + if (original == nullptr) + return override; + + std::vector> validators = { original, override }; + return std::make_shared( + validators, tesseract_common::CombinedContactAllowedValidatorType::OR); + } default: // LCOV_EXCL_LINE return original; // LCOV_EXCL_LINE } diff --git a/tesseract_collision/fcl/include/tesseract_collision/fcl/fcl_discrete_managers.h b/tesseract_collision/fcl/include/tesseract_collision/fcl/fcl_discrete_managers.h index 7b90cb45146..a8f1f146aad 100644 --- a/tesseract_collision/fcl/include/tesseract_collision/fcl/fcl_discrete_managers.h +++ b/tesseract_collision/fcl/include/tesseract_collision/fcl/fcl_discrete_managers.h @@ -113,9 +113,10 @@ class FCLDiscreteBVHManager : public DiscreteContactManager const CollisionMarginData& getCollisionMarginData() const override final; - void setIsContactAllowedFn(IsContactAllowedFn fn) override final; + void + setContactAllowedValidator(std::shared_ptr validator) override final; - IsContactAllowedFn getIsContactAllowedFn() const override final; + std::shared_ptr getContactAllowedValidator() const override final; void contactTest(ContactResultMap& collisions, const ContactRequest& request) override final; @@ -138,8 +139,9 @@ class FCLDiscreteBVHManager : public DiscreteContactManager std::vector active_; /**< @brief A list of the active collision objects */ std::vector collision_objects_; /**< @brief A list of the collision objects */ CollisionMarginData collision_margin_data_; /**< @brief The contact distance threshold */ - IsContactAllowedFn fn_; /**< @brief The is allowed collision function */ - std::size_t fcl_co_count_{ 0 }; /**< @brief The number fcl collision objects */ + std::shared_ptr validator_; /**< @brief The is allowed collision + function */ + std::size_t fcl_co_count_{ 0 }; /**< @brief The number fcl collision objects */ /** @brief This is used to store static collision objects to update */ std::vector static_update_; diff --git a/tesseract_collision/fcl/src/fcl_discrete_managers.cpp b/tesseract_collision/fcl/src/fcl_discrete_managers.cpp index 0876241e6cd..22c040b4282 100644 --- a/tesseract_collision/fcl/src/fcl_discrete_managers.cpp +++ b/tesseract_collision/fcl/src/fcl_discrete_managers.cpp @@ -40,6 +40,7 @@ */ #include +#include namespace tesseract_collision::tesseract_collision_fcl { @@ -64,7 +65,7 @@ DiscreteContactManager::UPtr FCLDiscreteBVHManager::clone() const manager->setActiveCollisionObjects(active_); manager->setCollisionMarginData(collision_margin_data_); - manager->setIsContactAllowedFn(fn_); + manager->setContactAllowedValidator(validator_); return manager; } @@ -308,12 +309,20 @@ void FCLDiscreteBVHManager::setPairCollisionMarginData(const std::string& name1, } const CollisionMarginData& FCLDiscreteBVHManager::getCollisionMarginData() const { return collision_margin_data_; } -void FCLDiscreteBVHManager::setIsContactAllowedFn(IsContactAllowedFn fn) { fn_ = fn; } -IsContactAllowedFn FCLDiscreteBVHManager::getIsContactAllowedFn() const { return fn_; } +void FCLDiscreteBVHManager::setContactAllowedValidator( + std::shared_ptr validator) +{ + validator_ = std::move(validator); +} +std::shared_ptr +FCLDiscreteBVHManager::getContactAllowedValidator() const +{ + return validator_; +} void FCLDiscreteBVHManager::contactTest(ContactResultMap& collisions, const ContactRequest& request) { - ContactTestData cdata(active_, collision_margin_data_, fn_, request, collisions); + ContactTestData cdata(active_, collision_margin_data_, validator_, request, collisions); if (collision_margin_data_.getMaxCollisionMargin() > 0) { // TODO: Should the order be flipped? diff --git a/tesseract_collision/fcl/src/fcl_utils.cpp b/tesseract_collision/fcl/src/fcl_utils.cpp index b310ef484d4..cda021c0662 100644 --- a/tesseract_collision/fcl/src/fcl_utils.cpp +++ b/tesseract_collision/fcl/src/fcl_utils.cpp @@ -217,7 +217,7 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi bool needs_collision = cd1->m_enabled && cd2->m_enabled && (cd1->m_collisionFilterGroup & cd2->m_collisionFilterMask) && // NOLINT (cd2->m_collisionFilterGroup & cd1->m_collisionFilterMask) && // NOLINT - !isContactAllowed(cd1->getName(), cd2->getName(), cdata->fn, false); + !isContactAllowed(cd1->getName(), cd2->getName(), cdata->validator, false); assert(std::find(cdata->active->begin(), cdata->active->end(), cd1->getName()) != cdata->active->end() || std::find(cdata->active->begin(), cdata->active->end(), cd2->getName()) != cdata->active->end()); @@ -285,7 +285,7 @@ bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void bool needs_collision = cd1->m_enabled && cd2->m_enabled && (cd1->m_collisionFilterGroup & cd2->m_collisionFilterMask) && // NOLINT (cd2->m_collisionFilterGroup & cd1->m_collisionFilterMask) && // NOLINT - !isContactAllowed(cd1->getName(), cd2->getName(), cdata->fn, false); + !isContactAllowed(cd1->getName(), cd2->getName(), cdata->validator, false); assert(std::find(cdata->active->begin(), cdata->active->end(), cd1->getName()) != cdata->active->end() || std::find(cdata->active->begin(), cdata->active->end(), cd2->getName()) != cdata->active->end()); diff --git a/tesseract_collision/test/collision_core_unit.cpp b/tesseract_collision/test/collision_core_unit.cpp index 5f0420a0905..2ec44bd4ff2 100644 --- a/tesseract_collision/test/collision_core_unit.cpp +++ b/tesseract_collision/test/collision_core_unit.cpp @@ -6,8 +6,19 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include #include +class TestContactAllowedValidator : public tesseract_common::ContactAllowedValidator +{ +public: + bool operator()(const std::string& s1, const std::string& s2) const override + { + return (tesseract_common::makeOrderedLinkPair("base_link", "link_1") == + tesseract_common::makeOrderedLinkPair(s1, s2)); + } +}; + TEST(TesseractCoreUnit, getCollisionObjectPairsUnit) // NOLINT { std::vector active_links{ "link_1", "link_2", "link_3" }; @@ -30,10 +41,7 @@ TEST(TesseractCoreUnit, getCollisionObjectPairsUnit) // NOLINT EXPECT_TRUE(tesseract_common::isIdentical(pairs, check_pairs, false)); // Now check provided a is contact allowed function - auto acm = [](const std::string& s1, const std::string& s2) { - return (tesseract_common::makeOrderedLinkPair("base_link", "link_1") == - tesseract_common::makeOrderedLinkPair(s1, s2)); - }; + auto validator = std::make_shared(); check_pairs.clear(); check_pairs.push_back(tesseract_common::makeOrderedLinkPair("link_1", "link_2")); @@ -45,21 +53,18 @@ TEST(TesseractCoreUnit, getCollisionObjectPairsUnit) // NOLINT check_pairs.push_back(tesseract_common::makeOrderedLinkPair("part_link", "link_2")); check_pairs.push_back(tesseract_common::makeOrderedLinkPair("part_link", "link_3")); - pairs = tesseract_collision::getCollisionObjectPairs(active_links, static_links, acm); + pairs = tesseract_collision::getCollisionObjectPairs(active_links, static_links, validator); EXPECT_TRUE(tesseract_common::isIdentical(pairs, check_pairs, false)); } TEST(TesseractCoreUnit, isContactAllowedUnit) // NOLINT { - auto acm = [](const std::string& s1, const std::string& s2) { - return (tesseract_common::makeOrderedLinkPair("base_link", "link_1") == - tesseract_common::makeOrderedLinkPair(s1, s2)); - }; + auto validator = std::make_shared(); - EXPECT_TRUE(tesseract_collision::isContactAllowed("base_link", "base_link", acm, false)); - EXPECT_FALSE(tesseract_collision::isContactAllowed("base_link", "link_2", acm, false)); - EXPECT_TRUE(tesseract_collision::isContactAllowed("base_link", "link_1", acm, true)); + EXPECT_TRUE(tesseract_collision::isContactAllowed("base_link", "base_link", validator, false)); + EXPECT_FALSE(tesseract_collision::isContactAllowed("base_link", "link_2", validator, false)); + EXPECT_TRUE(tesseract_collision::isContactAllowed("base_link", "link_1", validator, true)); } TEST(TesseractCoreUnit, scaleVerticesUnit) // NOLINT diff --git a/tesseract_collision/test/contact_managers_config_unit.cpp b/tesseract_collision/test/contact_managers_config_unit.cpp index f8e7b84d38a..581e66db014 100644 --- a/tesseract_collision/test/contact_managers_config_unit.cpp +++ b/tesseract_collision/test/contact_managers_config_unit.cpp @@ -38,6 +38,42 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP using namespace tesseract_collision; +class AlwaysTrueContactAllowedValidator : public tesseract_common::ContactAllowedValidator +{ +public: + bool operator()(const std::string&, const std::string&) const override { return true; } +}; + +class AlwaysFalseContactAllowedValidator : public tesseract_common::ContactAllowedValidator +{ +public: + bool operator()(const std::string&, const std::string&) const override { return false; } +}; + +class TestOrigContactAllowedValidator : public tesseract_common::ContactAllowedValidator +{ +public: + bool operator()(const std::string& s1, const std::string& s2) const override + { + if (s1 == "link_1" && s2 == "link_2") + return true; + + if (s1 == "link_1" && s2 == "link_3") + return true; + + return false; + } +}; + +class TestOvrdContactAllowedValidator : public tesseract_common::ContactAllowedValidator +{ +public: + bool operator()(const std::string& s1, const std::string& s2) const override + { + return (s1 == "link_1" && s2 == "link_2"); + } +}; + TEST(TesseractCollisionUnit, BulletDiscreteSimpleContactManagerConfigUnit) // NOLINT { tesseract_collision_bullet::BulletDiscreteSimpleManager checker; @@ -71,79 +107,47 @@ TEST(TesseractCollisionUnit, FCLDiscreteBVHContactManagerConfigUnit) // NOLINT TEST(TesseractCollisionUnit, CombineContactAllowedFnUnit) // NOLINT { { // tesseract_collision::ACMOverrideType::NONE - tesseract_collision::IsContactAllowedFn orig = [](const std::string& /*link_name1*/, - const std::string& /*link_name2*/) { return true; }; - - tesseract_collision::IsContactAllowedFn ovrd = [](const std::string& /*link_name1*/, - const std::string& /*link_name2*/) { return false; }; + auto orig = std::make_shared(); + auto ovrd = std::make_shared(); - tesseract_collision::IsContactAllowedFn comb = - tesseract_collision::combineContactAllowedFn(orig, ovrd, tesseract_collision::ACMOverrideType::NONE); - EXPECT_TRUE(comb("", "")); + auto comb = combineContactAllowedValidators(orig, ovrd, tesseract_collision::ACMOverrideType::NONE); + EXPECT_TRUE((*comb)("", "")); } { // tesseract_collision::ACMOverrideType::ASSIGN - tesseract_collision::IsContactAllowedFn orig = [](const std::string& /*link_name1*/, - const std::string& /*link_name2*/) { return true; }; - - tesseract_collision::IsContactAllowedFn ovrd = [](const std::string& /*link_name1*/, - const std::string& /*link_name2*/) { return false; }; + auto orig = std::make_shared(); + auto ovrd = std::make_shared(); - tesseract_collision::IsContactAllowedFn comb = - tesseract_collision::combineContactAllowedFn(orig, ovrd, tesseract_collision::ACMOverrideType::ASSIGN); - EXPECT_FALSE(comb("", "")); + auto comb = combineContactAllowedValidators(orig, ovrd, tesseract_collision::ACMOverrideType::ASSIGN); + EXPECT_FALSE((*comb)("", "")); } { // tesseract_collision::ACMOverrideType::AND - auto orig = [](const std::string& link_name1, const std::string& link_name2) { - if (link_name1 == "link_1" && link_name2 == "link_2") - return true; - - if (link_name1 == "link_1" && link_name2 == "link_3") - return true; - - return false; - }; - - auto ovrd = [](const std::string& link_name1, const std::string& link_name2) { - return (link_name1 == "link_1" && link_name2 == "link_2"); - }; + auto orig = std::make_shared(); + auto ovrd = std::make_shared(); - auto comb = tesseract_collision::combineContactAllowedFn(orig, ovrd, tesseract_collision::ACMOverrideType::AND); - EXPECT_TRUE(comb("link_1", "link_2")); - EXPECT_FALSE(comb("link_1", "link_3")); - EXPECT_FALSE(comb("abc", "def")); + auto comb = combineContactAllowedValidators(orig, ovrd, tesseract_collision::ACMOverrideType::AND); + EXPECT_TRUE((*comb)("link_1", "link_2")); + EXPECT_FALSE((*comb)("link_1", "link_3")); + EXPECT_FALSE((*comb)("abc", "def")); - auto comb1 = tesseract_collision::combineContactAllowedFn(nullptr, ovrd, tesseract_collision::ACMOverrideType::AND); - EXPECT_FALSE(comb1("link_1", "link_2")); - EXPECT_FALSE(comb1("link_1", "link_3")); - EXPECT_FALSE(comb1("abc", "def")); + auto comb1 = combineContactAllowedValidators(nullptr, ovrd, tesseract_collision::ACMOverrideType::AND); + EXPECT_TRUE(comb1 == nullptr); } { // tesseract_collision::ACMOverrideType::AND - auto orig = [](const std::string& link_name1, const std::string& link_name2) { - if (link_name1 == "link_1" && link_name2 == "link_2") - return true; - - if (link_name1 == "link_1" && link_name2 == "link_3") - return true; - - return false; - }; - - auto ovrd = [](const std::string& link_name1, const std::string& link_name2) { - return (link_name1 == "link_1" && link_name2 == "link_2"); - }; - - auto comb = tesseract_collision::combineContactAllowedFn(orig, ovrd, tesseract_collision::ACMOverrideType::OR); - EXPECT_TRUE(comb("link_1", "link_2")); - EXPECT_TRUE(comb("link_1", "link_3")); - EXPECT_FALSE(comb("abc", "def")); - - auto comb1 = tesseract_collision::combineContactAllowedFn(nullptr, ovrd, tesseract_collision::ACMOverrideType::OR); - EXPECT_TRUE(comb1("link_1", "link_2")); - EXPECT_FALSE(comb1("link_1", "link_3")); - EXPECT_FALSE(comb1("abc", "def")); + auto orig = std::make_shared(); + auto ovrd = std::make_shared(); + + auto comb = combineContactAllowedValidators(orig, ovrd, tesseract_collision::ACMOverrideType::OR); + EXPECT_TRUE((*comb)("link_1", "link_2")); + EXPECT_TRUE((*comb)("link_1", "link_3")); + EXPECT_FALSE((*comb)("abc", "def")); + + auto comb1 = combineContactAllowedValidators(nullptr, ovrd, tesseract_collision::ACMOverrideType::OR); + EXPECT_TRUE((*comb1)("link_1", "link_2")); + EXPECT_FALSE((*comb1)("link_1", "link_3")); + EXPECT_FALSE((*comb1)("abc", "def")); } } diff --git a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_box_box_cast_unit.hpp b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_box_box_cast_unit.hpp index b70986e2dad..6cf0e698921 100644 --- a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_box_box_cast_unit.hpp +++ b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_box_box_cast_unit.hpp @@ -129,7 +129,7 @@ inline void runTest(ContinuousContactManager& checker) std::vector check_active_links = checker.getActiveCollisionObjects(); EXPECT_TRUE(tesseract_common::isIdentical(active_links, check_active_links, false)); - EXPECT_TRUE(checker.getIsContactAllowedFn() == nullptr); + EXPECT_TRUE(checker.getContactAllowedValidator() == nullptr); checker.setDefaultCollisionMarginData(0.1); EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.1, 1e-5); diff --git a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_box_box_unit.hpp b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_box_box_unit.hpp index c58c481186a..524cb3557a8 100644 --- a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_box_box_unit.hpp +++ b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_box_box_unit.hpp @@ -136,7 +136,7 @@ inline void runTestTyped(DiscreteContactManager& checker, ContactTestType test_t std::vector check_active_links = checker.getActiveCollisionObjects(); EXPECT_TRUE(tesseract_common::isIdentical(active_links, check_active_links, false)); - EXPECT_TRUE(checker.getIsContactAllowedFn() == nullptr); + EXPECT_TRUE(checker.getContactAllowedValidator() == nullptr); checker.setCollisionMarginData(CollisionMarginData(0.1)); EXPECT_NEAR(checker.getCollisionMarginData().getPairCollisionMargin("box_link", "second_box_link"), 0.1, 1e-5); diff --git a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_box_capsule_unit.hpp b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_box_capsule_unit.hpp index 8af0faf2582..0fbdc0b975a 100644 --- a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_box_capsule_unit.hpp +++ b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_box_capsule_unit.hpp @@ -115,7 +115,7 @@ inline void runTest(DiscreteContactManager& checker) std::vector check_active_links = checker.getActiveCollisionObjects(); EXPECT_TRUE(tesseract_common::isIdentical(active_links, check_active_links, false)); - EXPECT_TRUE(checker.getIsContactAllowedFn() == nullptr); + EXPECT_TRUE(checker.getContactAllowedValidator() == nullptr); checker.setDefaultCollisionMarginData(0.1); EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.1, 1e-5); diff --git a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_box_cone_unit.hpp b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_box_cone_unit.hpp index ebec0bd8544..5aeba87aede 100644 --- a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_box_cone_unit.hpp +++ b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_box_cone_unit.hpp @@ -118,7 +118,7 @@ inline void runTest(DiscreteContactManager& checker) std::vector check_active_links = checker.getActiveCollisionObjects(); EXPECT_TRUE(tesseract_common::isIdentical(active_links, check_active_links, false)); - EXPECT_TRUE(checker.getIsContactAllowedFn() == nullptr); + EXPECT_TRUE(checker.getContactAllowedValidator() == nullptr); checker.setCollisionMarginData(CollisionMarginData(0.5)); EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.5, 1e-5); diff --git a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_box_cylinder_unit.hpp b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_box_cylinder_unit.hpp index d465d26da62..747891e6bdc 100644 --- a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_box_cylinder_unit.hpp +++ b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_box_cylinder_unit.hpp @@ -116,7 +116,7 @@ inline void runTest(DiscreteContactManager& checker) std::vector check_active_links = checker.getActiveCollisionObjects(); EXPECT_TRUE(tesseract_common::isIdentical(active_links, check_active_links, false)); - EXPECT_TRUE(checker.getIsContactAllowedFn() == nullptr); + EXPECT_TRUE(checker.getContactAllowedValidator() == nullptr); checker.setCollisionMarginData(CollisionMarginData(0.1)); EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.1, 1e-5); diff --git a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_box_sphere_unit.hpp b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_box_sphere_unit.hpp index c44c275addd..5fd3523d006 100644 --- a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_box_sphere_unit.hpp +++ b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_box_sphere_unit.hpp @@ -135,7 +135,7 @@ inline void runTestPrimitive(DiscreteContactManager& checker) std::vector check_active_links = checker.getActiveCollisionObjects(); EXPECT_TRUE(tesseract_common::isIdentical(active_links, check_active_links, false)); - EXPECT_TRUE(checker.getIsContactAllowedFn() == nullptr); + EXPECT_TRUE(checker.getContactAllowedValidator() == nullptr); checker.setDefaultCollisionMarginData(0.1); EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.1, 1e-5); @@ -248,7 +248,7 @@ inline void runTestConvex(DiscreteContactManager& checker) std::vector check_active_links = checker.getActiveCollisionObjects(); EXPECT_TRUE(tesseract_common::isIdentical(active_links, check_active_links, false)); - EXPECT_TRUE(checker.getIsContactAllowedFn() == nullptr); + EXPECT_TRUE(checker.getContactAllowedValidator() == nullptr); checker.setDefaultCollisionMarginData(0.1); EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.1, 1e-5); diff --git a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_clone_unit.hpp b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_clone_unit.hpp index e776100c037..089e7395dca 100644 --- a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_clone_unit.hpp +++ b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_clone_unit.hpp @@ -127,7 +127,7 @@ runTest(DiscreteContactManager& checker, double dist_tol = 0.001, double nearest std::vector check_active_links = checker.getActiveCollisionObjects(); EXPECT_TRUE(tesseract_common::isIdentical(active_links, check_active_links, false)); - EXPECT_TRUE(checker.getIsContactAllowedFn() == nullptr); + EXPECT_TRUE(checker.getContactAllowedValidator() == nullptr); checker.setCollisionMarginData(CollisionMarginData(0.5)); EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.5, 1e-5); diff --git a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_compound_compound_unit.hpp b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_compound_compound_unit.hpp index 7cefc9c4c60..5c973714ef2 100644 --- a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_compound_compound_unit.hpp +++ b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_compound_compound_unit.hpp @@ -82,7 +82,7 @@ inline void runTestCompound(DiscreteContactManager& checker) std::vector check_active_links = checker.getActiveCollisionObjects(); EXPECT_TRUE(tesseract_common::isIdentical(active_links, check_active_links, false)); - EXPECT_TRUE(checker.getIsContactAllowedFn() == nullptr); + EXPECT_TRUE(checker.getContactAllowedValidator() == nullptr); checker.setCollisionMarginData(CollisionMarginData(0.25)); EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.25, 1e-5); @@ -117,7 +117,7 @@ inline void runTestCompound(ContinuousContactManager& checker) std::vector check_active_links = checker.getActiveCollisionObjects(); EXPECT_TRUE(tesseract_common::isIdentical(active_links, check_active_links, false)); - EXPECT_TRUE(checker.getIsContactAllowedFn() == nullptr); + EXPECT_TRUE(checker.getContactAllowedValidator() == nullptr); checker.setCollisionMarginData(CollisionMarginData(0.5)); EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.5, 1e-5); diff --git a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_compound_mesh_sphere_unit.hpp b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_compound_mesh_sphere_unit.hpp index 84812daf7e7..21f452f83c2 100644 --- a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_compound_mesh_sphere_unit.hpp +++ b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_compound_mesh_sphere_unit.hpp @@ -128,7 +128,7 @@ inline void runTest(DiscreteContactManager& checker) std::vector check_active_links = checker.getActiveCollisionObjects(); EXPECT_TRUE(tesseract_common::isIdentical(active_links, check_active_links, false)); - EXPECT_TRUE(checker.getIsContactAllowedFn() == nullptr); + EXPECT_TRUE(checker.getContactAllowedValidator() == nullptr); checker.setDefaultCollisionMarginData(0.1); EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.1, 1e-5); diff --git a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_large_dataset_unit.hpp b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_large_dataset_unit.hpp index ae29ef9f646..fdbe4bf771a 100644 --- a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_large_dataset_unit.hpp +++ b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_large_dataset_unit.hpp @@ -74,7 +74,7 @@ inline void runTest(DiscreteContactManager& checker, bool use_convex_mesh = fals std::vector check_active_links = checker.getActiveCollisionObjects(); EXPECT_TRUE(tesseract_common::isIdentical(link_names, check_active_links, false)); - EXPECT_TRUE(checker.getIsContactAllowedFn() == nullptr); + EXPECT_TRUE(checker.getContactAllowedValidator() == nullptr); checker.setCollisionMarginData(CollisionMarginData(0.1)); EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.1, 1e-5); diff --git a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_mesh_mesh_unit.hpp b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_mesh_mesh_unit.hpp index 6894a6e2c48..8e26eaa076b 100644 --- a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_mesh_mesh_unit.hpp +++ b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_mesh_mesh_unit.hpp @@ -134,7 +134,7 @@ inline void runTest(DiscreteContactManager& checker) std::vector check_active_links = checker.getActiveCollisionObjects(); EXPECT_TRUE(tesseract_common::isIdentical(active_links, check_active_links, false)); - EXPECT_TRUE(checker.getIsContactAllowedFn() == nullptr); + EXPECT_TRUE(checker.getContactAllowedValidator() == nullptr); checker.setDefaultCollisionMarginData(0); EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.0, 1e-5); diff --git a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_multi_threaded_unit.hpp b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_multi_threaded_unit.hpp index d3ef3d7dc2e..c9055b4d95d 100644 --- a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_multi_threaded_unit.hpp +++ b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_multi_threaded_unit.hpp @@ -73,7 +73,7 @@ inline void runTest(DiscreteContactManager& checker, bool use_convex_mesh = fals std::vector check_active_links = checker.getActiveCollisionObjects(); EXPECT_TRUE(tesseract_common::isIdentical(link_names, check_active_links, false)); - EXPECT_TRUE(checker.getIsContactAllowedFn() == nullptr); + EXPECT_TRUE(checker.getContactAllowedValidator() == nullptr); checker.setCollisionMarginData(CollisionMarginData(0.1)); EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.1, 1e-5); diff --git a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_octomap_mesh_unit.hpp b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_octomap_mesh_unit.hpp index 5cb40981130..ce76c6b3289 100644 --- a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_octomap_mesh_unit.hpp +++ b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_octomap_mesh_unit.hpp @@ -84,7 +84,7 @@ inline void runTest(DiscreteContactManager& checker, const std::string& file_pat std::vector check_active_links = checker.getActiveCollisionObjects(); EXPECT_TRUE(tesseract_common::isIdentical(active_links, check_active_links, false)); - EXPECT_TRUE(checker.getIsContactAllowedFn() == nullptr); + EXPECT_TRUE(checker.getContactAllowedValidator() == nullptr); checker.setCollisionMarginData(CollisionMarginData(0.5)); EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.5, 1e-5); diff --git a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_octomap_octomap_unit.hpp b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_octomap_octomap_unit.hpp index e16ba22880d..9413cb003fc 100644 --- a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_octomap_octomap_unit.hpp +++ b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_octomap_octomap_unit.hpp @@ -82,7 +82,7 @@ inline void runTestOctomap(DiscreteContactManager& checker, ContactTestType test std::vector check_active_links = checker.getActiveCollisionObjects(); EXPECT_TRUE(tesseract_common::isIdentical(active_links, check_active_links, false)); - EXPECT_TRUE(checker.getIsContactAllowedFn() == nullptr); + EXPECT_TRUE(checker.getContactAllowedValidator() == nullptr); checker.setCollisionMarginData(CollisionMarginData(0.25)); EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.25, 1e-5); @@ -117,7 +117,7 @@ inline void runTestOctomap(ContinuousContactManager& checker, ContactTestType te std::vector check_active_links = checker.getActiveCollisionObjects(); EXPECT_TRUE(tesseract_common::isIdentical(active_links, check_active_links, false)); - EXPECT_TRUE(checker.getIsContactAllowedFn() == nullptr); + EXPECT_TRUE(checker.getContactAllowedValidator() == nullptr); checker.setCollisionMarginData(CollisionMarginData(0.25)); EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.25, 1e-5); diff --git a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_octomap_sphere_unit.hpp b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_octomap_sphere_unit.hpp index f56938d696e..bd2687db0e3 100644 --- a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_octomap_sphere_unit.hpp +++ b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_octomap_sphere_unit.hpp @@ -95,7 +95,7 @@ inline void runTestTyped(DiscreteContactManager& checker, double tol, ContactTes std::vector check_active_links = checker.getActiveCollisionObjects(); EXPECT_TRUE(tesseract_common::isIdentical(active_links, check_active_links, false)); - EXPECT_TRUE(checker.getIsContactAllowedFn() == nullptr); + EXPECT_TRUE(checker.getContactAllowedValidator() == nullptr); checker.setDefaultCollisionMarginData(0.1); EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.1, 1e-5); diff --git a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_sphere_sphere_cast_unit.hpp b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_sphere_sphere_cast_unit.hpp index 807c3d2514a..422638d6d47 100644 --- a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_sphere_sphere_cast_unit.hpp +++ b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_sphere_sphere_cast_unit.hpp @@ -151,7 +151,7 @@ inline void runTestPrimitive(ContinuousContactManager& checker) std::vector check_active_links = checker.getActiveCollisionObjects(); EXPECT_TRUE(tesseract_common::isIdentical(active_links, check_active_links, false)); - EXPECT_TRUE(checker.getIsContactAllowedFn() == nullptr); + EXPECT_TRUE(checker.getContactAllowedValidator() == nullptr); checker.setCollisionMarginData(CollisionMarginData(0.1)); EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.1, 1e-5); @@ -307,7 +307,7 @@ inline void runTestConvex(ContinuousContactManager& checker) std::vector check_active_links = checker.getActiveCollisionObjects(); EXPECT_TRUE(tesseract_common::isIdentical(active_links, check_active_links, false)); - EXPECT_TRUE(checker.getIsContactAllowedFn() == nullptr); + EXPECT_TRUE(checker.getContactAllowedValidator() == nullptr); checker.setCollisionMarginData(CollisionMarginData(0.1)); EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.1, 1e-5); diff --git a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_sphere_sphere_unit.hpp b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_sphere_sphere_unit.hpp index b067e9b7ad1..9c18a8b4d51 100644 --- a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_sphere_sphere_unit.hpp +++ b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_sphere_sphere_unit.hpp @@ -141,7 +141,7 @@ inline void runTestPrimitive(DiscreteContactManager& checker) std::vector check_active_links = checker.getActiveCollisionObjects(); EXPECT_TRUE(tesseract_common::isIdentical(active_links, check_active_links, false)); - EXPECT_TRUE(checker.getIsContactAllowedFn() == nullptr); + EXPECT_TRUE(checker.getContactAllowedValidator() == nullptr); checker.setCollisionMarginData(CollisionMarginData(0.1)); EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.1, 1e-5); @@ -258,7 +258,7 @@ inline void runTestPrimitiveDistanceDisabled(DiscreteContactManager& checker) std::vector check_active_links = checker.getActiveCollisionObjects(); EXPECT_TRUE(tesseract_common::isIdentical(active_links, check_active_links, false)); - EXPECT_TRUE(checker.getIsContactAllowedFn() == nullptr); + EXPECT_TRUE(checker.getContactAllowedValidator() == nullptr); checker.setCollisionMarginData(CollisionMarginData(0.1)); EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.1, 1e-5); @@ -377,7 +377,7 @@ inline void runTestConvex1(DiscreteContactManager& checker) std::vector check_active_links = checker.getActiveCollisionObjects(); EXPECT_TRUE(tesseract_common::isIdentical(active_links, check_active_links, false)); - EXPECT_TRUE(checker.getIsContactAllowedFn() == nullptr); + EXPECT_TRUE(checker.getContactAllowedValidator() == nullptr); checker.setCollisionMarginData(CollisionMarginData(0.1)); EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.1, 1e-5); @@ -455,7 +455,7 @@ inline void runTestConvex2(DiscreteContactManager& checker) std::vector check_active_links = checker.getActiveCollisionObjects(); EXPECT_TRUE(tesseract_common::isIdentical(active_links, check_active_links, false)); - EXPECT_TRUE(checker.getIsContactAllowedFn() == nullptr); + EXPECT_TRUE(checker.getContactAllowedValidator() == nullptr); tesseract_common::TransformMap location; location["sphere_link"] = Eigen::Isometry3d::Identity(); @@ -505,7 +505,7 @@ inline void runTestConvex3(DiscreteContactManager& checker) std::vector check_active_links = checker.getActiveCollisionObjects(); EXPECT_TRUE(tesseract_common::isIdentical(active_links, check_active_links, false)); - EXPECT_TRUE(checker.getIsContactAllowedFn() == nullptr); + EXPECT_TRUE(checker.getContactAllowedValidator() == nullptr); checker.setCollisionMarginData(CollisionMarginData(0.1)); EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.1, 1e-5); diff --git a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/contact_manager_config_unit.hpp b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/contact_manager_config_unit.hpp index 92f3cfccd3c..ca0b87bcc70 100644 --- a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/contact_manager_config_unit.hpp +++ b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/contact_manager_config_unit.hpp @@ -127,7 +127,7 @@ inline void runTest(DiscreteContactManager& checker) std::vector check_active_links = checker.getActiveCollisionObjects(); EXPECT_TRUE(tesseract_common::isIdentical(active_links, check_active_links, false)); - EXPECT_TRUE(checker.getIsContactAllowedFn() == nullptr); + EXPECT_TRUE(checker.getContactAllowedValidator() == nullptr); checker.setDefaultCollisionMarginData(0.5); EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.5, 1e-5); @@ -235,7 +235,7 @@ inline void runTest(ContinuousContactManager& checker) std::vector check_active_links = checker.getActiveCollisionObjects(); EXPECT_TRUE(tesseract_common::isIdentical(active_links, check_active_links, false)); - EXPECT_TRUE(checker.getIsContactAllowedFn() == nullptr); + EXPECT_TRUE(checker.getContactAllowedValidator() == nullptr); checker.setDefaultCollisionMarginData(0.5); EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.5, 1e-5); diff --git a/tesseract_common/CMakeLists.txt b/tesseract_common/CMakeLists.txt index c13544902b1..b2e61c964da 100644 --- a/tesseract_common/CMakeLists.txt +++ b/tesseract_common/CMakeLists.txt @@ -74,6 +74,7 @@ add_library( src/any_poly.cpp src/calibration_info.cpp src/collision_margin_data.cpp + src/contact_allowed_validator.cpp src/joint_state.cpp src/manipulator_info.cpp src/kinematic_limits.cpp diff --git a/tesseract_common/include/tesseract_common/contact_allowed_validator.h b/tesseract_common/include/tesseract_common/contact_allowed_validator.h new file mode 100644 index 00000000000..12cd93138dc --- /dev/null +++ b/tesseract_common/include/tesseract_common/contact_allowed_validator.h @@ -0,0 +1,116 @@ +/** + * @file contact_allowed_validators.h + * @brief The contact allowed validator + * + * @author Levi Armstrong + * @date Jan 2, 2025 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2025, Levi Armstrong + * + * @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. + */ + +#ifndef TESSERACT_COMMON_CONTACT_ALLOWED_VALIDATOR_H +#define TESSERACT_COMMON_CONTACT_ALLOWED_VALIDATOR_H + +#include +#include +#include + +#include + +namespace tesseract_common +{ +/** @brief Should return true if links are allowed to be in collision, otherwise false. */ +class ContactAllowedValidator +{ +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + using UPtr = std::unique_ptr; + using ConstUPtr = std::unique_ptr; + + virtual ~ContactAllowedValidator() = default; + + virtual bool operator()(const std::string&, const std::string&) const = 0; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int version); // NOLINT +}; + +class ACMContactAllowedValidator : public ContactAllowedValidator +{ +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + using UPtr = std::unique_ptr; + using ConstUPtr = std::unique_ptr; + + ACMContactAllowedValidator() = default; // Required for serialization + ACMContactAllowedValidator(tesseract_common::AllowedCollisionMatrix acm); + + bool operator()(const std::string& link_name1, const std::string& link_name2) const override; + +protected: + tesseract_common::AllowedCollisionMatrix acm_; + + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int version); // NOLINT +}; + +/** @brief Identify how the two should be combined */ +enum class CombinedContactAllowedValidatorType +{ + /** @brief Combines the two ContactAllowedValidator with AND operator */ + AND, + /** @brief Combines the two ContactAllowedValidator with OR operator */ + OR, +}; + +class CombinedContactAllowedValidator : public ContactAllowedValidator +{ +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + using UPtr = std::unique_ptr; + using ConstUPtr = std::unique_ptr; + + CombinedContactAllowedValidator() = default; // Required for serialization + CombinedContactAllowedValidator(std::vector> validators, + CombinedContactAllowedValidatorType type); + + bool operator()(const std::string& link_name1, const std::string& link_name2) const override; + +protected: + std::vector> validators_; + CombinedContactAllowedValidatorType type_{ CombinedContactAllowedValidatorType::OR }; + + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int version); // NOLINT +}; + +} // namespace tesseract_common + +BOOST_CLASS_EXPORT_KEY(tesseract_common::ContactAllowedValidator) +BOOST_CLASS_EXPORT_KEY(tesseract_common::ACMContactAllowedValidator) +BOOST_CLASS_EXPORT_KEY(tesseract_common::CombinedContactAllowedValidator) + +#endif // TESSERACT_COMMON_CONTACT_ALLOWED_VALIDATOR_H diff --git a/tesseract_common/include/tesseract_common/fwd.h b/tesseract_common/include/tesseract_common/fwd.h index 5da074e5c4f..3fbc89e1737 100644 --- a/tesseract_common/include/tesseract_common/fwd.h +++ b/tesseract_common/include/tesseract_common/fwd.h @@ -47,6 +47,11 @@ struct ClassLoader; // allowed_collision_matrix class AllowedCollisionMatrix; +// contact_allowed_validator +class ContactAllowedValidator; +class ACMContactAllowedValidator; +class CombinedContactAllowedValidator; + // collision_margin_data.h enum class CollisionMarginOverrideType; class CollisionMarginData; diff --git a/tesseract_common/src/contact_allowed_validator.cpp b/tesseract_common/src/contact_allowed_validator.cpp new file mode 100644 index 00000000000..e3116a10f39 --- /dev/null +++ b/tesseract_common/src/contact_allowed_validator.cpp @@ -0,0 +1,100 @@ +/** + * @file contact_allowed_validators.cpp + * @brief The contact allowed validator + * + * @author Levi Armstrong + * @date Jan 2, 2025 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2025, Levi Armstrong + * + * @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 +#include +#include +#include + +namespace tesseract_common +{ +template +void ContactAllowedValidator::serialize(Archive& /*ar*/, const unsigned int /*version*/) +{ +} + +ACMContactAllowedValidator::ACMContactAllowedValidator(tesseract_common::AllowedCollisionMatrix acm) + : acm_(std::move(acm)) +{ +} + +bool ACMContactAllowedValidator::operator()(const std::string& link_name1, const std::string& link_name2) const +{ + return acm_.isCollisionAllowed(link_name1, link_name2); +} + +template +void ACMContactAllowedValidator::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(ContactAllowedValidator); + ar& BOOST_SERIALIZATION_NVP(acm_); +} + +CombinedContactAllowedValidator::CombinedContactAllowedValidator( + std::vector> validators, + CombinedContactAllowedValidatorType type) + : validators_(std::move(validators)), type_(type) +{ +} + +bool CombinedContactAllowedValidator::operator()(const std::string& link_name1, const std::string& link_name2) const +{ + assert(!validators_.empty()); + if (type_ == CombinedContactAllowedValidatorType::OR) + { + bool value{ false }; + for (const auto& validator : validators_) + value = value || (*validator)(link_name1, link_name2); + + return value; + } + + bool value{ true }; + for (const auto& validator : validators_) + value = value && (*validator)(link_name1, link_name2); + + return value; +} + +template +void CombinedContactAllowedValidator::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(ContactAllowedValidator); + ar& BOOST_SERIALIZATION_NVP(validators_); + ar& BOOST_SERIALIZATION_NVP(type_); +} + +} // namespace tesseract_common + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_common::ContactAllowedValidator) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_common::ContactAllowedValidator) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_common::ACMContactAllowedValidator) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_common::ACMContactAllowedValidator) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_common::CombinedContactAllowedValidator) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_common::CombinedContactAllowedValidator) diff --git a/tesseract_environment/include/tesseract_environment/environment.h b/tesseract_environment/include/tesseract_environment/environment.h index 3d5539080b7..d77415506bc 100644 --- a/tesseract_environment/include/tesseract_environment/environment.h +++ b/tesseract_environment/include/tesseract_environment/environment.h @@ -50,6 +50,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include namespace boost::serialization { @@ -556,6 +557,7 @@ class Environment /** @brief This should only be used by the clone method */ explicit Environment(std::unique_ptr impl); }; + } // namespace tesseract_environment BOOST_CLASS_EXPORT_KEY(tesseract_environment::Environment) TESSERACT_ANY_EXPORT_KEY(std::shared_ptr, diff --git a/tesseract_environment/src/environment.cpp b/tesseract_environment/src/environment.cpp index ec2522baa0f..c76a790f7b7 100644 --- a/tesseract_environment/src/environment.cpp +++ b/tesseract_environment/src/environment.cpp @@ -74,6 +74,32 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_environment { +class EnvironmentContactAllowedValidator : public tesseract_common::ContactAllowedValidator +{ +public: + EnvironmentContactAllowedValidator() = default; // Required for serialization + EnvironmentContactAllowedValidator(std::shared_ptr scene_graph) + : scene_graph_(std::move(scene_graph)) + { + } + + bool operator()(const std::string& link_name1, const std::string& link_name2) const override + { + return scene_graph_->isCollisionAllowed(link_name1, link_name2); + } + +protected: + std::shared_ptr scene_graph_; + + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int /*version*/) // NOLINT + { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(ContactAllowedValidator); + ar& BOOST_SERIALIZATION_NVP(scene_graph_); + } +}; + void getCollisionObject(std::vector>& shapes, tesseract_common::VectorIsometry3d& shape_poses, const tesseract_scene_graph::Link& link) @@ -171,10 +197,9 @@ struct Environment::Implementation std::unique_ptr state_solver{ nullptr }; /** - * @brief The function used to determine if two objects are allowed in collision - * @todo This needs to be switched to class so it may be serialized + * @brief The validator used to determine if two objects are allowed in collision */ - std::function is_contact_allowed_fn; + tesseract_common::ContactAllowedValidator::ConstPtr contact_allowed_validator; /** * @brief A vector of user defined callbacks for locating tool center point @@ -378,6 +403,8 @@ struct Environment::Implementation ar& BOOST_SERIALIZATION_NVP(commands); ar& BOOST_SERIALIZATION_NVP(init_revision); ar& BOOST_SERIALIZATION_NVP(current_state); + // No need to serialize the contact allowed validator because it cannot be modified and is constructed internally + // from the scene graph ar& boost::serialization::make_nvp("timestamp", boost::serialization::make_binary_object(×tamp, sizeof(timestamp))); ar& boost::serialization::make_nvp( @@ -400,6 +427,9 @@ struct Environment::Implementation ar& boost::serialization::make_nvp("current_state", current_state); setState(current_state.joints); + // No need to serialize the contact allowed validator because it cannot be modified and is constructed internally + // from the scene graph + ar& boost::serialization::make_nvp("timestamp", boost::serialization::make_binary_object(×tamp, sizeof(timestamp))); ar& boost::serialization::make_nvp( @@ -436,8 +466,10 @@ bool Environment::Implementation::operator==(const Environment::Implementation& equal &= timestamp == rhs.timestamp; equal &= current_state_timestamp == rhs.current_state_timestamp; + // No need to check contact_allowed validator because it is constructed internally from the scene graph and cannot be + // changed + /** @todo uncomment after serialized */ - // equal &= is_contact_allowed_fn == rhs.is_contact_allowed_fn; // equal &= find_tcp_cb == rhs.find_tcp_cb; return equal; @@ -483,20 +515,17 @@ std::unique_ptr Environment::Implementation::clone( cloned_env->group_joint_names_cache = group_joint_names_cache; // NOLINTNEXTLINE - cloned_env->is_contact_allowed_fn = std::bind(&tesseract_scene_graph::SceneGraph::isCollisionAllowed, - cloned_env->scene_graph, - std::placeholders::_1, - std::placeholders::_2); + cloned_env->contact_allowed_validator = std::make_shared(cloned_env->scene_graph); if (discrete_manager) { cloned_env->discrete_manager = discrete_manager->clone(); - cloned_env->discrete_manager->setIsContactAllowedFn(cloned_env->is_contact_allowed_fn); + cloned_env->discrete_manager->setContactAllowedValidator(cloned_env->contact_allowed_validator); } if (continuous_manager) { cloned_env->continuous_manager = continuous_manager->clone(); - cloned_env->continuous_manager->setIsContactAllowedFn(cloned_env->is_contact_allowed_fn); + cloned_env->continuous_manager->setContactAllowedValidator(cloned_env->contact_allowed_validator); } cloned_env->contact_managers_plugin_info = contact_managers_plugin_info; @@ -522,9 +551,7 @@ bool Environment::Implementation::initHelper(const std::vector( std::static_pointer_cast(commands.at(0))->getSceneGraph()->getName()); - is_contact_allowed_fn = [this](const std::string& l1, const std::string& l2) { - return scene_graph->isCollisionAllowed(l1, l2); - }; + contact_allowed_validator = std::make_shared(scene_graph); if (!applyCommandsHelper(commands)) { @@ -603,7 +630,7 @@ void Environment::Implementation::clear() state_solver = nullptr; current_state = tesseract_scene_graph::SceneState(); commands.clear(); - is_contact_allowed_fn = nullptr; + contact_allowed_validator = nullptr; collision_margin_data = tesseract_collision::CollisionMarginData(); kinematics_information.clear(); contact_managers_plugin_info.clear(); @@ -990,7 +1017,7 @@ Environment::Implementation::getDiscreteContactManagerHelper(const std::string& if (manager == nullptr) return nullptr; - manager->setIsContactAllowedFn(is_contact_allowed_fn); + manager->setContactAllowedValidator(contact_allowed_validator); if (scene_graph != nullptr) { for (const auto& link : scene_graph->getLinks()) @@ -1023,7 +1050,7 @@ Environment::Implementation::getContinuousContactManagerHelper(const std::string if (manager == nullptr) return nullptr; - manager->setIsContactAllowedFn(is_contact_allowed_fn); + manager->setContactAllowedValidator(contact_allowed_validator); if (scene_graph != nullptr) { for (const auto& link : scene_graph->getLinks()) @@ -2693,6 +2720,9 @@ void Environment::serialize(Archive& ar, const unsigned int version) } // namespace tesseract_environment #include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_environment::EnvironmentContactAllowedValidator) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_environment::EnvironmentContactAllowedValidator) + TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_environment::Environment) BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_environment::Environment) diff --git a/tesseract_environment/test/tesseract_environment_unit.cpp b/tesseract_environment/test/tesseract_environment_unit.cpp index 00de706e59c..51e2da0770d 100644 --- a/tesseract_environment/test/tesseract_environment_unit.cpp +++ b/tesseract_environment/test/tesseract_environment_unit.cpp @@ -278,8 +278,8 @@ Environment::Ptr getEnvironment(EnvironmentInitType init_type = EnvironmentInitT EXPECT_EQ(group_names_ki[1], "manipulator_joint_group"); // Check allowed collision matrix is not nullptr - EXPECT_TRUE(env->getDiscreteContactManager()->getIsContactAllowedFn() != nullptr); - EXPECT_TRUE(env->getContinuousContactManager()->getIsContactAllowedFn() != nullptr); + EXPECT_TRUE(env->getDiscreteContactManager()->getContactAllowedValidator() != nullptr); + EXPECT_TRUE(env->getContinuousContactManager()->getContactAllowedValidator() != nullptr); // Get active contact managers { diff --git a/tesseract_environment/test/tesseract_environment_utils.cpp b/tesseract_environment/test/tesseract_environment_utils.cpp index b4d9d5c6b82..ab4ae991f61 100644 --- a/tesseract_environment/test/tesseract_environment_utils.cpp +++ b/tesseract_environment/test/tesseract_environment_utils.cpp @@ -49,8 +49,8 @@ void checkIsAllowedFnOverride(std::unique_ptr manager) config.contact_manager_config.acm.addAllowedCollision("allowed_link_1a", "allowed_link_2a", "Unit test"); config.contact_manager_config.acm_override_type = ACMOverrideType::ASSIGN; manager->applyContactManagerConfig(config.contact_manager_config); - auto fn = manager->getIsContactAllowedFn(); - EXPECT_TRUE(fn("allowed_link_1a", "allowed_link_2a")); + auto fn = manager->getContactAllowedValidator(); + EXPECT_TRUE((*fn)("allowed_link_1a", "allowed_link_2a")); } // NONE @@ -59,8 +59,8 @@ void checkIsAllowedFnOverride(std::unique_ptr manager) config.contact_manager_config.acm.addAllowedCollision("allowed_link_1b", "allowed_link_2b", "Unit test"); config.contact_manager_config.acm_override_type = ACMOverrideType::NONE; manager->applyContactManagerConfig(config.contact_manager_config); - auto fn = manager->getIsContactAllowedFn(); - EXPECT_FALSE(fn("allowed_link_1b", "allowed_link_2b")); + auto fn = manager->getContactAllowedValidator(); + EXPECT_FALSE((*fn)("allowed_link_1b", "allowed_link_2b")); } // OR @@ -69,9 +69,9 @@ void checkIsAllowedFnOverride(std::unique_ptr manager) config.contact_manager_config.acm.addAllowedCollision("allowed_link_1c", "allowed_link_2c", "Unit test"); config.contact_manager_config.acm_override_type = ACMOverrideType::OR; manager->applyContactManagerConfig(config.contact_manager_config); - auto fn = manager->getIsContactAllowedFn(); - EXPECT_TRUE(fn("allowed_link_1a", "allowed_link_2a")); - EXPECT_TRUE(fn("allowed_link_1c", "allowed_link_2c")); + auto fn = manager->getContactAllowedValidator(); + EXPECT_TRUE((*fn)("allowed_link_1a", "allowed_link_2a")); + EXPECT_TRUE((*fn)("allowed_link_1c", "allowed_link_2c")); } // AND @@ -80,9 +80,9 @@ void checkIsAllowedFnOverride(std::unique_ptr manager) config.contact_manager_config.acm.removeAllowedCollision("allowed_link_1a", "allowed_link_2a"); config.contact_manager_config.acm_override_type = ACMOverrideType::AND; manager->applyContactManagerConfig(config.contact_manager_config); - auto fn = manager->getIsContactAllowedFn(); - EXPECT_FALSE(fn("allowed_link_1a", "allowed_link_2a")); - EXPECT_TRUE(fn("allowed_link_1c", "allowed_link_2c")); + auto fn = manager->getContactAllowedValidator(); + EXPECT_FALSE((*fn)("allowed_link_1a", "allowed_link_2a")); + EXPECT_TRUE((*fn)("allowed_link_1c", "allowed_link_2c")); } }