from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare


def generate_launch_description() -> LaunchDescription:
    arm_debug_launch = PathJoinSubstitution([
        FindPackageShare("xr_rm_bringup"),
        "launch",
        "arm_debug.launch.py",
    ])

    return LaunchDescription([
        DeclareLaunchArgument("arm", default_value="right"),
        DeclareLaunchArgument("udp_host", default_value="0.0.0.0"),
        DeclareLaunchArgument("udp_port", default_value="15000"),
        DeclareLaunchArgument("left_robot_ip", default_value="192.168.192.18"),
        # 兼容旧命令：robot_ip 仍作为右臂 IP 覆盖入口。
        DeclareLaunchArgument("robot_ip", default_value="192.168.192.19"),
        DeclareLaunchArgument("robot_port", default_value="8080"),
        DeclareLaunchArgument("move_to_initial_pose_on_connect", default_value="false"),
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(arm_debug_launch),
            launch_arguments={
                "arm": LaunchConfiguration("arm"),
                "use_mock": "false",
                "udp_host": LaunchConfiguration("udp_host"),
                "udp_port": LaunchConfiguration("udp_port"),
                "left_robot_ip": LaunchConfiguration("left_robot_ip"),
                "right_robot_ip": LaunchConfiguration("robot_ip"),
                "robot_port": LaunchConfiguration("robot_port"),
                "move_to_initial_pose_on_connect": LaunchConfiguration("move_to_initial_pose_on_connect"),
            }.items(),
        ),
    ])