from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from pathlib import Path


def generate_launch_description():
    pkg_share = FindPackageShare("saltybot_sensor_fusion")
    config_dir = Path(str(pkg_share)) / "config"
    config_file = str(config_dir / "sensor_fusion_params.yaml")

    lidar_topic_arg = DeclareLaunchArgument(
        "lidar_topic",
        default_value="/scan",
        description="RPLIDAR topic"
    )

    depth_topic_arg = DeclareLaunchArgument(
        "depth_topic",
        default_value="/depth_scan",
        description="RealSense depth_to_laserscan topic"
    )

    sensor_fusion_node = Node(
        package="saltybot_sensor_fusion",
        executable="sensor_fusion",
        name="sensor_fusion",
        parameters=[
            config_file,
            {"lidar_topic": LaunchConfiguration("lidar_topic")},
            {"depth_topic": LaunchConfiguration("depth_topic")},
        ],
        output="screen",
    )

    return LaunchDescription([
        lidar_topic_arg,
        depth_topic_arg,
        sensor_fusion_node,
    ])
