From 340458e525101160b9386fc1999de675b801d3f7 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Wed, 25 Oct 2023 10:33:09 -0500 Subject: [PATCH] Keep active links updated in contact managers when adding and removing links --- .../bullet/src/bullet_cast_bvh_manager.cpp | 13 +++++- .../bullet/src/bullet_cast_simple_manager.cpp | 6 +++ .../src/bullet_discrete_bvh_manager.cpp | 4 ++ .../src/bullet_discrete_simple_manager.cpp | 6 +++ .../fcl/src/fcl_discrete_managers.cpp | 2 + .../test_suite/collision_box_box_unit.hpp | 43 +++++++++++++++++++ 6 files changed, 73 insertions(+), 1 deletion(-) diff --git a/tesseract_collision/bullet/src/bullet_cast_bvh_manager.cpp b/tesseract_collision/bullet/src/bullet_cast_bvh_manager.cpp index a69a52e1209..289b7a66ab1 100644 --- a/tesseract_collision/bullet/src/bullet_cast_bvh_manager.cpp +++ b/tesseract_collision/bullet/src/bullet_cast_bvh_manager.cpp @@ -164,6 +164,8 @@ bool BulletCastBVHManager::removeCollisionObject(const std::string& name) removeCollisionObjectFromBroadphase(cow2, broadphase_, dispatcher_); link2castcow_.erase(name); + active_.erase(std::find(active_.begin(), active_.end(), name)); + return true; } @@ -484,7 +486,16 @@ void BulletCastBVHManager::addCollisionObject(const COW::Ptr& cow) // Add it to the cast map link2castcow_[cast_cow->getName()] = cast_cow; - const COW::Ptr& selected_cow = (cow->m_collisionFilterGroup == btBroadphaseProxy::KinematicFilter) ? cast_cow : cow; + COW::Ptr selected_cow; + if (cow->m_collisionFilterGroup == btBroadphaseProxy::KinematicFilter) + { + active_.push_back(cow->getName()); + selected_cow = cast_cow; + } + else + { + selected_cow = cow; + } btVector3 aabb_min, aabb_max; selected_cow->getAABB(aabb_min, aabb_max); diff --git a/tesseract_collision/bullet/src/bullet_cast_simple_manager.cpp b/tesseract_collision/bullet/src/bullet_cast_simple_manager.cpp index fc2f8e74de9..275ea3f74c1 100644 --- a/tesseract_collision/bullet/src/bullet_cast_simple_manager.cpp +++ b/tesseract_collision/bullet/src/bullet_cast_simple_manager.cpp @@ -139,6 +139,7 @@ bool BulletCastSimpleManager::removeCollisionObject(const std::string& name) collision_objects_.erase(std::find(collision_objects_.begin(), collision_objects_.end(), name)); link2cow_.erase(name); link2castcow_.erase(name); + active_.erase(std::find(active_.begin(), active_.end(), name)); return true; } @@ -448,9 +449,14 @@ void BulletCastSimpleManager::addCollisionObject(const COW::Ptr& cow) link2castcow_[cast_cow->getName()] = cast_cow; if (cow->m_collisionFilterGroup == btBroadphaseProxy::KinematicFilter) + { cows_.insert(cows_.begin(), cast_cow); + active_.push_back(cow->getName()); + } else + { cows_.push_back(cow); + } } void BulletCastSimpleManager::onCollisionMarginDataChanged() diff --git a/tesseract_collision/bullet/src/bullet_discrete_bvh_manager.cpp b/tesseract_collision/bullet/src/bullet_discrete_bvh_manager.cpp index 00dd1845136..4880f9c5209 100644 --- a/tesseract_collision/bullet/src/bullet_discrete_bvh_manager.cpp +++ b/tesseract_collision/bullet/src/bullet_discrete_bvh_manager.cpp @@ -154,6 +154,7 @@ bool BulletDiscreteBVHManager::removeCollisionObject(const std::string& name) collision_objects_.erase(std::find(collision_objects_.begin(), collision_objects_.end(), name)); removeCollisionObjectFromBroadphase(it->second, broadphase_, dispatcher_); link2cow_.erase(name); + active_.erase(std::find(active_.begin(), active_.end(), name)); return true; } @@ -298,6 +299,9 @@ void BulletDiscreteBVHManager::addCollisionObject(const COW::Ptr& cow) link2cow_[cow->getName()] = cow; collision_objects_.push_back(cow->getName()); + if (cow->m_collisionFilterGroup == btBroadphaseProxy::KinematicFilter) + active_.push_back(cow->getName()); + // Add collision object to broadphase addCollisionObjectToBroadphase(cow, broadphase_, dispatcher_); } diff --git a/tesseract_collision/bullet/src/bullet_discrete_simple_manager.cpp b/tesseract_collision/bullet/src/bullet_discrete_simple_manager.cpp index fb3523393d9..3d6ee366576 100644 --- a/tesseract_collision/bullet/src/bullet_discrete_simple_manager.cpp +++ b/tesseract_collision/bullet/src/bullet_discrete_simple_manager.cpp @@ -140,6 +140,7 @@ bool BulletDiscreteSimpleManager::removeCollisionObject(const std::string& name) cows_.erase(std::find(cows_.begin(), cows_.end(), it->second)); collision_objects_.erase(std::find(collision_objects_.begin(), collision_objects_.end(), name)); link2cow_.erase(name); + active_.erase(std::find(active_.begin(), active_.end(), name)); return true; } @@ -327,9 +328,14 @@ void BulletDiscreteSimpleManager::addCollisionObject(const COW::Ptr& cow) collision_objects_.push_back(cow->getName()); if (cow->m_collisionFilterGroup == btBroadphaseProxy::KinematicFilter) + { cows_.insert(cows_.begin(), cow); + active_.push_back(cow->getName()); + } else + { cows_.push_back(cow); + } } void BulletDiscreteSimpleManager::onCollisionMarginDataChanged() diff --git a/tesseract_collision/fcl/src/fcl_discrete_managers.cpp b/tesseract_collision/fcl/src/fcl_discrete_managers.cpp index 4656f5f9d0f..3f0813e7dc8 100644 --- a/tesseract_collision/fcl/src/fcl_discrete_managers.cpp +++ b/tesseract_collision/fcl/src/fcl_discrete_managers.cpp @@ -137,6 +137,7 @@ bool FCLDiscreteBVHManager::removeCollisionObject(const std::string& name) collision_objects_.erase(std::find(collision_objects_.begin(), collision_objects_.end(), name)); link2cow_.erase(name); + active_.erase(std::find(active_.begin(), active_.end(), name)); return true; } return false; @@ -354,6 +355,7 @@ void FCLDiscreteBVHManager::addCollisionObject(const COW::Ptr& cow) } else { + active_.push_back(cow->getName()); for (auto& co : objects) dynamic_manager_->registerObject(co.get()); } 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 faaa3b9c7ac..1b88a049a53 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 @@ -13,6 +13,8 @@ namespace detail { inline void addCollisionObjects(DiscreteContactManager& checker, bool use_convex_mesh = false) { + const std::size_t current_size = checker.getActiveCollisionObjects().size(); + ////////////////////// // Add box to checker ////////////////////// @@ -27,6 +29,17 @@ inline void addCollisionObjects(DiscreteContactManager& checker, bool use_convex checker.addCollisionObject("box_link", 0, obj1_shapes, obj1_poses, false); checker.enableCollisionObject("box_link"); + std::vector active = checker.getActiveCollisionObjects(); + if (current_size == 0) + { + EXPECT_EQ(active.size(), 1); + EXPECT_TRUE(std::find(active.begin(), active.end(), "box_link") != active.end()); + } + else + { + EXPECT_EQ(active.size(), 3); + EXPECT_TRUE(std::find(active.begin(), active.end(), "box_link") != active.end()); + } ///////////////////////////////////////////// // Add thin box to checker which is disabled @@ -42,6 +55,19 @@ inline void addCollisionObjects(DiscreteContactManager& checker, bool use_convex checker.addCollisionObject("thin_box_link", 0, obj2_shapes, obj2_poses); checker.disableCollisionObject("thin_box_link"); + active = checker.getActiveCollisionObjects(); + if (current_size == 0) + { + EXPECT_EQ(active.size(), 2); + EXPECT_TRUE(std::find(active.begin(), active.end(), "box_link") != active.end()); + EXPECT_TRUE(std::find(active.begin(), active.end(), "thin_box_link") != active.end()); + } + else + { + EXPECT_EQ(active.size(), 3); + EXPECT_TRUE(std::find(active.begin(), active.end(), "box_link") != active.end()); + EXPECT_TRUE(std::find(active.begin(), active.end(), "thin_box_link") != active.end()); + } ///////////////////////////////////////////////////////////////// // Add second box to checker. If use_convex_mesh = true then this @@ -75,6 +101,11 @@ inline void addCollisionObjects(DiscreteContactManager& checker, bool use_convex obj3_poses.push_back(second_box_pose); checker.addCollisionObject("second_box_link", 0, obj3_shapes, obj3_poses); + active = checker.getActiveCollisionObjects(); + EXPECT_EQ(active.size(), 3); + EXPECT_TRUE(std::find(active.begin(), active.end(), "box_link") != active.end()); + EXPECT_TRUE(std::find(active.begin(), active.end(), "thin_box_link") != active.end()); + EXPECT_TRUE(std::find(active.begin(), active.end(), "second_box_link") != active.end()); ///////////////////////////////////////////// // Add box and remove @@ -91,8 +122,20 @@ inline void addCollisionObjects(DiscreteContactManager& checker, bool use_convex checker.addCollisionObject("remove_box_link", 0, obj4_shapes, obj4_poses); EXPECT_TRUE(checker.getCollisionObjects().size() == 4); EXPECT_TRUE(checker.hasCollisionObject("remove_box_link")); + active = checker.getActiveCollisionObjects(); + EXPECT_EQ(active.size(), 4); + EXPECT_TRUE(std::find(active.begin(), active.end(), "box_link") != active.end()); + EXPECT_TRUE(std::find(active.begin(), active.end(), "thin_box_link") != active.end()); + EXPECT_TRUE(std::find(active.begin(), active.end(), "second_box_link") != active.end()); + EXPECT_TRUE(std::find(active.begin(), active.end(), "remove_box_link") != active.end()); checker.removeCollisionObject("remove_box_link"); EXPECT_FALSE(checker.hasCollisionObject("remove_box_link")); + active = checker.getActiveCollisionObjects(); + EXPECT_EQ(active.size(), 3); + EXPECT_TRUE(std::find(active.begin(), active.end(), "box_link") != active.end()); + EXPECT_TRUE(std::find(active.begin(), active.end(), "thin_box_link") != active.end()); + EXPECT_TRUE(std::find(active.begin(), active.end(), "second_box_link") != active.end()); + EXPECT_TRUE(std::find(active.begin(), active.end(), "remove_box_link") == active.end()); ///////////////////////////////////////////// // Try functions on a link that does not exist