#!/usr/bin/env python3
"""
人脸跟随和自动避障启动文件

启动节点：
1. 人脸检测识别节点（带位置信息发布）
2. 人脸跟随和避障节点
"""

from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration


def generate_launch_description():
    """生成启动描述"""
    # 声明启动参数
    image_topic_arg = DeclareLaunchArgument(
        'image_topic',
        default_value='/camera/camera/color/image_raw',
        description='彩色相机话题'
    )
    
    depth_topic_arg = DeclareLaunchArgument(
        'depth_topic',
        default_value='/camera/camera/depth/image_rect_raw',
        description='深度相机话题'
    )
    
    camera_info_topic_arg = DeclareLaunchArgument(
        'camera_info_topic',
        default_value='/camera/camera/depth/camera_info',
        description='相机内参话题'
    )
    
    db_path_arg = DeclareLaunchArgument(
        'db_path',
        default_value='/home/ubuntu/eyes/src/eye_ros/face_database/face_database.pkl',
        description='人脸数据库路径'
    )
    
    model_name_arg = DeclareLaunchArgument(
        'model_name',
        default_value='buffalo_l',
        description='人脸识别模型名称'
    )
    
    body_vel_cmd_topic_arg = DeclareLaunchArgument(
        'body_vel_cmd_topic',
        default_value='/motion/body_vel_cmd',
        description='机器人速度命令话题'
    )
    
    # 创建人脸识别节点
    face_recognition_node = Node(
        package='eye_ros',
        executable='face_recognition_node.py',
        name='face_recognition_node',
        parameters=[
            {
                'image_topic': LaunchConfiguration('image_topic'),
                'db_path': LaunchConfiguration('db_path'),
                'model_name': LaunchConfiguration('model_name')
            }
        ],
        output='screen'
    )
    
    # 创建人脸跟随和避障节点
    face_tracking_obstacle_avoidance_node = Node(
        package='eye_ros',
        executable='face_tracking_obstacle_avoidance_node.py',
        name='face_tracking_obstacle_avoidance_node',
        parameters=[
            {
                'depth_topic': LaunchConfiguration('depth_topic'),
                'camera_info_topic': LaunchConfiguration('camera_info_topic'),
                'face_name_topic': 'face_recognition/name',
                'face_position_topic': 'face_recognition/position',
                'body_vel_cmd_topic': LaunchConfiguration('body_vel_cmd_topic')
            }
        ],
        output='screen'
    )
    
    # 创建启动描述
    ld = LaunchDescription()
    
    # 添加启动参数
    ld.add_action(image_topic_arg)
    ld.add_action(depth_topic_arg)
    ld.add_action(camera_info_topic_arg)
    ld.add_action(db_path_arg)
    ld.add_action(model_name_arg)
    ld.add_action(body_vel_cmd_topic_arg)
    
    # 添加节点
    ld.add_action(face_recognition_node)
    ld.add_action(face_tracking_obstacle_avoidance_node)
    
    return ld
