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.

include/ur_modern_driver/ros/action_server.h

@@ -39,7 +39,7 @@ private:
   std::condition_variable tj_cv_;
   std::thread tj_thread_;
 
-  TrajectoryFollower& follower_;
+  TrajectoryFollowerInterface& follower_;
 
   RobotState state_;
   std::array<double, 6> q_actual_, qd_actual_;
@@ -61,7 +61,7 @@ private:
   bool updateState(RTShared& data);
 
 public:
-  ActionServer(TrajectoryFollower& follower, std::vector<std::string>& joint_names, double max_velocity);
+  ActionServer(TrajectoryFollowerInterface& follower, std::vector<std::string>& joint_names, double max_velocity);
 
   void start();
   virtual void onRobotStateChange(RobotState state);
@@ -70,4 +70,4 @@ public:
   virtual bool consume(RTState_V1_8& state);
   virtual bool consume(RTState_V3_0__1& state);
   virtual bool consume(RTState_V3_2__3& state);
-};
+};

include/ur_modern_driver/ros/safe_trajectory_follower.h

@@ -0,0 +1,50 @@
+#pragma once
+
+#include <inttypes.h>
+#include <array>
+#include <atomic>
+#include <cstddef>
+#include <cstring>
+#include <string>
+#include <thread>
+#include <vector>
+#include "ur_modern_driver/log.h"
+#include "ur_modern_driver/ur/commander.h"
+#include "ur_modern_driver/ur/server.h"
+#include "ur_modern_driver/ros/trajectory_follower_interface.h"
+
+class SafeTrajectoryFollower:  public TrajectoryFollowerInterface
+{
+private:
+  std::atomic<bool> running_;
+  std::array<double, 6> last_positions_;
+  URCommander &commander_;
+  URServer server_;
+
+  double time_interval_, servoj_time_, servoj_time_waiting_, max_waiting_time_, \
+         servoj_gain_, servoj_lookahead_time_;
+  bool debug_, more_debug_;
+
+  std::string program_;
+
+  template <typename T>
+  size_t append(uint8_t *buffer, T &val)
+  {
+    size_t s = sizeof(T);
+    std::memcpy(buffer, &val, s);
+    return s;
+  }
+
+  bool execute(const std::array<double, 6> &positions,
+               const std::array<double, 6> &velocities,
+               double sample_number, double time_in_seconds);
+
+public:
+  SafeTrajectoryFollower(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() {};
+};

include/ur_modern_driver/ros/trajectory_follower.h

@@ -11,24 +11,9 @@
 #include "ur_modern_driver/log.h"
 #include "ur_modern_driver/ur/commander.h"
 #include "ur_modern_driver/ur/server.h"
+#include "ur_modern_driver/ros/trajectory_follower_interface.h"
 
-struct TrajectoryPoint
-{
-  std::array<double, 6> positions;
-  std::array<double, 6> velocities;
-  std::chrono::microseconds time_from_start;
-
-  TrajectoryPoint()
-  {
-  }
-
-  TrajectoryPoint(std::array<double, 6> &pos, std::array<double, 6> &vel, std::chrono::microseconds tfs)
-    : positions(pos), velocities(vel), time_from_start(tfs)
-  {
-  }
-};
-
-class TrajectoryFollower
+class TrajectoryFollower : public TrajectoryFollowerInterface
 {
 private:
   std::atomic<bool> running_;
@@ -57,5 +42,6 @@ public:
   bool execute(std::array<double, 6> &positions);
   bool execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt);
   void stop();
-  void interrupt();
-};
+
+  virtual ~TrajectoryFollower() {};
+};

include/ur_modern_driver/ros/trajectory_follower_interface.h

@@ -0,0 +1,32 @@
+#pragma once
+
+#include <inttypes.h>
+#include <array>
+#include <atomic>
+#include <cstddef>
+#include <vector>
+
+struct TrajectoryPoint
+{
+  std::array<double, 6> positions;
+  std::array<double, 6> velocities;
+  std::chrono::microseconds time_from_start;
+
+  TrajectoryPoint()
+  {
+  }
+
+  TrajectoryPoint(std::array<double, 6> &pos, std::array<double, 6> &vel, std::chrono::microseconds tfs)
+    : positions(pos), velocities(vel), time_from_start(tfs)
+  {
+  }
+};
+
+class TrajectoryFollowerInterface
+{
+public:
+  virtual bool start() = 0;
+  virtual bool execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt) = 0;
+  virtual void stop() = 0;
+  virtual ~TrajectoryFollowerInterface() {};
+};

include/ur_modern_driver/tcp_socket.h

@@ -46,6 +46,7 @@ public:
 
   std::string getIP();
 
+  bool read(char *character);
   bool read(uint8_t *buf, size_t buf_len, size_t &read);
   bool write(const uint8_t *buf, size_t buf_len, size_t &written);
 

include/ur_modern_driver/ur/server.h

@@ -8,6 +8,8 @@
 #include <string>
 #include "ur_modern_driver/tcp_socket.h"
 
+#define MAX_SERVER_BUF_LEN 50
+
 class URServer : private TCPSocket
 {
 private:
@@ -24,5 +26,6 @@ public:
   bool bind();
   bool accept();
   void disconnectClient();
+  bool readLine(char* buffer, size_t buf_len);
   bool write(const uint8_t *buf, size_t buf_len, size_t &written);
-};
+};