#!/usr/bin/env python3
"""视觉抓取 bringup：机械臂控制 + 视觉抓取 + (可选) YOLO 方框检测。

组件依赖：arm_control → vision_grasp → box_detection_grasp
三个节点通过 ROS 服务通信，启动顺序由 lifecycle 自发处理。

用法：
  ros2 launch udp_teleop vision_grasp.launch.py
  ros2 launch udp_teleop vision_grasp.launch.py udp_ip:=192.168.4.1 detection:=true
  ros2 launch udp_teleop vision_grasp.launch.py detection:=true auto_grasp:=true
"""

import os

from ament_index_python.packages import get_package_share_directory
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():
    pkg_share = get_package_share_directory('udp_teleop')

    arm_params = os.path.join(pkg_share, 'config', 'arm_control.yaml')
    vision_params = os.path.join(pkg_share, 'config', 'vision_grasp.yaml')
    detection_params = os.path.join(pkg_share, 'config', 'box_detection_grasp.yaml')

    detection = LaunchConfiguration('detection')
    auto_grasp = LaunchConfiguration('auto_grasp')
    udp_ip = LaunchConfiguration('udp_ip')
    show_debug = LaunchConfiguration('show_debug')

    return LaunchDescription([
        DeclareLaunchArgument('detection', default_value='false',
                              description='是否启动 YOLO 方框检测节点'),
        DeclareLaunchArgument('auto_grasp', default_value='false',
                              description='检测到方框后自动抓取'),
        DeclareLaunchArgument('udp_ip', default_value='192.168.245.67',
                              description='ESP32 UDP IP 地址'),
        DeclareLaunchArgument('show_debug', default_value='false',
                              description='显示 YOLO 检测调试窗口'),

        # ===== arm_control =====
        Node(
            package='udp_teleop',
            executable='arm_control',
            name='arm_control',
            output='screen',
            parameters=[arm_params, {'udp_ip': udp_ip}],
        ),

        # ===== vision_grasp =====
        Node(
            package='udp_teleop',
            executable='vision_grasp',
            name='vision_grasp',
            output='screen',
            parameters=[vision_params],
        ),

        # ===== box_detection_grasp (可选) =====
        Node(
            package='udp_teleop',
            executable='box_detection_grasp',
            name='box_detection_grasp',
            output='screen',
            parameters=[detection_params, {
                'auto_grasp': auto_grasp,
                'show_debug_window': show_debug,
            }],
            condition=IfCondition(detection),
        ),
    ])
