diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index abf7ec416d..0dded1ed2f 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -686,6 +686,10 @@ class ignition::gazebo::systems::PhysicsPrivate /// through the GUI model editor, so that we can avoid premature creation /// of joints. This also lets us suppress some invalid error messages. public: std::set jointAddedToModel; + + /// \brief Flag to store whether the names of colliding entities should + /// be populated in the contact points. + public: bool contactsEntityNames = true; }; ////////////////////////////////////////////////// @@ -732,6 +736,18 @@ void Physics::Configure(const Entity &_entity, [](const std::string &_a, const std::string &_b){return _a == _b;}); } + // Check if entity names should be populated in contact points. + if (_sdf->HasElement("contacts")) + { + auto sdfClone = _sdf->Clone(); + auto contactsElement = sdfClone->GetElement("contacts"); + if (contactsElement->HasElement("include_entity_names")) + { + this->dataPtr->contactsEntityNames = contactsElement->Get( + "include_entity_names"); + } + } + // Find engine shared library // Look in: // * Paths from environment variable @@ -3259,11 +3275,14 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) { msgs::Contact *contactMsg = contactsComp.add_contact(); contactMsg->mutable_collision1()->set_id(_collEntity1); - contactMsg->mutable_collision1()->set_name( - removeParentScope(scopedName(_collEntity1, _ecm, "::", 0), "::")); contactMsg->mutable_collision2()->set_id(collEntity2); - contactMsg->mutable_collision2()->set_name( - removeParentScope(scopedName(collEntity2, _ecm, "::", 0), "::")); + if (this->contactsEntityNames) + { + contactMsg->mutable_collision1()->set_name( + removeParentScope(scopedName(_collEntity1, _ecm, "::", 0), "::")); + contactMsg->mutable_collision2()->set_name( + removeParentScope(scopedName(collEntity2, _ecm, "::", 0), "::")); + } for (const auto &contact : contactData) { auto *position = contactMsg->add_position(); diff --git a/test/integration/contact_system.cc b/test/integration/contact_system.cc index 7fdc88599a..ac2aed5ce3 100644 --- a/test/integration/contact_system.cc +++ b/test/integration/contact_system.cc @@ -40,6 +40,82 @@ class ContactSystemTest : public InternalFixture<::testing::Test> { }; +///////////////////////////////////////////////// +// This test verifies that colliding entity names are not populated in +// the contact points message. +TEST_F(ContactSystemTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(DisableCollidingEntityNames)) +{ + // Start server + ServerConfig serverConfig; + const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/contact_without_entity_names.sdf"; + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + using namespace std::chrono_literals; + server.SetUpdatePeriod(1ns); + + std::mutex contactMutex; + std::vector contactMsgs; + + auto contactCb = [&](const msgs::Contacts &_msg) -> void + { + std::lock_guard lock(contactMutex); + contactMsgs.push_back(_msg); + }; + + // subscribe to contacts topic + transport::Node node; + // Have to create an lvalue here for Node::Subscribe to work. + auto callbackFunc = std::function(contactCb); + node.Subscribe("/test_multiple_collisions", callbackFunc); + + // Run server + size_t iters = 1000; + server.Run(true, iters, false); + { + std::lock_guard lock(contactMutex); + ASSERT_GE(contactMsgs.size(), 1u); + } + + // "contact_model" has one contact sensor, but the sensor uses two collisions + // as sensors. Therefore, once the "contact_model" model has fallen and has + // stabilized, we expect that each collision will have 1 contact point. + { + std::lock_guard lock(contactMutex); + const auto &lastContacts = contactMsgs.back(); + EXPECT_EQ(4, lastContacts.contact_size()); + + for (const auto &contact : lastContacts.contact()) + { + ASSERT_EQ(1, contact.position_size()); + bool entityNameFound = + contact.collision1().name() == + "contact_model::link::collision_sphere1" || + contact.collision1().name() == + "contact_model::link::collision_sphere2"; + EXPECT_FALSE(entityNameFound); + } + } + + // Remove the colliding boxes and check that contacts are no longer generated. + server.RequestRemoveEntity("box1"); + server.RequestRemoveEntity("box2"); + // Run once to remove entities + server.Run(true, 1, false); + + contactMsgs.clear(); + server.Run(true, 10, false); + { + std::lock_guard lock(contactMutex); + EXPECT_EQ(0u, contactMsgs.size()); + } +} + ///////////////////////////////////////////////// // The test checks that contacts are published by the contact system // See https://github.com/ignitionrobotics/ign-gazebo/issues/1175 diff --git a/test/worlds/contact.sdf b/test/worlds/contact.sdf index 10c4afb1da..7c346e4aa5 100644 --- a/test/worlds/contact.sdf +++ b/test/worlds/contact.sdf @@ -8,6 +8,9 @@ + + true + + + + + 0 + + + + + false + + + + + + + 0 0 3.0 0 0.0 0 + + + 0 1 0.0 0 0.0 0 + + + 0.5 + + + + + 0 -1 0.0 0 0.0 0 + + + 0.5 + + + + + 0 1 0.0 0 0.0 0 + + + 0.5 + + + + + 0 -1 0.0 0 0.0 0 + + + 0.5 + + + + + + collision_sphere1 + collision_sphere2 + /test_multiple_collisions + + 1 + 1000 + + + + + + 1 + -0.75 0 0.5 0 0 0 + + + + + 1 4 1 + + + + + + + 1 4 1 + + + + + + + + 1 + 0.75 0 0.5 0 0 0 + + + + + 1 4 1 + + + + + + + 1 4 1 + + + + + + +