# 导入 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 open_file(file_name, context):
    try:
        package_share = FindPackageShare(package='rm_arm_control').perform(context)
        # 1. 计算SRDF文件路径并解析
        file_path = PathJoinSubstitution(
            [package_share, 'urdf', file_name]  # 替换为实际的SRDF路径
        ).perform(context)  # context 是LaunchContext对象，通常在generate_launch_description中可用

        # 2. 检查路径是否存在
        if not os.path.exists(file_path):
            raise FileNotFoundError(f"SRDF文件不存在: {file_path}")

        # 3. 检查路径是否为文件（避免目录路径）
        if not os.path.isfile(file_path):
            raise IsADirectoryError(f"指定路径是目录，不是文件: {file_path}")

        # 4. 尝试打开并读取文件
        with open(file_path, 'r') as f:
            file_content = f.read()

        # 5. 检查文件内容是否为空
        if not file_content.strip():  # 忽略纯空白内容
            raise ValueError(f"文件内容为空: {file_path}")

        # 6. （可选）简单验证XML格式是否合法（检查根节点）
        if '<robot' not in file_content:
            raise SyntaxError(f"文件格式无效，未找到<robot>根节点: {file_path}")

        # 读取成功后的逻辑（如加载到参数服务器）
        print(f"成功读取SRDF文件: {file_path}")

        return file_content  # 返回读取的内容，或根据需要处理

    except FileNotFoundError as e:
        # 文件不存在错误
        print(f"错误: {str(e)}")
        print("请检查路径是否正确，确保SRDF文件存在于指定目录")
        # 可根据需要抛出异常终止launch，或使用return None等方式处理
        raise  # 抛出异常终止launch启动

    except IsADirectoryError as e:
        # 路径是目录而非文件
        print(f"错误: {str(e)}")
        print("请检查路径是否指向具体的SRDF文件，而非目录")
        raise

    except PermissionError:
        # 无权限读取文件
        print(f"错误: 没有权限读取SRDF文件: {file_path}")
        print("请检查文件权限，确保当前用户有读取权限")
        raise

    except ValueError as e:
        # 文件内容为空
        print(f"错误: {str(e)}")
        print("请检查SRDF文件是否被正确生成，内容不能为空")
        raise

    except SyntaxError as e:
        # XML格式初步验证失败
        print(f"错误: {str(e)}")
        print("请检查SRDF文件是否为有效的XML格式")
        raise

    except Exception as e:
        # 捕获其他未预料的错误
        print(f"读取SRDF文件时发生未知错误: {str(e)}")
        raise

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

    srdf_content = open_file('FHrobot.srdf', context)
    urdf_content = open_file('FHrobot.urdf', context)
    xml_content = open_file('FHrobot.xml', context)

    robot_controller = Node(
        package='rm_arm_control',
        executable='rm_arm_control',
        name='rm_arm_control',
        output='screen',
        parameters=[{
            'robot_description': urdf_content,
            'robot_description_semantic': srdf_content,
            'robot_description_xml': xml_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)
    ])
