<?xml version="1.0"?>
<launch>
  <!-- Gazebo options -->
  <arg name="gazebo_gui" default="true" doc="Start Gazebo GUI"/>
  <arg name="paused" default="false" doc="Start Gazebo paused"/>
  <arg name="world_name" default="worlds/empty.world" doc="Gazebo world file"/>
  <arg name="world_pose" default="-x 0 -y 0 -z 0 -R 0 -P 0 -Y 0" doc="Pose to spawn the robot at"/>
  <arg name="initial_joint_positions" default=" -J joint1 0 -J joint2 0 -J joint3 0 -J joint4 0 -J joint5 0 -J joint6 0" doc="Initial joint configuration of the robot"/>

  <!-- Start Gazebo paused to allow the controllers to pickup the initial pose -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch" pass_all_args="true">
    <arg name="paused" value="true"/>
  </include>

  <!-- Set the robot urdf on the parameter server -->
  <param name="robot_description" textfile="$(find lstrobot_moveit_config_0605)/config/gazebo_mr12urdf20240605.urdf" />

  <!-- Unpause the simulation after loading the robot model -->
  <arg name="unpause" value="$(eval '' if arg('paused') else '-unpause')" />

  <!-- Spawn the robot in Gazebo -->
  <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 the controller parameters onto the parameter server -->
  <rosparam file="$(find lstrobot_moveit_config_0605)/config/gazebo_controllers.yaml" />
  <include file="$(dirname)/ros_controllers.launch"/>

  <!-- Spawn the Gazebo ROS controllers -->
  <node name="gazebo_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller" />

  <!-- Given the published joint states, publish tf for the robot links -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
</launch>
