Add example documentation for RTDE client

Author: urfeex

doc/examples.rst

@@ -20,4 +20,5 @@ may be running forever until manually stopped.
    examples/instruction_executor
    examples/primary_pipeline
    examples/primary_pipeline_calibration
+   examples/rtde_client
    examples/ur_driver

doc/examples/rtde_client.rst

@@ -0,0 +1,89 @@
+:github_url: https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/examples/rtde_client.rst
+
+.. _rtde_client_example:
+
+RTDE Client example
+===================
+
+This example shows how to use the ``RTDEClient`` class for the robot's `Real-Time Data Exchange
+(RTDE) interface
+<https://www.universal-robots.com/articles/ur/interface-communication/real-time-data-exchange-rtde-guide/>`_.
+
+The RTDE client has to be initialized with a list of keys that should be streamed from the robot
+and a list of keys that should be sent to the robot. The client will then start a background thread
+establishing communication.
+
+In this example, those keys are stored in two text files relative to this repository's root:
+``rtde_input_keys.txt`` and ``rtde_output_keys.txt``. The example will read those files and use them
+to initialize the RTDE client.
+
+.. literalinclude:: ../../examples/rtde_client.cpp
+   :language: c++
+   :caption: examples/rtde_client.cpp
+   :linenos:
+   :lineno-match:
+   :start-at: const std::string OUTPUT_RECIPE
+   :end-at: const std::string INPUT_RECIPE
+
+
+Internally, the RTDE client uses the same producer / consumer architecture as show in the
+:ref:`primary_pipeline_example` example. However, it doesn't have a consumer thread, so data has to
+be read by the user to avoid the pipeline's queue from overflowing.
+
+Creating an RTDE Client
+-----------------------
+
+An RTDE client can be directly created passing the robot's IP address, a ``INotifier`` object, an
+output and an input recipe. Optionally, a communication frequency can be passed as well. If that is
+omitted, RTDE communication will be established at the robot's control frequency.
+
+.. literalinclude:: ../../examples/rtde_client.cpp
+   :language: c++
+   :caption: examples/rtde_client.cpp
+   :linenos:
+   :lineno-match:
+   :start-at: comm::INotifier notifier;
+   :end-at: my_client.init();
+
+An RTDE data package containing every key-value pair from the output recipe can be fetched using
+the ``getDataPackage()`` method. This method will block until a new package is available.
+
+
+Reading data from the RTDE client
+---------------------------------
+
+Once the RTDE client is initialized, we'll have to start communication separately. As mentioned
+above, we'll have to read data from the client once communication is started, hence we start
+communication right before a loop reading data.
+
+.. literalinclude:: ../../examples/rtde_client.cpp
+   :language: c++
+   :caption: examples/rtde_client.cpp
+   :linenos:
+   :lineno-match:
+   :start-at: // Once RTDE communication is started
+   :end-before: // Change the speed slider
+
+Writing Data to the RTDE client
+-------------------------------
+
+In this example, we use the RTDE client to oscillate the speed slider on the teach pendant between
+0 and 1. While this doesn't bear any practical use it shows how sending data to the RTDE interface
+works.
+
+To send data to the RTDE client, we can use ``RTDEWriter`` object stored in the RTDE client. This
+has methods implemented for each data type that can be sent to the robot. The input recipe used to
+initialize the RTDE client has to contain the keys necessary to send that specific data.
+
+.. literalinclude:: ../../examples/rtde_client.cpp
+   :language: c++
+   :caption: examples/rtde_client.cpp
+   :linenos:
+   :lineno-match:
+   :start-at: my_client.getWriter().sendSpeedSlider
+   :end-at: }
+
+
+.. note:: Many RTDE inputs require setting up the data key and a mask key. See the `RTDE guide
+   <https://www.universal-robots.com/articles/ur/interface-communication/real-time-data-exchange-rtde-guide/>`_
+   for more information.

examples/rtde_client.cpp

@@ -27,6 +27,7 @@
 
 #include <ur_client_library/rtde/rtde_client.h>
 
+#include <cmath>
 #include <iostream>
 #include <memory>
 #include <ctime>
@@ -40,6 +41,20 @@ const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt";
 const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt";
 const std::chrono::milliseconds READ_TIMEOUT{ 100 };
 
+void printFraction(const double fraction, const std::string& label, const size_t width = 20)
+{
+  std::cout << "\r" << label << ": [";
+  for (size_t i = 0; i < std::ceil(fraction * width); i++)
+  {
+    std::cout << "#";
+  }
+  for (size_t i = 0; i < std::floor((1.0 - fraction) * width); i++)
+  {
+    std::cout << "-";
+  }
+  std::cout << "]" << std::flush;
+}
+
 int main(int argc, char* argv[])
 {
   // Parse the ip arguments if given
@@ -56,22 +71,25 @@ int main(int argc, char* argv[])
     second_to_run = std::stoi(argv[2]);
   }
 
-  // TODO: Write good docstring for notifier
   comm::INotifier notifier;
-  rtde_interface::RTDEClient my_client(robot_ip, notifier, OUTPUT_RECIPE, INPUT_RECIPE);
+  const double rtde_frequency = 50;  // Hz
+  rtde_interface::RTDEClient my_client(robot_ip, notifier, OUTPUT_RECIPE, INPUT_RECIPE, rtde_frequency);
   my_client.init();
 
   // We will use the speed_slider_fraction as an example how to write to RTDE
   double speed_slider_fraction = 1.0;
+  double target_speed_fraction = 1.0;
   double speed_slider_increment = 0.01;
 
   // Once RTDE communication is started, we have to make sure to read from the interface buffer, as
   // otherwise we will get pipeline overflows. Therefor, do this directly before starting your main
   // loop.
   my_client.start();
 
-  unsigned long start_time = clock();
-  while (second_to_run < 0 || ((clock() - start_time) / CLOCKS_PER_SEC) < static_cast<unsigned int>(second_to_run))
+  auto start_time = std::chrono::steady_clock::now();
+  while (second_to_run <= 0 ||
+         std::chrono::duration_cast<std::chrono::seconds>(std::chrono::steady_clock::now() - start_time).count() <
+             second_to_run)
   {
     // Read latest RTDE package. This will block for READ_TIMEOUT, so the
     // robot will effectively be in charge of setting the frequency of this loop unless RTDE
@@ -81,22 +99,19 @@ int main(int argc, char* argv[])
     std::unique_ptr<rtde_interface::DataPackage> data_pkg = my_client.getDataPackage(READ_TIMEOUT);
     if (data_pkg)
     {
-      std::cout << data_pkg->toString() << std::endl;
+      // Data fields in the data package are accessed by their name. Only names present in the
+      // output recipe can be accessed. Otherwise this function will return false.
+      data_pkg->getData("target_speed_fraction", target_speed_fraction);
+      printFraction(target_speed_fraction, "target_speed_fraction");
     }
     else
     {
+      // The client isn't connected properly anymore / doesn't receive any data anymore. Stop the
+      // program.
       std::cout << "Could not get fresh data package from robot" << std::endl;
       return 1;
     }
 
-    if (!my_client.getWriter().sendSpeedSlider(speed_slider_fraction))
-    {
-      // This will happen for example, when the required keys are not configured inside the input
-      // recipe.
-      std::cout << "\033[1;31mSending RTDE data failed." << "\033[0m\n" << std::endl;
-      return 1;
-    }
-
     // Change the speed slider so that it will move between 0 and 1 all the time. This is for
     // demonstration purposes only and gains no real value.
     if (speed_slider_increment > 0)
@@ -111,6 +126,14 @@ int main(int argc, char* argv[])
       speed_slider_increment *= -1;
     }
     speed_slider_fraction += speed_slider_increment;
+
+    if (!my_client.getWriter().sendSpeedSlider(speed_slider_fraction))
+    {
+      // This will happen for example, when the required keys are not configured inside the input
+      // recipe.
+      std::cout << "\033[1;31mSending RTDE data failed." << "\033[0m\n" << std::endl;
+      return 1;
+    }
   }
 
   // Resetting the speedslider back to 100%