"""
visual_odom.launch.py — Visual odometry + Kalman fusion stack.

Starts:
  visual_odom    — D435i IR stereo + CUDA optical flow VO → /saltybot/visual_odom
  odom_fusion    — EKF: visual + wheel + slip failover + RTAB loop closure
                 → /saltybot/odom_fused + odom→base_link TF

Launch args:
  wheel_odom_topic   str    '/saltybot/rover_odom'  (or /saltybot/tank_odom)
  max_features       int    '300'
  loop_closure_thr   float  '0.30'
  publish_debug      bool   'false'

Verify:
  ros2 topic hz /saltybot/visual_odom        # ~30 Hz
  ros2 topic hz /saltybot/odom_fused         # ~30 Hz
  ros2 topic echo /saltybot/visual_odom_status
  ros2 run tf2_tools view_frames             # verify odom→base_link TF

RTAB-Map integration:
  RTAB-Map's odom_frame / map_frame should match the odom_frame parameter.
  In slam_rtabmap.launch.py / rtabmap_params.yaml set:
    odom_frame_id: 'odom'
  RTAB-Map will auto-subscribe to /saltybot/odom_fused via the odom remapping
  or set subscribe_odom:=true with the odom topic pointing here.
"""

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def generate_launch_description():
    args = [
        DeclareLaunchArgument('wheel_odom_topic',   default_value='/saltybot/rover_odom'),
        DeclareLaunchArgument('max_features',        default_value='300'),
        DeclareLaunchArgument('loop_closure_thr',    default_value='0.30'),
        DeclareLaunchArgument('loop_closure_alpha',  default_value='0.40'),
        DeclareLaunchArgument('fusion_hz',           default_value='30.0'),
    ]

    visual_odom = Node(
        package='saltybot_visual_odom',
        executable='visual_odom',
        name='visual_odom',
        output='screen',
        parameters=[{
            'max_features':   LaunchConfiguration('max_features'),
            'min_features':   80,
            'frame_id':       'odom',
            'child_frame_id': 'camera_link',
        }],
    )

    odom_fusion = Node(
        package='saltybot_visual_odom',
        executable='odom_fusion',
        name='odom_fusion',
        output='screen',
        parameters=[{
            'fusion_hz':             LaunchConfiguration('fusion_hz'),
            'wheel_odom_topic':      LaunchConfiguration('wheel_odom_topic'),
            'loop_closure_thr_m':    LaunchConfiguration('loop_closure_thr'),
            'loop_closure_alpha':    LaunchConfiguration('loop_closure_alpha'),
            'slip_noise_multiplier': 10.0,
            'odom_frame':            'odom',
            'base_frame':            'base_link',
        }],
    )

    return LaunchDescription(args + [visual_odom, odom_fusion])
