Added support for std::vector strings in UrDriver (#408)

Since it is a functionality allowed by the RTDEClient, seems reasonable
to also add it to the UrDriver.

Author: PabloDArandaR

include/ur_client_library/example_robot_wrapper.h

@@ -76,6 +76,26 @@ class ExampleRobotWrapper
   ExampleRobotWrapper(const std::string& robot_ip, const std::string& output_recipe_file,
                       const std::string& input_recipe_file, const bool headless_mode = true,
                       const std::string& autostart_program = "", const std::string& script_file = SCRIPT_FILE);
+  /*!
+   * \brief Construct a new Example Robot Wrapper object
+   *
+   * This will connect to a robot and initialize it. In headless mode the program will be running
+   * instantly, in teach pendant mode the from \p autostart_program will be started.
+   *
+   * Note: RTDE communication has to be started separately.
+   *
+   * \param robot_ip IP address of the robot to connect to
+   * \param output_recipe Output recipe vector for RTDE communication
+   * \param input_recipe Input recipe vector for RTDE communication
+   * \param headless_mode Should the driver be started in headless mode or not?
+   * \param autostart_program Program to start automatically after initialization when not in
+   * headless mode. This flag is ignored in headless mode.
+   * \param script_file URScript file to send to the robot. That should be script code
+   * communicating to the driver's reverse interface and trajectory interface.
+   */
+  ExampleRobotWrapper(const std::string& robot_ip, const std::vector<std::string> output_recipe,
+                      const std::vector<std::string> input_recipe, const bool headless_mode = true,
+                      const std::string& autostart_program = "", const std::string& script_file = SCRIPT_FILE);
   ~ExampleRobotWrapper();
 
   /**

include/ur_client_library/rtde/rtde_client.h

@@ -222,6 +222,19 @@ class RTDEClient
     return output_recipe_;
   }
 
+  /*!
+   * \brief Getter for the RTDE input recipe.
+   *
+   * \returns The input recipe
+   */
+  std::vector<std::string> getInputRecipe()
+  {
+    return input_recipe_;
+  }
+
+  // Reads output or input recipe from a file
+  static std::vector<std::string> readRecipe(const std::string& recipe_file);
+
 private:
   comm::URStream<RTDEPackage> stream_;
   std::vector<std::string> output_recipe_;
@@ -247,9 +260,6 @@ class RTDEClient
   constexpr static const double CB3_MAX_FREQUENCY = 125.0;
   constexpr static const double URE_MAX_FREQUENCY = 500.0;
 
-  // Reads output or input recipe from a file
-  std::vector<std::string> readRecipe(const std::string& recipe_file) const;
-
   // Helper function to ensure that timestamp is present in the output recipe. The timestamp is needed to ensure that
   // the robot is booted.
   std::vector<std::string> ensureTimestampIsPresent(const std::vector<std::string>& output_recipe) const;

include/ur_client_library/ur/ur_driver.h

@@ -52,18 +52,20 @@ namespace urcl
  */
 struct UrDriverConfiguration
 {
-  std::string robot_ip;            //!< IP-address under which the robot is reachable.
-  std::string script_file;         //!< URScript file that should be sent to the robot.
-  std::string output_recipe_file;  //!< Filename where the output recipe is stored in.
-  std::string input_recipe_file;   //!< Filename where the input recipe is stored in.
+  std::string robot_ip;                    //!< IP-address under which the robot is reachable.
+  std::string script_file;                 //!< URScript file that should be sent to the robot.
+  std::string output_recipe_file;          //!< Filename where the output recipe is stored in.
+  std::string input_recipe_file;           //!< Filename where the input recipe is stored in.
+  std::vector<std::string> output_recipe;  //!< Vector with the output recipe fields.
+  std::vector<std::string> input_recipe;   //!< Vector with the input recipe fields.
 
   /*!
    * \brief Function handle to a callback on program state changes.
    *
    * For this to work, the URScript program will have to send keepalive signals to the \p
    * reverse_port.
    */
-  std::function<void(bool)> handle_program_state;
+  std::function<void(bool)> handle_program_state = nullptr;
   bool headless_mode;  //!< Parameter to control if the driver should be started in headless mode.
 
   std::unique_ptr<ToolCommSetup> tool_comm_setup = nullptr;  //!< Configuration for using the tool communication.
@@ -837,6 +839,13 @@ class UrDriver
    */
   std::vector<std::string> getRTDEOutputRecipe();
 
+  /*!
+   * \brief Getter for the RTDE input recipe used in the RTDE client.
+   *
+   * \returns The used RTDE input recipe
+   */
+  std::vector<std::string> getRTDEInputRecipe();
+
   /*!
    * \brief Set the Keepalive count. This will set the number of allowed timeout reads on the robot.
    *
@@ -987,6 +996,7 @@ class UrDriver
 private:
   void init(const UrDriverConfiguration& config);
 
+  void setupRTDEClient(const UrDriverConfiguration& config);
   void initRTDE();
   void setupReverseInterface(const uint32_t reverse_port);
 
@@ -1023,4 +1033,4 @@ class UrDriver
   VersionInformation robot_version_;
 };
 }  // namespace urcl
-#endif  // ifndef UR_CLIENT_LIBRARY_UR_UR_DRIVER_H_INCLUDED
+#endif  // ifndef UR_CLIENT_LIBRARY_UR_UR_DRIVER_H_INCLUDED

src/control/reverse_interface.cpp

@@ -47,7 +47,10 @@ ReverseInterface::ReverseInterface(const ReverseInterfaceConfig& config)
   , step_time_(config.step_time)
   , keep_alive_count_modified_deprecated_(false)
 {
-  handle_program_state_(false);
+  if (handle_program_state_)
+  {
+    handle_program_state_(false);
+  }
   server_.setMessageCallback(std::bind(&ReverseInterface::messageCallback, this, std::placeholders::_1,
                                        std::placeholders::_2, std::placeholders::_3));
   server_.setConnectCallback(std::bind(&ReverseInterface::connectionCallback, this, std::placeholders::_1));
@@ -224,7 +227,10 @@ void ReverseInterface::connectionCallback(const socket_t filedescriptor)
   {
     URCL_LOG_INFO("Robot connected to reverse interface. Ready to receive control commands.");
     client_fd_ = filedescriptor;
-    handle_program_state_(true);
+    if (handle_program_state_)
+    {
+      handle_program_state_(true);
+    }
   }
   else
   {
@@ -237,7 +243,10 @@ void ReverseInterface::disconnectionCallback(const socket_t filedescriptor)
 {
   URCL_LOG_INFO("Connection to reverse interface dropped.", filedescriptor);
   client_fd_ = INVALID_SOCKET;
-  handle_program_state_(false);
+  if (handle_program_state_)
+  {
+    handle_program_state_(false);
+  }
   for (auto handler : disconnect_callbacks_)
   {
     handler.function(filedescriptor);
@@ -251,4 +260,4 @@ void ReverseInterface::messageCallback(const socket_t filedescriptor, char* buff
 }
 
 }  // namespace control
-}  // namespace urcl
+}  // namespace urcl

src/rtde/rtde_client.cpp

@@ -664,7 +664,7 @@ bool RTDEClient::sendPause()
   throw UrException(ss.str());
 }
 
-std::vector<std::string> RTDEClient::readRecipe(const std::string& recipe_file) const
+std::vector<std::string> RTDEClient::readRecipe(const std::string& recipe_file)
 {
   std::vector<std::string> recipe;
   std::ifstream file(recipe_file);

src/ur/ur_driver.cpp

@@ -32,13 +32,16 @@
 //----------------------------------------------------------------------
 
 #include "ur_client_library/ur/ur_driver.h"
+#include "ur_client_library/rtde/rtde_client.h"
 #include "ur_client_library/control/script_reader.h"
 #include "ur_client_library/exceptions.h"
 #include "ur_client_library/helpers.h"
 #include "ur_client_library/primary/primary_parser.h"
 #include "ur_client_library/helpers.h"
 #include <memory>
 #include <sstream>
+#include <stdexcept>
+#include <filesystem>
 
 #include <ur_client_library/ur/calibration_checker.h>
 
@@ -76,14 +79,12 @@ void UrDriver::init(const UrDriverConfiguration& config)
 
   URCL_LOG_DEBUG("Initializing urdriver");
   URCL_LOG_DEBUG("Initializing RTDE client");
-  rtde_client_.reset(
-      new rtde_interface::RTDEClient(robot_ip_, notifier_, config.output_recipe_file, config.input_recipe_file));
 
   primary_client_.reset(new urcl::primary_interface::PrimaryClient(robot_ip_, notifier_));
 
   get_packet_timeout_ = non_blocking_read_ ? 0 : 100;
 
-  initRTDE();
+  setupRTDEClient(config);
   setupReverseInterface(config.reverse_port);
 
   // Figure out the ip automatically if the user didn't provide it
@@ -623,6 +624,11 @@ std::vector<std::string> UrDriver::getRTDEOutputRecipe()
   return rtde_client_->getOutputRecipe();
 }
 
+std::vector<std::string> UrDriver::getRTDEInputRecipe()
+{
+  return rtde_client_->getInputRecipe();
+}
+
 void UrDriver::setKeepaliveCount(const uint32_t count)
 {
   URCL_LOG_WARN("DEPRECATION NOTICE: Setting the keepalive count has been deprecated. Instead use the "
@@ -684,4 +690,40 @@ std::deque<urcl::primary_interface::ErrorCode> UrDriver::getErrorCodes()
 {
   return primary_client_->getErrorCodes();
 }
+
+void UrDriver::setupRTDEClient(const UrDriverConfiguration& config)
+{
+  auto output_recipe = config.output_recipe;
+  if (config.output_recipe_file.empty() && config.output_recipe.size() == 0)
+  {
+    throw UrException("Neither output recipe file nor output recipe have been defined. An output recipe is required.");
+  }
+  if (!config.output_recipe_file.empty())
+  {
+    if (config.output_recipe.size() != 0)
+    {
+      URCL_LOG_WARN("Both output recipe file and output recipe vector are  used. Defaulting to output recipe vector");
+    }
+    else
+    {
+      output_recipe = rtde_interface::RTDEClient::readRecipe(config.output_recipe_file);
+    }
+  }
+
+  auto input_recipe = config.input_recipe;
+  if (!config.input_recipe_file.empty())
+  {
+    if (config.input_recipe.size() != 0)
+    {
+      URCL_LOG_WARN("Both input recipe file and input recipe vector are  used. Defaulting to input recipe vector.");
+    }
+    else
+    {
+      input_recipe = rtde_interface::RTDEClient::readRecipe(config.input_recipe_file);
+    }
+  }
+
+  resetRTDEClient(output_recipe, input_recipe);
+}
+
 }  // namespace urcl

tests/test_ur_driver.cpp

@@ -33,13 +33,51 @@
 #include <ur_client_library/ur/dashboard_client.h>
 #include <ur_client_library/ur/ur_driver.h>
 #include <ur_client_library/example_robot_wrapper.h>
+#include <algorithm>
 #include "test_utils.h"
 
 using namespace urcl;
 
 const std::string SCRIPT_FILE = "../resources/external_control.urscript";
 const std::string OUTPUT_RECIPE = "resources/rtde_output_recipe.txt";
 const std::string INPUT_RECIPE = "resources/rtde_input_recipe.txt";
+const std::vector<std::string> OUTPUT_RECIPE_VECTOR = { "timestamp",
+                                                        "actual_qd",
+                                                        "speed_scaling",
+                                                        "target_speed_fraction",
+                                                        "runtime_state",
+                                                        "actual_TCP_force",
+                                                        "actual_TCP_pose",
+                                                        "actual_digital_input_bits",
+                                                        "actual_digital_output_bits",
+                                                        "standard_analog_input0",
+                                                        "standard_analog_input1",
+                                                        "standard_analog_output0",
+                                                        "standard_analog_output1",
+                                                        "analog_io_types",
+                                                        "tool_mode",
+                                                        "tool_analog_input_types",
+                                                        "tool_analog_input0",
+                                                        "tool_analog_input1",
+                                                        "tool_output_voltage",
+                                                        "tool_output_current",
+                                                        "tool_temperature",
+                                                        "robot_mode",
+                                                        "safety_mode",
+                                                        "robot_status_bits",
+                                                        "safety_status_bits",
+                                                        "actual_current",
+                                                        "tcp_offset" };
+const std::vector<std::string> INPUT_RECIPE_VECTOR = {
+  "speed_slider_mask",           "standard_digital_output_mask",
+  "standard_digital_output",     "configurable_digital_output_mask",
+  "configurable_digital_output", "tool_digital_output_mask",
+  "tool_digital_output",         "standard_analog_output_mask",
+  "standard_analog_output_type", "standard_analog_output_0",
+  "standard_analog_output_1"
+};
+const std::string OUTPUT_RECIPE_VECTOR_EXCLUDED_VALUE = "actual_q";
+const std::string INPUT_RECIPE_VECTOR_EXCLUDED_VALUE = "speed_slider_fraction";
 const std::string CALIBRATION_CHECKSUM = "calib_12788084448423163542";
 std::string g_ROBOT_IP = "192.168.56.101";
 bool g_HEADLESS = true;
@@ -310,6 +348,78 @@ TEST(UrDriverInitTest, setting_connection_limits_works_correctly)
   EXPECT_THROW(UrDriver ur_driver(config), UrException);
 }
 
+TEST(UrDriverInitTest, no_recipe_throws_error)
+{
+  UrDriverConfiguration config;
+  config.socket_reconnect_attempts = 1;
+  config.socket_reconnection_timeout = std::chrono::milliseconds(200);
+  config.robot_ip = g_ROBOT_IP;  // That IP address should not exist on the test network
+  config.headless_mode = g_HEADLESS;
+
+  EXPECT_THROW(UrDriver ur_driver(config), UrException);
+}
+
+TEST(UrDriverInitTest, initialization_from_vectors)
+{
+  UrDriverConfiguration config;
+  config.socket_reconnect_attempts = 1;
+  config.socket_reconnection_timeout = std::chrono::milliseconds(200);
+  config.robot_ip = g_ROBOT_IP;  // That IP address should not exist on the test network
+  config.input_recipe = INPUT_RECIPE_VECTOR;
+  config.output_recipe = OUTPUT_RECIPE_VECTOR;
+  config.headless_mode = g_HEADLESS;
+  config.script_file = SCRIPT_FILE;
+
+  EXPECT_NO_THROW(UrDriver ur_driver(config));
+}
+
+TEST(UrDriverInitTest, non_existing_output_recipe_file_throws_exception)
+{
+  UrDriverConfiguration config;
+  config.socket_reconnect_attempts = 1;
+  config.socket_reconnection_timeout = std::chrono::milliseconds(200);
+  config.robot_ip = g_ROBOT_IP;  // That IP address should not exist on the test network
+  config.input_recipe_file = INPUT_RECIPE;
+  config.output_recipe_file = " ";
+  config.headless_mode = g_HEADLESS;
+
+  EXPECT_THROW(UrDriver ur_driver(config), UrException);
+}
+
+TEST(UrDriverInitTest, non_existing_input_recipe_file_does_not_throw_exception)
+{
+  UrDriverConfiguration config;
+  config.socket_reconnect_attempts = 1;
+  config.socket_reconnection_timeout = std::chrono::milliseconds(200);
+  config.robot_ip = g_ROBOT_IP;  // That IP address should not exist on the test network
+  config.output_recipe_file = OUTPUT_RECIPE;
+  config.headless_mode = g_HEADLESS;
+  config.script_file = SCRIPT_FILE;
+
+  EXPECT_NO_THROW(UrDriver ur_driver(config));
+}
+
+TEST(UrDriverInitTest, both_recipe_file_and_vector_select_vector)
+{
+  UrDriverConfiguration config;
+  config.socket_reconnect_attempts = 1;
+  config.socket_reconnection_timeout = std::chrono::milliseconds(200);
+  config.robot_ip = g_ROBOT_IP;  // That IP address should not exist on the test network
+  config.input_recipe_file = INPUT_RECIPE;
+  config.output_recipe_file = OUTPUT_RECIPE;
+  config.input_recipe = INPUT_RECIPE_VECTOR;
+  config.output_recipe = OUTPUT_RECIPE_VECTOR;
+  config.headless_mode = g_HEADLESS;
+  config.script_file = SCRIPT_FILE;
+
+  auto driver = UrDriver(config);
+  auto output_recipe = driver.getRTDEOutputRecipe();
+  EXPECT_TRUE(std::find(output_recipe.begin(), output_recipe.end(), OUTPUT_RECIPE_VECTOR_EXCLUDED_VALUE) ==
+              output_recipe.end());
+  auto input_recipe = driver.getRTDEInputRecipe();
+  EXPECT_TRUE(std::find(input_recipe.begin(), input_recipe.end(), INPUT_RECIPE_VECTOR_EXCLUDED_VALUE) ==
+              input_recipe.end());
+}
 // TODO we should add more tests for the UrDriver class.
 
 int main(int argc, char* argv[])
@@ -332,4 +442,4 @@ int main(int argc, char* argv[])
   }
 
   return RUN_ALL_TESTS();
-}
+}