saltylab-firmware/jetson/config/rtabmap_params.yaml
sl-perception 6b798cc890 feat: update SLAM stack for Jetson Orin Nano Super (67 TOPS, JetPack 6)
Platform upgrade: Jetson Nano 4GB → Orin Nano Super 8GB (March 1, 2026)
All Nano-era constraints removed — power/rate/resolution limits obsolete.

Dockerfile: l4t-jetpack:r36.2.0 (JetPack 6 / Ubuntu 22.04 / CUDA 12.x),
  ROS2 Humble via native apt, added ros-humble-rtabmap-ros,
  ros-humble-v4l2-camera for future IMX219 CSI (Phase 2c)

New: slam_rtabmap.launch.py — Orin primary SLAM entry point
  RTAB-Map with subscribe_scan (RPLIDAR) + subscribe_rgbd (D435i)
  Replaces slam_toolbox as docker-compose default

New: config/rtabmap_params.yaml — Orin-optimized
  DetectionRate 10Hz, MaxFeatures 1000, Grid/3D true,
  TimeThr 0 (no limit), Mem/STMSize 0 (unlimited)

Updated: config/realsense_d435i.yaml — 848x480x30, pointcloud enabled
Updated: config/slam_toolbox_params.yaml — 10Hz rate, 1s map interval
Updated: SLAM-SETUP-PLAN.md — full rewrite for Orin: arch diagram,
  Phase 2c IMX219 plan (4x 160° CSI surround), 25W power budget

docker-compose.yml: image tag jetson-orin, default → slam_rtabmap.launch.py

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-02-28 21:13:55 -05:00

79 lines
4.5 KiB
YAML

# RTAB-Map configuration — Jetson Orin Nano Super
#
# Hardware context: 67 TOPS, 1024-core Ampere GPU, 8GB RAM
# All Nano-era constraints (2Hz cap, 400 features, no 3D) removed.
#
# Sensor inputs:
# /scan LaserScan from RPLIDAR A1M8 ~5.5 Hz
# /camera/color/image_raw RGB from RealSense D435i 30 Hz
# /camera/depth/image_rect_raw Depth from D435i 30 Hz
# /camera/color/camera_info Camera intrinsics 30 Hz
#
# Outputs:
# /rtabmap/map OccupancyGrid (2D occupancy)
# /rtabmap/cloud_map PointCloud2 (3D point cloud, enabled)
# /rtabmap/odom Odometry (visual-inertial odometry)
# /rtabmap/mapData Map data for serialization/loop closure
#
# Frame assumptions (match sensors.launch.py static TF):
# map → odom → base_link → laser
# → camera_link
rtabmap:
ros__parameters:
# ── Core rate and timing ───────────────────────────────────────────────────
# Process at 10Hz — far below D435i 30fps ceiling, still 5x faster than Nano
Rtabmap/DetectionRate: "10"
# No processing time limit — Orin has headroom (Nano was 700ms)
Rtabmap/TimeThr: "0"
# Update map every 5cm or ~3° rotation (was 10cm / 5°)
RGBD/LinearUpdate: "0.05"
RGBD/AngularUpdate: "0.05"
# ── Visual features ────────────────────────────────────────────────────────
# 1000 ORB features per frame (Nano was 400)
Kp/MaxFeatures: "1000"
Vis/MaxFeatures: "1000"
# ── Memory ────────────────────────────────────────────────────────────────
# Unlimited short-term memory — 8GB RAM (Nano was 30 keyframes)
Mem/STMSize: "0"
# Keep full map in working memory
Mem/IncrementalMemory: "true"
# ── Map generation ────────────────────────────────────────────────────────
# 3D occupancy grid enabled — Ampere GPU handles point cloud generation
Grid/3D: "true"
Grid/CellSize: "0.05" # 5cm voxels
Grid/RangeMin: "0.3" # ignore points closer than 30cm
Grid/RangeMax: "8.0" # clip at A1M8 reliable range
# ── Sensor subscriptions ──────────────────────────────────────────────────
# Subscribe to both RPLIDAR scan (fast 2D) and D435i depth (3D)
subscribe_scan: "true"
subscribe_rgbd: "true"
subscribe_odom_info: "false"
# ── Loop closure ──────────────────────────────────────────────────────────
# More aggressive loop closure — Orin can handle it
RGBD/LoopClosureReextractFeatures: "true"
Kp/IncrementalFlann: "true"
# ── Odometry ──────────────────────────────────────────────────────────────
# Use F2M (frame-to-map) visual odometry — more accurate on Orin
Odom/Strategy: "0" # 0=F2M, 1=F2F
Odom/ResetCountdown: "1"
OdomF2M/MaxSize: "2000"
# ── RGBD node input topics ─────────────────────────────────────────────────
rgb_topic: /camera/color/image_raw
depth_topic: /camera/depth/image_rect_raw
camera_info_topic: /camera/color/camera_info
depth_camera_info_topic: /camera/depth/camera_info
scan_topic: /scan
# ── Output ────────────────────────────────────────────────────────────────
# Publish 3D point cloud map
cloud_output_voxelized: "true"
cloud_voxel_size: "0.05" # match Grid/CellSize