from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare


def generate_launch_description() -> LaunchDescription:
    config_file = PathJoinSubstitution([
        FindPackageShare("xr_rm_teleop"),
        "config",
        "single_arm.yaml",
    ])

    return LaunchDescription([
        DeclareLaunchArgument("udp_host", default_value="0.0.0.0"),
        DeclareLaunchArgument("udp_port", default_value="15000"),
        Node(
            package="xr_rm_input",
            executable="udp_controller_receiver",
            name="udp_controller_receiver",
            output="screen",
            parameters=[{
                "udp_host": LaunchConfiguration("udp_host"),
                "udp_port": LaunchConfiguration("udp_port"),
                "topic": "/xr/right_controller",
            }],
        ),
        Node(
            package="xr_rm_teleop",
            executable="single_arm_velocity_teleop",
            name="single_arm_velocity_teleop",
            output="screen",
            parameters=[config_file, {"use_mock": True}],
        ),
    ])