Templated the communication structure to prepare for RTDE interface

Note: This was merged from a branch, as individual commits might not build

CMakeLists.txt

@@ -67,7 +67,6 @@ include_directories(
 )
 
 add_library(ur_rtde_driver
-    src/comm/stream.cpp
     src/comm/tcp_socket.cpp
     src/ros/service_stopper.cpp
     src/ur/commander.cpp

include/ur_rtde_driver/comm/pipeline.h

@@ -1,4 +1,6 @@
 /*
+ * Copyright 2019, FZI Forschungszentrum Informatik (templating)
+ *
  * Copyright 2017, 2018 Simon Rasmussen (refactor)
  *
  * Copyright 2015, 2016 Thomas Timm Andersen (original version)
@@ -24,6 +26,7 @@
 #include <vector>
 #include "ur_rtde_driver/log.h"
 #include "ur_rtde_driver/queue/readerwriterqueue.h"
+#include "ur_rtde_driver/comm/package.h"
 
 namespace ur_driver
 {
@@ -104,7 +107,7 @@ public:
   }
 };
 
-template <typename T>
+template <typename HeaderT>
 class IProducer
 {
 public:
@@ -118,7 +121,7 @@ public:
   {
   }
 
-  virtual bool tryGet(std::vector<std::unique_ptr<T>>& products) = 0;
+  virtual bool tryGet(std::vector<std::unique_ptr<URPackage<HeaderT>>>& products) = 0;
 };
 
 class INotifier
@@ -132,24 +135,25 @@ public:
   }
 };
 
-template <typename T>
+template <typename HeaderT>
 class Pipeline
 {
 private:
   typedef std::chrono::high_resolution_clock Clock;
   typedef Clock::time_point Time;
-  IProducer<T>& producer_;
-  IConsumer<T>& consumer_;
+  using _package_type = URPackage<HeaderT>;
+  IProducer<HeaderT>& producer_;
+  IConsumer<_package_type>& consumer_;
   std::string name_;
   INotifier& notifier_;
-  BlockingReaderWriterQueue<std::unique_ptr<T>> queue_;
+  BlockingReaderWriterQueue<std::unique_ptr<_package_type>> queue_;
   std::atomic<bool> running_;
   std::thread pThread_, cThread_;
 
   void run_producer()
   {
     producer_.setupProducer();
-    std::vector<std::unique_ptr<T>> products;
+    std::vector<std::unique_ptr<_package_type>> products;
     while (running_)
     {
       if (!producer_.tryGet(products))
@@ -177,7 +181,7 @@ private:
   void run_consumer()
   {
     consumer_.setupConsumer();
-    std::unique_ptr<T> product;
+    std::unique_ptr<_package_type> product;
     while (running_)
     {
       // timeout was chosen because we should receive messages
@@ -201,7 +205,7 @@ private:
   }
 
 public:
-  Pipeline(IProducer<T>& producer, IConsumer<T>& consumer, std::string name, INotifier& notifier)
+  Pipeline(IProducer<HeaderT>& producer, IConsumer<_package_type>& consumer, std::string name, INotifier& notifier)
     : producer_(producer), consumer_(consumer), name_(name), notifier_(notifier), queue_{ 32 }, running_{ false }
   {
   }

include/ur_rtde_driver/comm/producer.h

@@ -1,4 +1,6 @@
 /*
+ * Copyright 2019, FZI Forschungszentrum Informatik (templating)
+ *
  * Copyright 2017, 2018 Simon Rasmussen (refactor)
  *
  * Copyright 2015, 2016 Thomas Timm Andersen (original version)
@@ -21,19 +23,22 @@
 #include "ur_rtde_driver/comm/pipeline.h"
 #include "ur_rtde_driver/comm/parser.h"
 #include "ur_rtde_driver/comm/stream.h"
+#include "ur_rtde_driver/comm/package.h"
 
 namespace ur_driver
 {
-template <typename T>
-class URProducer : public comm::IProducer<T>
+namespace comm
+{
+template <typename HeaderT>
+class URProducer : public IProducer<HeaderT>
 {
 private:
-  comm::URStream& stream_;
-  comm::URParser<T>& parser_;
+  URStream<HeaderT>& stream_;
+  URParser<HeaderT>& parser_;
   std::chrono::seconds timeout_;
 
 public:
-  URProducer(comm::URStream& stream, comm::URParser<T>& parser) : stream_(stream), parser_(parser), timeout_(1)
+  URProducer(URStream<HeaderT>& stream, URParser<HeaderT>& parser) : stream_(stream), parser_(parser), timeout_(1)
   {
   }
 
@@ -50,7 +55,7 @@ public:
     stream_.disconnect();
   }
 
-  bool tryGet(std::vector<std::unique_ptr<T>>& products)
+  bool tryGet(std::vector<std::unique_ptr<URPackage<HeaderT>>>& products)
   {
     // 4KB should be enough to hold any packet received from UR
     uint8_t buf[4096];
@@ -83,4 +88,5 @@ public:
     return parser_.parse(bp, products);
   }
 };
+}  // namespace comm
 }  // namespace ur_driver

include/ur_rtde_driver/comm/stream.h

@@ -1,4 +1,6 @@
 /*
+ * Copyright 2019, FZI Forschungszentrum Informatik (templating)
+ *
  * Copyright 2017, 2018 Simon Rasmussen (refactor)
  *
  * Copyright 2015, 2016 Thomas Timm Andersen (original version)
@@ -30,6 +32,7 @@ namespace ur_driver
 {
 namespace comm
 {
+template <typename HeaderT>
 class URStream : public TCPSocket
 {
 private:
@@ -66,5 +69,44 @@ public:
   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);
 };
+
+template <typename HeaderT>
+bool URStream<HeaderT>::write(const uint8_t* buf, size_t buf_len, size_t& written)
+{
+  std::lock_guard<std::mutex> lock(write_mutex_);
+  return TCPSocket::write(buf, buf_len, written);
+}
+
+template <typename HeaderT>
+bool URStream<HeaderT>::read(uint8_t* buf, size_t buf_len, size_t& total)
+{
+  std::lock_guard<std::mutex> lock(read_mutex_);
+
+  bool initial = true;
+  uint8_t* buf_pos = buf;
+  size_t remainder = sizeof(HeaderT::_package_size_type);
+  size_t read = 0;
+
+  while (remainder > 0 && TCPSocket::read(buf_pos, remainder, read))
+  {
+    TCPSocket::setOptions(getSocketFD());
+    if (initial)
+    {
+      remainder = HeaderT::getPackageLength(buf);
+      if (remainder >= (buf_len - sizeof(HeaderT::_package_size_type)))
+      {
+        LOG_ERROR("Packet size %zd is larger than buffer %zu, discarding.", remainder, buf_len);
+        return false;
+      }
+      initial = false;
+    }
+
+    total += read;
+    buf_pos += read;
+    remainder -= read;
+  }
+
+  return remainder == 0;
+}
 }  // namespace comm
 }  // namespace ur_driver

include/ur_rtde_driver/ur/commander.h

@@ -1,4 +1,6 @@
 /*
+ * Copyright 2019, FZI Forschungszentrum Informatik (templating)
+ *
  * Copyright 2017, 2018 Simon Rasmussen (refactor)
  *
  * Copyright 2015, 2016 Thomas Timm Andersen (original version)
@@ -21,20 +23,21 @@
 #include <iomanip>
 #include <sstream>
 #include "ur_rtde_driver/comm/stream.h"
+#include "ur_rtde_driver/primary/package_header.h"
 
 namespace ur_driver
 {
 class URCommander
 {
 private:
-  comm::URStream& stream_;
+  comm::URStream<primary_interface::PackageHeader>& stream_;
 
 protected:
   bool write(const std::string& s);
   void formatArray(std::ostringstream& out, std::array<double, 6>& values);
 
 public:
-  URCommander(comm::URStream& stream) : stream_(stream)
+  URCommander(comm::URStream<primary_interface::PackageHeader>& stream) : stream_(stream)
   {
   }
 
@@ -53,7 +56,7 @@ public:
 class URCommander_V1_X : public URCommander
 {
 public:
-  URCommander_V1_X(comm::URStream& stream) : URCommander(stream)
+  URCommander_V1_X(comm::URStream<primary_interface::PackageHeader>& stream) : URCommander(stream)
   {
   }
 
@@ -65,7 +68,7 @@ public:
 class URCommander_V3_X : public URCommander
 {
 public:
-  URCommander_V3_X(comm::URStream& stream) : URCommander(stream)
+  URCommander_V3_X(comm::URStream<primary_interface::PackageHeader>& stream) : URCommander(stream)
   {
   }
 
@@ -77,7 +80,7 @@ public:
 class URCommander_V3_1__2 : public URCommander_V3_X
 {
 public:
-  URCommander_V3_1__2(comm::URStream& stream) : URCommander_V3_X(stream)
+  URCommander_V3_1__2(comm::URStream<primary_interface::PackageHeader>& stream) : URCommander_V3_X(stream)
   {
   }
 
@@ -87,7 +90,7 @@ public:
 class URCommander_V3_3 : public URCommander_V3_X
 {
 public:
-  URCommander_V3_3(comm::URStream& stream) : URCommander_V3_X(stream)
+  URCommander_V3_3(comm::URStream<primary_interface::PackageHeader>& stream) : URCommander_V3_X(stream)
   {
   }
 

src/comm/stream.cpp

@@ -1,68 +0,0 @@
-/*
- * Copyright 2017, 2018 Simon Rasmussen (refactor)
- *
- * Copyright 2015, 2016 Thomas Timm Andersen (original version)
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#include <endian.h>
-#include <netinet/tcp.h>
-#include <unistd.h>
-#include <cstring>
-
-#include "ur_rtde_driver/log.h"
-#include "ur_rtde_driver/comm/stream.h"
-
-namespace ur_driver
-{
-namespace comm
-{
-bool URStream::write(const uint8_t* buf, size_t buf_len, size_t& written)
-{
-  std::lock_guard<std::mutex> lock(write_mutex_);
-  return TCPSocket::write(buf, buf_len, written);
-}
-
-bool URStream::read(uint8_t* buf, size_t buf_len, size_t& total)
-{
-  std::lock_guard<std::mutex> lock(read_mutex_);
-
-  bool initial = true;
-  uint8_t* buf_pos = buf;
-  size_t remainder = sizeof(int32_t);
-  size_t read = 0;
-
-  while (remainder > 0 && TCPSocket::read(buf_pos, remainder, read))
-  {
-    TCPSocket::setOptions(getSocketFD());
-    if (initial)
-    {
-      remainder = be32toh(*(reinterpret_cast<int32_t*>(buf)));
-      if (remainder >= (buf_len - sizeof(int32_t)))
-      {
-        LOG_ERROR("Packet size %zd is larger than buffer %zu, discarding.", remainder, buf_len);
-        return false;
-      }
-      initial = false;
-    }
-
-    total += read;
-    buf_pos += read;
-    remainder -= read;
-  }
-
-  return remainder == 0;
-}
-}  // namespace comm
-}  // namespace ur_driver