added RTDE writer class and required interfaces for use in the hardware interface

ur_rtde_driver/src/ros/hardware_interface.cpp

@@ -55,19 +55,25 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h
   joint_efforts_ = { { 0, 0, 0, 0, 0, 0 } };
   robot_ip_ = robot_hw_nh.param<std::string>("robot_ip", "192.168.56.101");
   std::string script_filename;
-  std::string recipe_filename;
+  std::string output_recipe_filename;
+  std::string input_recipe_filename;
   if (!robot_hw_nh.getParam("script_file", script_filename))
   {
     ROS_ERROR_STREAM("Required parameter " << robot_hw_nh.resolveName("script_file") << " not given.");
     return false;
   }
 
-  if (!robot_hw_nh.getParam("recipe_file", recipe_filename))
+  if (!robot_hw_nh.getParam("output_recipe_file", output_recipe_filename))
   {
-    ROS_ERROR_STREAM("Required parameter " << robot_hw_nh.resolveName("recipe_file") << " not given.");
+    ROS_ERROR_STREAM("Required parameter " << robot_hw_nh.resolveName("output_recipe_file") << " not given.");
     return false;
   }
 
+  if (!robot_hw_nh.getParam("input_recipe_file", input_recipe_filename))
+  {
+    ROS_ERROR_STREAM("Required parameter " << robot_hw_nh.resolveName("input_recipe_file") << " not given.");
+    return false;
+  }
   std::string tf_prefix = robot_hw_nh.param<std::string>("tf_prefix", "");
 
   program_state_pub_ = robot_hw_nh.advertise<std_msgs::Bool>("robot_program_running", 10, true);
@@ -135,7 +141,7 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h
   ROS_INFO_STREAM("Initializing urdriver");
   try
   {
-    ur_driver_.reset(new UrDriver(robot_ip_, script_filename, recipe_filename,
+    ur_driver_.reset(new UrDriver(robot_ip_, script_filename, output_recipe_filename, input_recipe_filename,
                                   std::bind(&HardwareInterface::handleRobotProgramState, this, std::placeholders::_1),
                                   std::move(tool_comm_setup), calibration_checksum));
   }

ur_rtde_driver/src/rtde/control_package_setup_inputs.cpp

@@ -46,5 +46,25 @@ std::string ControlPackageSetupInputs::toString() const
 
   return ss.str();
 }
+
+size_t ControlPackageSetupInputsRequest::generateSerializedRequest(uint8_t* buffer,
+                                                                   std::vector<std::string> variable_names)
+{
+  if (variable_names.size() == 0)
+  {
+    return 0;
+  }
+  std::string variables;
+  for (const auto& piece : variable_names)
+    variables += (piece + ",");
+  variables.pop_back();
+  uint16_t payload_size = sizeof(double) + variables.size();
+
+  size_t size = 0;
+  size += PackageHeader::serializeHeader(buffer, PACKAGE_TYPE, payload_size);
+  size += comm::PackageSerializer::serialize(buffer + size, variables);
+
+  return size;
+}
 }  // namespace rtde_interface
 }  // namespace ur_driver

ur_rtde_driver/src/rtde/rtde_client.cpp

@@ -32,12 +32,14 @@ namespace ur_driver
 {
 namespace rtde_interface
 {
-RTDEClient::RTDEClient(std::string robot_ip, comm::INotifier& notifier, const std::string& recipe_file)
+RTDEClient::RTDEClient(std::string robot_ip, comm::INotifier& notifier, const std::string& output_recipe_file,
+                       const std::string& input_recipe_file)
   : stream_(robot_ip, UR_RTDE_PORT)
-  , recipe_(readRecipe(recipe_file))
+  , recipe_(readRecipe(output_recipe_file))
   , parser_(recipe_)
   , prod_(stream_, parser_)
   , pipeline_(prod_, PIPELINE_NAME, notifier)
+  , writer_(&stream_, input_recipe_file)
   , max_frequency_(URE_MAX_FREQUENCY)
 {
 }
@@ -137,5 +139,10 @@ std::string RTDEClient::getIP() const
 {
   return stream_.getIP();
 }
+
+RTDEWriter& RTDEClient::getWriter()
+{
+  return writer_;
+}
 }  // namespace rtde_interface
 }  // namespace ur_driver

ur_rtde_driver/src/rtde/rtde_writer.cpp

@@ -0,0 +1,69 @@
+// 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-07-25
+ *
+ */
+//----------------------------------------------------------------------
+
+#include "ur_rtde_driver/rtde/rtde_writer.h"
+
+namespace ur_driver
+{
+namespace rtde_interface
+{
+RTDEWriter::RTDEWriter(comm::URStream<PackageHeader>* stream, const std::string& recipe_file) : stream_(stream)
+{
+}
+
+bool RTDEWriter::init(uint8_t recipe_id)
+{
+  return false;
+}
+bool RTDEWriter::start()
+{
+  return false;
+}
+
+bool RTDEWriter::sendSpeedSlider(double speed_slider_fraction)
+{
+  return false;
+}
+bool RTDEWriter::sendStandardDigitalOutput(uint8_t output_pin, bool value)
+{
+  return false;
+}
+bool RTDEWriter::sendConfigurableDigitalOutput(uint8_t output_pin, bool value)
+{
+  return false;
+}
+bool RTDEWriter::sendToolDigitalOutput(bool value)
+{
+  return false;
+}
+bool RTDEWriter::sendStandardAnalogOuput(uint8_t output_pin, bool value)
+{
+  return false;
+}
+
+}  // namespace rtde_interface
+}  // namespace ur_driver

ur_rtde_driver/src/ur/ur_driver.cpp

@@ -47,7 +47,8 @@ static const std::string SERVER_IP_REPLACE("{{SERVER_IP_REPLACE}}");
 static const std::string SERVER_PORT_REPLACE("{{SERVER_PORT_REPLACE}}");
 
 ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_file,
-                              const std::string& recipe_file, std::function<void(bool)> handle_program_state,
+                              const std::string& output_recipe_file, const std::string& input_recipe_file,
+                              std::function<void(bool)> handle_program_state,
                               std::unique_ptr<ToolCommSetup> tool_comm_setup, const std::string& calibration_checksum)
   : servoj_time_(0.008)
   , servoj_gain_(2000)
@@ -58,7 +59,7 @@ ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& sc
 {
   LOG_DEBUG("Initializing urdriver");
   LOG_DEBUG("Initializing RTDE client");
-  rtde_client_.reset(new rtde_interface::RTDEClient(robot_ip_, notifier_, recipe_file));
+  rtde_client_.reset(new rtde_interface::RTDEClient(robot_ip_, notifier_, output_recipe_file, input_recipe_file));
 
   LOG_INFO("Checking if calibration data matches connected robot.");
   checkCalibration(calibration_checksum);
@@ -234,4 +235,9 @@ void UrDriver::checkCalibration(const std::string& checksum)
   ROS_DEBUG_STREAM("Got calibration information from robot.");
 }
 
+rtde_interface::RTDEWriter& UrDriver::getRTDEWriter()
+{
+  return rtde_client_->getWriter();
+}
+
 }  // namespace ur_driver