<launch>
    <param name="robot_description" command="$(find xacro)/xacro $(find franka_description)/robots/panda_arm_hand.urdf.xacro" />

    <!-- GAZEBO arguments -->
    <arg name="paused" default="false" />
    <arg name="use_sim_time" default="true" />
    <arg name="gui" default="true" />
    <arg name="headless" default="false" />
    <arg name="debug" default="false" />
    <arg name="load_gripper" default="true" />

    <!--launch GAZEBO with own world configuration -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <!-- <arg name="world_name" value="$(find robotics_assisted_tomography)/worlds/tomography.world"/> -->
        <arg name="debug" value="$(arg debug)" />
        <arg name="gui" value="$(arg gui)" />
        <arg name="paused" value="$(arg paused)" />
        <arg name="use_sim_time" value="$(arg use_sim_time)" />
        <arg name="headless" value="$(arg headless)" />
    </include>

    <node name="rqt_console" pkg="rqt_console" type="rqt_console" />
    <!-- <node name="rqt_logger_level" pkg="rqt_logger_level" type="rqt_logger_level" /> -->

    <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model panda" />

    <!-- Load joint controller configurations from YAML file to parameter server -->
    <rosparam file="$(find panda_simulation)/config/panda_control.yaml" command="load" />


    <!-- load the controllers -->
    <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller panda_arm_controller" />
    <node if="$(arg load_gripper)" name="controller_spawner_hand" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="panda_hand_controller" />


    <!-- convert joint states to TF transforms for rviz, etc -->
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />

    <include file="$(find panda_moveit_config)/launch/planning_context.launch">
        <arg name="load_robot_description" value="true" />
        <arg name="load_gripper" value="$(arg load_gripper)" />
    </include>
    <include file="$(find panda_moveit_config)/launch/move_group.launch">
        <arg name="load_gripper" value="$(arg load_gripper)" />
    </include>
    <group if="$(arg gui)">
        <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />
    </group>

    <node name="joint_state_desired_publisher" pkg="topic_tools" type="relay" args="joint_states joint_states_desired" />

    <!-- launch robot control node for moveit motion planning -->
    <node pkg="panda_simulation" type="robot_control_node" name="robot_control_node" output="screen" />

    <!-- load (not start!) custom joint position controller -->
    <node pkg="controller_manager" type="spawner" name="joint_position_launcher" args="--stopped joint_position_controller" />

    <!-- run custom node for automatic intialization -->
    <node pkg="panda_simulation" type="robot_state_initializer_node" name="robot_state_initializer_node" />

    <node pkg="sample_applications" type="SampleSimpleMotion" name="SampleSimpleMotion" />

</launch>