import os
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    # 获取包的共享目录
    driver_pkg_dir = get_package_share_directory('driver_pkg')
    perception_pkg_dir = get_package_share_directory('perception_pkg')
    decision_pkg_dir = get_package_share_directory('decision_pkg')
    control_pkg_dir = get_package_share_directory('control_pkg')
    interaction_pkg_dir = get_package_share_directory('interaction_pkg')
    
    # 配置文件路径
    nav2_config = os.path.join(control_pkg_dir, 'config', 'nav2_config.yaml')
    
    return LaunchDescription([
        # ========== 驱动层节点 ==========
        # 电机驱动
        Node(
            package='driver_pkg',
            executable='motor_driver_node',
            name='motor_driver',
            output='screen'
        ),
        
        # 激光雷达驱动
        Node(
            package='driver_pkg',
            executable='lidar_driver_node',
            name='lidar_driver',
            output='screen'
        ),
        
        # 相机驱动
        Node(
            package='driver_pkg',
            executable='camera_driver_node',
            name='camera_driver',
            output='screen'
        ),
        
        # ========== 感知层节点 ==========
        # 点云处理
        Node(
            package='perception_pkg',
            executable='point_cloud_processor',
            name='point_cloud_processor',
            output='screen'
        ),
        
        # 目标检测
        Node(
            package='perception_pkg',
            executable='object_detection_node.py',
            name='object_detection',
            output='screen'
        ),
        
        # SLAM (使用Cartographer)
        Node(
            package='cartographer_ros',
            executable='cartographer_node',
            name='cartographer_node',
            output='screen',
            arguments=[
                '-configuration_directory', os.path.join(perception_pkg_dir, 'config'),
                '-configuration_basename', 'cartographer_config.lua'
            ]
        ),
        
        Node(
            package='cartographer_ros',
            executable='occupancy_grid_node',
            name='occupancy_grid_node',
            output='screen',
            arguments=['-resolution', '0.05']
        ),
        
        # ========== 决策层节点 ==========
        # 行为树
        Node(
            package='decision_pkg',
            executable='behavior_tree_node.py',
            name='behavior_tree',
            output='screen'
        ),
        
        # 任务规划
        Node(
            package='decision_pkg',
            executable='task_planner_node.py',
            name='task_planner',
            output='screen'
        ),
        
        # 路径规划 (使用Nav2)
        Node(
            package='nav2_bringup',
            executable='nav2_lifecycle_manager',
            name='lifecycle_manager_navigation',
            output='screen',
            parameters=[{
                'autostart': True,
                'node_names': [
                    'controller_server',
                    'planner_server',
                    'recoveries_server',
                    'bt_navigator',
                    'waypoint_follower'
                ]
            }]
        ),
        
        Node(
            package='nav2_planner',
            executable='planner_server',
            name='planner_server',
            output='screen',
            parameters=[nav2_config]
        ),
        
        Node(
            package='nav2_controller',
            executable='controller_server',
            name='controller_server',
            output='screen',
            parameters=[nav2_config]
        ),
        
        # ========== 控制层节点 ==========
        # 底盘控制器
        Node(
            package='control_pkg',
            executable='base_controller',
            name='base_controller',
            output='screen'
        ),
        
        # ========== 交互层节点 ==========
        # 状态监控
        Node(
            package='interaction_pkg',
            executable='robot_monitor.py',
            name='robot_monitor',
            output='screen'
        ),
        
        # Web桥接器
        Node(
            package='rosbridge_server',
            executable='rosbridge_websocket',
            name='rosbridge_websocket',
            output='screen'
        )
    ])