JdeRobot / JdeRobot/RoboticsAcademy

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

Open
#3,932 3 comments 0 reactions 0 assignees View on GitHub
Dominant language
Python
Stars
492
Forks
357
Avg merge
1h 58m
Merged PRs (30d)
25

Description

## 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_` 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).

https://github.com/user-attachments/assets/f2d238f4-4fb3-423c-ae2e-efd6d886683a

Contributor guide

Open the contributing guide

Research direction

Start with RoboticsInfrastructure/Industrial/drone_gripper/src/drone_gripper.cpp, especially ISystemPreUpdate, ISystemReset, TryAttach, Detach, and HandleResetDetach. Trace the reset sequence through manager/manager.py::reset_sim() and manager/launcher/launcher_gzsim.py::LauncherGzsim.reset(), then reproduce repeated mid-carry resets. Done means the box can be physically re-grabbed after reset without stale attached state or a gz-sim crash.

Written by the indexing model from the issue text.

Assessment

Tech stack
cpp, python
Domain
robotics
Issue type
Bug
Difficulty
5/5
Estimated time
Over a week
Activity status
Active
Clarity
Mostly clear
Newbie friendliness
35/100

Get new issues in your inbox

A short digest of beginner-friendly GitHub issues.