Add ContactAllowedValidator and ContactResultValidator

tesseract-robotics · Jan 3, 2025 · 302ee29 · 302ee29
1 parent 5416ec6
commit 302ee29
Showing 55 changed files with 677 additions and 239 deletions.
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
@@ -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<const tesseract_common::ContactAllowedValidator> validator) override final;
 
-  IsContactAllowedFn getIsContactAllowedFn() const override final;
+  std::shared_ptr<const tesseract_common::ContactAllowedValidator> 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
@@ -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<const tesseract_common::ContactAllowedValidator> validator) override final;
 
-  IsContactAllowedFn getIsContactAllowedFn() const override final;
+  std::shared_ptr<const tesseract_common::ContactAllowedValidator> 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
@@ -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<const tesseract_common::ContactAllowedValidator> validator) override final;
 
-  IsContactAllowedFn getIsContactAllowedFn() const override final;
+  std::shared_ptr<const tesseract_common::ContactAllowedValidator> getContactAllowedValidator() const override final;
 
   void contactTest(ContactResultMap& collisions, const ContactRequest& request) override final;
 

diff --git a/...ract_collision/bullet/include/tesseract_collision/bullet/bullet_discrete_simple_manager.h b/...ract_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<const tesseract_common::ContactAllowedValidator> validator) override final;
 
-  IsContactAllowedFn getIsContactAllowedFn() const override final;
+  std::shared_ptr<const tesseract_common::ContactAllowedValidator> 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
@@ -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<const tesseract_common::ContactAllowedValidator>& 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
@@ -39,7 +39,8 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include "tesseract_collision/bullet/bullet_cast_bvh_manager.h"
+#include <tesseract_collision/bullet/bullet_cast_bvh_manager.h>
+#include <tesseract_common/contact_allowed_validator.h>
 
 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_.fn);
 
   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<const tesseract_common::ContactAllowedValidator> validator)
+{
+  contact_test_data_.fn = std::move(validator);
+}
+std::shared_ptr<const tesseract_common::ContactAllowedValidator>
+BulletCastBVHManager::getContactAllowedValidator() const
+{
+  return contact_test_data_.fn;
+}
 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
@@ -39,7 +39,8 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include "tesseract_collision/bullet/bullet_cast_simple_manager.h"
+#include <tesseract_collision/bullet/bullet_cast_simple_manager.h>
+#include <tesseract_common/contact_allowed_validator.h>
 
 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_.fn);
 
   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<const tesseract_common::ContactAllowedValidator> validator)
+{
+  contact_test_data_.fn = std::move(validator);
+}
+std::shared_ptr<const tesseract_common::ContactAllowedValidator>
+BulletCastSimpleManager::getContactAllowedValidator() const
+{
+  return contact_test_data_.fn;
+}
 void BulletCastSimpleManager::contactTest(ContactResultMap& collisions, const ContactRequest& request)
 {
   contact_test_data_.res = &collisions;

diff --git 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 <tesseract_collision/bullet/bullet_discrete_bvh_manager.h>
+#include <tesseract_common/contact_allowed_validator.h>
 
 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_.fn);
 
   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<const tesseract_common::ContactAllowedValidator> validator)
+{
+  contact_test_data_.fn = std::move(validator);
+}
+std::shared_ptr<const tesseract_common::ContactAllowedValidator>
+BulletDiscreteBVHManager::getContactAllowedValidator() const
+{
+  return contact_test_data_.fn;
+}
 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
@@ -39,7 +39,8 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include "tesseract_collision/bullet/bullet_discrete_simple_manager.h"
+#include <tesseract_collision/bullet/bullet_discrete_simple_manager.h>
+#include <tesseract_common/contact_allowed_validator.h>
 
 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_.fn);
 
   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<const tesseract_common::ContactAllowedValidator> validator)
+{
+  contact_test_data_.fn = std::move(validator);
+}
+std::shared_ptr<const tesseract_common::ContactAllowedValidator>
+BulletDiscreteSimpleManager::getContactAllowedValidator() const
+{
+  return contact_test_data_.fn;
+}
 void BulletDiscreteSimpleManager::contactTest(ContactResultMap& collisions, const ContactRequest& request)
 {
   contact_test_data_.res = &collisions;

diff --git 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<const tesseract_common::ContactAllowedValidator>& 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,

diff --git 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
@@ -33,6 +33,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_collision/core/types.h>
+#include <tesseract_common/contact_allowed_validator.h>
 
 namespace tesseract_collision
 {
@@ -43,12 +44,13 @@ using ObjectPairKey = std::pair<std::string, std::string>;
  * @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<ObjectPairKey> getCollisionObjectPairs(const std::vector<std::string>& active_links,
-                                                   const std::vector<std::string>& static_links,
-                                                   const IsContactAllowedFn& acm = nullptr);
+std::vector<ObjectPairKey>
+getCollisionObjectPairs(const std::vector<std::string>& active_links,
+                        const std::vector<std::string>& static_links,
+                        const std::shared_ptr<const tesseract_common::ContactAllowedValidator>& 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<std::string>& 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<const tesseract_common::ContactAllowedValidator>& 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
@@ -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 <memory>
+#include <boost/serialization/access.hpp>
+#include <boost/serialization/export.hpp>
+
+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<ContactResultValidator>;
+  using ConstPtr = std::shared_ptr<const ContactResultValidator>;
+  using UPtr = std::unique_ptr<ContactResultValidator>;
+  using ConstUPtr = std::unique_ptr<const ContactResultValidator>;
+
+  virtual ~ContactResultValidator() = default;
+
+  virtual bool operator()(const ContactResult&) const = 0;
+
+protected:
+  friend class boost::serialization::access;
+  template <class Archive>
+  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
@@ -34,6 +34,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_collision/core/types.h>
+#include <tesseract_common/fwd.h>
 
 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<const tesseract_common::ContactAllowedValidator> 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<const tesseract_common::ContactAllowedValidator> 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
@@ -34,6 +34,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_collision/core/types.h>
+#include <tesseract_common/fwd.h>
 
 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<const tesseract_common::ContactAllowedValidator> 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<const tesseract_common::ContactAllowedValidator> getContactAllowedValidator() const = 0;
 
   /**
    * @brief Perform a contact test for all objects based