Fix issue #2175

gazebosim · May 12, 2017 · 69b2841 · 69b2841
1 parent 4a10aa2
commit 69b2841
Showing 1 changed file with 12 additions and 10 deletions.
diff --git a/gazebo/physics/ode/ODEPhysics.cc b/gazebo/physics/ode/ODEPhysics.cc
@@ -960,16 +960,6 @@ void ODEPhysics::CollisionCallback(void *_data, dGeomID _o1, dGeomID _o2)
     ODECollision *collision1 = nullptr;
     ODECollision *collision2 = nullptr;
 
-    // Exit if both bodies are not enabled
-    if (dGeomGetCategoryBits(_o1) != GZ_SENSOR_COLLIDE &&
-        dGeomGetCategoryBits(_o2) != GZ_SENSOR_COLLIDE &&
-        ((b1 && b2 && !dBodyIsEnabled(b1) && !dBodyIsEnabled(b2)) ||
-        (!b2 && b1 && !dBodyIsEnabled(b1)) ||
-        (!b1 && b2 && !dBodyIsEnabled(b2))))
-    {
-      return;
-    }
-
     // Get pointers to the underlying collisions
     if (dGeomGetClass(_o1) == dGeomTransformClass)
       collision1 =
@@ -983,6 +973,18 @@ void ODEPhysics::CollisionCallback(void *_data, dGeomID _o1, dGeomID _o2)
     else
       collision2 = static_cast<ODECollision*>(dGeomGetData(_o2));
 
+    // Exit if both bodies are not enabled
+    if (dGeomGetCategoryBits(_o1) != GZ_SENSOR_COLLIDE &&
+        dGeomGetCategoryBits(_o2) != GZ_SENSOR_COLLIDE &&
+        !self->contactManager->NeverDropContacts() &&
+        !self->contactManager->SubscribersConnected(collision1, collision2) &&
+        ((b1 && b2 && !dBodyIsEnabled(b1) && !dBodyIsEnabled(b2)) ||
+        (!b2 && b1 && !dBodyIsEnabled(b1)) ||
+        (!b1 && b2 && !dBodyIsEnabled(b2))))
+    {
+      return;
+    }
+
     // Make sure both collision pointers are valid.
     if (collision1 && collision2)
     {