feat(#158): docking station auto-return with ArUco/IR detection and visual servo
Two new ROS2 packages implementing Issue #158:
saltybot_docking_msgs (ament_cmake)
- DockingStatus.msg: stamp, state, dock_detected, distance_m, lateral_error_m,
battery_pct, charging, aligned
- Dock.srv / Undock.srv: force + resume_mission flags
saltybot_docking (ament_python, 20 Hz)
- dock_detector.py: ArucoDetector (cv2.aruco PnP → DockPose) + IRBeaconDetector
(EMA envelope with amplitude threshold); both gracefully degrade if unavailable
- visual_servo.py: IBVS proportional controller — v = k_lin×(d−target),
ω = −k_ang×yaw; aligned when |lateral| < 5mm AND d < contact_distance
- charge_monitor.py: edge-triggered events (CHARGING_STARTED/STOPPED,
THRESHOLD_LOW at 15%, THRESHOLD_HIGH at 80%)
- docking_state_machine.py: 7-state FSM (IDLE→DETECTING→NAV2_APPROACH→
VISUAL_SERVO→CONTACT→CHARGING→UNDOCKING); mission_resume signal on
auto-dock cycle; contact retry on timeout; lost-detection timeout
- docking_node.py: 20Hz ROS2 node; Nav2 NavigateToPose action client (optional);
/saltybot/dock + /saltybot/undock services; /saltybot/docking_cmd_vel;
/saltybot/resume_mission; /saltybot/docking_status
- config/docking_params.yaml, launch/docking.launch.py
Tests: 64/64 passing (IRBeaconDetector, VisualServo, ChargeMonitor,
DockingStateMachine — all state transitions and guard conditions covered)
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
sl-controls
783dedf4d4