Populate names of colliding entities in contact points message

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<Entity> 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<bool>(
+      "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();

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 : <include_entity_names>. When set
+  /// to false, the name of colliding entities is not populated in
+  /// the contacts. Remains true by default. Usage :
+  /// ```
+  ///  <plugin
+  ///    filename="ignition-gazebo-physics-system"
+  ///    name="ignition::gazebo::systems::Physics">
+  ///    <contacts>
+  ///      <include_entity_names>false</include_entity_names>
+  ///    </contacts>
+  ///  </plugin>
+  ///  ```
+
   class Physics:
     public System,
     public ISystemConfigure,

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<msgs::Contacts> contactMsgs;
+
+  auto contactCb = [&](const msgs::Contacts &_msg) -> void
+  {
+    std::lock_guard<std::mutex> 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<void(const msgs::Contacts &)>(contactCb);
+  node.Subscribe("/test_multiple_collisions", callbackFunc);
+
+  // Run server
+  size_t iters = 1000;
+  server.Run(true, iters, false);
+  {
+    std::lock_guard<std::mutex> 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<std::mutex> 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<std::mutex> 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<msgs::Contacts> contactMsgs;
+
+  auto contactCb = [&](const msgs::Contacts &_msg) -> void
+  {
+    std::lock_guard<std::mutex> 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<void(const msgs::Contacts &)>(contactCb);
+  node.Subscribe("/test_multiple_collisions", callbackFunc);
+
+  // Run server
+  size_t iters = 1000;
+  server.Run(true, iters, false);
+  {
+    std::lock_guard<std::mutex> 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<std::mutex> 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<std::mutex> 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);
     }
   }
 

test/worlds/contact_with_entity_names.sdf

@@ -0,0 +1,109 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <world name="contact_sensor">
+    <physics name="fast" type="ignored">
+      <real_time_factor>0</real_time_factor>
+    </physics>
+
+    <plugin
+      filename="ignition-gazebo-physics-system"
+      name="ignition::gazebo::systems::Physics">
+      <contacts>
+        <include_entity_names>true</include_entity_names>
+      </contacts>
+    </plugin>
+    <plugin
+      filename="ignition-gazebo-contact-system"
+      name="ignition::gazebo::systems::Contact">
+    </plugin>
+
+    <model name="contact_model">
+      <pose>0 0 3.0 0 0.0 0</pose>
+      <link name="link">
+        <collision name="collision_sphere1">
+          <pose>0 1 0.0 0 0.0 0</pose>
+          <geometry>
+            <sphere>
+              <radius>0.5</radius>
+            </sphere>
+          </geometry>
+        </collision>
+        <collision name="collision_sphere2">
+          <pose>0 -1 0.0 0 0.0 0</pose>
+          <geometry>
+            <sphere>
+              <radius>0.5</radius>
+            </sphere>
+          </geometry>
+        </collision>
+        <visual name="visual_sphere1">
+          <pose>0 1 0.0 0 0.0 0</pose>
+          <geometry>
+            <sphere>
+              <radius>0.5</radius>
+            </sphere>
+          </geometry>
+        </visual>
+        <visual name="visual_sphere2">
+          <pose>0 -1 0.0 0 0.0 0</pose>
+          <geometry>
+            <sphere>
+              <radius>0.5</radius>
+            </sphere>
+          </geometry>
+        </visual>
+        <sensor name='sensor_contact' type='contact'>
+          <contact>
+            <collision>collision_sphere1</collision>
+            <collision>collision_sphere2</collision>
+            <topic>/test_multiple_collisions</topic>
+          </contact>
+          <always_on>1</always_on>
+          <update_rate>1000</update_rate>
+        </sensor>
+      </link>
+    </model>
+
+    <model name="box1">
+      <static>1</static>
+      <pose>-0.75 0 0.5 0 0 0</pose>
+      <link name="link">
+        <collision name="collision_box1_box">
+          <geometry>
+            <box>
+              <size>1 4 1</size>
+            </box>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <box>
+              <size>1 4 1</size>
+            </box>
+          </geometry>
+        </visual>
+      </link>
+    </model>
+
+    <model name="box2">
+      <static>1</static>
+      <pose>0.75 0 0.5 0 0 0</pose>
+      <link name="link">
+        <collision name="collision_box2_box">
+          <geometry>
+            <box>
+              <size>1 4 1</size>
+            </box>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <box>
+              <size>1 4 1</size>
+            </box>
+          </geometry>
+        </visual>
+      </link>
+    </model>
+  </world>
+</sdf>