System plugins (contact sensor) still not working when spawned

[yschulz]

Environment

• OS Version: Ubuntu 22.04
• Source or binary build? tested in both (harmonic)
• If this is a GUI or sensor rendering bug, describe your GPU and rendering system. Otherwise delete this section.
  • Rendering plugin: [ogre | ogre2].
    • [ ] Sensor rendering error.
    • [ ] GUI rendering error.
  • EGL headless mode:
    • [ ] Running in EGL headless mode
  • Generally, mention all circumstances that might affect rendering capabilities:
    • [ ] running on a dual GPU machine (integrated GPU + discrete GPU)
    • [ ] running on a multi-GPU machine (it has multiple discrete GPUs)
    • [x] running on real hardware
    • [ ] running in virtual machine
    • [ ] running in Docker/Singularity
    • [ ] running remotely (e.g. via SSH)
    • [ ] running in a cloud
    • [ ] using VirtualGL, XVFB, Xdummy, XVNC or other indirect rendering utilities
    • [ ] GPU is concurrently used for other tasks
    • [ ] desktop acceleration
    • [ ] video decoding (i.e. a playing Youtube video)
    • [ ] video encoding
    • [ ] CUDA/ROCm computations (Tensorflow, Torch, Caffe running)
    • [ ] multiple simulators running at the same time
    • [ ] other...
  • Rendering system info:
    • On Linux, provide the outputs of the following commands:

01:00.0 VGA compatible controller [0300]: NVIDIA Corporation GA106 [GeForce RTX 3060 Lite Hash Rate] [10de:2504] (rev a1)
:1
direct rendering: Yes
OpenGL vendor string: NVIDIA Corporation
OpenGL renderer string: NVIDIA GeForce RTX 3060/PCIe/SSE2
OpenGL core profile version string: 4.6.0 NVIDIA 530.30.02
OpenGL core profile shading language version string: 4.60 NVIDIA
OpenGL core profile context flags: (none)
OpenGL core profile profile mask: core profile
OpenGL version string: 4.6.0 NVIDIA 530.30.02
OpenGL shading language version string: 4.60 NVIDIA
OpenGL context flags: (none)
OpenGL profile mask: (none)
OpenGL ES profile version string: OpenGL ES 3.2 NVIDIA 530.30.02
OpenGL ES profile shading language version string: OpenGL ES GLSL ES 3.20
yannick     2441  0.3  0.6 26805656 203056 tty2  Sl+  okt31  10:42 /usr/lib/xorg/Xorg vt2 -displayfd 3 -auth /run/user/1000/gdm/Xauthority -nolisten tcp -background none -noreset -keeptty -novtswitch -verbose 3
yannick   120706  0.0  0.0   9216  2688 pts/3    S+   08:34   0:00 grep --color=auto Xorg

X.Org X Server 1.21.1.4
X Protocol Version 11, Revision 0
Current Operating System: Linux rome 6.2.0-35-generic #35~22.04.1-Ubuntu SMP PREEMPT_DYNAMIC Fri Oct  6 10:23:26 UTC 2 x86_64
Kernel command line: BOOT_IMAGE=/boot/vmlinuz-6.2.0-35-generic root=UUID=e6bb636d-9fbd-4f7d-94d5-9019a795bb47 ro quiet splash vt.handoff=7
xorg-server 2:21.1.4-2ubuntu1.7~22.04.2 (For technical support please see http://www.ubuntu.com/support) 
Current version of pixman: 0.40.0
        Before reporting problems, check http://wiki.x.org
        to make sure that you have the latest version.

Description

• Expected behavior: I expect to see the topic of the contact sensor to show-up and be callable via gz topic -l regardless of spawning a model through the world file or through create service
• Actual behavior: When model is spawned through entity creation, the sensor does exist in the gui component manager, but the topic doesn't show up
• I know this has been an issue in the past in #1245 and #1281 and other linked issues, but it is persistent with the contact sensor.

Steps to reproduce

I was using an xacro/urdf description, but converted manually to sdf for testing Combined test world:

<sdf version='1.11'>
<world name="test_world">
    <plugin filename="gz-sim-contact-system" name="gz::sim::systems::Contact" />
    <plugin filename="gz-sim-user-commands-system" name="gz::sim::systems::UserCommands" />
    <plugin filename="gz-sim-scene-broadcaster-system" name="gz::sim::systems::SceneBroadcaster" />
  <model name='test_model'>
    <link name='base_footprint'>
      <collision name='base_footprint_fixed_joint_lump__base_link_robot_collision'>
        <pose>0 0 0.12 0 0 0</pose>
        <geometry>
          <box>
            <size>2 0.79700000000000004 0.12</size>
          </box>
        </geometry>
        <surface>
          <contact>
            <ode/>
          </contact>
          <friction>
            <ode>
              <mu>1</mu>
              <mu2>1</mu2>
            </ode>
          </friction>
        </surface>
      </collision>
      <visual name='base_footprint_fixed_joint_lump__base_link_robot_visual'>
        <pose>0 0 0 0 0 0</pose>
        <geometry>
            <box>
                <size>1.5 0.4 0.4</size>
            </box>
        </geometry>
      </visual>
      <gravity>true</gravity>
      <self_collide>false</self_collide>
    </link>
    <joint name='bumper_front_joint' type='fixed'>
      <pose relative_to='base_footprint'>1.0236099999999999 0 0.03977 0 0 0</pose>
      <parent>base_footprint</parent>
      <child>bumper_front</child>
      <axis>
        <dynamics>
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
        <xyz>0 0 1</xyz>
        <limit>
          <lower>-inf</lower>
          <upper>inf</upper>
        </limit>
      </axis>
      <physics>
        <ode>
          <limit>
            <cfm>0</cfm>
            <erp>0.20000000000000001</erp>
          </limit>
        </ode>
      </physics>
    </joint>
    <link name='bumper_front'>
      <pose relative_to='bumper_front_joint'>0 0 0 0 0 0</pose>
      <collision name='bumper_front_collision_collision'>
        <pose>0.017000000000000001 0 0 0 0 0</pose>
        <geometry>
          <box>
            <size>0.059999999999999998 0.81000000000000005 0.059999999999999998</size>
          </box>
        </geometry>
        <surface>
          <contact>
            <ode/>
          </contact>
          <friction>
            <ode/>
          </friction>
        </surface>
      </collision>
      <visual name='bumper_front_visual'>
        <pose>0 0 0 0 0 0</pose>
        <geometry>
          <box>
            <size>0.059999999999999998 0.81000000000000005 0.059999999999999998</size>
          </box>
        </geometry>
        <material>
          <ambient>1 1 0 1</ambient>
          <diffuse>1 1 0 1</diffuse>
          <specular>0 0 0 0</specular>
          <emissive>0 0 0 1</emissive>
        </material>
      </visual>
      <sensor name='bumper-front' type='contact'>
        <update_rate>10</update_rate>
        <always_on>true</always_on>
        <contact>
          <collision>bumper_front_collision_collision</collision>
          <topic>__default_topic__</topic>
        </contact>
      </sensor>
    </link>
    <frame name='base_link_robot_joint' attached_to='base_footprint'>
      <pose>0 0 0 0 0 0</pose>
    </frame>
    <frame name='base_link_robot' attached_to='base_link_robot_joint'>
      <pose>0 0 0 0 0 0</pose>
    </frame>
  </model>
  </world>
</sdf>

To avoid clutter, I leave it at that. Just separate model and world and spawn via gz service -s /world/test_world/create --reqtype gz.msgs.EntityFactory --reptype gz.msgs.Boolean --timeout 200 --req 'sdf_filename: "[path_to_model_sdf]" '

[jrutgeer]

I can confirm this:

• Topic is present when loading the model through the world file (gz sim world.sdf),
• Topic is not present when loading an empty world and spawning the model through the /world/test_world/create service.

[azeey]

I am able to reproduce this as well. I think the issue is that Contact uses EachNew in the PreUpdate, which is fine when entities are created by SimulationRunner because the entity creation happens before any of the *Update calls. But when a model is spawned through the create service, it uses the UserCommands system which creates entities in the PreUpdate phase, so the new entities will not be included in the EachNew call. In fact, the documentation for EachNew states that it should not be used in the PreUpdate for this reason.

I see three solutions, with increasing complexity:

1. Change Contact::PreUpdate to Contact::Update in the Contact system. This will ensure that new entities created by UserCommands will be captured by EachNew in the Contact system. We usually reserve the Update calls to the Physics system, but I don't see any harm in doing this since the Contact system would mostly be initializing internal state during this call. If the Physics system happens to run before the Contact system, we would miss contacts from the first iteration immediately after the model is spawned.
2. If we are not okay with using Contact::Update, use a combination of Contact::PostUpdate and Contact::PreUpdate. The PostUpdate would be used to initialize internal state and the PreUpdate would be used to create ContactSensorData components on the collision entities identified in the PostUpdate.
3. Update the ECM so that entity creation can be done by request, just like entity removal. The request would be processed outside of the *Update calls. This would avoid any ordering issues like this for all systems, not just Contact, but would be a lot more work.

[yschulz]

I could fix 1 or 2, especially since 1523 did the same as 2, I believe. I cannot answer though if we are ok with 1 ¯_(ツ)_/¯

Thanks for taking a look though!

[yschulz]

@azeey I was just implementing your 2nd suggested version when I ran into runtime issues and started testing back and forth with the new and previous version. I suddenly wasn't able to reproduce the issue anymore. It turns out, if you put the plugin tag in the sdf for the UserCommand before the one of Contact, the issue disappears.

This would suggest that it doesn't matter when the entities are created in the sequence. What's important is that the UserCommand system is loaded before the Contact system. That also sounds strange to me, but also like a separate issue to the call to EachNew inside PreUpdate.

[azeey]

The UserCommand system is what listens to /world/test_world/create and spawns the model, so it's not that it doesn't matter when the entities are created in the sequence. When you put the plugin tag for UserCommand before Contact, it means UserCommand::PreUpdate will be called before Contact::PreUpdate, so the entities will have been created before Contact::PreUpdate is called.