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 Gazebo
    gazebo = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            PathJoinSubstitution([
                FindPackageShare("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=[
            "-file", xacro_file,
            "-entity", "rasmt"
        ],
        output="screen"
    )
    cube_spawn = Node(
        package="gazebo_ros",
        executable="spawn_entity.py",
        arguments=[
            "-file", sdf_file,
            "-entity", "cube_station"
        ]
    )

    return LaunchDescription(
    [
        gazebo,
        spawn_entity,
        #cube_spawn
    ])