import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
import xacro


def generate_launch_description():
    
    pkg_path=get_package_share_directory('robotic_arm')
    xacro_file=os.path.join(pkg_path,'urdf','robotic_arm.urdf.xacro')
    # 解析xacro文件,展开所有宏,常量,变量,include等,得到最终的URDF字符串
    robot_description_config=xacro.process_file(xacro_file)
    # 将URDF字符串封装成ROS参数字典,键为'robot_description',值为URDF字符串
    robot_description = {'robot_description': robot_description_config.toxml()}
    
    # 发布机器人状态的节点
    rsp=Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        output='screen',
        parameters=[robot_description]
    )
    
    # 关节状态发布器
    jsp=Node(
        package='joint_state_publisher_gui',
        executable='joint_state_publisher_gui',
        output='screen'
    )
    
    # 启动rviz2,并加载预设的rviz配置文件
    rvz=Node(
        package='rviz2',
        executable='rviz2',
        output='screen',
        arguments=['-d', os.path.join(pkg_path,'rviz','display.rviz')]
    )
    
    return LaunchDescription([
        rsp,
        jsp,
        rvz
    ])