Populate names of colliding entities in contact points message

[adityapande-1995]

🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸

🎉 New feature

Summary

Currently, the contact points message has a Enitity member whose name field is not populated and just contains the id. This PR populates the name with the entity's full name, for e.g. world:shapes:model:ground_plane:link:link:collision:collision so it is clear in the message which pair of entities is colliding.

Test it

Modified an existing test case to check the full name of a colliding entity (contact_system.cc integration test)

Checklist

• Signed all commits for DCO
• Added tests
• Added example and/or tutorial
• Updated documentation (as needed)
• Updated migration guide (as needed)
• codecheck passed (See contributing)
• All tests passed (See test coverage)
• While waiting for a review on your PR, please help review another open pull request to support the maintainers

Note to maintainers: Remember to use Squash-Merge and edit the commit message to match the pull request summary while retaining Signed-off-by messages.

🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸

[azeey]

Can you also fix the codecheck errors?

[codecov]

Codecov Report

Merging #1351 (84faae9) into ign-gazebo6 (d03a77e) will increase coverage by 0.01%.
The diff coverage is 100.00%.

@@               Coverage Diff               @@
##           ign-gazebo6    #1351      +/-   ##
===============================================
+ Coverage        62.98%   62.99%   +0.01%     
===============================================
  Files              301      301              
  Lines            24199    24208       +9     
===============================================
+ Hits             15242    15251       +9     
  Misses            8957     8957              

Impacted Files	Coverage Δ
src/systems/physics/Physics.hh	100.00% <ø> (ø)
src/systems/physics/Physics.cc	71.75% <100.00%> (+0.19%)	⬆️

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update d03a77e...84faae9. Read the comment docs.

[peci1]

Thanks for the PR, it looks useful! However, I would really like to see a performance analysis of this. Try to create a world with say 10 000 contacts and measure the update rate with and without this PR...

[adityapande-1995]

Performance benchmarking :

Generate world file

I used the following erb template to generate a test world file :

<?xml version="1.0" ?>
<!--
  To generate sdf file using this erb template, run `erb -T 1 shapes_population.sdf.erb > shapes_population.sdf`
-->
<sdf version="1.6">
  <world name="shapes">
    <physics name="1ms" type="ignored">
      <max_step_size>0.001</max_step_size>
      <real_time_factor>1.0</real_time_factor>
    </physics>
    <plugin
      filename="ignition-gazebo-physics-system"
      name="ignition::gazebo::systems::Physics">
    </plugin>
    <plugin
      filename="ignition-gazebo-sensors-system"
      name="ignition::gazebo::systems::Sensors">
      <render_engine>ogre2</render_engine>
    </plugin>
    <plugin
      filename="ignition-gazebo-scene-broadcaster-system"
      name="ignition::gazebo::systems::SceneBroadcaster">
    </plugin>
    <plugin
      filename="ignition-gazebo-contact-system"
      name="ignition::gazebo::systems::Contact">
    </plugin>

    <scene>
      <ambient>1.0 1.0 1.0</ambient>
      <background>0.8 0.8 0.8</background>
    </scene>

    <light type="directional" name="sun">
      <cast_shadows>true</cast_shadows>
      <pose>0 0 10 0 0 0</pose>
      <diffuse>0.8 0.8 0.8 1</diffuse>
      <specular>0.8 0.8 0.8 1</specular>
      <attenuation>
        <range>1000</range>
        <constant>0.9</constant>
        <linear>0.01</linear>
        <quadratic>0.001</quadratic>
      </attenuation>
      <direction>-0.5 0.1 -0.9</direction>
    </light>

    <model name="ground_plane">
      <static>true</static>
      <link name="link">
        <collision name="collision">
          <geometry>
            <plane>
              <normal>0 0 1</normal>
              <size>100 100</size>
            </plane>
          </geometry>
        </collision>
        <visual name="visual">
          <geometry>
            <plane>
              <normal>0 0 1</normal>
              <size>100 100</size>
            </plane>
          </geometry>
          <material>
            <ambient>0.8 0.8 0.8 1</ambient>
            <diffuse>0.8 0.8 0.8 1</diffuse>
            <specular>0.8 0.8 0.8 1</specular>
          </material>
        </visual>
      </link>
    </model>

    <%
      # number of population
      n = 100
      for m in (0..n)
        geom_1 = 'box'
    %>

    <model name="<%= geom_1 %>_<%= m.to_s %>">
      <pose><%= 0 + m*1.5 %> 0 0.5 0 0 0</pose>
      <link name="box_link">
        <inertial>
          <inertia>
            <ixx>1</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>1</iyy>
            <iyz>0</iyz>
            <izz>1</izz>
          </inertia>
          <mass>1.0</mass>
        </inertial>
        <collision name="box_collision">
          <geometry>
            <box>
              <size>1 1 1</size>
            </box>
          </geometry>
        </collision>
        
        <sensor name='sensor_contact_<%= m.to_s %>' type='contact'>
	<contact>
	  <collision>box_collision</collision>
	  <topic>/robot_collision</topic>
        </contact>
        <always_on>1</always_on>
        <update_rate>1000</update_rate>
       </sensor>

        <visual name="box_visual">
          <geometry>
            <box>
              <size>1 1 1</size>
            </box>
          </geometry>
          <material>
            <ambient>1 0 0 1</ambient>
            <diffuse>1 0 0 1</diffuse>
            <specular>1 0 0 1</specular>
          </material>
        </visual>
      </link>
    </model>

    <%
      end
    %>
  </world>
</sdf>

Then, erb -T 1 template.erb > test.sdf ; ign gazebo -r -s test.sdf runs the simulation.

Data collection

I collected the real time factor data in a text file (let the simulation run for around 30 seconds) : ign topic -e -t /stats | grep real_time_factor | cut -c 19- > data.txt, and got an average of that data using : awk '{ sum += $1; n++ } END { if (n > 0) print sum / n; print n}' data.txt

Results

Trails	Avg real time factor gazebo6 branch	Avg real time factor current branch	% change
1.	0.0918426	0.0894021	-2.65
2.	0.0914473	0.0910263	-0.46
3.	0.0927287	0.0912242	-1.62
4.	0.0926891	0.0915801 ​	-1.19

On an average, the real time factor for this branch seems to be slower by 1.5%

[adityapande-1995]

Added an optional parameter so that the names of colliding entities will not be populated and performance won't be affected. This parameter remains true by default (so the names will be populated).

[azeey]

Looks great! Just a couple of minor comments.

[azeey]

Also could you document the parameter in Physics.hh?

[adityapande-1995]

I've added the docstring, not sure if that is the preferred style though.

[azeey]

This is good. For other systems we have been listing the parameters in a bullet list, but since this is the only parameter here, I think the format is fine.

[peci1]

Performance benchmarking :

Thanks a lot for the benchmark, it's good to see the performance isn't affected that heavily. In normal situations with just a few contacts, the difference would be negligible.

[osrf-triage]

This pull request has been mentioned on Gazebo Community. There might be relevant details there:

https://community.gazebosim.org/t/new-ignition-releases-2022-03-25-fortress-edifice-citadel/1343/1