# 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

    # ── Persistence + Multi-session (Issue #123) ───────────────────────────
    # Database path — persistent storage on NVMe.
    # Do NOT use --delete_db_on_start; fresh_start:=true at launch clears it.
    database_path:            "/mnt/nvme/saltybot/maps/current.db"

    # On startup, load ALL previously mapped nodes into working memory.
    # This enables relocalization: RTAB-Map matches incoming frames against
    # the full prior map before adding new keyframes.
    Mem/InitWMWithAllNodes:   "true"

    # Append new data to the existing map across sessions (multi-session).
    # RTAB-Map will attempt loop closure against prior-session keyframes.
    Rtabmap/StartNewMapOnLoopClosure: "false"

    # ── Memory management ─────────────────────────────────────────────────
    # Prune near-duplicate keyframes (rehearsal): merge if similarity > 0.6.
    # Reduces DB size without losing map coverage.
    Mem/RehearsalSimilarity:  "0.6"

    # Maximum nodes retrieved from long-term memory per iteration.
    # Limits CPU/RAM during large-map loop closure searches.
    Rtabmap/MaxRetrieved:     "50"

    # Transfer nodes from short-term to long-term memory when STM > this limit.
    # 0 = unlimited STM; set a limit to bound RAM use on very long sessions.
    # At ~1 keyframe/5cm, 5000 nodes ≈ 250m of travel before LTM transfer begins.
    Mem/STMSize:              "0"        # keep unlimited for Orin 8GB RAM

    # ── Keyframe pruning ──────────────────────────────────────────────────
    # Remove keyframes that have been rehearsed and are no longer needed.
    Mem/RehearsedNodesKept:   "false"   # discard rehearsed nodes to save space