Adds Safe Trajectory Follower implementation

Safe Trajectory Follower implements different approach for controlling
the robot. Rather than calculate the interpolation steps in the driver
and send the small interpolated steps over the network to the URScript
program with 500Hz frequency, the coarser MoveIt trajectory is sent
(with few Hz) and the interpolation steps are calculated by the
URScript.

The algorithm for time progress has also built-in protection against
any delays induced by load on the driver, network or URControl - it
will never "catch-up" dangerously when such delay are introduced,
It will rather pause and wait for the next small interpolation step
instructions and re-start the move slower - never skipping any
interpolated steps.

Those changes make Safe Trajectory Follower much more resilient to
network communication problems and removes any superficial requirements
for the network setup, kernel latency and no-load-requirement for the
driver's PC - making it much more suitable for research, development
and quick iteration loops. It works reliably even over WiFi.

launch/ur_common.launch

@@ -2,7 +2,7 @@
 <!--
   Universal robot common bringup.  Starts ur driver node and robot state
   publisher (translates joint positions to propper tfs).
-  
+
   Usage:
     ur_common.launch robot_ip:=<value>
 -->
@@ -12,14 +12,23 @@
   <arg name="min_payload" />
   <arg name="max_payload" />
   <arg name="prefix" default="" />
+  <arg name="use_ros_control" default="false"/>
+  <arg name="use_safe_trajectory_follower" default="false"/>
+  <arg name="time_interval" default="0.008"/>
   <arg name="servoj_time" default="0.008" />
+  <arg name="servoj_time_waiting" default="0.001" />
+  <arg name="max_waiting_time" default="2.0" />
+  <arg name="debug" default="false" />
+  <arg name="more_debug" default="false" />
+  <arg name="servoj_gain" default="100." />
+  <arg name="servoj_lookahead_time" default="1." />
   <arg name="base_frame" default="$(arg prefix)base" />
   <arg name="tool_frame" default="$(arg prefix)tool0_controller" />
   <arg name="shutdown_on_disconnect" default="true" />
 
   <!-- require_activation defines when the service /ur_driver/robot_enable needs to be called. -->
   <arg name="require_activation" default="Never" /> <!-- Never, Always, OnStartup -->
-    
+
   <!-- The max_velocity parameter is only used for debugging in the ur_driver. It's not related to actual velocity limits -->
   <arg name="max_velocity" default="10.0"/> <!-- [rad/s] -->
 
@@ -30,10 +39,19 @@
   <!-- copy the specified IP address to be consistant with ROS-Industrial spec. -->
     <param name="prefix" type="str" value="$(arg prefix)" />
     <param name="robot_ip_address" type="str" value="$(arg robot_ip)" />
+    <param name="use_ros_control" type="bool" value="$(arg use_ros_control)"/>
+    <param name="use_safe_trajectory_follower" type="bool" value="$(arg use_safe_trajectory_follower)"/>
     <param name="min_payload" type="double" value="$(arg min_payload)" />
     <param name="max_payload" type="double" value="$(arg max_payload)" />
     <param name="max_velocity" type="double" value="$(arg max_velocity)" />
+    <param name="time_interval" type="double" value="$(arg time_interval)" />
     <param name="servoj_time" type="double" value="$(arg servoj_time)" />
+    <param name="servoj_time_waiting" type="double" value="$(arg servoj_time_waiting)" />
+    <param name="max_waiting_time" type="double" value="$(arg max_waiting_time)" />
+    <param name="debug" type="bool" value="$(arg debug)"/>
+    <param name="more_debug" type="bool" value="$(arg more_debug)"/>
+    <param name="servoj_gain" type="double" value="$(arg servoj_gain)" />
+    <param name="servoj_lookahead_time" type="double" value="$(arg servoj_lookahead_time)" />
     <param name="base_frame" type="str" value="$(arg base_frame)"/>
     <param name="tool_frame" type="str" value="$(arg tool_frame)"/>
     <param name="require_activation" type="str" value="$(arg require_activation)" />