"""robot_tf2_launch.py - 启动 URDF + JointStatePublisher + robot_state_publisher + Tf2Listener。

设计思想:
    - 用 xacro 解析 URDF(ROS2 工业标准,简单 URDF 可直接读)
    - robot_state_publisher 订阅 /joint_states + URDF → 动态 TF → /tf
    - Tf2Listener 查 gripper 在 base_link 下位姿
    - 跨包: urdf 来自 cpp_robot_tf2, robot_state_publisher 来自 ros-humble-robot-state-publisher
"""
from launch import LaunchDescription
from launch.substitutions import Command, FindExecutable, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare


def generate_launch_description() -> LaunchDescription:
    """生成启动描述:URDF + JointState + robot_state_publisher + TF listener。

    Returns:
        LaunchDescription: 启动描述实例。
    """
    # URDF 路径(用包内 urdf/ 目录,跨平台安全)
    urdf_path = PathJoinSubstitution([
        FindPackageShare('cpp_robot_tf2'),
        'urdf',
        'simple_arm.urdf',
    ])

    # 用 cat 读 URDF 内容(给 robot_state_publisher 用)
    robot_description = Command([
        FindExecutable(name='cat'),
        ' ',
        urdf_path,
    ])

    return LaunchDescription([
        # 1) JointStatePublisher(C++ 发布关节角度)
        Node(
            package='cpp_robot_tf2',
            executable='joint_state_publisher_cpp',
            name='joint_state_publisher',
            output='screen',
        ),

        # 2) robot_state_publisher(ROS2 系统包) — URDF + /joint_states → /tf
        Node(
            package='robot_state_publisher',
            executable='robot_state_publisher',
            name='robot_state_publisher',
            output='screen',
            parameters=[{'robot_description': robot_description}],
        ),

        # 3) TF2 listener(C++ 查 gripper 位姿)
        Node(
            package='cpp_robot_tf2',
            executable='tf2_listener_cpp',
            name='tf2_listener',
            output='screen',
        ),
    ])