Remove detachable joints when a model is removed

Signed-off-by: Ian Chen <[email protected]>
gazebosim · Nov 23, 2024 · 4f26be5 · 4f26be5
1 parent af92e29
commit 4f26be5
Show file tree

Hide file tree

Showing 3 changed files with 123 additions and 34 deletions.
diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc
@@ -34,6 +34,7 @@
 #include "gz/sim/components/CanonicalLink.hh"
 #include "gz/sim/components/ChildLinkName.hh"
 #include "gz/sim/components/Component.hh"
+#include "gz/sim/components/DetachableJoint.hh"
 #include "gz/sim/components/Factory.hh"
 #include "gz/sim/components/Joint.hh"
 #include "gz/sim/components/Link.hh"
@@ -711,6 +712,25 @@ void EntityComponentManager::RequestRemoveEntity(Entity _entity,
   else
   {
     this->dataPtr->InsertEntityRecursive(_entity, tmpToRemoveEntities);
+
+    // remove detachable joint entities that are connected to
+    // any of the entities to be removed
+    std::unordered_set<Entity> detachableJoints;
+    this->Each<components::DetachableJoint>(
+        [&](const Entity &_jointEntity,
+            const components::DetachableJoint *_jointInfo) -> bool
+        {
+          Entity parentLinkEntity = _jointInfo->Data().parentLink;
+          Entity childLinkEntity = _jointInfo->Data().childLink;
+          if (tmpToRemoveEntities.find(parentLinkEntity) !=
+              tmpToRemoveEntities.end() ||
+              tmpToRemoveEntities.find(childLinkEntity) !=
+              tmpToRemoveEntities.end())
+            detachableJoints.insert(_jointEntity);
+          return true;
+        });
+    tmpToRemoveEntities.insert(detachableJoints.begin(),
+                               detachableJoints.end());
   }
 
   // Remove entities from tmpToRemoveEntities that are marked as

diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc
@@ -2003,6 +2003,43 @@ void PhysicsPrivate::CreateBatteryEntities(const EntityComponentManager &_ecm)
 //////////////////////////////////////////////////
 void PhysicsPrivate::RemovePhysicsEntities(const EntityComponentManager &_ecm)
 {
+  // Remove detachable joints. Do this before removing models otherwise the
+  // physics engine may not find the links associated with the detachable
+  // joints.
+  _ecm.EachRemoved<components::DetachableJoint>(
+      [&](const Entity &_entity, const components::DetachableJoint *) -> bool
+      {
+        if (!this->entityJointMap.HasEntity(_entity))
+        {
+          gzwarn << "Failed to find joint [" << _entity
+                  << "]." << std::endl;
+          return true;
+        }
+
+        auto castEntity =
+            this->entityJointMap.EntityCast<DetachableJointFeatureList>(
+                _entity);
+        if (!castEntity)
+        {
+          static bool informed{false};
+          if (!informed)
+          {
+            gzdbg << "Attempting to detach a joint, but the physics "
+                   << "engine doesn't support feature "
+                   << "[DetachJointFeature]. Joint won't be detached."
+                   << std::endl;
+            informed = true;
+          }
+
+          // Break Each call since no DetachableJoints can be processed
+          return false;
+        }
+
+        gzdbg << "Detaching joint [" << _entity << "]" << std::endl;
+        castEntity->Detach();
+        return true;
+      });
+
   // Assume the world will not be erased
   // Only removing models is supported by gz-physics right now so we only
   // remove links, joints and collisions if they are children of the removed
@@ -2063,40 +2100,6 @@ void PhysicsPrivate::RemovePhysicsEntities(const EntityComponentManager &_ecm)
         }
         return true;
       });
-
-  _ecm.EachRemoved<components::DetachableJoint>(
-      [&](const Entity &_entity, const components::DetachableJoint *) -> bool
-      {
-        if (!this->entityJointMap.HasEntity(_entity))
-        {
-          gzwarn << "Failed to find joint [" << _entity
-                  << "]." << std::endl;
-          return true;
-        }
-
-        auto castEntity =
-            this->entityJointMap.EntityCast<DetachableJointFeatureList>(
-                _entity);
-        if (!castEntity)
-        {
-          static bool informed{false};
-          if (!informed)
-          {
-            gzdbg << "Attempting to detach a joint, but the physics "
-                   << "engine doesn't support feature "
-                   << "[DetachJointFeature]. Joint won't be detached."
-                   << std::endl;
-            informed = true;
-          }
-
-          // Break Each call since no DetachableJoints can be processed
-          return false;
-        }
-
-        gzdbg << "Detaching joint [" << _entity << "]" << std::endl;
-        castEntity->Detach();
-        return true;
-      });
 }
 
 //////////////////////////////////////////////////

diff --git a/test/integration/entity_erase.cc b/test/integration/entity_erase.cc
@@ -21,9 +21,11 @@
 
 #include <gz/utils/ExtraTestMacros.hh>
 
+#include "gz/sim/components/DetachableJoint.hh"
 #include "gz/sim/Server.hh"
 #include "test_config.hh"  // NOLINT(build/include)
 #include "../helpers/EnvTestFixture.hh"
+#include "../helpers/Relay.hh"
 
 using namespace gz;
 using namespace sim;
@@ -84,3 +86,67 @@ TEST_F(PhysicsSystemFixture,
   EXPECT_TRUE(server.HasEntity("ellipsoid"));
   EXPECT_FALSE(server.HasEntity("sphere"));
 }
+
+/////////////////////////////////////////////////
+// See https://github.com/gazebosim/gz-sim/issues/1175
+TEST_F(PhysicsSystemFixture,
+       GZ_UTILS_TEST_DISABLED_ON_WIN32(RemoveModelWithDetachableJoints))
+{
+  ServerConfig serverConfig;
+
+  serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) +
+    "/examples/worlds/detachable_joint.sdf");
+
+  Server server(serverConfig);
+
+  unsigned int detachableJointCount = 0u;
+  test::Relay testSystem;
+  testSystem.OnPostUpdate([&](const UpdateInfo &,
+    const EntityComponentManager &_ecm)
+    {
+      _ecm.Each<components::DetachableJoint>(
+              [&](const Entity &,
+                  const components::DetachableJoint *) -> bool
+              {
+                detachableJointCount++;
+                return true;
+              });
+    });
+  server.AddSystem(testSystem.systemPtr);
+
+  server.Run(true, 1, false);
+  EXPECT_TRUE(server.HasEntity("vehicle_blue"));
+  EXPECT_TRUE(server.HasEntity("chassis"));
+  EXPECT_TRUE(server.HasEntity("front_left_wheel"));
+  EXPECT_TRUE(server.HasEntity("front_right_wheel"));
+  EXPECT_TRUE(server.HasEntity("rear_left_wheel"));
+  EXPECT_TRUE(server.HasEntity("rear_right_wheel"));
+  EXPECT_TRUE(server.HasEntity("front_left_wheel_joint"));
+  EXPECT_TRUE(server.HasEntity("front_right_wheel_joint"));
+  EXPECT_TRUE(server.HasEntity("rear_left_wheel_joint"));
+  EXPECT_TRUE(server.HasEntity("rear_right_wheel_joint"));
+  EXPECT_TRUE(server.HasEntity("B1"));
+  EXPECT_TRUE(server.HasEntity("B2"));
+  EXPECT_TRUE(server.HasEntity("B3"));
+  EXPECT_EQ(3u, detachableJointCount);
+
+  EXPECT_TRUE(server.RequestRemoveEntity("vehicle_blue"));
+  server.Run(true, 1, false);
+
+  detachableJointCount = 0u;
+  server.Run(true, 1, false);
+  EXPECT_FALSE(server.HasEntity("vehicle_blue"));
+  EXPECT_FALSE(server.HasEntity("chassis"));
+  EXPECT_FALSE(server.HasEntity("front_left_wheel"));
+  EXPECT_FALSE(server.HasEntity("front_right_wheel"));
+  EXPECT_FALSE(server.HasEntity("rear_left_wheel"));
+  EXPECT_FALSE(server.HasEntity("rear_right_wheel"));
+  EXPECT_FALSE(server.HasEntity("front_left_wheel_joint"));
+  EXPECT_FALSE(server.HasEntity("front_right_wheel_joint"));
+  EXPECT_FALSE(server.HasEntity("rear_left_wheel_joint"));
+  EXPECT_FALSE(server.HasEntity("rear_right_wheel_joint"));
+  EXPECT_TRUE(server.HasEntity("B1"));
+  EXPECT_TRUE(server.HasEntity("B2"));
+  EXPECT_TRUE(server.HasEntity("B3"));
+  EXPECT_EQ(0u, detachableJointCount);
+}