sl-perception
dc01efe323
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>
168 lines
5.5 KiB
Markdown
168 lines
5.5 KiB
Markdown
# SLAM Setup Plan — Jetson Orin Nano Super
|
||
|
||
**Bead:** bd-wax (plan), bd-a2j (sensor drivers done PR #17)
|
||
**Phase:** 2 | **Owner:** sl-perception
|
||
**Updated:** 2026-03-01 — revised for Jetson Orin Nano Super (replaces Nano 4GB)
|
||
|
||
> All Nano-era constraints (10W cap, 2Hz detection, 400 features, no 3D) are obsolete.
|
||
|
||
---
|
||
|
||
## Hardware
|
||
|
||
| Component | Specs |
|
||
|-----------|-------|
|
||
| AI Brain | **Jetson Orin Nano Super 8GB** — 6-core A78AE, 1024-core Ampere, **67 TOPS**, JetPack 6 |
|
||
| Depth Cam | Intel RealSense D435i — 848×480 @ 90fps, BMI055 IMU |
|
||
| LIDAR | RPLIDAR A1M8 — 360° 2D, 12m range, ~5.5 Hz |
|
||
| Wide Cams | 4× IMX219 160° CSI — front/right/rear/left 90° intervals *(arriving)* |
|
||
| FC | STM32F722 — UART bridge `/dev/ttyACM0` @ 921600 |
|
||
|
||
---
|
||
|
||
## 1. OS & ROS2
|
||
|
||
JetPack 6 = Ubuntu 22.04 → ROS2 Humble via **native apt** (no Docker workarounds needed).
|
||
|
||
```bash
|
||
sudo apt install ros-humble-desktop ros-humble-rtabmap-ros \
|
||
ros-humble-rplidar-ros ros-humble-realsense2-camera \
|
||
ros-humble-slam-toolbox ros-humble-nav2-bringup
|
||
```
|
||
|
||
Docker still supported for CI. Updated `Dockerfile` uses `nvcr.io/nvidia/l4t-jetpack:r36.2.0`.
|
||
|
||
---
|
||
|
||
## 2. SLAM Stack
|
||
|
||
### Primary: RTAB-Map (RGB-D + 2D LIDAR fusion)
|
||
|
||
| Parameter | Nano 4GB (old) | **Orin Nano Super** |
|
||
|-----------|---------------|---------------------|
|
||
| Detection rate | 2 Hz | **10 Hz** |
|
||
| Visual features | 400 | **1000** |
|
||
| D435i profile | 640×480×15fps | **848×480×30fps** |
|
||
| 3D point cloud | disabled | **enabled** |
|
||
| Map type | 2D only | **2D + 3D** |
|
||
| Processing time limit | 700ms | **none** |
|
||
| Short-term memory | 30 keyframes | **unlimited** |
|
||
|
||
Fusion: RPLIDAR `/scan` (fast 2D loop closure) + D435i depth (3D reconstruction + visual odometry).
|
||
|
||
### Secondary: slam_toolbox (LIDAR-only localization / pre-built map mode)
|
||
|
||
---
|
||
|
||
## 3. Architecture
|
||
|
||
```
|
||
Jetson Orin Nano Super (Ubuntu 22.04 / JetPack 6 / CUDA 12.x)
|
||
|
||
realsense2_camera rplidar_ros
|
||
848×480×30fps /scan ~5.5Hz 360°
|
||
/camera/color │
|
||
/camera/depth │
|
||
/camera/imu ~400Hz │
|
||
│ │
|
||
└──────────┬─────────┘
|
||
▼
|
||
rtabmap_ros
|
||
10Hz | 3D cloud | 1000 features
|
||
→ /rtabmap/map (OccupancyGrid)
|
||
→ /rtabmap/cloud_map (PointCloud2)
|
||
→ /rtabmap/odom (Odometry)
|
||
│
|
||
▼
|
||
Nav2 stack (Phase 2b)
|
||
20Hz costmap
|
||
/cmd_vel → STM32
|
||
|
||
4× IMX219 CSI (Phase 2c — pending hardware)
|
||
front/right/rear/left 160°
|
||
→ panoramic stitch, person tracking
|
||
```
|
||
|
||
---
|
||
|
||
## 4. Phases
|
||
|
||
| Phase | Status | Description |
|
||
|-------|--------|-------------|
|
||
| 2a | ✅ Done (PR #17) | Sensor drivers — `saltybot_bringup` package |
|
||
| 2a+ | ✅ Done (PR #36) | Orin update: Dockerfile JetPack 6, RTAB-Map launch + config |
|
||
| 2b | ✅ Done (PR #49) | Nav2 integration — path planning + obstacle avoidance |
|
||
| 2c | ✅ Done (this PR) | 4× IMX219 surround vision + Nav2 camera obstacle layer |
|
||
|
||
---
|
||
|
||
## 5. RTAB-Map Config (Orin)
|
||
|
||
Full config: `jetson/config/rtabmap_params.yaml`
|
||
|
||
```yaml
|
||
Rtabmap/DetectionRate: "10" # was 2 on Nano
|
||
Kp/MaxFeatures: "1000" # was 400
|
||
RGBD/LinearUpdate: "0.05" # 5cm (was 10cm)
|
||
RGBD/AngularUpdate: "0.05" # ~3° (was 5°)
|
||
Grid/3D: "true" # 3D cloud enabled (was false)
|
||
Rtabmap/TimeThr: "0" # no limit (was 700ms)
|
||
Mem/STMSize: "0" # unlimited (was 30)
|
||
```
|
||
|
||
---
|
||
|
||
## 6. 4× IMX219 Layout (Phase 2c)
|
||
|
||
```
|
||
FRONT (CSI0) 160°
|
||
LEFT (CSI3) × RIGHT (CSI1)
|
||
REAR (CSI2) 160°
|
||
```
|
||
|
||
90° between cameras, 160° FOV → ~70° overlap at each boundary, full 360° coverage.
|
||
|
||
ROS topics (planned): `/camera/{front,right,rear,left}/image_raw` @ 30Hz,
|
||
`/camera/panoramic/image_raw` @ 15Hz (stitched equirectangular).
|
||
|
||
---
|
||
|
||
## 7. Power Budget (Orin Nano Super)
|
||
|
||
| Scenario | Total |
|
||
|----------|-------|
|
||
| SLAM active (RTAB-Map + D435i + RPLIDAR) | ~16W |
|
||
| + 4× IMX219 | ~17W |
|
||
| + Nav2 + TensorRT person detection | ~22W |
|
||
|
||
Orin Nano Super TDP: **25W max**. Recommended PSU: 5V 5A (25W) from robot buck converter.
|
||
No power gating needed. Run `sudo nvpmodel -m 0 && sudo jetson_clocks` for full performance.
|
||
|
||
---
|
||
|
||
## 8. Milestones
|
||
|
||
- [ ] Flash JetPack 6 on Orin (arriving March 1)
|
||
- [ ] `sudo apt install ros-humble-desktop ros-humble-rtabmap-ros ...`
|
||
- [ ] Verify D435i: `lsusb | grep "8086:0b3a"`
|
||
- [ ] Verify RPLIDAR: `ls /dev/rplidar`
|
||
- [ ] `colcon build --packages-select saltybot_bringup`
|
||
- [ ] `ros2 launch saltybot_bringup sensors.launch.py` — verify topics
|
||
- [ ] `ros2 launch saltybot_bringup slam_rtabmap.launch.py` — verify `/rtabmap/map`
|
||
- [ ] `ros2 topic hz /rtabmap/cloud_map` — verify 3D cloud
|
||
- [ ] Record rosbag, monitor `tegrastats` for thermal headroom
|
||
- [ ] Update static TF with real mount measurements
|
||
- [ ] Open bead: Phase 2b Nav2
|
||
- [ ] Open bead: Phase 2c IMX219 (after hardware arrives)
|
||
|
||
---
|
||
|
||
## 9. References
|
||
|
||
- [Jetson Orin Nano Super](https://www.nvidia.com/en-us/autonomous-machines/embedded-systems/jetson-orin/)
|
||
- [JetPack 6 / L4T R36](https://developer.nvidia.com/embedded/jetpack)
|
||
- [dusty-nv/jetson-containers JetPack 6](https://github.com/dusty-nv/jetson-containers)
|
||
- [rtabmap_ros ROS2](https://github.com/introlab/rtabmap_ros)
|
||
- [realsense-ros](https://github.com/IntelRealSense/realsense-ros)
|
||
- [IMX219 / nvarguscamerasrc on Jetson](https://developer.ridgerun.com/wiki/index.php/NVIDIA_Jetson_ISP_Control)
|