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

feat: Depth-to-costmap plugin for RealSense D435i (Issue #532) #540

Closed
sl-perception wants to merge 0 commits from sl-perception/issue-532-depth-costmap into main
pull from: sl-perception/issue-532-depth-costmap
merge into: seb:main
Conversation 0 Commits - Files Changed -
sl-perception commented 2026-03-07 09:53:00 -05:00
Collaborator
Copy Link

Summary

Implements saltybot_depth_costmap — a Nav2 costmap2d plugin that converts RealSense D435i depth images directly into obstacle markings on the Nav2 local and global costmaps.

Pipeline

  1. Subscribe to /camera/depth/image_rect_raw (16UC1, mm) + /camera/depth/camera_info
  2. Back-project depth pixels to 3D using pinhole camera intrinsics (fx, fy, cx, cy)
  3. Transform 3D points to costmap global_frame via TF2
  4. Apply configurable height filter (min_height..max_height above ground)
  5. Mark obstacle cells as LETHAL_OBSTACLE
  6. Inflate neighbouring cells within inflation_radius as INSCRIBED_INFLATED_OBSTACLE

Parameters

Parameter Default Description
min_height 0.05 m Floor clearance — ignores ground returns
max_height 0.80 m Ceiling cutoff — ignores lights/overhead
obstacle_range 3.5 m Max marking distance from camera
clearing_range 4.0 m Max range processed at all
inflation_radius 0.10 m In-layer inflation (runs before inflation_layer)
downsample_factor 4 Process 1 of N rows+cols (~19k pts @ 640×480)

Integration with #478

  • depth_costmap_layer added to local_costmap and global_costmap plugin lists in nav2_params.yaml
  • Plugin registered via pluginlib (plugin.xml) — drops in alongside existing voxel_layer and obstacle_layer

Files

  • jetson/ros2_ws/src/saltybot_depth_costmap/ — new Nav2 plugin package
    • CMakeLists.txt, package.xml, plugin.xml
    • include/saltybot_depth_costmap/depth_costmap_layer.hpp
    • src/depth_costmap_layer.cpp
  • jetson/ros2_ws/src/saltybot_bringup/config/nav2_params.yaml — adds depth_costmap_layer to both costmaps

Test plan

  • colcon build --packages-select saltybot_depth_costmap succeeds
  • Plugin loads in Nav2: check ros2 run pluginlib pluginlib_headers saltybot_depth_costmap (or inspect Nav2 logs)
  • /camera/depth/image_rect_raw + /camera/depth/camera_info publishing
  • TF: camera_depth_optical_frame → odom transform available
  • ros2 topic echo /local_costmap/costmap shows obstacles from depth camera
  • Height filter: floor returns ignored (zw < 0.05 m), ceiling ignored (zw > 0.80 m)
  • Inflation: neighbouring cells show INSCRIBED_INFLATED_OBSTACLE (253) around LETHAL (254) cells
  • Downsample factor 4 gives acceptable CPU load on Jetson Orin Nano Super

Closes #532

## Summary Implements `saltybot_depth_costmap` — a Nav2 costmap2d plugin that converts RealSense D435i depth images directly into obstacle markings on the Nav2 local and global costmaps. ### Pipeline 1. Subscribe to `/camera/depth/image_rect_raw` (16UC1, mm) + `/camera/depth/camera_info` 2. Back-project depth pixels to 3D using pinhole camera intrinsics (fx, fy, cx, cy) 3. Transform 3D points to costmap `global_frame` via TF2 4. Apply configurable height filter (`min_height`..`max_height` above ground) 5. Mark obstacle cells as `LETHAL_OBSTACLE` 6. Inflate neighbouring cells within `inflation_radius` as `INSCRIBED_INFLATED_OBSTACLE` ### Parameters | Parameter | Default | Description | |-----------|---------|-------------| | `min_height` | 0.05 m | Floor clearance — ignores ground returns | | `max_height` | 0.80 m | Ceiling cutoff — ignores lights/overhead | | `obstacle_range` | 3.5 m | Max marking distance from camera | | `clearing_range` | 4.0 m | Max range processed at all | | `inflation_radius` | 0.10 m | In-layer inflation (runs before `inflation_layer`) | | `downsample_factor` | 4 | Process 1 of N rows+cols (~19k pts @ 640×480) | ### Integration with #478 - `depth_costmap_layer` added to `local_costmap` and `global_costmap` plugin lists in `nav2_params.yaml` - Plugin registered via `pluginlib` (`plugin.xml`) — drops in alongside existing `voxel_layer` and `obstacle_layer` ### Files - `jetson/ros2_ws/src/saltybot_depth_costmap/` — new Nav2 plugin package - `CMakeLists.txt`, `package.xml`, `plugin.xml` - `include/saltybot_depth_costmap/depth_costmap_layer.hpp` - `src/depth_costmap_layer.cpp` - `jetson/ros2_ws/src/saltybot_bringup/config/nav2_params.yaml` — adds `depth_costmap_layer` to both costmaps ## Test plan - [ ] `colcon build --packages-select saltybot_depth_costmap` succeeds - [ ] Plugin loads in Nav2: check `ros2 run pluginlib pluginlib_headers saltybot_depth_costmap` (or inspect Nav2 logs) - [ ] `/camera/depth/image_rect_raw` + `/camera/depth/camera_info` publishing - [ ] TF: `camera_depth_optical_frame` → `odom` transform available - [ ] `ros2 topic echo /local_costmap/costmap` shows obstacles from depth camera - [ ] Height filter: floor returns ignored (zw < 0.05 m), ceiling ignored (zw > 0.80 m) - [ ] Inflation: neighbouring cells show INSCRIBED_INFLATED_OBSTACLE (253) around LETHAL (254) cells - [ ] Downsample factor 4 gives acceptable CPU load on Jetson Orin Nano Super Closes #532
sl-perception added 1 commit 2026-03-07 09:53:01 -05:00 
Add saltybot_depth_costmap — a Nav2 costmap2d plugin that converts
D435i depth images directly into obstacle markings on both local and
global costmaps.

Pipeline:
  1. Subscribe to /camera/depth/image_rect_raw (16UC1 mm) + camera_info
  2. Back-project depth pixels to 3D using pinhole camera intrinsics
  3. Transform points to costmap global_frame via TF2
  4. Apply configurable height filter (min_height..max_height above ground)
  5. Mark obstacle cells as LETHAL_OBSTACLE
  6. Inflate neighbours within inflation_radius as INSCRIBED_INFLATED_OBSTACLE

Parameters:
  min_height: 0.05 m       — floor clearance (ignores ground returns)
  max_height: 0.80 m       — ceiling cutoff (ignores lights/ceiling)
  obstacle_range: 3.5 m    — max marking distance from camera
  clearing_range: 4.0 m    — max distance processed at all
  inflation_radius: 0.10 m — in-layer inflation (works before inflation_layer)
  downsample_factor: 4     — process 1 of N rows+cols (~19k pts @ 640×480)

Integration (#478):
  - Added depth_costmap_layer to local_costmap plugins list
  - Added depth_costmap_layer to global_costmap plugins list
  - Plugin registered via pluginlib (plugin.xml)

Files:
  jetson/ros2_ws/src/saltybot_depth_costmap/
    CMakeLists.txt, package.xml, plugin.xml
    include/saltybot_depth_costmap/depth_costmap_layer.hpp
    src/depth_costmap_layer.cpp
  jetson/ros2_ws/src/saltybot_bringup/config/nav2_params.yaml (updated)

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
sl-jetson closed this pull request 2026-03-07 13:47:03 -05:00

Pull request closed

Please reopen this pull request to perform a merge.
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#540
No description provided.
Delete Branch "sl-perception/issue-532-depth-costmap"

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?