seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests 3 Actions Packages Projects Releases Wiki Activity

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

Merged
sl-jetson merged 1 commits from sl-controls/issue-158-docking into main 2026-03-02 10:26:18 -05:00
Conversation 0 Commits 1 Files Changed 18 +2030
sl-controls commented 2026-03-02 10:19:48 -05:00
Collaborator
Copy Link

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
## 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 - [x] 64/64 pytest passing - [x] IRBeaconDetector: envelope EMA, threshold, abs(sample), reset - [x] VisualServo: proportional gains, no-reverse, clamps, alignment (±5 mm), stop() - [x] ChargeMonitor: edge-triggered events, no duplicates, is_depleted/is_charged - [x] DockingStateMachine: all 7 states, battery/request triggers, Nav2 arrived/failed, lost timeout, contact retry, mission_resume vs explicit undock
sl-controls added 1 commit 2026-03-02 10:19:48 -05:00 
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-jetson merged commit 77b3d614dc into main 2026-03-02 10:26:18 -05:00
Sign in to join this conversation.
Reviewers
No Reviewers
Labels
No items
No Label
Milestone
No items
No Milestone
Projects
Clear projects
No project
Assignees
Clear assignees
shaytac sl-controls sl-firmware sl-jetson sl-mechanical sl-perception sl-uwb sl-webui seb
No Assignees
1 Participants
Notifications
Due Date
No due date set.
Dependencies

No dependencies set.

Reference: seb/saltylab-firmware#165
No description provided.
Delete Branch "sl-controls/issue-158-docking"

Deleting a branch is permanent. Although the deleted branch may continue to exist for a short time before it actually gets removed, it CANNOT be undone in most cases. Continue?