Merge pull request #9 from NoMagicAi/SAFE_TRAJECTORY_FOLLOWER

Adds Low Bandwith Trajectory Follower implementation

include/ur_modern_driver/ros/action_server.h

@@ -39,7 +39,7 @@ private:
   std::condition_variable tj_cv_;
   std::thread tj_thread_;
 
-  TrajectoryFollower& follower_;
+  ActionTrajectoryFollowerInterface& 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(ActionTrajectoryFollowerInterface& 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/action_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 ActionTrajectoryFollowerInterface
+{
+public:
+  virtual bool start() = 0;
+  virtual bool execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt) = 0;
+  virtual void stop() = 0;
+  virtual ~ActionTrajectoryFollowerInterface() {};
+};

include/ur_modern_driver/ros/lowbandwidth_trajectory_follower.h

@@ -0,0 +1,41 @@
+#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/action_trajectory_follower_interface.h"
+
+class LowBandwidthTrajectoryFollower:  public ActionTrajectoryFollowerInterface
+{
+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_, max_joint_difference_;
+
+  std::string program_;
+
+  bool execute(const std::array<double, 6> &positions,
+               const std::array<double, 6> &velocities,
+               double sample_number, double time_in_seconds);
+
+public:
+  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 ~LowBandwidthTrajectoryFollower() {};
+};

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/action_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 ActionTrajectoryFollowerInterface
 {
 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/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);
-};
+};