Package delivery, drone magnet fails to re-grab the box after a reset performed mid-carry #3932

Branches

• RoboticsAcademy: branch package-delivery
• RoboticsInfrastructure (submodule): branch drone-amazon-delivery

Summary

In the package_delivery exercise, the drone uses an electromagnet (drone_gripper
plugin, RoboticsInfrastructure/Industrial/drone_gripper/src/drone_gripper.cpp) to pick
up a box (package_box_01) via a gz::sim::components::DetachableJoint created
dynamically between the drone's magnet link and the box's canonical link.

Symptom: the first time the drone grabs the box in a session, everything works
correctly (both the C++ and Python exercise variants). If at any point a reset is
triggered while the drone is carrying the box, from then on the magnet silently stops
being able to re-grab it: no errors are raised, and in some cases the state topic even
reports attached: true for several seconds straight without the box moving physically.

Architecture before vs. now

• Before commit 6f32e1be4 ("make robot, world independent"), the drone was a static
  part of the world, just like the box. A reset never destroyed or recreated either
  entity iit only restored poses/velocities via gz-sim's native reset
  (WorldReset(all=true), equivalent to a SetState restore against the initial
  snapshot). The DetachableJoint was always created and destroyed between a parent
  (drone) and a child (box) that both stayed alive across resets.

• Now, the reset (orchestrated from RoboticsApplicationManager,
  manager/manager.py::reset_sim() + manager/launcher/launcher_gzsim.py::LauncherGzsim.reset())
  does, in this order:

  1. WorldControl(pause=True) — pause the world.
  2. /world/default/remove on the robot entity — fully deletes the drone model.
  3. WorldControl(pause=True, reset=WorldReset(all=True)) — native reset, restoring any
     entity present in the world's initial snapshot (including the box, which is never
     deleted, only pose-reset).
  4. The backend respawns the drone with a fresh ros2 launch (a brand-new spawn, new
     entity).

DetachableJoint connects two bodies by internally reparenting (at the physics-engine
level, dartsim) the box's body under the drone's skeleton while attached. On detach, the
plugin only removes the ECM DetachableJoint component
(_ecm.RequestRemoveEntity(activeJoint)), asking Physics to undo that reparenting.

The problem is the timing window: that detach happens in the same cycle (world paused) in
which, almost immediately after, the parent side (drone) gets fully destroyed so it
can be recreated from scratch. If the Physics system doesn't get to fully process/apply
the box's "un-reparenting" before the parent skeleton disappears, the box is left with an
inconsistent internal physics state: the ECM correctly reflects that there's no joint
anymore, but the box's body in the physics engine ends up in a corrupted state relative to
a parent-child relationship that no longer exists. A later DetachableJoint on the same
link is created fine at the data level (which is why the state topic can report
attached: true), but the physics engine never actually enforces the constraint.

DetachableJoint is not designed or tested for the case "one of the two ends literally
stops existing mid-detach" only for the case "both ends persist and get their pose
reset", which is exactly the old architecture's scenario.

History of previous fix attempts (all in RoboticsInfrastructure, drone-amazon-delivery branch)

Previously merged commits that progressively hardened the plugin for this transition:

• fc107209a — adds ISystemReset, detach + state cleanup on reset.
• 27c376629 — detects the drone being removed in the SAME cycle (EachRemoved) instead
  of a cycle late, so the joint doesn't outlive its links.
• 691692a78 — prevents the plugin's ROS thread from becoming a zombie (duplicate
  drone_gripper_<model> node) that blocks the respawn.
• 777732771 — detach while the world is paused, before the drone gets removed.
• fbad6a019 — forces magnetEnabled = false while paused (otherwise TryAttach would
  auto-reattach to a stale object right on unpause).
• 596d81f5c — explicit ROS node teardown (dead_ flag) to avoid zombies once the
  drone's entity is removed.

Additional fix from this session (applied and currently active):

• Publish attached: false immediately on a reset-forced detach, instead of waiting
  for the periodic heartbeat (which never runs, since the instance dies first) — avoids
  the ROS2-side HAL being left with a stale is_carrying() == True after reset.

Attempt that did NOT work (reverted)

In addition to removing the joint, an attempt was made to also delete the box entity
at the same point (inside HandleResetDetach, paused/GripperGoneOrRemoving path),
relying on the WorldReset(all=true) the backend issues right after to recreate the box
from the initial snapshot with a clean physics body — matching the box's treatment to the
drone's own (which is also deleted and recreated).

Conceptually this was the right direction (if the parent side is destroyed and recreated,
the child side should be too, avoiding the asymmetry described above). In practice,
though, after several repeated "grab box → reset mid-carry" cycles, gzserver itself
(the gz sim -s process) ended up dying completely (stops showing up in ps aux, and
the scene launcher (ros2 launch package_delivery.launch.py) never relaunches it).
Suspicion: deleting a model entity within that same paused window competes/interferes with
gz-sim's own internal restore mechanism, more severely than deleting just the joint. No
crash log was captured (gzserver's stdout doesn't land in any accessible file in the dev
container — see the "How to debug" section below) — this is a strong correlation-based
hypothesis, not a confirmed cause backed by a stack trace.

This part has been reverted (only the joint removal + immediate attached:false
publish remain). With that, reset no longer brings down gzserver, but the original bug
(the box can't be re-grabbed after a reset performed mid-carry) is still unresolved.

Reproduction

1. Launch the package_delivery exercise.
2. Have the drone grab the box (enable_magnet() near the box until is_carrying()
   becomes true).
3. With the box attached, trigger a reset from the RA UI (or directly via
   reset_sim()/LauncherGzsim.reset()).
4. Try to grab the box again (enable_magnet() near it).
5. Observed result: either it never reattaches (physical symptom: the box stays on the
   ground, the drone flies off alone), or the attached topic reports true with no real
   physical movement of the box (confirmed live with ros2 topic echo: attached: true
   held for ~5.5s straight while the box never moved in the viewport).

files

• RoboticsInfrastructure/Industrial/drone_gripper/src/drone_gripper.cpp — the magnet
  plugin (ISystemPreUpdate/ISystemReset, TryAttach/Detach/HandleResetDetach).
• RoboticsInfrastructure/database/worlds.sql — the "Quadrotor Gripper" robot row
  (id 31): entity=drone, extra_config includes gripper:=true.
• RoboticsInfrastructure/CustomRobots/quadrotor/models/quadrotor/quadrotor_common.urdf.xacro
  — magnet geometry (magnet_ring_seg, magnet link, magnetic_gripper).
• RoboticsApplicationManager/manager/manager.py::reset_sim() and
  manager/launcher/launcher_gzsim.py::LauncherGzsim.reset() — the actual reset
  orchestration (pause → remove robot → WorldReset(all=true) → respawn robot). Not
  freely modifiable from this repo (separate project).