import os
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import ExecuteProcess, IncludeLaunchDescription
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([
        #IncludeLaunchDescription(
        #    PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')),
        #    launch_arguments={'world': world_path}.items()
        #),
        
        ExecuteProcess(
            cmd=[[
                'LIBGL_ALWAYS_SOFTWARE=true ros2 launch gazebo_ros gzserver.launch.py "world:='+world_path+'"'
            ]],
            shell=True
        ),


        # 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"]
        # )
    ])


<?xml version="1.0"?>
<launch>
  <arg name="paused" default="false"/>
  <arg name="gazebo_gui" default="true"/>
  <arg name="initial_joint_positions" doc="Initial joint configuration of the robot"
       default=" -J base_link1_joint 0 -J base_rot 0 -J link1_link2_joint 0 -J link2_link3_joint 0 -J link2_rot 0 -J link3_rot 0"/>

  <!-- startup simulated world -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <let name="world_name" default="worlds/empty.world"/>
    <let name="paused" value="true"/>
    <let name="gui" value="$(arg gazebo_gui)"/>
  </include>

  <!-- send robot urdf to param server -->
  <param name="robot_description" textfile="$(find gaz_simulation)/urdf/iisy.urdf" />

  <!-- unpause only after loading robot model -->
  <let name="unpause" value="$(eval '' if arg('paused') else '-unpause')" />
  <!-- push robot_description to factory and spawn robot in gazebo at the origin, change x,y,z arguments to spawn in a different position -->
  <arg name="world_pose" value="-x 0 -y 0 -z 0" />
  <node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot $(arg unpause) $(arg world_pose) $(arg initial_joint_positions)"
    respawn="false" output="screen" />

  <!-- Load joint controller parameters for Gazebo -->
  <param from="$(find iisy_config)/config/gazebo_controllers.yaml" />
  <!-- Spawn Gazebo ROS controllers -->
  <node name="gazebo_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller" />
  <!-- Load ROS controllers -->
  <include file="$(dirname)/ros_controllers.launch"/>

</launch>