sl-controls created pull request seb/saltylab-firmware#165

sl-controls
Mon, 02 Mar 2026 10:19:48 -0500

165#feat(#158): docking station auto-return — ArUco/IR detection, visual servo, charge monitoring#

Summary

Implements Issue #158 — autonomous docking station return.

Two new ROS2 packages:

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-override and resume_mission flags

saltybot_docking (ament_python, 20 Hz)

Pure modules (no ROS2 dep):

dock_detector.py: ArucoDetector (cv2.aruco PnP solve → DockPose: distance, yaw, lateral) + IRBeaconDetector (EMA envelope follower with amplitude threshold); both degrade gracefully if OpenCV/IR unavailable

visual_servo.py: IBVS proportional controller — v = k_lin × (d − target), ω = −k_ang × yaw; aligned when |lateral| < 5 mm AND d < contact_distance

charge_monitor.py: edge-triggered events: CHARGING_STARTED/STOPPED, THRESHOLD_LOW (15%), THRESHOLD_HIGH (80%)

docking_state_machine.py: 7-state FSM:

IDLE → DETECTING → NAV2_APPROACH → VISUAL_SERVO → CONTACT → CHARGING → UNDOCKING → IDLE

Interrupts mission at 15% battery; emits mission_resume on full charge cycle

Lost-detection timeout in VISUAL_SERVO (2 s → retry DETECTING)

Contact timeout (1 s without charging → realign)

All abort paths via undock_requested

ROS2 node:

docking_node.py: 20 Hz; Nav2 NavigateToPose action client (optional, falls back gracefully); /saltybot/dock + /saltybot/undock services; publishes /saltybot/docking_cmd_vel (servo + undock), /saltybot/resume_mission, /saltybot/docking_status

config/docking_params.yaml, launch/docking.launch.py

Test plan

64/64 pytest passing

IRBeaconDetector: envelope EMA, threshold, abs(sample), reset

VisualServo: proportional gains, no-reverse, clamps, alignment (±5 mm), stop()

ChargeMonitor: edge-triggered events, no duplicates, is_depleted/is_charged

DockingStateMachine: all 7 states, battery/request triggers, Nav2 arrived/failed, lost timeout, contact retry, mission_resume vs explicit undock