import os
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable, UnsetEnvironmentVariable
from launch_ros.substitutions import FindPackageShare
from launch.substitutions import Command
from launch.launch_description_sources import PythonLaunchDescriptionSource


def generate_launch_description():
    robot_name = "iisy"
    package_name = "gaz_simulation"
    pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros')
    path = FindPackageShare(package=package_name).find(package_name)
    world_path = os.path.join(path, "world/test2.xml")
    urdf_path = os.path.join(path, "urdf/iisy.urdf")
    return LaunchDescription([
        SetEnvironmentVariable(name='LIBGL_ALWAYS_SOFTWARE', value='true'),

        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')),
            launch_arguments={'world': world_path}.items()
        ),

        UnsetEnvironmentVariable(name='LIBGL_ALWAYS_SOFTWARE'),

        # Start Gazebo client
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py'))
        ),
        Node(
            package='tf2_ros',
            namespace='gaz_simulation',
            executable='static_transform_publisher',
            name='lidar_1_broadcaster',
            arguments=["1", "-1.9", "1", "1.5707963267949", "0", "0", "map", "lidar_1_link"]
        ),
        Node(
            package='tf2_ros',
            namespace='gaz_simulation',
            executable='static_transform_publisher',
            name='lidar_2_broadcaster',
            arguments=["-1.9", "1", "1", "0", "0", "0", "map", "lidar_2_link"]
        ),
        #Node(
        #    package='gazebo_ros',
        #    executable='spawn_entity.py',
        #    arguments=['-entity', robot_name,
        #               '-topic', 'robot_description',
        #               '-x', '-1',
        #               '-y', '-1',
        #               '-z', '1',
        #               '-Y', '0'],  # Yaw
        #    output='screen'
        #),
        # Subscribe to the joint states of the robot, and publish the 3D pose of each link.
        #Node(
        #    package='robot_state_publisher',
        #    executable='robot_state_publisher',
        #    parameters=[{'robot_description': Command(['xacro ', urdf_path])}]
        #),

        # Publish the joint states of the robot
        #Node(
        #    package='joint_state_publisher',
        #    executable='joint_state_publisher',
        #    name='joint_state_publisher'
        #)

        # ,
        # Node(
        #    namespace='turtlesim1',
        #    executable='gazebo',
        #    name='gazebo',
        #    arguments=["test.xml"]
        # )
    ])