"""
uwb_imu_fusion.launch.py — Launch UWB-IMU EKF fusion node (Issue #573)

Usage:
  ros2 launch saltybot_uwb_imu_fusion uwb_imu_fusion.launch.py
  ros2 launch saltybot_uwb_imu_fusion uwb_imu_fusion.launch.py publish_tf:=false
  ros2 launch saltybot_uwb_imu_fusion uwb_imu_fusion.launch.py sigma_uwb_pos_m:=0.20
"""

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def generate_launch_description():
    pkg = get_package_share_directory("saltybot_uwb_imu_fusion")
    cfg = os.path.join(pkg, "config", "fusion_params.yaml")

    return LaunchDescription([
        DeclareLaunchArgument("publish_tf",          default_value="true"),
        DeclareLaunchArgument("use_uwb_pose",        default_value="true"),
        DeclareLaunchArgument("sigma_uwb_pos_m",     default_value="0.12"),
        DeclareLaunchArgument("max_dead_reckoning_s", default_value="10.0"),

        Node(
            package="saltybot_uwb_imu_fusion",
            executable="uwb_imu_fusion",
            name="uwb_imu_fusion",
            output="screen",
            parameters=[
                cfg,
                {
                    "publish_tf":           LaunchConfiguration("publish_tf"),
                    "use_uwb_pose":         LaunchConfiguration("use_uwb_pose"),
                    "sigma_uwb_pos_m":      LaunchConfiguration("sigma_uwb_pos_m"),
                    "max_dead_reckoning_s": LaunchConfiguration("max_dead_reckoning_s"),
                },
            ],
        ),
    ])
