import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, ExecuteProcess
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
import xacro

def generate_launch_description():
    pkg_name = 'calibration_sim'
    
    # 1. 解析 URDF
    infra_xacro = os.path.join(get_package_share_directory(pkg_name), 'urdf', 'workshop_sensors.xacro')
    infra_doc = xacro.process_file(infra_xacro)
    infra_xml = infra_doc.toxml()

    agv_xacro = os.path.join(get_package_share_directory(pkg_name), 'urdf', 'agv.xacro')
    agv_doc = xacro.process_file(agv_xacro)
    agv_xml = agv_doc.toxml()

    world_path = os.path.join(
        get_package_share_directory('calibration_sim'), 
        'worlds', 
        'calibration_room.world'
    )

    # 2. 启动 Gazebo
    gazebo = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(
            get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py')]),
        launch_arguments={'world': world_path}.items(), # 【关键】指定 world 参数
    )

    # 3. 生成基础设施 (4个雷达)
    spawn_infra = Node(
        package='gazebo_ros', executable='spawn_entity.py',
        arguments=['-topic', 'robot_description_infra', '-entity', 'workshop_sensors'],
        output='screen'
    )
    
    # 发布基础设施的 State Publisher (为了TF树)
    infra_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        name='infra_state_publisher',
        parameters=[{'robot_description': infra_xml}],
        remappings=[('robot_description', 'robot_description_infra')]
    )

    # 4. 生成 AGV
    spawn_agv = Node(
        package='gazebo_ros', executable='spawn_entity.py',
        arguments=['-topic', 'robot_description_agv', '-entity', 'my_agv', '-z', '0.1'],
        output='screen'
    )

    # 发布 AGV 的 State Publisher
    agv_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        name='agv_state_publisher',
        parameters=[{'robot_description': agv_xml}],
        remappings=[('robot_description', 'robot_description_agv')]
    )

    return LaunchDescription([
        gazebo,
        infra_state_publisher,
        spawn_infra,
        agv_state_publisher,
        spawn_agv,
        # 这里还可以加上 RViz 的节点
    ])
