WIP

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_;