diff --git a/CMakeLists.txt b/CMakeLists.txt
index 459aef7..18ee84e 100644
--- a/CMakeLists.txt
+++ b/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
diff --git a/include/ur_rtde_driver/comm/stream.h b/include/ur_rtde_driver/comm/stream.h
index 9f8c3ff..7cfd37d 100644
--- a/include/ur_rtde_driver/comm/stream.h
+++ b/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
diff --git a/src/comm/stream.cpp b/src/comm/stream.cpp
deleted file mode 100644
index 9ecb880..0000000
--- a/src/comm/stream.cpp
+++ /dev/null
@@ -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