Autodocking package discussion

[BotshareAI]

🚀 Autodocking Architecture Discussion (Linorobot2 / OpenAMR)

We are opening this thread to discuss the new autodocking plan for the OpenAMR / Linorobot2 platform running ROS 2 on Raspberry Pi 5. The objective is to finalize a reliable docking workflow integrating Nav2, AprilTag-based detection, and automated charging logic.

📌 Summary of the Proposed System

• Robot navigates normally using Nav2 on a static map.
• On low battery, it moves to a staging pose near the dock.
• From there, a Docking Server takes control and switches to a vision-based alignment procedure.
• AprilTag (tag36h11) on the dock provides precise pose correction.
• The robot performs alignment, slow forward approach, verifies charging, waits for battery full, undocks, and returns to operations.

🧩 Core Components

• Nav2 for global navigation.
• OpenNav Docking Server (opennav_docking).
• SimpleChargingDock plugin for alignment and approach.
• AprilTag detection (apriltag_ros).
• dock_pose_bridge node to convert tag detections into a clean detected_dock_pose.
• charge_monitor to detect CHARGING → FULL transitions.
• twist_mux to grant control to docking behavior when active.
• Static map with predefined dock pose.
• TF tree including camera_link → camera_optical_frame.

🔌📥 Docking sequence

🛠 Workflow Overview

1. Normal operation with Nav2.
2. Battery drops below threshold → trigger docking.
3. Navigate to predefined staging pose.
4. Switch to Docking Server.
5. Use AprilTag pose to refine dock position.
6. Align, approach, and connect to the charger.
7. Verify charging via BatteryState or hardware signal.
8. On battery full → reverse to undock.
9. Return control to Nav2 and resume tasks.

🔧 Implementation Tasks

• Implement dock_pose_bridge (filter tag by ID, compute pose, publish).
• Implement charge_monitor (battery full detection, optional GPIO sensing).
• Configure Docking Server + plugin (staging, alignment, approach parameters).
• Integrate twist_mux with priority switching.
• Combine into unified bringup launch and YAML configuration.

✔ Topics for Discussion

• Tag placement and geometric offsets.
• Whether charging detection is BatteryState-only or hardware-assisted.
• Alignment and approach parameters (speed, timeouts, backoff).
• Retry strategy for failed docking attempts.
• Required success rate and test scenarios.

Please share comments and improvements so we can finalize the autodocking implementation.

🔍📝 More detailed description:

Autodocking_Linorobot2_ROS2_RPi.pdf

[SHuttooo]

Auto-docking package — simulation testing report

Wanted to share what I've been working on the past few days. I set up a full Gazebo Harmonic simulation of the openamrobot_docking package so I could validate the AprilTag-based docking pipeline end-to-end before touching the real robot. Sharing the experience here because a few of the things that tripped me up might be useful to others doing the same.

The setup

Ubuntu 24.04, ROS 2 Jazzy, Gazebo Harmonic. A 10×10 m room with the OMR differential-drive robot, a 0.40 m AprilTag 36h11 mounted on a panel against the north wall, and the full pipeline running: apriltag_ros reading from the simulated camera, detected_dock_pose_publisher republishing the tag TF as a PoseStamped, Nav2 with slam_toolbox in mapping mode, and the docking trigger.

One thing worth flagging upfront: on Jazzy I had to switch from FastDDS to CycloneDDS. The default FastDDS has a Python-side bug that makes dock_trigger.py exit silently when sending action goals — no error, no log, just dead. Spent a while chasing that before realising the RMW was the issue. A simple export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp in every terminal fixed it.

Why I ended up writing a custom sequencer

I started with opennav_docking's controlled_approach, but the graceful_controller consistently produced a curved trajectory that came in at the dock from the side rather than head-on. Lateral offset would accumulate along the curve, and the robot would end up parked at an angle.

So I replaced the single DockRobot action with a 7-phase sequence in scripts/dock_trigger.py. The idea is to break the approach into geometrically clean steps instead of relying on the controller to do everything at once:

1. NavigateToPose to a rough staging zone.
2. Hold for a second, robot fully still.
3. Read a filtered tag pose — 20 samples averaged, with the quaternion re-normalised.
4. Compute a "parallel spot" perpendicular to the tag at staging distance. The tag normal is extracted from the quaternion; the sign ambiguity that solvePnP can introduce is resolved by a dot product with the (robot − tag) vector.
5. Rotate-drive to that spot. This is what corrects any lateral offset accumulated during phase 1.
6. Spin in place to face the tag perpendicularly.
7. Advance in a straight line, with the target yaw continuously re-derived from the live tag detection (low-pass filtered with α=0.15). Stop when the distance to the tag drops to 0.60 m.

The advance phase is the part I'm most happy with — it auto-calibrates as it goes, so even if the alignment from phase 6 leaves a small error, the robot converges to the true perpendicular by the time it reaches the dock.

How it performs

End-to-end works: spawn in the south-west corner, navigate to staging, snap to the parallel spot, align, advance, stop cleanly at 0.60 m from the tag. The trajectory is now a sequence of clean straight lines and rotations, not a curve.

The residual error at the final position is a few centimetres laterally and a few degrees in yaw. After looking at it for a while I'm convinced this is dominated by AprilTag detection noise rather than anything in the controller — the 20-sample filtering in phase 3 helps, and bumping the tag from 0.25 m to 0.40 m made a big difference (~125 px per side at 1.5 m distance, which is comfortably in sub-pixel solvePnP territory).

Things that broke along the way

A few problems I hit that aren't obvious from the docs, in case they save someone time:

• Robot tipping during navigation. The drive wheels were lifting off briefly under acceleration and the casters couldn't catch it. Fixed by sizing the caster spheres so they don't actually contact the ground at rest (radius 0.016 m, so they sit ~0.4 mm above) — the drive wheels carry 100% of the weight, traction is full, and velocity_smoother with conservative accels (1.2 m/s² linear, 1.5 rad/s² angular) does the rest.
• Planner failing at startup with "Failed to create plan with tolerance of: 0.500000". Switched to A* with tolerance: 1.0, and dropped static_layer from the global costmap (it was waiting for /map, which SLAM hadn't published yet).
• Camera dropping frames. Tried bumping the camera to 1280×720 for accuracy. CameraInfo kept publishing fine but Image dropped to ~0.2 Hz — the gz plugin or the bridge couldn't keep up. Reverted to 640×480 @ 15 Hz and compensated with the bigger tag instead.
• collision_monitor clamping cmd_vel to zero on phantom near-obstacles whenever the robot tilted slightly and the lidar registered its own body. Disabled FootprintApproach.
• Nav2's Spin behaviour refusing to rotate during alignment, same root cause (phantom collisions). Phases 6 and 7 of the sequencer now publish directly to /cmd_vel_nav, bypassing the behavior server's collision check.
• Zombie processes after Ctrl-C — gz-sim, parameter_bridge, slam_toolbox not always cleaning up, preventing the next launch. Added scripts/kill_sim.sh to SIGKILL everything in one go.

Documentation

I also wrote up the whole thing in 14 markdown files under openamrobot_docking/docs/, covering setup, calibration, the TF chain, a parameter reference, troubleshooting, the full step-by-step story of building the sim (every gotcha included), and a detailed walkthrough of what each phase of the sequencer does and why.

To reproduce

sudo apt install ros-jazzy-rmw-cyclonedds-cpp
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
cd ~/Downloads/openamrobot-docking-main
colcon build --packages-select openamrobot_docking
source install/setup.bash
ros2 launch openamrobot_docking simulation.launch.py
# in another terminal (with CycloneDDS exported there too):
ros2 topic pub /dock_trigger std_msgs/msg/Bool "{data: true}" --once

Since I don't have write access to the upstream repo, I pushed the work to my own fork: https://github.com/SHuttooo/openamrobot-docking. Everything is on branch matthieu/jazzy-setup-docs, latest commit edbe57f. Happy to open a PR if there's interest in merging any of it back. Also happy to answer questions or take feedback on the sequencer design — especially curious whether anyone else has hit the same FastDDS issue on Jazzy.