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 目录路径
    share_dir = get_package_share_directory(pkg_name)
    
    # 1. 路径定义
    # 定位 xacro 模型文件 (用于 robot_state_publisher)
    xacro_file = os.path.join(share_dir, 'urdf', 'soft_arm.urdf.xacro')
    # 定位 yaml 配置文件 (用于模拟器参数)
    config_file = os.path.join(share_dir, 'config', 'soft_arm_params.yaml')

    # 2. Robot State Publisher 节点
    # 作用：发布静态 TF 树 (如 base_link)，并向 Rviz 提供 robot_description 话题。
    # Command(['xacro ', xacro_file]) 会在运行时动态解析 xacro 文件生成 XML 字符串。
    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}]
    )

    # 3. 自定义仿真节点 (Simulator)
    # 这是我们编写的核心 Python 节点。
    node_simulator = Node(
        package=pkg_name,
        executable='simulator',    # setup.py 中 entry_points 定义的可执行文件名
        name='soft_arm_simulator', # 节点名，必须与 yaml 文件中的根键一致，否则参数加载失败
        output='screen',
        parameters=[config_file]   # <--- 关键：在这里加载 .yaml 参数文件
    )
    
    # 4. Rviz2 节点
    # 启动可视化界面
    node_rviz = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        # (可选) 可以在这里添加 arguments=['-d', rviz_config_path] 来加载保存的 rviz 配置
    )

    # 返回 Launch 描述符，ROS 2 会并行启动列表中的所有节点
    return LaunchDescription([
        node_robot_state_publisher,
        node_simulator,
        node_rviz
    ])