Skip to content

Commit 11d935b

Browse files
Levi-ArmstrongLevi-Armstrong
authored
Add ContactAllowedValidator and ContactResultValidator (#1095)
1 parent 16a4193 commit 11d935b

File tree

56 files changed

+682
-244
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

56 files changed

+682
-244
lines changed

‎tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_cast_bvh_manager.h‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -125,9 +125,10 @@ class BulletCastBVHManager : public ContinuousContactManager
125125

126126
const CollisionMarginData& getCollisionMarginData() const override final;
127127

128-
void setIsContactAllowedFn(IsContactAllowedFn fn) override final;
128+
void
129+
setContactAllowedValidator(std::shared_ptr<const tesseract_common::ContactAllowedValidator> validator) override final;
129130

130-
IsContactAllowedFn getIsContactAllowedFn() const override final;
131+
std::shared_ptr<const tesseract_common::ContactAllowedValidator> getContactAllowedValidator() const override final;
131132

132133
void contactTest(ContactResultMap& collisions, const ContactRequest& request) override final;
133134

‎tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_cast_simple_manager.h‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -125,9 +125,10 @@ class BulletCastSimpleManager : public ContinuousContactManager
125125
const std::string& name2,
126126
double collision_margin) override final;
127127

128-
void setIsContactAllowedFn(IsContactAllowedFn fn) override final;
128+
void
129+
setContactAllowedValidator(std::shared_ptr<const tesseract_common::ContactAllowedValidator> validator) override final;
129130

130-
IsContactAllowedFn getIsContactAllowedFn() const override final;
131+
std::shared_ptr<const tesseract_common::ContactAllowedValidator> getContactAllowedValidator() const override final;
131132

132133
void contactTest(ContactResultMap& collisions, const ContactRequest& request) override final;
133134

‎tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_discrete_bvh_manager.h‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -114,9 +114,10 @@ class BulletDiscreteBVHManager : public DiscreteContactManager
114114

115115
const CollisionMarginData& getCollisionMarginData() const override final;
116116

117-
void setIsContactAllowedFn(IsContactAllowedFn fn) override final;
117+
void
118+
setContactAllowedValidator(std::shared_ptr<const tesseract_common::ContactAllowedValidator> validator) override final;
118119

119-
IsContactAllowedFn getIsContactAllowedFn() const override final;
120+
std::shared_ptr<const tesseract_common::ContactAllowedValidator> getContactAllowedValidator() const override final;
120121

121122
void contactTest(ContactResultMap& collisions, const ContactRequest& request) override final;
122123

‎tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_discrete_simple_manager.h‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -114,9 +114,10 @@ class BulletDiscreteSimpleManager : public DiscreteContactManager
114114

115115
const CollisionMarginData& getCollisionMarginData() const override final;
116116

117-
void setIsContactAllowedFn(IsContactAllowedFn fn) override final;
117+
void
118+
setContactAllowedValidator(std::shared_ptr<const tesseract_common::ContactAllowedValidator> validator) override final;
118119

119-
IsContactAllowedFn getIsContactAllowedFn() const override final;
120+
std::shared_ptr<const tesseract_common::ContactAllowedValidator> getContactAllowedValidator() const override final;
120121

121122
void contactTest(ContactResultMap& collisions, const ContactRequest& request) override final;
122123

‎tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_utils.h‎

Lines changed: 5 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -209,11 +209,14 @@ btTransform getLinkTransformFromCOW(const btCollisionObjectWrapper* cow);
209209
* @brief This is used to check if a collision check is required between the provided two collision objects
210210
* @param cow1 The first collision object
211211
* @param cow2 The second collision object
212-
* @param acm The contact allowed function pointer
212+
* @param validator The contact allowed validator
213213
* @param verbose Indicate if verbose information should be printed to the terminal
214214
* @return True if the two collision objects should be checked for collision, otherwise false
215215
*/
216-
bool needsCollisionCheck(const COW& cow1, const COW& cow2, const IsContactAllowedFn& acm, bool verbose = false);
216+
bool needsCollisionCheck(const COW& cow1,
217+
const COW& cow2,
218+
const std::shared_ptr<const tesseract_common::ContactAllowedValidator>& validator,
219+
bool verbose = false);
217220

218221
btScalar addDiscreteSingleResult(btManifoldPoint& cp,
219222
const btCollisionObjectWrapper* colObj0Wrap,

‎tesseract_collision/bullet/src/bullet_cast_bvh_manager.cpp‎

Lines changed: 13 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -39,7 +39,8 @@
3939
* POSSIBILITY OF SUCH DAMAGE.
4040
*/
4141

42-
#include "tesseract_collision/bullet/bullet_cast_bvh_manager.h"
42+
#include <tesseract_collision/bullet/bullet_cast_bvh_manager.h>
43+
#include <tesseract_common/contact_allowed_validator.h>
4344

4445
extern btScalar gDbvtMargin; // NOLINT
4546

@@ -104,7 +105,7 @@ ContinuousContactManager::UPtr BulletCastBVHManager::clone() const
104105

105106
manager->setActiveCollisionObjects(active_);
106107
manager->setCollisionMarginData(contact_test_data_.collision_margin_data);
107-
manager->setIsContactAllowedFn(contact_test_data_.fn);
108+
manager->setContactAllowedValidator(contact_test_data_.validator);
108109

109110
return manager;
110111
}
@@ -451,8 +452,16 @@ const CollisionMarginData& BulletCastBVHManager::getCollisionMarginData() const
451452
{
452453
return contact_test_data_.collision_margin_data;
453454
}
454-
void BulletCastBVHManager::setIsContactAllowedFn(IsContactAllowedFn fn) { contact_test_data_.fn = fn; }
455-
IsContactAllowedFn BulletCastBVHManager::getIsContactAllowedFn() const { return contact_test_data_.fn; }
455+
void BulletCastBVHManager::setContactAllowedValidator(
456+
std::shared_ptr<const tesseract_common::ContactAllowedValidator> validator)
457+
{
458+
contact_test_data_.validator = std::move(validator);
459+
}
460+
std::shared_ptr<const tesseract_common::ContactAllowedValidator>
461+
BulletCastBVHManager::getContactAllowedValidator() const
462+
{
463+
return contact_test_data_.validator;
464+
}
456465
void BulletCastBVHManager::contactTest(ContactResultMap& collisions, const ContactRequest& request)
457466
{
458467
contact_test_data_.res = &collisions;

‎tesseract_collision/bullet/src/bullet_cast_simple_manager.cpp‎

Lines changed: 14 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -39,7 +39,8 @@
3939
* POSSIBILITY OF SUCH DAMAGE.
4040
*/
4141

42-
#include "tesseract_collision/bullet/bullet_cast_simple_manager.h"
42+
#include <tesseract_collision/bullet/bullet_cast_simple_manager.h>
43+
#include <tesseract_common/contact_allowed_validator.h>
4344

4445
namespace tesseract_collision::tesseract_collision_bullet
4546
{
@@ -84,7 +85,7 @@ ContinuousContactManager::UPtr BulletCastSimpleManager::clone() const
8485

8586
manager->setActiveCollisionObjects(active_);
8687
manager->setCollisionMarginData(contact_test_data_.collision_margin_data);
87-
manager->setIsContactAllowedFn(contact_test_data_.fn);
88+
manager->setContactAllowedValidator(contact_test_data_.validator);
8889

8990
return manager;
9091
}
@@ -364,8 +365,16 @@ const CollisionMarginData& BulletCastSimpleManager::getCollisionMarginData() con
364365
{
365366
return contact_test_data_.collision_margin_data;
366367
}
367-
void BulletCastSimpleManager::setIsContactAllowedFn(IsContactAllowedFn fn) { contact_test_data_.fn = fn; }
368-
IsContactAllowedFn BulletCastSimpleManager::getIsContactAllowedFn() const { return contact_test_data_.fn; }
368+
void BulletCastSimpleManager::setContactAllowedValidator(
369+
std::shared_ptr<const tesseract_common::ContactAllowedValidator> validator)
370+
{
371+
contact_test_data_.validator = std::move(validator);
372+
}
373+
std::shared_ptr<const tesseract_common::ContactAllowedValidator>
374+
BulletCastSimpleManager::getContactAllowedValidator() const
375+
{
376+
return contact_test_data_.validator;
377+
}
369378
void BulletCastSimpleManager::contactTest(ContactResultMap& collisions, const ContactRequest& request)
370379
{
371380
contact_test_data_.res = &collisions;
@@ -401,7 +410,7 @@ void BulletCastSimpleManager::contactTest(ContactResultMap& collisions, const Co
401410

402411
if (aabb_check)
403412
{
404-
bool needs_collision = needsCollisionCheck(*cow1, *cow2, contact_test_data_.fn, false);
413+
bool needs_collision = needsCollisionCheck(*cow1, *cow2, contact_test_data_.validator, false);
405414

406415
if (needs_collision)
407416
{

‎tesseract_collision/bullet/src/bullet_discrete_bvh_manager.cpp‎

Lines changed: 12 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -40,6 +40,7 @@
4040
*/
4141

4242
#include <tesseract_collision/bullet/bullet_discrete_bvh_manager.h>
43+
#include <tesseract_common/contact_allowed_validator.h>
4344

4445
extern btScalar gDbvtMargin; // NOLINT
4546

@@ -100,7 +101,7 @@ DiscreteContactManager::UPtr BulletDiscreteBVHManager::clone() const
100101

101102
manager->setActiveCollisionObjects(active_);
102103
manager->setCollisionMarginData(contact_test_data_.collision_margin_data);
103-
manager->setIsContactAllowedFn(contact_test_data_.fn);
104+
manager->setContactAllowedValidator(contact_test_data_.validator);
104105

105106
return manager;
106107
}
@@ -272,8 +273,16 @@ const CollisionMarginData& BulletDiscreteBVHManager::getCollisionMarginData() co
272273
{
273274
return contact_test_data_.collision_margin_data;
274275
}
275-
void BulletDiscreteBVHManager::setIsContactAllowedFn(IsContactAllowedFn fn) { contact_test_data_.fn = fn; }
276-
IsContactAllowedFn BulletDiscreteBVHManager::getIsContactAllowedFn() const { return contact_test_data_.fn; }
276+
void BulletDiscreteBVHManager::setContactAllowedValidator(
277+
std::shared_ptr<const tesseract_common::ContactAllowedValidator> validator)
278+
{
279+
contact_test_data_.validator = std::move(validator);
280+
}
281+
std::shared_ptr<const tesseract_common::ContactAllowedValidator>
282+
BulletDiscreteBVHManager::getContactAllowedValidator() const
283+
{
284+
return contact_test_data_.validator;
285+
}
277286
void BulletDiscreteBVHManager::contactTest(ContactResultMap& collisions, const ContactRequest& request)
278287
{
279288
contact_test_data_.res = &collisions;

‎tesseract_collision/bullet/src/bullet_discrete_simple_manager.cpp‎

Lines changed: 14 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -39,7 +39,8 @@
3939
* POSSIBILITY OF SUCH DAMAGE.
4040
*/
4141

42-
#include "tesseract_collision/bullet/bullet_discrete_simple_manager.h"
42+
#include <tesseract_collision/bullet/bullet_discrete_simple_manager.h>
43+
#include <tesseract_common/contact_allowed_validator.h>
4344

4445
namespace tesseract_collision::tesseract_collision_bullet
4546
{
@@ -86,7 +87,7 @@ DiscreteContactManager::UPtr BulletDiscreteSimpleManager::clone() const
8687

8788
manager->setActiveCollisionObjects(active_);
8889
manager->setCollisionMarginData(contact_test_data_.collision_margin_data);
89-
manager->setIsContactAllowedFn(contact_test_data_.fn);
90+
manager->setContactAllowedValidator(contact_test_data_.validator);
9091

9192
return manager;
9293
}
@@ -250,8 +251,16 @@ const CollisionMarginData& BulletDiscreteSimpleManager::getCollisionMarginData()
250251
{
251252
return contact_test_data_.collision_margin_data;
252253
}
253-
void BulletDiscreteSimpleManager::setIsContactAllowedFn(IsContactAllowedFn fn) { contact_test_data_.fn = fn; }
254-
IsContactAllowedFn BulletDiscreteSimpleManager::getIsContactAllowedFn() const { return contact_test_data_.fn; }
254+
void BulletDiscreteSimpleManager::setContactAllowedValidator(
255+
std::shared_ptr<const tesseract_common::ContactAllowedValidator> validator)
256+
{
257+
contact_test_data_.validator = std::move(validator);
258+
}
259+
std::shared_ptr<const tesseract_common::ContactAllowedValidator>
260+
BulletDiscreteSimpleManager::getContactAllowedValidator() const
261+
{
262+
return contact_test_data_.validator;
263+
}
255264
void BulletDiscreteSimpleManager::contactTest(ContactResultMap& collisions, const ContactRequest& request)
256265
{
257266
contact_test_data_.res = &collisions;
@@ -287,7 +296,7 @@ void BulletDiscreteSimpleManager::contactTest(ContactResultMap& collisions, cons
287296

288297
if (aabb_check)
289298
{
290-
bool needs_collision = needsCollisionCheck(*cow1, *cow2, contact_test_data_.fn, false);
299+
bool needs_collision = needsCollisionCheck(*cow1, *cow2, contact_test_data_.validator, false);
291300

292301
if (needs_collision)
293302
{

‎tesseract_collision/bullet/src/bullet_utils.cpp‎

Lines changed: 8 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -694,11 +694,14 @@ btTransform getLinkTransformFromCOW(const btCollisionObjectWrapper* cow)
694694
return cow->getWorldTransform();
695695
}
696696

697-
bool needsCollisionCheck(const COW& cow1, const COW& cow2, const IsContactAllowedFn& acm, bool verbose)
697+
bool needsCollisionCheck(const COW& cow1,
698+
const COW& cow2,
699+
const std::shared_ptr<const tesseract_common::ContactAllowedValidator>& validator,
700+
bool verbose)
698701
{
699702
return cow1.m_enabled && cow2.m_enabled && (cow2.m_collisionFilterGroup & cow1.m_collisionFilterMask) && // NOLINT
700703
(cow1.m_collisionFilterGroup & cow2.m_collisionFilterMask) && // NOLINT
701-
!isContactAllowed(cow1.getName(), cow2.getName(), acm, verbose);
704+
!isContactAllowed(cow1.getName(), cow2.getName(), validator, verbose);
702705
}
703706

704707
btScalar addDiscreteSingleResult(btManifoldPoint& cp,
@@ -979,7 +982,7 @@ BroadphaseContactResultCallback::BroadphaseContactResultCallback(ContactTestData
979982
bool BroadphaseContactResultCallback::needsCollision(const CollisionObjectWrapper* cow0,
980983
const CollisionObjectWrapper* cow1) const
981984
{
982-
return !collisions_.done && needsCollisionCheck(*cow0, *cow1, collisions_.fn, verbose_);
985+
return !collisions_.done && needsCollisionCheck(*cow0, *cow1, collisions_.validator, verbose_);
983986
}
984987

985988
DiscreteBroadphaseContactResultCallback::DiscreteBroadphaseContactResultCallback(ContactTestData& collisions,
@@ -1182,7 +1185,7 @@ bool DiscreteCollisionCollector::needsCollision(btBroadphaseProxy* proxy0) const
11821185
{
11831186
return !collisions_.done &&
11841187
needsCollisionCheck(
1185-
*cow_, *(static_cast<CollisionObjectWrapper*>(proxy0->m_clientObject)), collisions_.fn, verbose_);
1188+
*cow_, *(static_cast<CollisionObjectWrapper*>(proxy0->m_clientObject)), collisions_.validator, verbose_);
11861189
}
11871190

11881191
CastCollisionCollector::CastCollisionCollector(ContactTestData& collisions,
@@ -1215,7 +1218,7 @@ bool CastCollisionCollector::needsCollision(btBroadphaseProxy* proxy0) const
12151218
{
12161219
return !collisions_.done &&
12171220
needsCollisionCheck(
1218-
*cow_, *(static_cast<CollisionObjectWrapper*>(proxy0->m_clientObject)), collisions_.fn, verbose_);
1221+
*cow_, *(static_cast<CollisionObjectWrapper*>(proxy0->m_clientObject)), collisions_.validator, verbose_);
12191222
}
12201223

12211224
COW::Ptr makeCastCollisionObject(const COW::Ptr& cow)

0 commit comments

Comments
 (0)