WIP

include/ur_modern_driver/bin_parser.h

@@ -68,7 +68,7 @@ public:
   template <typename T>
   T peek()
   {
-    assert(buf_pos_ <= buf_end_);
+    assert(buf_pos_ + sizeof(T) <= buf_end_);
     T val;
     std::memcpy(&val, buf_pos_, sizeof(T));
     return decode(val);

include/ur_modern_driver/ros/trajectory_follower.h

@@ -49,12 +49,11 @@ 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);
+  TrajectoryFollower(URCommander &commander, std::string& reverse_ip, int reverse_port, bool version_3);
 
   bool start();
   bool execute(std::array<double, 6> &positions);

include/ur_modern_driver/tcp_socket.h

@@ -29,8 +29,6 @@ protected:
   
   
   bool setup(std::string &host, int port);
-  void close();
-
 
 public:
   TCPSocket();
@@ -41,6 +39,10 @@ public:
   int getSocketFD() { return socket_fd_; }
   bool setSocketFD(int socket_fd);
 
+  std::string getIP();
+
   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);
+
+  void close();  
 };

include/ur_modern_driver/ur/producer.h

@@ -50,6 +50,10 @@ public:
 
       LOG_WARN("Failed to read from stream, reconnecting in %ld seconds...", timeout_.count());
       std::this_thread::sleep_for(timeout_);
+
+      if(stream_.connect())
+        continue;
+
       auto next = timeout_ * 2;
       if(next <= std::chrono::seconds(120))
         timeout_ = next;

include/ur_modern_driver/ur/server.h

@@ -12,7 +12,6 @@ class URServer : private TCPSocket
 {
 private:
   int port_;
-  SocketState state_;
   TCPSocket client_;
 
 protected:
@@ -28,5 +27,6 @@ public:
   std::string getIP();
   bool bind();
   bool accept();
+  void disconnectClient();
   bool write(const uint8_t* buf, size_t buf_len, size_t &written);
 };

include/ur_modern_driver/ur/state_parser.h

@@ -34,7 +34,7 @@ public:
     bp.parse(type);
 
     //quietly ignore the intial version message
-    if (type == message_type::ROBOT_MESSAGE)
+    if (type == message_type::ROBOT_MESSAGE || ((int)type) == 5)
     {
       bp.consume();
       return true;
@@ -42,7 +42,7 @@ public:
 
     if (type != message_type::ROBOT_STATE)
     {
-      LOG_WARN("Invalid message type recieved: %u", static_cast<uint8_t>(type));
+      LOG_WARN("Invalid state message type recieved: %u", static_cast<uint8_t>(type));
       return false;
     }
 
@@ -60,7 +60,7 @@ public:
         return false;
       }
 
-      LOG_DEBUG("sub-packet size: %" PRIu32, sub_size);
+      //LOG_DEBUG("sub-packet size: %" PRIu32, sub_size);
 
       // deconstruction of a sub parser will increment the position of the parent parser
       BinParser sbp(bp, sub_size);
@@ -73,7 +73,7 @@ public:
       if (packet == nullptr)
       {
         sbp.consume();
-        LOG_DEBUG("Skipping sub-packet of type %d", type);
+        //LOG_DEBUG("Skipping sub-packet of type %d", type);
         continue;
       }
 

include/ur_modern_driver/ur/stream.h

@@ -8,7 +8,7 @@
 #include "ur_modern_driver/log.h"
 #include "ur_modern_driver/tcp_socket.h"
 
-class URStream : private TCPSocket
+class URStream : public TCPSocket
 {
 private:
   std::string host_;