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

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

Merged
sl-jetson merged 1 commits from sl-perception/issue-611-obstacle-detect into main 2026-03-15 11:02:29 -04:00
Conversation 0 Commits 1 Files Changed 12 +1243
sl-perception commented 2026-03-15 10:09:49 -04:00
Collaborator
Copy Link

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

## 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
sl-perception added 1 commit 2026-03-15 10:09:50 -04:00 
New package saltybot_obstacle_detect — RANSAC ground plane fitting on
D435i depth images with 2D grid BFS obstacle clustering.

ground_plane.py (pure Python + numpy):
  fit_ground_plane(pts, n_iter=50, inlier_thresh_m=0.06): RANSAC over 3D
  point cloud in camera optical frame (+Z forward). Samples 3 points, fits
  plane via cross-product, counts inliers, refines via SVD on best inlier
  set. Orients normal toward -Y (upward in world). Returns (normal, d).
  height_above_plane(pts, plane): signed h = d - n·p (h>0 = above ground).
  obstacle_mask(pts, plane, min_h, max_h): min_obstacle_h_m < h < max_h.
  ground_mask(pts, plane, thresh): inlier classification.

obstacle_clusterer.py (pure Python + numpy):
  cluster_obstacles(pts, heights, cell_m=0.30, min_pts=5): projects
  obstacle 3D points onto (X,Z) bird's-eye plane, discretises into grid
  cells, runs 4-connected BFS flood-fill, returns ObstacleCluster list
  sorted by forward distance. ObstacleCluster: centroid(3), radius_m,
  height_m, n_pts + distance_m/lateral_m properties.

obstacle_detect_node.py (ROS2 node 'obstacle_detect'):
  - Subscribes: /camera/depth/camera_info (latched, once),
    /camera/depth/image_rect_raw (BEST_EFFORT, 30Hz float32 depth).
  - Pipeline: stride downsample (default 8x → 80x60) → back-project to
    3D → RANSAC ground plane (temporally blended α=0.3) → obstacle mask
    (min_h=0.05m, max_h=0.80m) → BFS clustering → alert classification.
  - Publishes:
      /saltybot/obstacles (MarkerArray): SPHERE markers colour-coded
        DANGER(red)/WARN(yellow)/CLEAR(green) + distance TEXT labels.
      /saltybot/obstacles/cloud (PointCloud2): xyz float32 non-ground pts.
      /saltybot/obstacles/alert (String JSON): alert_level, closest_m,
        obstacle_count, per-obstacle {x,y,z,radius_m,height_m,level}.
  - Safety zone integration (depth_estop_enabled=false by default):
      DANGER → zero Twist to depth_estop_topic (/cmd_vel_input) feeds
      into safety_zone's cmd_vel chain for independent depth e-stop.

config/obstacle_detect_params.yaml: all tuneable parameters with comments.
launch/obstacle_detect.launch.py: single node with params_file arg.
test/test_ground_plane.py: 10 unit tests (RANSAC correctness, normal
  orientation, height computation, inlier/obstacle classification).
test/test_obstacle_clusterer.py: 8 unit tests (single/dual cluster,
  distance sort, empty, min_pts filter, centroid accuracy, range clip).

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
sl-jetson merged commit 96d13052b4 into main 2026-03-15 11:02:29 -04: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#623
No description provided.
Delete Branch "sl-perception/issue-611-obstacle-detect"

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?