Major refactor

include/ur_modern_driver/pipeline.h

@@ -23,6 +23,9 @@ public:
   virtual void stopConsumer()
   {
   }
+  virtual void onTimeout()
+  {
+  }
 
   virtual bool consume(shared_ptr<T> product) = 0;
 };
@@ -59,6 +62,13 @@ public:
       con->stopConsumer();
     }
   }
+  virtual void onTimeout()
+  {
+    for(auto &con : consumers_)
+    {
+      con->onTimeout();
+    }
+  }
 
   bool consume(shared_ptr<T> product)
   {
@@ -93,6 +103,8 @@ template <typename T>
 class Pipeline
 {
 private:
+  typedef std::chrono::high_resolution_clock Clock;
+  typedef Clock::time_point Time;
   IProducer<T>& producer_;
   IConsumer<T>& consumer_;
   BlockingReaderWriterQueue<unique_ptr<T>> queue_;
@@ -129,6 +141,8 @@ private:
   {
     consumer_.setupConsumer();
     unique_ptr<T> product;
+    Time last_pkg = Clock::now();
+    Time last_warn = last_pkg;
     while (running_)
     {
       // 16000us timeout was chosen because we should
@@ -136,8 +150,18 @@ private:
       // 8ms so double it for some error margin
       if (!queue_.wait_dequeue_timed(product, std::chrono::milliseconds(16)))
       {
+        Time now = Clock::now();
+        auto pkg_diff = now - last_pkg;
+        auto warn_diff = now - last_warn;
+        if(pkg_diff > std::chrono::seconds(1) && warn_diff > std::chrono::seconds(1))
+        {
+          last_warn = now;
+          consumer_.onTimeout();
+        }
         continue;
       }
+
+      last_pkg = Clock::now();
       if (!consumer_.consume(std::move(product)))
         break;
     }

include/ur_modern_driver/ros/action_server.h

@@ -5,6 +5,7 @@
 #include <condition_variable>
 #include <mutex>
 #include <set>
+#include <thread>
 #include <ros/ros.h>
 #include <actionlib/server/action_server.h>
 #include <actionlib/server/server_goal_handle.h>
@@ -17,7 +18,7 @@
 #include "ur_modern_driver/ros/trajectory_follower.h"
 
 
-class ActionServer : public URRTPacketConsumer, public Service
+class ActionServer : public Service //,public URRTPacketConsumer
 {
 private:
   typedef control_msgs::FollowJointTrajectoryAction Action;
@@ -35,9 +36,11 @@ private:
 
   
   GoalHandle curr_gh_;
+  std::atomic<bool> interrupt_traj_;
   std::atomic<bool> has_goal_, running_;
   std::mutex tj_mutex_;
   std::condition_variable tj_cv_;
+  std::thread tj_thread_;
 
   TrajectoryFollower& follower_;
 
@@ -50,20 +53,16 @@ private:
   bool validateTrajectory(GoalHandle& gh, Result& res);
 
   bool try_execute(GoalHandle& gh, Result& res);
+  void interruptGoal(GoalHandle& gh);
 
   std::vector<size_t> reorderMap(std::vector<std::string> goal_joints);
-  double interp_cubic(double t, double T, double p0_pos, double p1_pos, double p0_vel, double p1_vel);
 
   void trajectoryThread();
 
-  template <typename U>
-  double toSec(U const& u)
-  {
-      return std::chrono::duration_cast<std::chrono::duration<double>>(u).count();
-  }
 
 public:
   ActionServer(TrajectoryFollower& follower, std::vector<std::string>& joint_names, double max_velocity);
 
+  void start();
   virtual void onRobotStateChange(RobotState state);
 };

include/ur_modern_driver/ros/controller.h

@@ -34,6 +34,7 @@ private:
   std::map<std::string, HardwareInterface*> available_interfaces_;
 
   std::atomic<bool> service_enabled_;
+  std::atomic<uint32_t> service_cooldown_;
 
   // helper functions to map interfaces
   template <typename T>
@@ -51,6 +52,7 @@ private:
   void read(RTShared& state);
   bool update(RTShared& state);
   bool write();
+  void reset();
 
 public:
   ROSController(URCommander& commander,  std::vector<std::string>& joint_names, double max_vel_change);

include/ur_modern_driver/ros/hardware_interface.h

@@ -13,6 +13,7 @@ public:
   virtual bool write() = 0;
   virtual void start() {}
   virtual void stop() {}
+  virtual void reset() {}
 };
 
 using hardware_interface::JointHandle;
@@ -48,6 +49,7 @@ private:
 public:
   VelocityInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface, std::vector<std::string> &joint_names, double max_vel_change);
   virtual bool write();
+  virtual void reset();
   typedef hardware_interface::VelocityJointInterface parent_type;
 };
 

include/ur_modern_driver/ros/io_service.h

@@ -37,6 +37,8 @@ private:
       case ur_msgs::SetIO::Request::FUN_SET_FLAG:
         res = commander_.setFlag(req.pin, flag);
       break;
+      default:
+        LOG_WARN("Invalid setIO function called (%d)", req.fun);
     }
 
     return (resp.success = res);

include/ur_modern_driver/ros/trajectory_follower.h

@@ -5,21 +5,39 @@
 #include <cstddef>
 #include <cstring>
 #include <string>
+#include <thread>
 #include <inttypes.h>
+#include "ur_modern_driver/log.h"
 #include "ur_modern_driver/ur/commander.h"
 #include "ur_modern_driver/ur/server.h"
-#include "ur_modern_driver/ur/stream.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 
 {
 private:
-  const int32_t MULT_JOINTSTATE_ = 1000000;
   double servoj_time_, servoj_lookahead_time_, servoj_gain_;
   std::atomic<bool> running_;
   std::array<double, 6> last_positions_;
   URCommander &commander_;
   URServer server_;
-  URStream stream_;
+  int reverse_port_;
   std::string program_;
 
   template <typename T>
@@ -30,15 +48,16 @@ private:
     return s;
   }
 
+  std::string buildProgram();
   bool execute(std::array<double, 6> &positions, bool keep_alive);  
+  double interpolate(double t, double T, double p0_pos, double p1_pos, double p0_vel, double p1_vel); 
 
 public:
   TrajectoryFollower(URCommander &commander, int reverse_port, bool version_3);
 
-  std::string buildProgram(bool version_3);
-
   bool start();
   bool execute(std::array<double, 6> &positions);
+  bool execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt);
   void stop();
-  void halt(); //maybe
+  void interrupt();
 };

include/ur_modern_driver/tcp_socket.h

@@ -0,0 +1,45 @@
+#pragma once 
+#include <netdb.h>
+#include <sys/socket.h>
+#include <sys/types.h>
+#include <mutex>
+#include <atomic>
+#include <string>
+
+enum class SocketState
+{
+  Invalid,
+  Connected,
+  Disconnected,
+  Closed
+};
+
+class TCPSocket
+{
+private:
+  std::atomic<int> socket_fd_;
+  std::atomic<SocketState> state_;
+
+protected:
+  virtual bool open(int socket_fd, struct sockaddr *address, size_t address_len)
+  {
+    return false;
+  }
+  
+  
+  bool setup(std::string &host, int port);
+  void close();
+
+
+public:
+  TCPSocket();
+  virtual ~TCPSocket();
+
+  SocketState getState() { return state_; }
+  
+  int getSocketFD() { return socket_fd_; }
+  bool setSocketFD(int socket_fd);
+
+  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/commander.h

@@ -11,18 +11,46 @@ private:
 
 protected:
   bool write(std::string& s);
+  void formatArray(std::ostringstream &out, std::array<double, 6> &values);
 
 public:
   URCommander(URStream& stream) : stream_(stream)
   {
   }
 
-  virtual bool uploadProg(std::string &s);
+  virtual bool speedj(std::array<double, 6> &speeds, double acceleration) = 0;
+  virtual bool setDigitalOut(uint8_t pin, bool value) = 0;
+  virtual bool setAnalogOut(uint8_t pin, double value) = 0;
+
+  //shared
+  bool uploadProg(std::string &s);
+  bool stopj(double a = 10.0);
+  bool setToolVoltage(uint8_t voltage);
+  bool setFlag(uint8_t pin, bool value);
+  bool setPayload(double value);
+};
+
+class URCommander_V1_X : public URCommander
+{
+public:
+  URCommander_V1_X(URStream& stream) : URCommander(stream)
+  {
+  }
+
+  virtual bool speedj(std::array<double, 6> &speeds, double acceleration);
+  virtual bool setDigitalOut(uint8_t pin, bool value);
+  virtual bool setAnalogOut(uint8_t pin, double value);
+};
+
+
+class URCommander_V3_X : public URCommander
+{
+public:
+  URCommander_V3_X(URStream& stream) : URCommander(stream)
+  {
+  }
+
   virtual bool speedj(std::array<double, 6> &speeds, double acceleration);
-  virtual bool stopj(double a = 10.0);
   virtual bool setDigitalOut(uint8_t pin, bool value);
   virtual bool setAnalogOut(uint8_t pin, double value);
-  virtual bool setToolVoltage(uint8_t voltage);
-  virtual bool setFlag(uint8_t pin, bool value);
-  virtual bool setPayload(double value);
 };

include/ur_modern_driver/ur/factory.h

@@ -71,6 +71,19 @@ public:
     prod.teardownProducer();
   }
 
+  bool isVersion3()
+  {
+    return major_version_ == 3;
+  }
+
+  std::unique_ptr<URCommander> getCommander(URStream &stream)
+  {
+    if(major_version_ == 1)
+      return std::unique_ptr<URCommander>(new URCommander_V1_X(stream));
+    else
+      return std::unique_ptr<URCommander>(new URCommander_V3_X(stream));    
+  }
+
   std::unique_ptr<URParser<StatePacket>> getStateParser()
   {
     if (major_version_ == 1)

include/ur_modern_driver/ur/producer.h

@@ -1,4 +1,5 @@
 #pragma once
+#include <chrono>
 #include "ur_modern_driver/pipeline.h"
 #include "ur_modern_driver/ur/parser.h"
 #include "ur_modern_driver/ur/stream.h"
@@ -9,9 +10,10 @@ class URProducer : public IProducer<T>
 private:
   URStream& stream_;
   URParser<T>& parser_;
+  std::chrono::seconds timeout_;
 
 public:
-  URProducer(URStream& stream, URParser<T>& parser) : stream_(stream), parser_(parser)
+  URProducer(URStream& stream, URParser<T>& parser) : stream_(stream), parser_(parser), timeout_(1)
   {
   }
 
@@ -32,24 +34,29 @@ public:
   {
     // 4KB should be enough to hold any packet received from UR
     uint8_t buf[4096];
-
-    // blocking call
-    ssize_t len = stream_.receive(buf, sizeof(buf));
-
-    // LOG_DEBUG("Read %d bytes from stream", len);
-
-    if (len == 0)
+    size_t read = 0;
+    //expoential backoff reconnects
+    while(true)
     {
-      LOG_WARN("Read nothing from stream");
-      return false;
-    }
-    else if (len < 0)
-    {
-      LOG_WARN("Stream closed");
-      return false;
+      if(stream_.read(buf, sizeof(buf), read))
+      {
+        //reset sleep amount
+        timeout_ = std::chrono::seconds(1);
+        break;
+      }
+
+      if(stream_.closed())
+        return false;
+
+      LOG_WARN("Failed to read from stream, reconnecting in %ld seconds...", timeout_.count());
+      std::this_thread::sleep_for(timeout_);
+      auto next = timeout_ * 2;
+      if(next <= std::chrono::seconds(120))
+        timeout_ = next;
     }
 
-    BinParser bp(buf, static_cast<size_t>(len));
+
+    BinParser bp(buf, read);
     return parser_.parse(bp, products);
   }
 };

include/ur_modern_driver/ur/server.h

@@ -6,14 +6,25 @@
 #include <mutex>
 #include <atomic>
 #include <string>
-#include "ur_modern_driver/ur/stream.h"
+#include "ur_modern_driver/tcp_socket.h"
 
-class URServer
+class URServer : private TCPSocket
 {
 private:
-  int socket_fd_ = -1;
+  int port_;
+  SocketState state_;
+  TCPSocket client_;
+
+protected:
+  virtual bool open(int socket_fd, struct sockaddr *address, size_t address_len)
+  {
+    return ::bind(socket_fd, address, address_len) == 0;
+  }
 
 public:
   URServer(int port);
-  URStream accept();
+  std::string getIP();
+  bool bind();
+  bool accept();
+  bool write(const uint8_t* buf, size_t buf_len, size_t &written);
 };

include/ur_modern_driver/ur/stream.h

@@ -5,56 +5,39 @@
 #include <mutex>
 #include <atomic>
 #include <string>
+#include "ur_modern_driver/log.h"
+#include "ur_modern_driver/tcp_socket.h"
 
-/// Encapsulates a TCP socket
-class URStream
+class URStream : private TCPSocket
 {
 private:
-  int socket_fd_ = -1;
   std::string host_;
   int port_;
+  std::mutex write_mutex_, read_mutex_;
 
-  std::atomic<bool> initialized_;
-  std::atomic<bool> stopping_;
-  std::mutex send_mutex_, receive_mutex_;
+protected:
+  virtual bool open(int socket_fd, struct sockaddr *address, size_t address_len)
+  {
+    return ::connect(socket_fd, address, address_len) == 0;
+  }
 
 public:
-  URStream() 
+  URStream(std::string& host, int port) : host_(host), port_(port)
   {
   }
 
-  URStream(std::string& host, int port) : host_(host), port_(port), initialized_(false), stopping_(false)
+  bool connect()
   {
+    return TCPSocket::setup(host_, port_);
+  }
+  void disconnect()
+  {
+    LOG_INFO("Disconnecting");
+    TCPSocket::close();
   }
 
-  URStream(int socket_fd) : socket_fd_(socket_fd), initialized_(true), stopping_(false)
-  {
-    
-  }
+  bool closed() { return getState() == SocketState::Closed; }
 
-  URStream(URStream&& other) noexcept : socket_fd_(other.socket_fd_), host_(other.host_), initialized_(other.initialized_.load()), stopping_(other.stopping_.load())
-  {
-    
-  }
-
-  ~URStream()
-  {
-    disconnect();
-  }
-
-  URStream& operator=(URStream&& other)
-  {
-    socket_fd_ = std::move(other.socket_fd_);
-    host_ = std::move(other.host_); 
-    initialized_ = std::move(other.initialized_.load());
-    stopping_ = std::move(other.stopping_.load());
-    return *this;
-  }
-
-  bool connect();
-  void disconnect();
-  void reconnect();
-
-  ssize_t send(const uint8_t* buf, size_t buf_len);
-  ssize_t receive(uint8_t* buf, size_t buf_len);
+  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);
 };