import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
from launch.substitutions import LaunchConfiguration

def generate_launch_description():
    # 设置参数
    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
    world_file_name = 'empty.world'
    world_path = os.path.join(get_package_share_directory('robot_simulation'), 'worlds', world_file_name)
    
    # URDF文件路径
    urdf_file_name = 'robot.urdf.xacro'
    urdf_path = os.path.join(get_package_share_directory('robot_simulation'), 'urdf', urdf_file_name)
    
    # 启动Gazebo服务器
    gazebo_server = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            os.path.join(get_package_share_directory('gazebo_ros'), 'launch'),
            '/gzserver.launch.py'
        ]),
        launch_arguments={'world': world_path}.items()
    )
    
    # 启动Gazebo客户端
    gazebo_client = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            os.path.join(get_package_share_directory('gazebo_ros'), 'launch'),
            '/gzclient.launch.py'
        ])
    )
    
    # 启动机器人状态发布器
    robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        name='robot_state_publisher',
        output='screen',
        parameters=[{'use_sim_time': use_sim_time, 'robot_description': open(urdf_path, 'r').read()}]
    )
    
    # 启动关节状态发布器
    joint_state_publisher = Node(
        package='joint_state_publisher',
        executable='joint_state_publisher',
        name='joint_state_publisher',
        output='screen',
        parameters=[{'use_sim_time': use_sim_time}]
    )
    
    # 在Gazebo中生成机器人
    spawn_entity = Node(
        package='gazebo_ros',
        executable='spawn_entity.py',
        arguments=['-entity', 'my_robot', '-topic', 'robot_description'],
        output='screen'
    )
    
    return LaunchDescription([
        gazebo_server,
        gazebo_client,
        robot_state_publisher,
        joint_state_publisher,
        spawn_entity
    ])