# 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