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

feat: 4x IMX219 surround vision + Nav2 costmap layer (Phase 2c) #52

Merged
seb merged 1 commits from sl-perception/surround-vision into main 2026-02-28 23:25:17 -05:00
Conversation 0 Commits 1 Files Changed 12 +894 -4
sl-perception commented 2026-02-28 23:19:47 -05:00
Collaborator
Copy Link

Summary

New package saltybot_surround — Phase 2c surround vision pipeline.

  • surround_costmap_node — subscribes to /camera/{front,left,rear,right}/image_raw, detects obstacles via Canny edge detection + ground back-projection, publishes /surround_vision/obstacles (PointCloud2 @ 5Hz in base_link). Catches obstacles RPLIDAR misses: glass walls, chair legs, people below scan plane.
  • surround_vision_node — IPM bird's-eye warp per camera, composites 4 patches into 240×240px 360° overhead image, publishes /surround_vision/birdseye (Image @ 10Hz).
  • surround_vision.launch.py — launches both nodes; start_cameras:=false when csi-cameras runs in its own container.
  • surround_vision_params.yaml — camera geometry, Canny thresholds, range, canvas size, rates.

Updated files:

  • jetson/config/nav2_params.yaml — add surround_cameras PointCloud2 observation source to both local and global costmap obstacle layers.
  • jetson/docker-compose.yml — add saltybot-surround service (depends_on: csi-cameras).
  • SLAM-SETUP-PLAN.md — Phase 2c ✅ Done.

Obstacle detection algorithm

Image → downsample 160×120 → fisheye undistort (placeholder cal) →
grayscale → equalizeHist → Canny(40,120) → morphClose →
edge pixels below horizon → back-project to ground plane (d = h*fy/(v-cy)) →
range filter [0.25m, 3.0m] → rotate to base_link by camera yaw →
publish PointCloud2 z=0.5m

Calibration note: IMX219 fisheye coefficients are zero placeholders.
Run ros2 run camera_calibration cameracalibrator per camera after hardware assembly.

Test plan

  • ros2 topic hz /surround_vision/obstacles — verify ~5Hz
  • ros2 topic hz /surround_vision/birdseye — verify ~10Hz
  • Place chair in front of robot (below LIDAR plane) — verify obstacle appears in /local_costmap/costmap
  • Hold glass pane in path — verify camera detects edges, LIDAR does not
  • ros2 run rqt_image_view rqt_image_view /surround_vision/birdseye — verify 360° overhead view

Generated with Claude Code

## Summary New package **`saltybot_surround`** — Phase 2c surround vision pipeline. - **`surround_costmap_node`** — subscribes to `/camera/{front,left,rear,right}/image_raw`, detects obstacles via Canny edge detection + ground back-projection, publishes `/surround_vision/obstacles` (PointCloud2 @ 5Hz in `base_link`). Catches obstacles RPLIDAR misses: glass walls, chair legs, people below scan plane. - **`surround_vision_node`** — IPM bird's-eye warp per camera, composites 4 patches into 240×240px 360° overhead image, publishes `/surround_vision/birdseye` (Image @ 10Hz). - **`surround_vision.launch.py`** — launches both nodes; `start_cameras:=false` when `csi-cameras` runs in its own container. - **`surround_vision_params.yaml`** — camera geometry, Canny thresholds, range, canvas size, rates. Updated files: - **`jetson/config/nav2_params.yaml`** — add `surround_cameras` PointCloud2 observation source to both local and global costmap obstacle layers. - **`jetson/docker-compose.yml`** — add `saltybot-surround` service (depends_on: csi-cameras). - **`SLAM-SETUP-PLAN.md`** — Phase 2c ✅ Done. ## Obstacle detection algorithm ``` Image → downsample 160×120 → fisheye undistort (placeholder cal) → grayscale → equalizeHist → Canny(40,120) → morphClose → edge pixels below horizon → back-project to ground plane (d = h*fy/(v-cy)) → range filter [0.25m, 3.0m] → rotate to base_link by camera yaw → publish PointCloud2 z=0.5m ``` Calibration note: IMX219 fisheye coefficients are zero placeholders. Run `ros2 run camera_calibration cameracalibrator` per camera after hardware assembly. ## Test plan - [ ] `ros2 topic hz /surround_vision/obstacles` — verify ~5Hz - [ ] `ros2 topic hz /surround_vision/birdseye` — verify ~10Hz - [ ] Place chair in front of robot (below LIDAR plane) — verify obstacle appears in `/local_costmap/costmap` - [ ] Hold glass pane in path — verify camera detects edges, LIDAR does not - [ ] `ros2 run rqt_image_view rqt_image_view /surround_vision/birdseye` — verify 360° overhead view Generated with Claude Code
sl-perception added 1 commit 2026-02-28 23:19:48 -05:00 
New ROS2 package saltybot_surround:

surround_costmap_node
  - Subscribes to /camera/{front,left,rear,right}/image_raw
  - Detects obstacles via Canny edge detection + ground projection
  - Pinhole back-projection: pixel row → forward distance (d = h*fy/(v-cy))
  - Rotates per-camera points to base_link frame using known camera yaws
  - Publishes /surround_vision/obstacles (PointCloud2, 5 Hz)
  - Catches chairs, glass walls, people that RPLIDAR misses
  - Placeholder IMX219 fisheye calibration (hook for real cal via cv2.fisheye)

surround_vision_node
  - IPM homography computed from camera height + pinhole model
  - 4× bird's-eye patches composited into 240×240px 360° overhead view
  - Publishes /surround_vision/birdseye (Image, 10 Hz)
  - Robot footprint + compass overlay

surround_vision.launch.py
  - Launches both nodes with surround_vision_params.yaml
  - start_cameras arg: set false when csi-cameras container runs separately

Updated:
- jetson/config/nav2_params.yaml   add surround_cameras PointCloud2 source
                                    to local + global costmap obstacle_layer
- jetson/docker-compose.yml        add saltybot-surround service
                                    (depends_on: csi-cameras, start_cameras:=false)
- projects/saltybot/SLAM-SETUP-PLAN.md  Phase 2c ✅ Done

Calibration TODO (run after hardware assembly):
  ros2 run camera_calibration cameracalibrator --size 8x6 --square 0.025 \
    image:=/camera/front/image_raw camera:=/camera/front
  Replace placeholder K/D in surround_costmap_node._undistort()

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
seb merged commit 2ffd462223 into main 2026-02-28 23:25:17 -05:00
seb deleted branch sl-perception/surround-vision 2026-02-28 23:25:17 -05: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#52
No description provided.
Delete Branch "sl-perception/surround-vision"

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?