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

def generate_launch_description():
    pkg_name = 'soft_arm_sim'
    share_dir = get_package_share_directory(pkg_name)
    
    # 获取 xacro 文件路径
    xacro_file = os.path.join(share_dir, 'urdf', 'soft_arm.urdf.xacro')

    # 1. Robot State Publisher
    # 使用 Command 进行转换，这样更稳定，且能在终端看到详细报错
    robot_description = Command(['xacro ', xacro_file])
    
    node_robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        output='screen',
        parameters=[{'robot_description': robot_description}]
    )

    # 2. 仿真节点
    node_simulator = Node(
        package=pkg_name,
        executable='simulator',
        output='screen'
    )
    
    # 3. Rviz2
    node_rviz = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
    )

    return LaunchDescription([
        node_robot_state_publisher,
        node_simulator,
        node_rviz
    ])