Renamed Safe Trajectory Follower to Low Bandwidth one

.gitignore

@@ -4,3 +4,4 @@ Makefile
 cmake_install.cmake
 install_manifest.txt
 *~
+.idea

CMakeLists.txt

@@ -174,7 +174,7 @@ set(${PROJECT_NAME}_SOURCES
     src/ros/rt_publisher.cpp
     src/ros/service_stopper.cpp
     src/ros/trajectory_follower.cpp
-    src/ros/safe_trajectory_follower.cpp
+    src/ros/lowbandwidth_trajectory_follower.cpp
     src/ros/urscript_handler.cpp
     src/ur/stream.cpp
     src/ur/server.cpp

README.md

@@ -39,18 +39,18 @@ A new driver for the UR3/UR5/UR10 robot arms from Universal Robots. It is design
 
 * Added activation modes: `Never`, `Always` and `OnStartup`. Sets wether a call to the `ur_driver/robot_enable` service is required at startup, at startup + on errors or never. Is intended as a safety feature to require some sort of manual intervention in case of driver crashes and robot safety faults.
 
-* **Safe Trajectory Follower** mode of execution. In this mode the real-time control loop for the robot is shifted from the client PC running the driver to URscript executed on the URControl PC of the robot with the following features:
+* **Low Bandwidth Trajectory Follower** mode of execution. In this mode the real-time control loop for the robot is shifted from the client PC running the driver to URscript executed on the URControl PC of the robot with the following features:
   * It works only with */follow\_joint\_trajectory* action for MoveIt integration. It is mutually exclusive with *ros_control*
   * It only implements "position + velocity" based control - it uses coarse positions and velocities calculated by MoveIt and performs cubic interpolation of joint positions (Bezier' curve) of positions. The positions are set using servoj command of URScript.
-  * It is much more safe and resilient to connectivity problems than other methods of ur_modern_driver. It is resilient to additional load on client PC, latency of network and kernel of the PC the driver runs on. This makes it much better suitable for development/research quick iteration loops without separate dedicated physical setup.
+  * It is much more resilient to connectivity problems than other methods of ur_modern_driver. It is resilient to additional load on client PC, latency of network and kernel of the PC the driver runs on. This makes it much better suitable for development/research quick iteration loops without separate dedicated physical setup.
     * Other methods of controlling the robot by ur_modern_driver are very fragile and require low-latency kernel, separated wired network and ideally no other network activity on the PC where ur_modern_driver runs.
-    * **Safe Trajectory Follower** will never make dangerous "catch-up" when move is delayed. Other methods prioritise execution time of a move which sometimes might lead to situation where robot gets out of control and speeds up to catch-up - often resulting in Protective Stop. For Safe Trajectory Follower safety of the move has priority over move execution time.
+    * **Low Bandwidth Trajectory Follower** will never make dangerous "catch-up" when move is delayed. Other methods prioritise execution time of a move which sometimes might lead to situation where robot gets out of control and speeds up to catch-up - often resulting in Protective Stop. For Low Bandwith Trajectory Follower predictability of the move has priority over move execution time.
-    * The amount of TCP/IP communication between the driver and UR robot (URControl PC) is two orders of magnitude (around 100x) less with **Safe Trajectory Follower** than with other methods- coarse MoveIt generated trajectory is sent to robot and cubic interpolation of the move is calculated by the URScript program run on URControl PC.
+    * The amount of TCP/IP communication between the driver and UR robot (URControl PC) is two orders of magnitude (around 100x) less with **Low Bandwidth Trajectory Follower** than with other methods- coarse MoveIt generated trajectory is sent to robot and cubic interpolation of the move is calculated by the URScript program run on URControl PC.
-    * Due to communication optimisations and **Safe Trajectory Follower** works reliably even over WiFi connection (!) which is impossible for other methods.
+    * Due to communication optimisations and **Low Bandwidth Trajectory Follower** works reliably even over WiFi connection (!) which is impossible for other methods.
   * Time flow for the URScript program might be independent from "real time" and can be controlled to speed up or slow down execution of planned moves.
-  * The Safe Trajectory Follower requires 3.0+ version firmware of the robot (servoj command must support lookahead_time and servoj_gain parameters)
+  * The Low Bandwidth Trajectory Follower requires 3.0+ version firmware of the robot (servoj command must support lookahead_time and servoj_gain parameters)
-  * There are several parameters that you can use to control Safe Trajectory Follower's behaviour:
+  * There are several parameters that you can use to control Low Bandwidth Trajectory Follower's behaviour:
-    * **use_safe_trajectory_follower** - should be set to `true` to enable the follower
+    * **use_lowbandwidth_trajectory_follower** - should be set to `true` to enable the follower
     * **time_interval** - time interval (in seconds) this is 'simulated' time for interpolation steps of the move. Together with *servoj_time* it can be used to change speed of moves even after they are planned by MoveIt. Default value is 0.008
     * **servoj_time** - time interval (real time) for which each interpolation step controls the robot (using servoj command). See below on examples of setting different time parameters. Default value is 0.008 (corresponds to expected 125Hz frequency of UR robot control)
     * **servoj_time_waiting** - time in seconds (real time) of internal active loop while the robot waits for new instructions in case of delays in communication. The smaller value, the faster robot restarts move after delay (but more stress is put on URControl processor). Default value is 0.001 (1000 Hz check frequency)
@@ -59,7 +59,7 @@ A new driver for the UR3/UR5/UR10 robot arms from Universal Robots. It is design
     * **more_debug** - displays even more debug information in the logs of Polyscope - details about interpolation loop calculations and execution. It adds a lot of overhead and is not recommended to be used with real robot (It's quite OK to run it with URSim though)
     * **servoj_gain** and **servoj_lookahead_time** - useful to control precision and speed of the position moves with servoj command (see URScript documentation for detailes)
 
-Here are some examples of manipulating the time flow for **Safe Trajectory Follower** mode. You can use other settings but you should do it on your own risk.
+Here are some examples of manipulating the time flow for **Low Bandwidth Trajectory Follower** mode. You can use other settings but you should do it on your own risk.
   * Default mode: *servoj_time* = 0.008, *time_interval* = 0.008 : interpolation time flows with the same speed as real time - moves are executed as planned
   * Slow-motion mode: *servoj_time* = 0.008, *time_interval* = 0.004 : interpolation time flows 2x slower than real time, so the move is executed 2x slower than planned. Requires configuring MoveIt to accept much slower moves than expected (otherwise MoveIt cancels such move mid-way) 
   * Fast-forward mode: *servoj_time* = 0.004, *time_interval* = 0.012 : interpolation time flows 3x faster than real time, so the move is 3x faster than planned. Might violate limits of the robot speed so use carefully and make sure you gradually increase the speed in-between to check how safe it is.

include/ur_modern_driver/ros/lowbandwidth_trajectory_follower.h

@@ -13,7 +13,7 @@
 #include "ur_modern_driver/ur/server.h"
 #include "ur_modern_driver/ros/trajectory_follower_interface.h"
 
-class SafeTrajectoryFollower:  public TrajectoryFollowerInterface
+class LowBandwidthTrajectoryFollower:  public TrajectoryFollowerInterface
 {
 private:
   std::atomic<bool> running_;
@@ -40,11 +40,11 @@ private:
                double sample_number, double time_in_seconds);
 
 public:
-  SafeTrajectoryFollower(URCommander &commander, std::string &reverse_ip, int reverse_port, bool version_3);
+  LowBandwidthTrajectoryFollower(URCommander &commander, std::string &reverse_ip, int reverse_port, bool version_3);
 
   bool start();
   bool execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt);
   void stop();
 
-  virtual ~SafeTrajectoryFollower() {};
+  virtual ~LowBandwidthTrajectoryFollower() {};
 };

launch/ur_common.launch

@@ -13,7 +13,7 @@
   <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="use_lowbandwidth_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" />
@@ -40,7 +40,7 @@
     <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="use_lowbandwidth_trajectory_follower" type="bool" value="$(arg use_lowbandwidth_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)" />

src/ros/lowbandwidth_trajectory_follower.cpp

@@ -1,4 +1,4 @@
-#include "ur_modern_driver/ros/safe_trajectory_follower.h"
+#include "ur_modern_driver/ros/lowbandwidth_trajectory_follower.h"
 #include <endian.h>
 #include <cmath>
 #include <ros/ros.h>
@@ -16,7 +16,7 @@ static const std::string SERVOJ_LOOKAHEAD_TIME("{{SERVOJ_LOOKAHEAD_TIME}}");
 static const std::string REVERSE_IP("{{REVERSE_IP}}");
 static const std::string REVERSE_PORT("{{REVERSE_PORT}}");
 static const std::string POSITION_PROGRAM = R"(
-def driveRobotSafeTrajectory():
+def driveRobotLowBandwidthTrajectory():
 
     global JOINT_NUM               = 6
     global TIME_INTERVAL           = {{TIME_INTERVAL}}
@@ -359,7 +359,7 @@ end
 
 )";
 
-SafeTrajectoryFollower::SafeTrajectoryFollower(URCommander &commander, std::string &reverse_ip, int reverse_port,
+LowBandwidthTrajectoryFollower::LowBandwidthTrajectoryFollower(URCommander &commander, std::string &reverse_ip, int reverse_port,
                                        bool version_3)
   : running_(false)
   , commander_(commander)
@@ -385,7 +385,7 @@ SafeTrajectoryFollower::SafeTrajectoryFollower(URCommander &commander, std::stri
   std::string res(POSITION_PROGRAM);
   std::ostringstream out;
   if (!version_3) {
-    LOG_ERROR("Safe Trajectory Follower only works for interface version > 3");
+    LOG_ERROR("Low Bandwidth Trajectory Follower only works for interface version > 3");
     std::exit(-1);
   }
   res.replace(res.find(TIME_INTERVAL), TIME_INTERVAL.length(), std::to_string(time_interval_));
@@ -405,12 +405,12 @@ SafeTrajectoryFollower::SafeTrajectoryFollower(URCommander &commander, std::stri
     LOG_ERROR("Failed to bind server, the port %d is likely already in use", reverse_port);
     std::exit(-1);
   }
-  LOG_INFO("Safe Trajectory Follower is initialized!");
+  LOG_INFO("Low Bandwidth Trajectory Follower is initialized!");
 }
 
-bool SafeTrajectoryFollower::start()
+bool LowBandwidthTrajectoryFollower::start()
 {
-  LOG_INFO("Starting SafeTrajectoryFollower");
+  LOG_INFO("Starting LowBandwidthTrajectoryFollower");
 
   if (running_)
     return true;  // not sure
@@ -435,7 +435,7 @@ bool SafeTrajectoryFollower::start()
   return (running_ = true);
 }
 
-bool SafeTrajectoryFollower::execute(const std::array<double, 6> &positions,
+bool LowBandwidthTrajectoryFollower::execute(const std::array<double, 6> &positions,
                                      const std::array<double, 6> &velocities,
                                      double sample_number, double time_in_seconds)
 {
@@ -468,7 +468,7 @@ bool SafeTrajectoryFollower::execute(const std::array<double, 6> &positions,
   return server_.write(buf, strlen(formatted_message) + 1, written);
 }
 
-bool SafeTrajectoryFollower::execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt)
+bool LowBandwidthTrajectoryFollower::execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt)
 {
   if (!running_)
     return false;
@@ -505,7 +505,7 @@ bool SafeTrajectoryFollower::execute(std::vector<TrajectoryPoint> &trajectory, s
   return res;
 }
 
-void SafeTrajectoryFollower::stop()
+void LowBandwidthTrajectoryFollower::stop()
 {
   if (!running_)
     return;

src/ros_main.cpp

@@ -13,7 +13,7 @@
 #include "ur_modern_driver/ros/rt_publisher.h"
 #include "ur_modern_driver/ros/service_stopper.h"
 #include "ur_modern_driver/ros/trajectory_follower.h"
-#include "ur_modern_driver/ros/safe_trajectory_follower.h"
+#include "ur_modern_driver/ros/lowbandwidth_trajectory_follower.h"
 #include "ur_modern_driver/ros/urscript_handler.h"
 #include "ur_modern_driver/ur/commander.h"
 #include "ur_modern_driver/ur/factory.h"
@@ -26,7 +26,7 @@
 static const std::string IP_ADDR_ARG("~robot_ip_address");
 static const std::string REVERSE_PORT_ARG("~reverse_port");
 static const std::string ROS_CONTROL_ARG("~use_ros_control");
-static const std::string SAFE_TRAJECTORY_FOLLOWER("~use_safe_trajectory_follower");
+static const std::string LOW_BANDWIDTH_TRAJECTORY_FOLLOWER("~use_lowbandwidth_trajectory_follower");
 static const std::string MAX_VEL_CHANGE_ARG("~max_vel_change");
 static const std::string PREFIX_ARG("~prefix");
 static const std::string BASE_FRAME_ARG("~base_frame");
@@ -55,7 +55,7 @@ public:
   double max_vel_change;
   int32_t reverse_port;
   bool use_ros_control;
-  bool use_safe_trajectory_follower;
+  bool use_lowbandwidth_trajectory_follower;
   bool shutdown_on_disconnect;
 };
 
@@ -95,7 +95,7 @@ bool parse_args(ProgArgs &args)
   ros::param::param(MAX_VEL_CHANGE_ARG, args.max_vel_change, 15.0);  // rad/s
   ros::param::param(MAX_VEL_CHANGE_ARG, args.max_velocity, 10.0);
   ros::param::param(ROS_CONTROL_ARG, args.use_ros_control, false);
-  ros::param::param(SAFE_TRAJECTORY_FOLLOWER, args.use_safe_trajectory_follower, false);
+  ros::param::param(LOW_BANDWIDTH_TRAJECTORY_FOLLOWER, args.use_lowbandwidth_trajectory_follower, false);
   ros::param::param(PREFIX_ARG, args.prefix, std::string());
   ros::param::param(BASE_FRAME_ARG, args.base_frame, args.prefix + "base_link");
   ros::param::param(TOOL_FRAME_ARG, args.tool_frame, args.prefix + "tool0_controller");
@@ -140,10 +140,10 @@ int main(int argc, char **argv)
 
   TrajectoryFollowerInterface *traj_follower(nullptr);
 
-  if (args.use_safe_trajectory_follower && !args.use_ros_control)
+  if (args.use_lowbandwidth_trajectory_follower && !args.use_ros_control)
   {
-    LOG_INFO("Use safe trajectory follower");
+    LOG_INFO("Use low bandwidth trajectory follower");
-    traj_follower = new SafeTrajectoryFollower(*rt_commander, local_ip, args.reverse_port,factory.isVersion3());
+    traj_follower = new LowBandwidthTrajectoryFollower(*rt_commander, local_ip, args.reverse_port,factory.isVersion3());
     // We are leaking the follower here, but it's OK as this is once-a-lifetime event
   }
   else