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


def load_urdf_file(file_path: str) -> str:
    if not os.path.exists(file_path):
        raise FileNotFoundError(f"URDF file does not exist: {file_path}")
    if not os.path.isfile(file_path):
        raise IsADirectoryError(f"URDF path is not a file: {file_path}")
    with open(file_path, "r") as f:
        content = f.read()
    if not content.strip():
        raise ValueError(f"URDF file is empty: {file_path}")
    if "<robot" not in content:
        raise ValueError(f"URDF file missing <robot> tag: {file_path}")
    return content


def generate_launch_description():
    config_path = os.path.join(
        get_package_share_directory("dual_arm_action_server"),
        "config",
        "dual_arm_action_server.yaml",
    )

    urdf_path = os.path.join(
        get_package_share_directory("dual_arm_moveit_action_server"),
        "urdf",
        "FHrobot.urdf",
    )
    urdf_content = load_urdf_file(urdf_path)

    return LaunchDescription([
        Node(
            package="dual_arm_action_server",
            executable="dual_arm_action_server_node",
            name="dual_arm_action_server",
            output="screen",
            parameters=[config_path],
        ),
        Node(
            package="robot_state_publisher",
            executable="robot_state_publisher",
            parameters=[{"robot_description": urdf_content}],
        ),
    ])

