Fix issue with removing skeletons without shapes

When `subscribeTo` is called with a skeleton in `addSkeleton`, the
skeleton is added to `mSkeletonSources`. However, inside
`removeSkeleton`, `unsubscribeFrom` is not called. Instead,
`removeShapeFrameOf` is called. This is fine for most cases, but if it
fails to remove the skeleton from `mSkeletonSources` if the skeleton
does not have any shapes within it. This causes `mSkeletonSources` to
grow indefinitely. A hazardous side effect of this is that when an
attempt is made to add a new skeleton via `subscribeTo`, the address of
the skeleton might be the same as one of the stale entries in
`mSkeletonSources`. Consequently, the new skeleton's shapes don't get
added to the collision detection engine and very surprising results
follow.

Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>

dart/collision/CollisionGroup.cpp

@@ -142,6 +142,18 @@ void CollisionGroup::removeShapeFramesOf()
   // Do nothing
 }
 
+//==============================================================================
+void CollisionGroup::unsubscribeFrom()
+{
+  // Do nothing
+}
+
+//==============================================================================
+bool CollisionGroup::isSubscribedTo()
+{
+  return true;
+}
+
 //==============================================================================
 void CollisionGroup::removeAllShapeFrames()
 {

dart/collision/CollisionGroup.hpp

@@ -214,6 +214,24 @@ class CollisionGroup
   void unsubscribeFrom(
       const dynamics::Skeleton* skeleton, const Others*... others);
 
+  /// Do nothing. This function is for terminating the recursive variadic
+  /// template.
+  void unsubscribeFrom();
+
+  // Check if this is subscribed to bodyNode and the other sources
+  template <typename... Others>
+  bool isSubscribedTo(
+      const dynamics::BodyNode* bodyNode, const Others*... others);
+
+  // Check if this is subscribed to skeleton and the other sources
+  template <typename... Others>
+  bool isSubscribedTo(
+      const dynamics::Skeleton* skeleton, const Others*... others);
+
+  // Return true. This function is for terminating the recursive variadic
+  // template
+  bool isSubscribedTo();
+
   /// Return true if this CollisionGroup contains shapeFrame
   bool hasShapeFrame(const dynamics::ShapeFrame* shapeFrame) const;
 

dart/collision/detail/CollisionGroup.hpp

@@ -280,6 +280,24 @@ void CollisionGroup::unsubscribeFrom(
   unsubscribeFrom(others...);
 }
 
+//==============================================================================
+template <typename... Others>
+bool CollisionGroup::isSubscribedTo(
+    const dynamics::BodyNode* bodyNode, const Others*... others)
+{
+  auto it = mBodyNodeSources.find(bodyNode);
+  return (it != mBodyNodeSources.end()) && isSubscribedTo(others...);
+}
+
+//==============================================================================
+template <typename... Others>
+bool CollisionGroup::isSubscribedTo(
+    const dynamics::Skeleton* skeleton, const Others*... others)
+{
+  auto it = mSkeletonSources.find(skeleton);
+  return (it != mSkeletonSources.end()) && isSubscribedTo(others...);
+}
+
 } // namespace collision
 } // namespace dart
 

dart/constraint/ConstraintSolver.cpp

@@ -142,7 +142,7 @@ void ConstraintSolver::removeSkeleton(const SkeletonPtr& skeleton)
            << "', which doesn't exist in the ConstraintSolver.\n";
   }
 
-  mCollisionGroup->removeShapeFramesOf(skeleton.get());
+  mCollisionGroup->unsubscribeFrom(skeleton.get());
   mSkeletons.erase(
       remove(mSkeletons.begin(), mSkeletons.end(), skeleton), mSkeletons.end());
   mConstrainedGroups.reserve(mSkeletons.size());

unittests/unit/test_CollisionGroups.cpp

@@ -263,6 +263,72 @@ TEST_P(CollisionGroupsTest, BodyNodeSubscription)
   EXPECT_FALSE(group->collide());
 }
 
+TEST_P(CollisionGroupsTest, RemovedSkeletonSubscription)
+{
+  if (!dart::collision::CollisionDetector::getFactory()->canCreate(GetParam()))
+  {
+    std::cout << "Skipping test for [" << GetParam() << "], because it is not "
+              << "available" << std::endl;
+    return;
+  }
+  else
+  {
+    std::cout << "Running CollisionGroups test for [" << GetParam() << "]"
+              << std::endl;
+  }
+  // Note: When skeletons are added to a world, the constraint solver will
+  // subscribe to them.
+  dart::simulation::WorldPtr world = dart::simulation::World::create();
+  auto cd
+      = dart::collision::CollisionDetector::getFactory()->create(GetParam());
+
+  world->getConstraintSolver()->setCollisionDetector(cd);
+
+  dart::dynamics::SkeletonPtr skel_A = dart::dynamics::Skeleton::create("A");
+  dart::dynamics::SkeletonPtr skel_B = dart::dynamics::Skeleton::create("B");
+
+  auto group = world->getConstraintSolver()->getCollisionGroup();
+
+  // Check that there are no subscribtions before adding the skeletons to the
+  // world
+  EXPECT_FALSE(group->isSubscribedTo(skel_A.get()));
+  EXPECT_FALSE(group->isSubscribedTo(skel_B.get()));
+
+  world->addSkeleton(skel_A);
+  world->addSkeleton(skel_B);
+
+  // Check that there are subscribtions after adding the skeletons to the
+  // world
+  EXPECT_TRUE(group->isSubscribedTo(skel_A.get()));
+  EXPECT_TRUE(group->isSubscribedTo(skel_B.get()));
+
+  // Add a shape to one of the skeletons to test that removal works for
+  // skeletons with and without shapes
+  auto boxShape = std::make_shared<dart::dynamics::BoxShape>(
+      Eigen::Vector3d::Constant(1.0));
+
+  auto pair = skel_B->createJointAndBodyNodePair<dart::dynamics::FreeJoint>();
+  auto sn = pair.second->createShapeNodeWith<dart::dynamics::CollisionAspect>(
+      boxShape);
+
+  // Needed to update subscribtions
+  world->step(1);
+
+  EXPECT_TRUE(group->hasShapeFrame(sn));
+  const auto* skel_A_ptr = skel_A.get();
+  const auto* skel_B_ptr = skel_B.get();
+  // Check that there are no subscribtions after removing the skeletons from the
+  // world
+  world->removeSkeleton(skel_A);
+  world->removeSkeleton(skel_B);
+
+  world->step(1);
+
+  EXPECT_FALSE(group->hasShapeFrame(sn));
+  EXPECT_FALSE(group->isSubscribedTo(skel_A_ptr));
+  EXPECT_FALSE(group->isSubscribedTo(skel_B_ptr));
+}
+
 INSTANTIATE_TEST_CASE_P(
     CollisionEngine,
     CollisionGroupsTest,