From aa76c3e1fd12c1e325c699f261c56699bab3748f Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 26 Nov 2024 09:46:42 -0800 Subject: [PATCH] Remove detachable joints when a model is removed (#2686) Signed-off-by: Ian Chen --- src/EntityComponentManager.cc | 20 +++++++++ src/systems/physics/Physics.cc | 71 +++++++++++++++++--------------- test/integration/entity_erase.cc | 66 +++++++++++++++++++++++++++++ 3 files changed, 123 insertions(+), 34 deletions(-) diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index 2fd4e03c6e..20c070b9ff 100644 --- 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 detachableJoints; + this->Each( + [&](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 index 6a2f3eda79..0bb135048d 100644 --- 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( + [&](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( + _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( - [&](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( - _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 index 8189f141d3..6f76691ca9 100644 --- a/test/integration/entity_erase.cc +++ b/test/integration/entity_erase.cc @@ -21,9 +21,11 @@ #include +#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(common::joinPaths(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( + [&](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); +}