from multiprocessing import context
from webbrowser import get
from launch.actions.declare_launch_argument import DeclareLaunchArgument
from launch.launch_description import LaunchDescription
#from launch.substitutions.launch_configuration import LaunchConfiguration
from launch_ros.actions import Node
import xacro
import os
from ament_index_python import get_package_share_directory

def generate_launch_description():

    launch_args = []
    launch_args.append(
        DeclareLaunchArgument(
            name="grip",
            default_value="true",
            description="On or OFF gripper for launch"
        )
    )
    launch_args.append(
        DeclareLaunchArgument(
            name="sim",
            default_value="true",
            description="On or OFF simulation"
        )
    )
    launch_args.append(
        DeclareLaunchArgument(
            name="robot_name",
            default_value="rasmt",
            description="Robot name"
        )
    )

    # For convert LaunchConfiguration to string
    # grip = LaunchConfiguration("grip").perform(context)

    # get xacro file path
    xacro_file = os.path.join(get_package_share_directory("rasmt_support"),"urdf/","rasmt.xacro")
    # get error if xacro file if missing
    assert os.path.exists(xacro_file), "The xacro file of neptun_description doesnt exist"+str(xacro_file)
    # parse xacro file from file with arguments
    robot_description = xacro.process_file(xacro_file, mappings={'grip':"true",'sim':"false",'robot_name':"rasmt"})
    # convert file to xml format
    robot_description_content = robot_description.toxml()

    rviz_config_file = os.path.join(get_package_share_directory("rasmt_support"),"config/","rasmt.rviz")
    rviz = Node(
        package="rviz2",
        executable="rviz2",
        parameters=[{'robot_description': robot_description_content}],
        arguments=[
            '-d', rviz_config_file
        ]
    )
    robot_state_publisher = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        output="screen",
        parameters=[{'robot_description': robot_description_content}]
    )
    joint_state_publisher =Node(
        package="joint_state_publisher_gui",
        executable="joint_state_publisher_gui",
        output="log",
        arguments=["--ros-args"],
    )

    launch_nodes = []
    launch_nodes.append(rviz)
    launch_nodes.append(robot_state_publisher)
    launch_nodes.append(joint_state_publisher)
    #launch_nodes.add_action(launch_args)
    

    return LaunchDescription(launch_nodes)