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

ur_rtde_driver/CMakeLists.txt

@@ -102,6 +102,7 @@ add_library(ur_rtde_driver
     src/ur/ur_driver.cpp
     src/ur/calibration_checker.cpp
     src/ur/tool_communication.cpp
+    src/rtde/rtde_writer.cpp
 )
 target_link_libraries(ur_rtde_driver ${catkin_LIBRARIES})
 add_dependencies(ur_rtde_driver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

ur_rtde_driver/include/ur_rtde_driver/rtde/control_package_setup_inputs.h

@@ -57,7 +57,12 @@ public:
   }
   virtual ~ControlPackageSetupInputsRequest() = default;
 
+  static size_t generateSerializedRequest(uint8_t* buffer, std::vector<std::string> variable_names);
+
   std::string variable_names_;
+
+private:
+  static const PackageType PACKAGE_TYPE = PackageType::RTDE_CONTROL_PACKAGE_SETUP_INPUTS;
 };
 
 }  // namespace rtde_interface

ur_rtde_driver/include/ur_rtde_driver/rtde/rtde_client.h

@@ -39,6 +39,7 @@
 #include "ur_rtde_driver/rtde/control_package_setup_outputs.h"
 #include "ur_rtde_driver/rtde/control_package_start.h"
 #include "ur_rtde_driver/log.h"
+#include "ur_rtde_driver/rtde/rtde_writer.h"
 
 static const int UR_RTDE_PORT = 30004;
 static const std::string PIPELINE_NAME = "RTDE Data Pipeline";
@@ -51,7 +52,8 @@ class RTDEClient
 {
 public:
   RTDEClient() = delete;
-  RTDEClient(std::string robot_ip, comm::INotifier& notifier, const std::string& recipe_file);
+  RTDEClient(std::string robot_ip, comm::INotifier& notifier, const std::string& output_recipe_file,
+             const std::string& input_recipe_file);
   ~RTDEClient() = default;
   bool init();
   bool start();
@@ -74,12 +76,15 @@ public:
    */
   std::string getIP() const;
 
+  RTDEWriter& getWriter();
+
 private:
   comm::URStream<PackageHeader> stream_;
   std::vector<std::string> recipe_;
   RTDEParser parser_;
   comm::URProducer<PackageHeader> prod_;
   comm::Pipeline<PackageHeader> pipeline_;
+  RTDEWriter writer_;
 
   VersionInformation urcontrol_version_;
 

ur_rtde_driver/include/ur_rtde_driver/rtde/rtde_writer.h

@@ -0,0 +1,63 @@
+// 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
+ *
+ */
+//----------------------------------------------------------------------
+
+#ifndef UR_RTDE_DRIVER_RTDE_WRITER_H_INCLUDED
+#define UR_RTDE_DRIVER_RTDE_WRITER_H_INCLUDED
+
+#include "ur_rtde_driver/rtde/package_header.h"
+#include "ur_rtde_driver/rtde/rtde_package.h"
+#include "ur_rtde_driver/comm/stream.h"
+
+namespace ur_driver
+{
+namespace rtde_interface
+{
+class RTDEWriter
+{
+public:
+  RTDEWriter() = delete;
+  RTDEWriter(comm::URStream<PackageHeader>* stream, const std::string& recipe_file);
+  ~RTDEWriter() = default;
+  bool init(uint8_t recipe_id);
+  bool start();
+
+  bool sendSpeedSlider(double speed_slider_fraction);
+  bool sendStandardDigitalOutput(uint8_t output_pin, bool value);
+  bool sendConfigurableDigitalOutput(uint8_t output_pin, bool value);
+  bool sendToolDigitalOutput(bool value);
+  bool sendStandardAnalogOuput(uint8_t output_pin, bool value);
+
+private:
+  comm::URStream<PackageHeader>* stream_;
+  std::vector<std::string> recipe_;
+  uint8_t recipe_id_;
+};
+
+}  // namespace rtde_interface
+}  // namespace ur_driver
+
+#endif  // UR_RTDE_DRIVER_RTDE_WRITER_H_INCLUDED

ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h

@@ -32,6 +32,7 @@
 #include "ur_rtde_driver/comm/script_sender.h"
 #include "ur_rtde_driver/ur/tool_communication.h"
 #include "ur_rtde_driver/primary/robot_message/version_message.h"
+#include "ur_rtde_driver/rtde/rtde_writer.h"
 
 namespace ur_driver
 {
@@ -51,19 +52,20 @@ public:
    * \param script_file URScript file that should be sent to the robot
    * \param tool_comm_setup Configuration for using the tool communication
    */
-  UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& recipe_file,
-           std::function<void(bool)> handle_program_state, std::unique_ptr<ToolCommSetup> tool_comm_setup,
-           const std::string& calibration_checksum = "");
+  UrDriver(const std::string& robot_ip, const std::string& script_file, 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 = "");
   /*!
    * \brief Constructs a new UrDriver object.
    *
    * \param robot_ip IP-address under which the robot is reachable.
    * \param script_file URScript file that should be sent to the robot
    */
-  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& calibration_checksum = "")
-    : UrDriver(robot_ip, script_file, recipe_file, handle_program_state, std::unique_ptr<ToolCommSetup>{},
-               calibration_checksum)
+  UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file,
+           const std::string& input_recipe_file, std::function<void(bool)> handle_program_state,
+           const std::string& calibration_checksum = "")
+    : UrDriver(robot_ip, script_file, output_recipe_file, input_recipe_file, handle_program_state,
+               std::unique_ptr<ToolCommSetup>{}, calibration_checksum)
   {
   }
 
@@ -115,6 +117,8 @@ public:
 
   void checkCalibration(const std::string& checksum);
 
+  rtde_interface::RTDEWriter& getRTDEWriter();
+
 private:
   std::string readScriptFile(const std::string& filename);
   std::string readKeepalive();

ur_rtde_driver/launch/ur_control.launch

@@ -14,7 +14,8 @@
   <arg name="controllers" default="joint_state_controller scaled_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller"/>
   <arg name="stopped_controllers" default="pos_traj_controller"/>
   <arg name="urscript_file" default="$(find ur_rtde_driver)/resources/servoj.urscript"/>
-  <arg name="rtde_recipe_file" default="$(find ur_rtde_driver)/resources/rtde_recipe.txt"/>
+  <arg name="rtde_output_recipe_file" default="$(find ur_rtde_driver)/resources/rtde_output_recipe.txt"/>
+  <arg name="rtde_input_recipe_file" default="$(find ur_rtde_driver)/resources/rtde_input_recipe.txt"/>
   <arg name="tool_voltage" default="0"/>
   <arg name="tool_parity" default="0"/>
   <arg name="tool_baud_rate" default="115200"/>
@@ -29,7 +30,8 @@
     <param name="robot_ip" type="str" value="$(arg robot_ip)"/>
     <rosparam command="load" file="$(arg kinematics_config)" />
     <param name="script_file" value="$(arg urscript_file)"/>
-    <param name="recipe_file" value="$(arg rtde_recipe_file)"/>
+    <param name="output_recipe_file" value="$(arg rtde_output_recipe_file)"/>
+    <param name="input_recipe_file" value="$(arg rtde_input_recipe_file)"/>
     <param name="tf_prefix" value="$(arg tf_prefix)"/>
     <param name="use_tool_communication" value="$(arg use_tool_communication)"/>
     <!--<param name="reverse_port" type="int" value="$(arg reverse_port)" />-->

ur_rtde_driver/resources/rtde_input_recipe.txt

@@ -0,0 +1,2 @@
+speed_slider_mask
+speed_slider_fraction

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