# 导入 ROS 2 launch 相关模块
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.substitutions import Command, FindExecutable, PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare
from launch.actions import OpaqueFunction
import os

def load_urdf(context, *args, **kwargs):
    nodeList = []

    # 获取包的共享目录路径（解析为实际字符串）
    package_share = FindPackageShare(package='rm_arm_control').perform(context)
    
    # 构建URDF文件的实际路径
    urdf_path = PathJoinSubstitution(
        [package_share, 'urdf', 'FHrobot.urdf']  # 替换为你的URDF路径
    ).perform(context)
    
    # 读取URDF文件内容
    with open(urdf_path, 'r') as f:
        urdf_content = f.read()

    robot_controller = Node(
        package='rm_arm_control',
        executable='rm_arm_control',
        name='rm_arm_control',
        output='screen',
        parameters=[{
            'robot_description': urdf_content,
            'use_sim_time': False
        }]
    )
    nodeList.append(robot_controller)
    
    # 创建机器人状态发布器节点
    robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description': urdf_content}]
    )
    nodeList.append(robot_state_publisher)

    '''joint_state_gui = Node(
        package='joint_state_publisher_gui',
        executable='joint_state_publisher_gui',
    )
    nodeList.append(joint_state_gui)'''
    
    # 创建RViz节点（可选）
    rviz_node = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        arguments=[
            '-d', os.path.join(
                FindPackageShare('rm_arm_control').find('rm_arm_control'),
                'launch', 'robot_view.rviz'  # 保存的RViz配置文件路径
            )
        ],
        parameters=[{
            # 这里可以设置一些RViz参数
        }]
    )
    nodeList.append(rviz_node)
    
    return nodeList

def generate_launch_description():
    return LaunchDescription([
        OpaqueFunction(function=load_urdf)
    ])
