From 8c9f79bc3b53ccf829961337fc7b4c479543d8ad Mon Sep 17 00:00:00 2001
From: Felix Mauch <mauch@fzi.de>
Date: Wed, 26 Jun 2019 08:46:21 +0200
Subject: [PATCH] turn around order of commands and keepalive

This way we can leave the commands empty very easily
---
 .../ur_rtde_driver/comm/reverse_interface.h   | 23 +++++++++++--------
 ur_rtde_driver/resources/servoj.urscript      | 16 ++++++++-----
 2 files changed, 23 insertions(+), 16 deletions(-)

diff --git a/ur_rtde_driver/include/ur_rtde_driver/comm/reverse_interface.h b/ur_rtde_driver/include/ur_rtde_driver/comm/reverse_interface.h
index 24a190b..d8ecfe1 100644
--- a/ur_rtde_driver/include/ur_rtde_driver/comm/reverse_interface.h
+++ b/ur_rtde_driver/include/ur_rtde_driver/comm/reverse_interface.h
@@ -57,20 +57,23 @@ public:
     server_.disconnectClient();
   }
 
-  bool write(const vector6d_t& positions)
+  bool write(const vector6d_t* positions, const int32_t type = 2)
   {
     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);
-    }
 
-    // TODO: Make this a parameter: Number of allowed missed packages
-    int32_t val = htobe32(5);
-    append(b_pos, val);
+    int32_t val = htobe32(type);
+    b_pos += append(b_pos, val);
+
+    if (positions != nullptr)
+    {
+      for (auto const& pos : *positions)
+      {
+        int32_t val = static_cast<int32_t>(pos * MULT_JOINTSTATE);
+        val = htobe32(val);
+        b_pos += append(b_pos, val);
+      }
+    }
 
     size_t written;
 
diff --git a/ur_rtde_driver/resources/servoj.urscript b/ur_rtde_driver/resources/servoj.urscript
index 0811219..f12b785 100644
--- a/ur_rtde_driver/resources/servoj.urscript
+++ b/ur_rtde_driver/resources/servoj.urscript
@@ -71,16 +71,20 @@ socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}}, "reverse_socket")
 
 thread_servo = run servoThread()
 keepalive = -2
-params_mult = socket_read_binary_integer(6+1, "reverse_socket")
-keepalive = params_mult[7]
+params_mult = socket_read_binary_integer(1+6, "reverse_socket")
+keepalive = params_mult[1]
 while keepalive > 0:
   enter_critical
   socket_send_line(1, "reverse_socket")
-  params_mult = socket_read_binary_integer(6+1, "reverse_socket", 0.02) # steptime could work as well, but does not work in simulation
+  params_mult = socket_read_binary_integer(1+6, "reverse_socket", 0.02) # steptime could work as well, but does not work in simulation
   if params_mult[0] > 0:
-    keepalive = params_mult[7]
-    q = [params_mult[1] / MULT_jointstate, params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate]
-    set_servo_setpoint(q)
+    if params_mult[1] > 1:
+      keepalive = params_mult[1]
+      q = [params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate, params_mult[7] / MULT_jointstate]
+      set_servo_setpoint(q)
+    else:
+      keepalive = 1
+    end
   else:
     keepalive = keepalive - 1
   end