implemented script_sender and moved sending of robot program

ur_rtde_driver/include/ur_rtde_driver/comm/script_sender.h

@@ -0,0 +1,125 @@
+
+// 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-05-22
+ *
+ */
+//----------------------------------------------------------------------
+
+#ifndef UR_RTDE_DRIVER_SCRIPT_SENDER_H_INCLUDED
+#define UR_RTDE_DRIVER_SCRIPT_SENDER_H_INCLUDED
+
+#include "ur_rtde_driver/comm/server.h"
+#include "ur_rtde_driver/log.h"
+
+namespace ur_driver
+{
+namespace comm
+{
+class ScriptSender
+{
+public:
+  ScriptSender() = delete;
+  ScriptSender(uint32_t port, const std::string& program) : server_(port), script_thread_(), program_(program)
+  {
+    if (!server_.bind())
+    {
+      throw std::runtime_error("Could not bind to server");
+    }
+  }
+
+  void start()
+  {
+    script_thread_ = std::thread(&ScriptSender::runScriptSender, this);
+  }
+
+private:
+  URServer server_;
+  std::thread script_thread_;
+  std::string program_;
+
+  const std::string PROGRAM_REQUEST = std::string("request_program\n");
+
+  void runScriptSender()
+  {
+    while (true)
+    {
+      if (!server_.accept())
+      {
+        throw std::runtime_error("Failed to accept robot connection");
+      }
+      if (requestRead())
+      {
+        LOG_INFO("Robot requested program");
+        sendProgram();
+      }
+      server_.disconnectClient();
+    }
+  }
+
+  bool requestRead()
+  {
+    size_t buf_len = 1024;
+    char buffer[buf_len];
+
+    bool read_successful = server_.readLine(buffer, buf_len);
+
+    if (read_successful)
+    {
+      if (std::string(buffer) == PROGRAM_REQUEST)
+      {
+        return true;
+      }
+      else
+      {
+        LOG_WARN("Received unexpected message on script request port ");
+        return false;
+      }
+    }
+    else
+    {
+      LOG_WARN("Could not read on script request port");
+    }
+  }
+
+  void sendProgram()
+  {
+    size_t len = program_.size();
+    const uint8_t* data = reinterpret_cast<const uint8_t*>(program_.c_str());
+    size_t written;
+
+    if (server_.write(data, len, written))
+    {
+      LOG_INFO("Sent program to robot");
+    }
+    else
+    {
+      LOG_ERROR("Could not send program to robot");
+    }
+  }
+};
+
+}  // namespace comm
+}  // namespace ur_driver
+
+#endif  // UR_RTDE_DRIVER_SCRIPT_SENDER_H_INCLUDED

ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h

@@ -27,6 +27,7 @@
 
 #include "ur_rtde_driver/rtde/rtde_client.h"
 #include "ur_rtde_driver/comm/reverse_interface.h"
+#include "ur_rtde_driver/comm/script_sender.h"
 
 namespace ur_driver
 {
@@ -71,6 +72,7 @@ private:
   comm::INotifier notifier_;
   std::unique_ptr<rtde_interface::RTDEClient> rtde_client_;
   std::unique_ptr<comm::ReverseInterface> reverse_interface_;
+  std::unique_ptr<comm::ScriptSender> script_sender_;
 
   double servoj_time_;
   uint32_t servoj_gain_;

ur_rtde_driver/src/ur/ur_driver.cpp

@@ -62,7 +62,8 @@ ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& sc
   stream.connect();
   std::string local_ip = stream.getIP();
 
-  uint32_t reverse_port = 50001;  // TODO: Make this a parameter
+  uint32_t reverse_port = 50001;        // TODO: Make this a parameter
+  uint32_t script_sender_port = 50002;  // TODO: Make this a parameter
 
   std::string prog = readScriptFile(script_file);
   prog.replace(prog.find(JOINT_STATE_REPLACE), JOINT_STATE_REPLACE.length(), std::to_string(MULT_JOINTSTATE_));
@@ -72,19 +73,10 @@ ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& sc
   prog.replace(prog.find(SERVO_J_REPLACE), SERVO_J_REPLACE.length(), out.str());
   prog.replace(prog.find(SERVER_IP_REPLACE), SERVER_IP_REPLACE.length(), local_ip);
   prog.replace(prog.find(SERVER_PORT_REPLACE), SERVER_PORT_REPLACE.length(), std::to_string(reverse_port));
-  size_t len = prog.size();
-  const uint8_t* data = reinterpret_cast<const uint8_t*>(prog.c_str());
-  size_t written;
-
-  if (stream.write(data, len, written))
-  {
-    LOG_INFO("Sent program to robot");
-  }
-  else
-  {
-    LOG_ERROR("Could not send program to robot");
-  }
 
+  script_sender_.reset(new comm::ScriptSender(script_sender_port, prog));
+  script_sender_->start();
+  LOG_INFO("Created script sender");
   reverse_interface_.reset(new comm::ReverseInterface(reverse_port));
 
   LOG_INFO("Created reverse interface");