From e5d4de84b807aef116f56de94f2e4b44cb0aaa8d Mon Sep 17 00:00:00 2001 From: Aditya Date: Thu, 17 Feb 2022 16:42:40 -0800 Subject: [PATCH 1/7] Add names to collision entities Signed-off-by: Aditya --- src/systems/physics/Physics.cc | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 18df36ba58..22bcbca02b 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -3259,7 +3259,11 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) { msgs::Contact *contactMsg = contactsComp.add_contact(); contactMsg->mutable_collision1()->set_id(_collEntity1); + auto _collEntity1Name = _ecm.Component(_collEntity1)->Data(); + contactMsg->mutable_collision1()->set_name(_collEntity1Name); contactMsg->mutable_collision2()->set_id(collEntity2); + auto collEntity2Name = _ecm.Component(collEntity2)->Data(); + contactMsg->mutable_collision2()->set_name(collEntity2Name); for (const auto &contact : contactData) { auto *position = contactMsg->add_position(); From c549793983765f05e7506c9eac7f5c3fe971d6a9 Mon Sep 17 00:00:00 2001 From: Aditya Date: Fri, 18 Feb 2022 15:49:56 -0800 Subject: [PATCH 2/7] Add complete names of entities Signed-off-by: Aditya --- src/systems/physics/Physics.cc | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 22bcbca02b..1d9d60da03 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -3259,11 +3259,11 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) { msgs::Contact *contactMsg = contactsComp.add_contact(); contactMsg->mutable_collision1()->set_id(_collEntity1); - auto _collEntity1Name = _ecm.Component(_collEntity1)->Data(); - contactMsg->mutable_collision1()->set_name(_collEntity1Name); + contactMsg->mutable_collision1()->set_name( + scopedName(_collEntity1, _ecm, ":", 1)); contactMsg->mutable_collision2()->set_id(collEntity2); - auto collEntity2Name = _ecm.Component(collEntity2)->Data(); - contactMsg->mutable_collision2()->set_name(collEntity2Name); + contactMsg->mutable_collision2()->set_name( + scopedName(collEntity2, _ecm, ":", 1)); for (const auto &contact : contactData) { auto *position = contactMsg->add_position(); From 04e04392f2f8b73a35ddd3281b45395a79be3c16 Mon Sep 17 00:00:00 2001 From: Aditya Date: Fri, 18 Feb 2022 16:37:04 -0800 Subject: [PATCH 3/7] Added test case Signed-off-by: Aditya --- test/integration/contact_system.cc | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/test/integration/contact_system.cc b/test/integration/contact_system.cc index 72e1f9e305..b09fd6fff2 100644 --- a/test/integration/contact_system.cc +++ b/test/integration/contact_system.cc @@ -100,6 +100,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() == + "world:contact_sensor:model:contact_model:link:link:collision:collision_sphere1" || + contact.collision1().name() == + "world:contact_sensor:model:contact_model:link:link:collision:collision_sphere2"; + EXPECT_TRUE(EntityNameFound); } } From b683fe7f345d983b0974fb0656cb4924e8042a6e Mon Sep 17 00:00:00 2001 From: Aditya Date: Tue, 22 Feb 2022 13:08:17 -0800 Subject: [PATCH 4/7] Changed delimiter, removed entity types from name, minor cleanup Signed-off-by: Aditya --- src/systems/physics/Physics.cc | 4 ++-- test/integration/contact_system.cc | 10 +++++----- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 1d9d60da03..abf7ec416d 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -3260,10 +3260,10 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) msgs::Contact *contactMsg = contactsComp.add_contact(); contactMsg->mutable_collision1()->set_id(_collEntity1); contactMsg->mutable_collision1()->set_name( - scopedName(_collEntity1, _ecm, ":", 1)); + removeParentScope(scopedName(_collEntity1, _ecm, "::", 0), "::")); contactMsg->mutable_collision2()->set_id(collEntity2); contactMsg->mutable_collision2()->set_name( - scopedName(collEntity2, _ecm, ":", 1)); + 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 b09fd6fff2..7fdc88599a 100644 --- a/test/integration/contact_system.cc +++ b/test/integration/contact_system.cc @@ -100,12 +100,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 = + bool entityNameFound = contact.collision1().name() == - "world:contact_sensor:model:contact_model:link:link:collision:collision_sphere1" || - contact.collision1().name() == - "world:contact_sensor:model:contact_model:link:link:collision:collision_sphere2"; - EXPECT_TRUE(EntityNameFound); + "contact_model::link::collision_sphere1" || + contact.collision1().name() == + "contact_model::link::collision_sphere2"; + EXPECT_TRUE(entityNameFound); } } From e08983ec9873a1eb6e7c7abaf87336f11ca657f8 Mon Sep 17 00:00:00 2001 From: Aditya Date: Thu, 24 Feb 2022 15:36:39 -0800 Subject: [PATCH 5/7] Added parameter to disable populating colliding entity names Signed-off-by: Aditya --- src/systems/physics/Physics.cc | 27 ++++- test/integration/contact_system.cc | 76 +++++++++++++ test/worlds/contact.sdf | 3 + test/worlds/contact_without_entity_names.sdf | 109 +++++++++++++++++++ 4 files changed, 211 insertions(+), 4 deletions(-) create mode 100644 test/worlds/contact_without_entity_names.sdf 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 + + + + + + + From ed305a9c1bab5d005bb052629b15b11f4ef3e4fb Mon Sep 17 00:00:00 2001 From: Aditya Date: Thu, 24 Feb 2022 16:25:15 -0800 Subject: [PATCH 6/7] Minor refactoring, added docstring Signed-off-by: Aditya --- src/systems/physics/Physics.cc | 12 ++++-------- src/systems/physics/Physics.hh | 13 +++++++++++++ test/integration/contact_system.cc | 9 +++------ 3 files changed, 20 insertions(+), 14 deletions(-) diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 0dded1ed2f..100c027356 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -737,15 +737,11 @@ void Physics::Configure(const Entity &_entity, } // Check if entity names should be populated in contact points. - if (_sdf->HasElement("contacts")) + auto contactsElement = _sdf->FindElement("contacts"); + if (contactsElement) { - auto sdfClone = _sdf->Clone(); - auto contactsElement = sdfClone->GetElement("contacts"); - if (contactsElement->HasElement("include_entity_names")) - { - this->dataPtr->contactsEntityNames = contactsElement->Get( - "include_entity_names"); - } + this->dataPtr->contactsEntityNames = contactsElement->Get( + "include_entity_names"); } // Find engine shared library 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 ac2aed5ce3..bbc6acbb07 100644 --- a/test/integration/contact_system.cc +++ b/test/integration/contact_system.cc @@ -93,12 +93,9 @@ TEST_F(ContactSystemTest, 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); + bool entityNameEmpty = + contact.collision1().name() == ""; + EXPECT_TRUE(entityNameEmpty); } } From 84faae9b8f133b722dbcadef85216a0696acb0b6 Mon Sep 17 00:00:00 2001 From: Aditya Date: Fri, 25 Feb 2022 19:21:14 -0800 Subject: [PATCH 7/7] Added default value of parameter, test case without the sdf tag Signed-off-by: Aditya --- src/systems/physics/Physics.cc | 2 +- test/integration/contact_system.cc | 76 +++++++++++++++ test/worlds/contact.sdf | 3 - test/worlds/contact_with_entity_names.sdf | 109 ++++++++++++++++++++++ 4 files changed, 186 insertions(+), 4 deletions(-) create mode 100644 test/worlds/contact_with_entity_names.sdf diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 33edd20952..b2deb586c4 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -741,7 +741,7 @@ void Physics::Configure(const Entity &_entity, if (contactsElement) { this->dataPtr->contactsEntityNames = contactsElement->Get( - "include_entity_names"); + "include_entity_names", true).first; } // Find engine shared library diff --git a/test/integration/contact_system.cc b/test/integration/contact_system.cc index bbc6acbb07..6b3cabfbfd 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 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. diff --git a/test/worlds/contact.sdf b/test/worlds/contact.sdf index 7c346e4aa5..10c4afb1da 100644 --- a/test/worlds/contact.sdf +++ b/test/worlds/contact.sdf @@ -8,9 +8,6 @@ - - true - + + + + 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 + + + + + + +