from launch.launch_description import LaunchDescription
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
from launch.substitutions import PathJoinSubstitution, LaunchConfiguration, FindExecutable, Command
from launch_ros.substitutions import FindPackageShare
from launch_ros.actions import Node
from launch.launch_description_sources import PythonLaunchDescriptionSource
import xacro
import os
from ament_index_python import get_package_share_directory


def generate_launch_description():

    launch_args = []
    launch_args.append(DeclareLaunchArgument(
        name="robot_name",
        default_value="rasmt",
        description="Set robot name in gazebo env"
    ))

    world_file = os.path.join(get_package_share_directory("rasmt_support"), "world", "robossembler.world")
    #test_world_file = "/home/bill-finger/gazebo_pkgs_ws/gazebo-pkgs/gazebo_grasp_plugin/test_world/test_world_full.world"

    # gzserver = IncludeLaunchDescription(
    #         PythonLaunchDescriptionSource(
    #             os.path.join(get_package_share_directory("gazebo_ros"), 'launch', 'gzserver.launch.py'),
    #         ), launch_arguments={'world':world_file}.items()
    # )
    # gzclient = IncludeLaunchDescription(
    #         PythonLaunchDescriptionSource(
    #             os.path.join(get_package_share_directory("gazebo_ros"), 'launch', 'gzclient.launch.py'),
    #         )
    # )

    gazebo = IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                os.path.join(get_package_share_directory("gazebo_ros"), 'launch', 'gazebo.launch.py'),
            )
    )
    
    """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 rasmt.xacro doesnt exist"+str(xacro_file)"""

    #sdf_file = os.path.join(get_package_share_directory("rasmt_support"),"urdf/","cube5x.sdf")


    spawn_entity = Node(
        package="gazebo_ros",
        executable="spawn_entity.py",
        arguments=[
            "-topic", "/robot_description",
            "-entity", LaunchConfiguration("robot_name")
        ],
        output="screen"
    )
    """cube_spawn = Node(
        package="gazebo_ros",
        executable="spawn_entity.py",
        arguments=[
            "-file", sdf_file,
            "-entity", "cube_station"
        ]
    )"""

    launch_nodes = []
    launch_nodes.append(gazebo)
    #launch_nodes.append(gzserver)
    #launch_nodes.append(gzclient)
    launch_nodes.append(spawn_entity)
    #launch_nodes.append(cube_spawn)

    return LaunchDescription(launch_args + launch_nodes)