import os
import xacro
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import ExecuteProcess, TimerAction
from launch_ros.actions import Node

def generate_launch_description():

    pkg_path = get_package_share_directory('robotic_arm')
    xacro_file = os.path.join(pkg_path, 'urdf', 'robotic_arm.urdf.xacro')
    robot_description_config = xacro.process_file(xacro_file)
    robot_description = {'robot_description': robot_description_config.toxml()}

    # 启动 Gazebo
    gazebo = ExecuteProcess(
        cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_factory.so'],
        output='screen'
    )

    # 发布机器人状态
    rsp = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        output='screen',
        parameters=[robot_description]
    )

    # 在 Gazebo 中生成机器人
    spawn = Node(
        package='gazebo_ros',
        executable='spawn_entity.py',
        output='screen',
        arguments=[
            '-topic', 'robot_description',
            '-entity', 'robotic_arm',
            '-x', '0',
            '-y', '0',
            '-z', '0.0',
        ],
    )

    # 加载关节状态控制器
    spawn_jsc = Node(
        package='controller_manager',
        executable='spawner',
        output='screen',
        arguments=['joint_state_broadcaster', '--controller-manager', '/controller_manager'],
    )

    # 加载机械臂位置控制器
    spawn_arm = Node(
        package='controller_manager',
        executable='spawner',
        output='screen',
        arguments=['arm_position_controller', '--controller-manager', '/controller_manager'],
    )

    return LaunchDescription([
        gazebo,
        rsp,
        TimerAction(period=3.0, actions=[spawn]),
        TimerAction(period=6.0, actions=[spawn_jsc, spawn_arm]),
    ])