From 34eb8dcf0829fd527c5dcccb3b42317e879ed253 Mon Sep 17 00:00:00 2001
From: Felix Mauch <mauch@fzi.de>
Date: Fri, 14 Jun 2019 10:31:52 +0200
Subject: [PATCH] split common and control

---
 .../launch/ur10e_ros_control.launch           | 15 ++--
 ur_rtde_driver/launch/ur_common.launch        | 70 +++++--------------
 ur_rtde_driver/launch/ur_control.launch       | 70 +++++++++++++++++++
 3 files changed, 93 insertions(+), 62 deletions(-)
 create mode 100644 ur_rtde_driver/launch/ur_control.launch

diff --git a/ur_rtde_driver/launch/ur10e_ros_control.launch b/ur_rtde_driver/launch/ur10e_ros_control.launch
index b48c870..dadd662 100644
--- a/ur_rtde_driver/launch/ur10e_ros_control.launch
+++ b/ur_rtde_driver/launch/ur10e_ros_control.launch
@@ -3,20 +3,16 @@
 
   <!-- 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" default="true"/>
   <arg name="controller_config_file" default="$(find ur_rtde_driver)/config/ur10e_controllers.yaml"/>
   <arg name="robot_description_file" default="$(find ur_e_description)/launch/ur10e_upload.launch"/>
+  <arg name="kinematics_config" default="$(find ur_e_description)/config/ur10e_default.yaml"/>
   <arg name="robot_ip"/>
-  <!--<arg name="reverse_port" default="50001"/>-->
   <arg name="limited" default="false"/>
-  <arg name="prefix" default="" />
+  <arg name="tf_prefix" default="" />
   <arg name="controllers" default="joint_state_controller pos_based_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller"/>
-  <!--<arg name="stopped_controllers" default=""/>-->
-  <arg name="urscript_file" default="$(find ur_rtde_driver)/resources/servoj.urscript"/>
-  <arg name="rtde_recipe_file" default="$(find ur_rtde_driver)/resources/rtde_recipe.txt"/>
+  <arg name="stopped_controllers" default=""/>
   <arg name="tool_voltage" default="12"/>
   <arg name="tool_parity" default="0"/>
   <arg name="tool_baud_rate" default="115200"/>
@@ -31,12 +27,11 @@
     <arg name="use_tool_communication" value="$(arg use_tool_communication)"/>
     <arg name="controller_config_file" value="$(arg controller_config_file)"/>
     <arg name="robot_description_file" value="$(arg robot_description_file)"/>
+    <arg name="kinematics_config" value="$(arg kinematics_config)"/>
     <arg name="robot_ip" value="$(arg robot_ip)"/>
     <arg name="limited" value="$(arg limited)"/>
-    <arg name="prefix" value="$(arg prefix)"/>
+    <arg name="tf_prefix" value="$(arg tf_prefix)"/>
     <arg name="controllers" value="$(arg controllers)"/>
-    <arg name="urscript_file" value="$(arg urscript_file)"/>
-    <arg name="rtde_recipe_file" value="$(arg rtde_recipe_file)"/>
     <arg name="tool_voltage" value="$(arg tool_voltage)"/>
     <arg name="tool_parity" value="$(arg tool_parity)"/>
     <arg name="tool_baud_rate" value="$(arg tool_baud_rate)"/>
diff --git a/ur_rtde_driver/launch/ur_common.launch b/ur_rtde_driver/launch/ur_common.launch
index 87becf8..abb058c 100644
--- a/ur_rtde_driver/launch/ur_common.launch
+++ b/ur_rtde_driver/launch/ur_common.launch
@@ -1,22 +1,15 @@
 <?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_description_file"/>
+  <arg name="kinematics_config"/>
   <arg name="robot_ip"/>
-  <!--<arg name="reverse_port" default="50001"/>-->
   <arg name="limited" default="false"/>
-  <arg name="prefix" default="" />
+  <arg name="tf_prefix" default="" />
   <arg name="controllers" default="joint_state_controller pos_based_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller"/>
-  <!--<arg name="stopped_controllers" default=""/>-->
-  <arg name="urscript_file" default="$(find ur_rtde_driver)/resources/servoj.urscript"/>
-  <arg name="rtde_recipe_file" default="$(find ur_rtde_driver)/resources/rtde_recipe.txt"/>
+  <arg name="stopped_controllers" default=""/>
   <arg name="tool_voltage" default="12"/>
   <arg name="tool_parity" default="0"/>
   <arg name="tool_baud_rate" default="115200"/>
@@ -29,51 +22,24 @@
   <!-- robot model -->
   <include file="$(arg robot_description_file)">
     <arg name="limited" value="$(arg limited)"/>
+    <arg name="kinematics_config" value="$(arg kinematics_config)"/>
   </include>
 
-
-  <!-- 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)"/>
-    <param name="script_file" value="$(arg urscript_file)"/>
-    <param name="recipe_file" value="$(arg rtde_recipe_file)"/>
-    <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>
-
-  <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="tool_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)" />-->
-
   <!-- Convert joint states to /tf tranforms -->
   <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
 
-  <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>
-
+  <include file="$(find ur_rtde_driver)/launch/ur_control.launch">
+    <arg name="debug" value="$(arg debug)"/>
+    <arg name="use_tool_communication" value="$(arg use_tool_communication)"/>
+    <arg name="controller_config_file" value="$(arg controller_config_file)"/>
+    <arg name="robot_ip" value="$(arg robot_ip)"/>
+    <arg name="tf_prefix" value="$(arg tf_prefix)"/>
+    <arg name="controllers" value="$(arg controllers)"/>
+    <arg name="tool_voltage" value="$(arg tool_voltage)"/>
+    <arg name="tool_parity" value="$(arg tool_parity)"/>
+    <arg name="tool_baud_rate" value="$(arg tool_baud_rate)"/>
+    <arg name="tool_stop_bits" value="$(arg tool_stop_bits)"/>
+    <arg name="tool_rx_idle_chars" value="$(arg tool_rx_idle_chars)"/>
+    <arg name="tool_tx_idle_chars" value="$(arg tool_tx_idle_chars)"/>
+  </include>
 </launch>
diff --git a/ur_rtde_driver/launch/ur_control.launch b/ur_rtde_driver/launch/ur_control.launch
new file mode 100644
index 0000000..5f99b8f
--- /dev/null
+++ b/ur_rtde_driver/launch/ur_control.launch
@@ -0,0 +1,70 @@
+<?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="tf_prefix" default="" />
+  <arg name="controllers" default="joint_state_controller pos_based_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller"/>
+  <arg name="stopped_controllers" default=""/>
+  <arg name="urscript_file" default="$(find ur_rtde_driver)/resources/servoj.urscript"/>
+  <arg name="rtde_recipe_file" default="$(find ur_rtde_driver)/resources/rtde_recipe.txt"/>
+  <arg name="tool_voltage" default="12"/>
+  <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)"/>
+    <param name="script_file" value="$(arg urscript_file)"/>
+    <param name="recipe_file" value="$(arg rtde_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="tool_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>