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

sl-perception
Sun, 15 Mar 2026 10:09:50 -0400

623#feat: RealSense obstacle detection (Issue #611)#

Summary

New package saltybot_obstacle_detect — RANSAC ground-plane fitting on D435i depth images, BFS obstacle clustering, MarkerArray centroids, PointCloud2 non-ground points, and safety_zone e-stop integration.

Pipeline (30 Hz)

1. Stride-downsample float32 depth image (default 8× → 80×60 px)

2. Back-project valid pixels to 3D camera-optical-frame point cloud

3. RANSAC ground plane (50 iter, 0.06 m inlier threshold, SVD refinement) — normal oriented upward (−Y), exponentially blended across frames (α=0.3)

4. Height filter: min_obstacle_h=0.05 m < h < max_obstacle_h=0.80 m

5. 2D grid BFS clustering on (X,Z) bird's-eye plane (cell=0.30 m, min_pts=5)

6. Alert classification per cluster: DANGER (<0.40 m), WARN (<1.20 m), CLEAR

Published topics

Topic | Type | Content
/saltybot/obstacles | MarkerArray | Sphere centroids, colour-coded DANGER/WARN/CLEAR + distance labels
/saltybot/obstacles/cloud | PointCloud2 | xyz float32 non-ground obstacle points
/saltybot/obstacles/alert | String (JSON) | alert_level, closest_m, obstacle_count, per-obstacle details

Safety zone integration

When depth_estop_enabled: true, DANGER obstacles publish zero Twist to depth_estop_topic (default /cmd_vel_input). This feeds into the saltybot_safety_zone cmd_vel chain, giving independent depth-based stopping for obstacles the LIDAR beam misses (low surfaces, floor-level hazards).

Pure-math modules (no ROS2, fully testable)

ground_plane.py: RANSAC + SVD fit, height_above_plane, obstacle_mask, ground_mask

obstacle_clusterer.py: 4-connected BFS on 2D occupancy grid, ObstacleCluster dataclass

Tests

test_ground_plane.py: 10 tests — flat/tilted ground, normal orientation, unit vector, height computation, obstacle/ground mask accuracy

test_obstacle_clusterer.py: 8 tests — single/dual cluster, sort order, empty input, min_pts filter, centroid accuracy, range clipping

Closes #611