sl-controls
783dedf4d4
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>
22 lines
814 B
Plaintext
22 lines
814 B
Plaintext
# DockingStatus.msg — Real-time docking state snapshot (Issue #158)
|
||
# Published by: /saltybot/docking_node
|
||
# Topic: /saltybot/docking_status
|
||
|
||
builtin_interfaces/Time stamp
|
||
|
||
# Current FSM state
|
||
# Values: "IDLE" | "DETECTING" | "NAV2_APPROACH" | "VISUAL_SERVO" | "CONTACT" | "CHARGING" | "UNDOCKING"
|
||
string state
|
||
|
||
# Dock detection
|
||
bool dock_detected # ArUco marker or IR beacon visible
|
||
float32 distance_m # distance to dock (m); NaN if unknown
|
||
float32 lateral_error_m # lateral offset to dock centre (m); +ve = dock is right
|
||
|
||
# Battery / charging
|
||
float32 battery_pct # state of charge [0–100]
|
||
bool charging # charge current detected on dock pins
|
||
|
||
# Alignment quality (active only during VISUAL_SERVO)
|
||
bool aligned # within ±5 mm lateral and contact distance
|