# slam_toolbox — online async SLAM configuration
# Tuned for Jetson Nano 4GB (constrained CPU/RAM, indoor mapping)
#
# Input:  /scan  (LaserScan from RPLIDAR A1M8, ~5.5 Hz)
# Output: /map   (OccupancyGrid, updated every map_update_interval seconds)
#
# Frame assumptions (must match sensors.launch.py static TF):
#   map → odom → base_link → laser
#   (odom not yet published — slam_toolbox handles this via scan matching)

slam_toolbox:
  ros__parameters:
    # ── Frames ───────────────────────────────────────────────────────────────
    odom_frame:   odom
    map_frame:    map
    base_frame:   base_link
    scan_topic:   /scan
    mode:         mapping    # 'mapping' or 'localization'

    # ── Map params ───────────────────────────────────────────────────────────
    resolution:            0.05    # 5 cm/cell — good balance for A1M8 angular res
    max_laser_range:       8.0     # clip to reliable range of A1M8 (spec: 12m)
    map_update_interval:   5.0     # seconds between full map publishes (saves CPU)
    minimum_travel_distance:  0.3  # only update after moving 30 cm
    minimum_travel_heading:   0.3  # or rotating ~17°

    # ── Performance (Nano-specific) ───────────────────────────────────────────
    # Reduce scan processing rate to stay within ~3.5W CPU budget
    minimum_time_interval:  0.5   # max 2 Hz scan processing (A1M8 is ~5.5 Hz)
    transform_timeout:      0.2
    tf_buffer_duration:     30.0
    stack_size_to_use:      40000000   # 40 MB stack
    enable_interactive_mode: false     # disable interactive editing (saves CPU)

    # ── Scan matching ─────────────────────────────────────────────────────────
    use_scan_matching:    true
    use_scan_barycenter:  true
    scan_buffer_size:     10
    scan_buffer_maximum_scan_distance: 10.0

    # ── Loop closure ──────────────────────────────────────────────────────────
    do_loop_closing:                      true
    loop_match_minimum_chain_size:        10
    loop_match_maximum_variance_coarse:   3.0
    loop_match_minimum_response_coarse:   0.35
    loop_match_minimum_response_fine:     0.45
    loop_search_maximum_distance:         3.0

    # ── Correlation (coarse scan matching) ───────────────────────────────────
    correlation_search_space_dimension:      0.5
    correlation_search_space_resolution:     0.01
    correlation_search_space_smear_deviation: 0.1

    # ── Loop search space ─────────────────────────────────────────────────────
    loop_search_space_dimension:      8.0
    loop_search_space_resolution:     0.05
    loop_search_space_smear_deviation: 0.03

    # ── Response expansion ────────────────────────────────────────────────────
    link_match_minimum_response_fine:  0.1
    link_scan_maximum_distance:        1.5
    use_response_expansion:            true

    # ── Penalties (scan matcher quality thresholds) ───────────────────────────
    distance_variance_penalty: 0.5
    angle_variance_penalty:    1.0
    fine_search_angle_offset:  0.00349    # ~0.2°
    coarse_search_angle_offset: 0.349     # ~20°
    coarse_angle_resolution:   0.0349     # ~2°
    minimum_angle_penalty:     0.9
    minimum_distance_penalty:  0.5

    # ── Solver ────────────────────────────────────────────────────────────────
    solver_plugin:           solver_plugins::CeresSolver
    ceres_linear_solver:     SPARSE_NORMAL_CHOLESKY
    ceres_preconditioner:    SCHUR_JACOBI
    ceres_trust_strategy:    LEVENBERG_MARQUARDT
    ceres_dogleg_type:       TRADITIONAL_DOGLEG
    ceres_loss_function:     None