Fix deadlock in model attachment

[anwei-dev]

Description

Fix a deadlock in the model attachment plugin when handling the attach service callback.

Problem

The attachCallback() function previously acquired Gazebo's PhysicsUpdateMutex (Mutex A) before proceeding with the attachment operation.

While Mutex A was held, World::SetPaused() was subsequently called to pause the simulation, which required acquiring another internal World mutex (Mutex B).

At the same time, Gazebo's physics update thread may acquire Mutex B during the normal physics update process and subsequently attempt to acquire Mutex A.

This results in the following lock-order inversion:

attachCallback():
    Mutex A → Mutex B

Physics update thread:
    Mutex B → Mutex A

resulting in a classic AB-BA mutex deadlock.

Fix

The fix changes the order of operations so that the simulation is paused before acquiring PhysicsUpdateMutex.

The existing PhysicsUpdateMutex protection scope is preserved, so the attachment operation remains fully protected by the mutex. A RAII-based PauseGuard was introduced to manage the simulation pause state, covering the entire protected operation and restoring the world's original paused state automatically after the operation completes.

This changes the ordering between the simulation pause and PhysicsUpdateMutex acquisition without reducing the mutex's protection scope.

Testing

The fix was tested in a ROS 2 Humble + Gazebo environment.

• Successfully built the modified plugin with colcon build.
• Successfully started Gazebo with the modified plugin.
• Successfully tested the /gazebo/attach service.
• Successfully tested the /gazebo/detach service.
• Confirmed that Gazebo remained responsive during attach/detach operations.
• Confirmed that the previously observed deadlock no longer occurred.

---

Debugging Details

The deadlock was reproduced by directly calling the /gazebo/attach service.

GDB confirmed a circular wait between two threads:

Attachment thread:
    LWP 1027319
    holds the World mutex
    waits for PhysicsUpdateMutex

Gazebo physics update thread:
    LWP 1027366
    holds PhysicsUpdateMutex
    waits for the World mutex

The attachment thread was blocked in:

ModelAttachmentPlugin::attach()
    ↓
World::SetPaused()
    ↓
pthread_mutex_lock()
    ↓
futex_wait()

The corresponding mutex information was:

Mutex A:
    address: 0x63624b6af830
    owner:   LWP 1027366

Mutex B:
    address: 0x63624b50abb0
    owner:   LWP 1027319

The Gazebo physics update thread was blocked in the gazebo_ros2_control joint write path:

World::RunLoop()
    ↓
World::Step()
    ↓
World::Update()
    ↓
gazebo_ros2_control::GazeboSystem::write()
    ↓
gazebo::physics::ODEJoint::SetPosition()

This confirmed the circular mutex dependency described above.

[anwei-dev]

Hi @brta-jc gentle ping on this PR. Thanks!

[brta-jc]

No problem with this, thank you for the contribution. Will adopt internally as well.