diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index bcd6146868..b2deb586c4 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,14 @@ 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. + auto contactsElement = _sdf->FindElement("contacts"); + if (contactsElement) + { + this->dataPtr->contactsEntityNames = contactsElement->Get( + "include_entity_names", true).first; + } + // Find engine shared library // Look in: // * Paths from environment variable @@ -3259,6 +3271,13 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) msgs::Contact *contactMsg = contactsComp.add_contact(); contactMsg->mutable_collision1()->set_id(_collEntity1); contactMsg->mutable_collision2()->set_id(collEntity2); + 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/src/systems/physics/Physics.hh b/src/systems/physics/Physics.hh index 2877bef288..98ebde22d5 100644 --- a/src/systems/physics/Physics.hh +++ b/src/systems/physics/Physics.hh @@ -64,6 +64,19 @@ namespace systems /// \class Physics Physics.hh ignition/gazebo/systems/Physics.hh /// \brief Base class for a System. + /// Includes optional parameter : . When set + /// to false, the name of colliding entities is not populated in + /// the contacts. Remains true by default. Usage : + /// ``` + /// + /// + /// false + /// + /// + /// ``` + class Physics: public System, public ISystemConfigure, diff --git a/test/integration/contact_system.cc b/test/integration/contact_system.cc index 72e1f9e305..6b3cabfbfd 100644 --- a/test/integration/contact_system.cc +++ b/test/integration/contact_system.cc @@ -40,6 +40,155 @@ class ContactSystemTest : public InternalFixture<::testing::Test> { }; +///////////////////////////////////////////////// +// This test verifies that colliding entity names are populated in +// the contact points message. +TEST_F(ContactSystemTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(EnableCollidingEntityNames)) +{ + // Start server + ServerConfig serverConfig; + const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/contact_with_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_TRUE(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()); + } +} + +///////////////////////////////////////////////// +// 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 entityNameEmpty = + contact.collision1().name() == ""; + EXPECT_TRUE(entityNameEmpty); + } + } + + // 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 @@ -100,6 +249,12 @@ TEST_F(ContactSystemTest, EXPECT_NEAR(0.25, std::abs(contact.position(0).x()), 5e-2); EXPECT_NEAR(1, std::abs(contact.position(0).y()), 5e-2); EXPECT_NEAR(1, contact.position(0).z(), 5e-2); + bool entityNameFound = + contact.collision1().name() == + "contact_model::link::collision_sphere1" || + contact.collision1().name() == + "contact_model::link::collision_sphere2"; + EXPECT_TRUE(entityNameFound); } } diff --git a/test/worlds/contact_with_entity_names.sdf b/test/worlds/contact_with_entity_names.sdf new file mode 100644 index 0000000000..7c346e4aa5 --- /dev/null +++ b/test/worlds/contact_with_entity_names.sdf @@ -0,0 +1,109 @@ + + + + + 0 + + + + + true + + + + + + + 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 + + + + + + + diff --git a/test/worlds/contact_without_entity_names.sdf b/test/worlds/contact_without_entity_names.sdf new file mode 100644 index 0000000000..d842486015 --- /dev/null +++ b/test/worlds/contact_without_entity_names.sdf @@ -0,0 +1,109 @@ + + + + + 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 + + + + + + +