Improved stream implementation

include/ur_modern_driver/ur/stream.h

@@ -2,6 +2,7 @@
 #include <netdb.h>
 #include <sys/socket.h>
 #include <sys/types.h>
+#include <mutex>
 #include <atomic>
 #include <string>
 
@@ -15,6 +16,7 @@ private:
 
   std::atomic<bool> initialized_;
   std::atomic<bool> stopping_;
+  std::mutex send_mutex_, receive_mutex_;
 
 public:
   URStream(std::string& host, int port) : host_(host), port_(port), initialized_(false), stopping_(false)
@@ -28,7 +30,8 @@ public:
 
   bool connect();
   void disconnect();
+  void reconnect();
 
-  ssize_t send(uint8_t* buf, size_t buf_len);
+  ssize_t send(const uint8_t* buf, size_t buf_len);
   ssize_t receive(uint8_t* buf, size_t buf_len);
 };

src/ur/stream.cpp

@@ -73,13 +73,22 @@ void URStream::disconnect()
   initialized_ = false;
 }
 
-ssize_t URStream::send(uint8_t* buf, size_t buf_len)
+void URStream::reconnect()
+{
+  disconnect();
+  stopping_ = false;
+  connect();
+}
+
+ssize_t URStream::send(const uint8_t* buf, size_t buf_len)
 {
   if (!initialized_)
     return -1;
   if (stopping_)
     return 0;
 
+  std::lock_guard<std::mutex> lock(send_mutex_);
+
   size_t total = 0;
   size_t remaining = buf_len;
 
@@ -104,6 +113,8 @@ ssize_t URStream::receive(uint8_t* buf, size_t buf_len)
   if (stopping_)
     return 0;
 
+  std::lock_guard<std::mutex> lock(receive_mutex_);    
+
   size_t remainder = sizeof(int32_t);
   uint8_t* buf_pos = buf;
   bool initial = true;