From 8a48b5019c8c598fa3f6ff70f63f112fd60fb794 Mon Sep 17 00:00:00 2001
From: Tristan Schnell <schnell@fzi.de>
Date: Tue, 30 Jul 2019 13:02:32 +0200
Subject: [PATCH] added RTDE write setup functionality to handshake

---
 .../include/ur_rtde_driver/rtde/rtde_client.h |  1 +
 ur_rtde_driver/src/rtde/rtde_client.cpp       | 19 +++++++++++++++++--
 2 files changed, 18 insertions(+), 2 deletions(-)

diff --git a/ur_rtde_driver/include/ur_rtde_driver/rtde/rtde_client.h b/ur_rtde_driver/include/ur_rtde_driver/rtde/rtde_client.h
index afcf62d..3264870 100644
--- a/ur_rtde_driver/include/ur_rtde_driver/rtde/rtde_client.h
+++ b/ur_rtde_driver/include/ur_rtde_driver/rtde/rtde_client.h
@@ -81,6 +81,7 @@ public:
 private:
   comm::URStream<PackageHeader> stream_;
   std::vector<std::string> recipe_;
+  std::vector<std::string> input_recipe_;
   RTDEParser parser_;
   comm::URProducer<PackageHeader> prod_;
   comm::Pipeline<PackageHeader> pipeline_;
diff --git a/ur_rtde_driver/src/rtde/rtde_client.cpp b/ur_rtde_driver/src/rtde/rtde_client.cpp
index 5c39e4b..9f256a1 100644
--- a/ur_rtde_driver/src/rtde/rtde_client.cpp
+++ b/ur_rtde_driver/src/rtde/rtde_client.cpp
@@ -36,6 +36,7 @@ RTDEClient::RTDEClient(std::string robot_ip, comm::INotifier& notifier, const st
                        const std::string& input_recipe_file)
   : stream_(robot_ip, UR_RTDE_PORT)
   , recipe_(readRecipe(output_recipe_file))
+  , input_recipe_(readRecipe(input_recipe_file))
   , parser_(recipe_)
   , prod_(stream_, parser_)
   , pipeline_(prod_, PIPELINE_NAME, notifier)
@@ -101,7 +102,22 @@ bool RTDEClient::init()
     size = ControlPackageSetupOutputsRequest::generateSerializedRequest(buffer, recipe_);
   }
   stream_.write(buffer, size, written);
-  return pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000));
+  pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000));
+
+  // sending input recipe
+  size = ControlPackageSetupInputsRequest::generateSerializedRequest(buffer, input_recipe_);
+  stream_.write(buffer, size, written);
+  bool success = pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000));
+  rtde_interface::ControlPackageSetupInputs* tmp_input =
+      dynamic_cast<rtde_interface::ControlPackageSetupInputs*>(package.get());
+  if (tmp_input == nullptr)
+  {
+    throw UrException("Could not setup RTDE inputs.");
+  }
+  writer_.init(tmp_input->input_recipe_id_);
+  pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000));
+
+  return success;
 }
 bool RTDEClient::start()
 {
@@ -125,7 +141,6 @@ std::vector<std::string> RTDEClient::readRecipe(const std::string& recipe_file)
   {
     recipe.push_back(line);
   }
-  recipe_ = recipe;
   return recipe;
 }