sl-perception created pull request seb/saltylab-firmware#638

sl-perception
Sun, 15 Mar 2026 14:37:49 -0400

638#feat: ArUco docking detection (Issue #627)#

Summary

New package saltybot_aruco_detect — detects all DICT_4X4_50 ArUco markers from the RealSense D435i RGB stream, estimates 6-DOF pose for each, and publishes a PoseArray + closest dock-target PoseStamped.

Detection pipeline

1. Subscribe /camera/color/image_raw (BGR8, 30 Hz)

2. Convert to greyscale → cv2.aruco.ArucoDetector.detectMarkers() (new OpenCV 4.x API)

3. Pose estimation: cv2.aruco.estimatePoseSingleMarkers (legacy, still present in cv2 4.x); per-marker solvePnP(IPPE_SQUARE) fallback

4. Select dock target: closest marker in dock_marker_ids within max_dock_range_m

Published topics

Topic | Type | Content
/saltybot/aruco/markers | PoseArray | All detected markers in camera_color_optical_frame
/saltybot/aruco/dock_target | PoseStamped | Closest dock candidate — position.z=forward dist, position.x=lateral offset
/saltybot/aruco/viz | MarkerArray | SPHERE+TEXT overlays per marker (dock=red, other=cyan)
/saltybot/aruco/status | String JSON (10 Hz) | detected_count, dock_distance_m, dock_yaw_deg, dock_lateral_m, per-marker details

aruco_math.py (pure Python, no ROS2)

rot_mat_to_quat(R) — Shepperd's method, all 4 quadrants, normalised

rvec_to_quat(rvec) — cv2.Rodrigues → quaternion; manual exponential-map fallback

tvec_distance/tvec_yaw_rad — distance + bearing error helpers

MarkerPose dataclass — lazy-cached distance_m, yaw_rad, lateral_m, forward_m, quat

Configuration

marker_size_m: 0.10 — physical marker side length

dock_marker_ids: [42] — default dock marker; empty = accept any detected marker

corner_refinement: CORNER_REFINE_SUBPIX — sub-pixel accuracy

draw_debug_image: false — optional annotated image with axes overlay

Tests

22 unit tests in test/test_aruco_math.py: identity/90°/180°/arbitrary rotations, unit-norm invariant, Rodrigues roundtrip, tvec distance/yaw sign + magnitude, MarkerPose accessor caching.

Closes #627