<?xml version="1.0"?>
<launch>

  <!-- GDB functionality -->
  <arg name="debug" default="false" />
  <arg unless="$(arg debug)" name="launch_prefix" value="" />
  <arg     if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />

  <arg name="use_tool_communication"/>
  <arg name="controller_config_file"/>
  <arg name="robot_ip"/>
  <arg name="kinematics_config"/>
  <arg name="tf_prefix" default="" />
  <arg name="controllers" default="joint_state_controller scaled_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller"/>
  <arg name="stopped_controllers" default="pos_traj_controller"/>
  <arg name="urscript_file" default="$(find ur_rtde_driver)/resources/servoj.urscript"/>
  <arg name="rtde_output_recipe_file" default="$(find ur_rtde_driver)/resources/rtde_output_recipe.txt"/>
  <arg name="rtde_input_recipe_file" default="$(find ur_rtde_driver)/resources/rtde_input_recipe.txt"/>
  <arg name="tool_voltage" default="0"/>
  <arg name="tool_parity" default="0"/>
  <arg name="tool_baud_rate" default="115200"/>
  <arg name="tool_stop_bits" default="1"/>
  <arg name="tool_rx_idle_chars" default="1.5"/>
  <arg name="tool_tx_idle_chars" default="3.5"/>
  <arg name="tool_device_name" default="/tmp/ttyUR"/>
  <arg name="tool_tcp_port" default="54321"/>

  <!-- Load hardware interface -->
  <node name="ur_hardware_interface" pkg="ur_rtde_driver" type="ur_rtde_driver_node" output="screen" launch-prefix="$(arg launch_prefix)" required="true">
    <param name="robot_ip" type="str" value="$(arg robot_ip)"/>
    <rosparam command="load" file="$(arg kinematics_config)" />
    <param name="script_file" value="$(arg urscript_file)"/>
    <param name="output_recipe_file" value="$(arg rtde_output_recipe_file)"/>
    <param name="input_recipe_file" value="$(arg rtde_input_recipe_file)"/>
    <param name="tf_prefix" value="$(arg tf_prefix)"/>
    <param name="use_tool_communication" value="$(arg use_tool_communication)"/>
    <!--<param name="reverse_port" type="int" value="$(arg reverse_port)" />-->
    <param name="tool_voltage" value="$(arg tool_voltage)"/>
    <param name="tool_parity" value="$(arg tool_parity)"/>
    <param name="tool_baud_rate" value="$(arg tool_baud_rate)"/>
    <param name="tool_stop_bits" value="$(arg tool_stop_bits)"/>
    <param name="tool_rx_idle_chars" value="$(arg tool_rx_idle_chars)"/>
    <param name="tool_tx_idle_chars" value="$(arg tool_tx_idle_chars)"/>
  </node>

  <!-- Starts socat to bridge the robot's tool communication interface to a local tty device -->
  <node if="$(arg use_tool_communication)" name="ur_tool_communication_bridge" pkg="ur_rtde_driver" type="tool_communication" respawn="false" output="screen">
    <param name="robot_ip" value="$(arg robot_ip)"/>
    <param name="device_name" value="$(arg tool_device_name)"/>
    <param name="tcp_port" value="$(arg tool_tcp_port)"/>
  </node>

  <!-- Load controller settings -->
  <rosparam file="$(arg controller_config_file)" command="load"/>

  <!-- spawn controller manager -->
  <node name="ros_control_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
    output="screen" args="$(arg controllers)" />

  <!-- load other controller -->
  <node name="ros_control_controller_manager" pkg="controller_manager" type="controller_manager" respawn="false"
    output="screen" args="load $(arg stopped_controllers)" />


  <node name="controller_stopper" pkg="controller_stopper" type="node" respawn="false" output="screen">
    <remap from="robot_running" to="ur_hardware_interface/robot_program_running"/>
    <rosparam param="consistent_controllers">
       - "joint_state_controller"
       - "speed_scaling_state_controller"
       - "force_torque_sensor_controller"
    </rosparam>
  </node>

</launch>
