Keep active links updated in contact managers when adding and removin…

…g links
tesseract-robotics · Oct 25, 2023 · 340458e · 340458e
1 parent 9c96ee7
commit 340458e
Show file tree

Hide file tree

Showing 6 changed files with 73 additions and 1 deletion.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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/...ct_collision/test_suite/include/tesseract_collision/test_suite/collision_box_box_unit.hpp b/...ct_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<std::string> 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