From 4c0aea4c4f76a8853f876e422cb1f8a5dcbeb42e Mon Sep 17 00:00:00 2001
From: Tristan Schnell <schnell@fzi.de>
Date: Thu, 11 Apr 2019 18:13:45 +0200
Subject: [PATCH] added reverse client to publish targets to the robot

---
 .../ur_rtde_driver/comm/reverse_interface.h   | 92 +++++++++++++++++++
 include/ur_rtde_driver/{ur => comm}/server.h  |  3 +
 src/{ur => comm}/server.cpp                   |  3 +
 3 files changed, 98 insertions(+)
 create mode 100644 include/ur_rtde_driver/comm/reverse_interface.h
 rename include/ur_rtde_driver/{ur => comm}/server.h (97%)
 rename src/{ur => comm}/server.cpp (98%)

diff --git a/include/ur_rtde_driver/comm/reverse_interface.h b/include/ur_rtde_driver/comm/reverse_interface.h
new file mode 100644
index 0000000..18dfc3c
--- /dev/null
+++ b/include/ur_rtde_driver/comm/reverse_interface.h
@@ -0,0 +1,92 @@
+// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
+
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+// Copyright 2019 FZI Forschungszentrum Informatik
+//
+// 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.
+// -- END LICENSE BLOCK ------------------------------------------------
+
+//----------------------------------------------------------------------
+/*!\file
+ *
+ * \author  Tristan Schnell schnell@fzi.de
+ * \date    2019-04-11
+ *
+ */
+//----------------------------------------------------------------------
+
+#ifndef UR_RTDE_DRIVER_REVERSE_INTERFACE_H_INCLUDED
+#define UR_RTDE_DRIVER_REVERSE_INTERFACE_H_INCLUDED
+
+#include "ur_rtde_driver/comm/server.h"
+#include "ur_rtde_driver/types.h"
+#include <cstring>
+#include <endian.h>
+
+namespace ur_driver
+{
+namespace comm
+{
+class ReverseInterface
+{
+public:
+  ReverseInterface() = delete;
+  ReverseInterface(uint32_t port) : server_(port)
+  {
+    if (!server_.bind())
+    {
+      throw std::runtime_error("Could not bind to server");
+    }
+    if (!server_.accept())
+    {
+      throw std::runtime_error("Failed to accept robot connection");
+    }
+  }
+  ~ReverseInterface() = default;
+
+  bool write(const vector6d_t& positions)
+  {
+    uint8_t buffer[sizeof(uint32_t) * 7];
+    uint8_t* b_pos = buffer;
+    for (auto const& pos : positions)
+    {
+      int32_t val = static_cast<int32_t>(pos * MULT_JOINTSTATE);
+      val = htobe32(val);
+      b_pos += append(b_pos, val);
+    }
+
+    int32_t val = htobe32(1);
+    append(b_pos, val);
+
+    size_t written;
+
+    return server_.write(buffer, sizeof(buffer), written);
+  }
+
+  template <typename T>
+  size_t append(uint8_t* buffer, T& val)
+  {
+    size_t s = sizeof(T);
+    std::memcpy(buffer, &val, s);
+    return s;
+  }
+
+private:
+  URServer server_;
+  static const int32_t MULT_JOINTSTATE = 1000000;
+};
+
+}  // namespace comm
+}  // namespace ur_driver
+
+#endif  // UR_RTDE_DRIVER_REVERSE_INTERFACE_H_INCLUDED
diff --git a/include/ur_rtde_driver/ur/server.h b/include/ur_rtde_driver/comm/server.h
similarity index 97%
rename from include/ur_rtde_driver/ur/server.h
rename to include/ur_rtde_driver/comm/server.h
index af687af..63f0fa7 100644
--- a/include/ur_rtde_driver/ur/server.h
+++ b/include/ur_rtde_driver/comm/server.h
@@ -28,6 +28,8 @@
 
 namespace ur_driver
 {
+namespace comm
+{
 #define MAX_SERVER_BUF_LEN 50
 
 class URServer : private comm::TCPSocket
@@ -49,4 +51,5 @@ public:
   bool readLine(char* buffer, size_t buf_len);
   bool write(const uint8_t* buf, size_t buf_len, size_t& written);
 };
+}  // namespace comm
 }  // namespace ur_driver
diff --git a/src/ur/server.cpp b/src/comm/server.cpp
similarity index 98%
rename from src/ur/server.cpp
rename to src/comm/server.cpp
index 40e72c0..0232f14 100644
--- a/src/ur/server.cpp
+++ b/src/comm/server.cpp
@@ -27,6 +27,8 @@
 
 namespace ur_driver
 {
+namespace comm
+{
 URServer::URServer(int port) : port_(port)
 {
 }
@@ -151,4 +153,5 @@ bool URServer::readLine(char* buffer, size_t buf_len)
   *current_pointer = '\0';
   return true;
 }
+}  // namespace comm
 }  // namespace ur_driver