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

sl-perception · 2026-03-15T14:37:49-04:00

sl-perception commented

2026-03-15 14:37:49 -04:00

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

Subscribe /camera/color/image_raw (BGR8, 30 Hz)
Convert to greyscale → cv2.aruco.ArucoDetector.detectMarkers() (new OpenCV 4.x API)
Pose estimation: cv2.aruco.estimatePoseSingleMarkers (legacy, still present in cv2 4.x); per-marker solvePnP(IPPE_SQUARE) fallback
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

## 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

sl-perception added 1 commit 2026-03-15 14:37:49 -04:00

feat: ArUco marker detection for docking (Issue #627 ) 1fd935b87e

New package saltybot_aruco_detect — DICT_4X4_50 ArUco detection from
RealSense D435i RGB, pose estimation, PoseArray + dock target output.

aruco_math.py (pure Python, no ROS2): rot_mat_to_quat (Shepperd),
  rvec_to_quat (Rodrigues + cv2 fallback), tvec_distance, tvec_yaw_rad,
  MarkerPose dataclass with lazy-cached distance_m/yaw_rad/lateral_m/quat.

aruco_detect_node.py (ROS2 node 'aruco_detect'):
  Subscribes: /camera/color/image_raw (30Hz BGR8) + /camera/color/camera_info.
  Converts to greyscale, cv2.aruco.ArucoDetector.detectMarkers().
  estimatePoseSingleMarkers (legacy API) with solvePnP(IPPE_SQUARE) fallback.
  Dock target: closest marker in dock_marker_ids (default=[42], empty=any),
    filtered to max_dock_range_m (3.0m).
  Publishes: /saltybot/aruco/markers (PoseArray — all detected, camera frame),
    /saltybot/aruco/dock_target (PoseStamped — closest dock candidate,
    position.z=forward, position.x=lateral), /saltybot/aruco/viz (MarkerArray
    — SPHERE + TEXT per marker, dock in red), /saltybot/aruco/status (JSON
    10Hz — detected_count, dock_distance_m, dock_yaw_deg, dock_lateral_m).
  Optional debug image with drawDetectedMarkers + drawFrameAxes.
  corner_refinement=CORNER_REFINE_SUBPIX.

config/aruco_detect_params.yaml, launch/aruco_detect.launch.py.
test/test_aruco_math.py: 22 unit tests (rotation/quat math, distance,
  yaw sign/magnitude, MarkerPose accessors + caching).

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

sl-jetson merged commit af46b15391 into main

2026-03-15 16:30:05 -04:00

sl-jetson referenced this issue from a commit

2026-03-15 16:30:06 -04:00

Merge pull request 'feat: ArUco docking detection (Issue #627)' (#638) from sl-perception/issue-627-aruco-docking into main

Sign in to join this conversation.

1 Participants

Notifications

Due Date

No due date set.

Dependencies

No dependencies set.

Reference: seb/saltylab-firmware#638