diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
index d0f3be1b..a327b0ea 100644
--- a/.github/workflows/ci.yml
+++ b/.github/workflows/ci.yml
@@ -34,7 +34,7 @@ jobs:
- uses: actions/checkout@v6
- name: start ursim
run: |
- scripts/start_ursim.sh -m $ROBOT_MODEL -v $URSIM_VERSION -p $PROGRAM_FOLDER -d
+ scripts/start_ursim.sh -m $ROBOT_MODEL -v $URSIM_VERSION -p $PROGRAM_FOLDER -d -f DISABLED
env:
DOCKER_RUN_OPTS: --network ursim_net
ROBOT_MODEL: ${{matrix.env.ROBOT_MODEL}}
@@ -49,6 +49,7 @@ jobs:
CFLAGS: -g -O2 -fprofile-arcs -ftest-coverage
LDFLAGS: -fprofile-arcs -ftest-coverage
- name: build
+ id: build
run: cmake --build build --config Debug
- name: Create folder for test artifacts
run: mkdir -p test_artifacts
@@ -57,7 +58,7 @@ jobs:
env:
URSIM_VERSION: ${{matrix.env.URSIM_VERSION}}
- name: Upload test results to Codecov
- if: ${{ !cancelled() }}
+ if: ${{ !cancelled() && steps.build.outcome == 'success' }}
uses: codecov/test-results-action@v1
with:
token: ${{ secrets.CODECOV_TOKEN }}
@@ -65,10 +66,13 @@ jobs:
- name: run examples
run: ./run_examples.sh "192.168.56.101" 1
- name: install gcovr
+ if: ${{ !cancelled() && steps.build.outcome == 'success' }}
run: sudo apt-get install -y gcovr
- name: gcovr
+ if: ${{ !cancelled() && steps.build.outcome == 'success' }}
run: cd build && gcovr -r .. --gcov-ignore-parse-errors negative_hits.warn_once_per_file --exclude "../3rdparty"
- name: Upload coverage to Codecov
+ if: ${{ !cancelled() && steps.build.outcome == 'success' }}
uses: codecov/codecov-action@v5
with:
fail_ci_if_error: true
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 146b74a0..14490827 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -43,6 +43,7 @@ add_library(urcl
src/rtde/get_urcontrol_version.cpp
src/rtde/request_protocol_version.cpp
src/rtde/rtde_package.cpp
+ src/rtde/rtde_parser.cpp
src/rtde/text_message.cpp
src/rtde/rtde_client.cpp
src/ur/ur_driver.cpp
diff --git a/doc/architecture/rtde_client.rst b/doc/architecture/rtde_client.rst
index 49790647..b4582397 100644
--- a/doc/architecture/rtde_client.rst
+++ b/doc/architecture/rtde_client.rst
@@ -7,17 +7,34 @@ RTDEClient
The Real Time Data Exchange Client, ``RTDEClient``, class serves as a standalone
`RTDE `_
-client. To use the RTDE-Client, you'll have to initialize and start it separately:
+client. To use the RTDE-Client, you'll have to initialize and start it separately. When starting
+it, it can be chosen whether data should be read in a background thread or if the user has to poll
+data in each cycle.
+
+- **Background read**: When background read is enabled (``start(true)``) (default), the RTDE client
+ will start a background thread that continuously reads data from the robot. The latest data
+ package can be fetched using the ``getDataPackage()`` method. This method returns immediately
+ with the latest data package received from the robot. If no data has been received since last
+ calling this function, it will block for a specified timeout waiting for new data to arrive.
+
+- **Blocking synchronous read**: When background read is not enabled (``start(false)``), data can
+ (and has to be) fetched using the ``getDataPackageBlocking()`` method. This call waits for a new
+ data package to arrive and parses that into the passed ``DataPackage`` object. This has to be
+ called with the RTDE control frequency, as the robot will shutdown RTDE communication if data is
+ not read by the client.
+
+The following example uses the background read method to fetch data from the RTDE interface. See
+the :ref:`rtde_client_example` for an example of the blocking read method.
.. code-block:: c++
rtde_interface::RTDEClient my_client(ROBOT_IP, notifier, OUTPUT_RECIPE_FILE, INPUT_RECIPE_FILE);
my_client.init();
- my_client.start();
+ my_client.start(true); // Start background read
+ rtde_interface::DataPackage data_pkg(my_client.getOutputRecipe());
while (true)
{
- std::unique_ptr data_pkg = my_client.getDataPackage(READ_TIMEOUT);
- if (data_pkg)
+ if (my_client.getDataPackage(data_pkg, READ_TIMEOUT))
{
std::cout << data_pkg->toString() << std::endl;
}
@@ -28,27 +45,32 @@ outputs. Please refer to the `RTDE
guide `_
on which elements are available.
-.. note::
+The recipes can be either passed as a filename or as a list of strings directly. E.g. the
+following will work
- The recipes can be either passed as a filename or as a list of strings directly. E.g. the
- following will work
+.. code-block:: c++
+
+ rtde_interface::RTDEClient my_client(
+ ROBOT_IP,
+ notifier,
+ {"timestamp", "actual_q"},
+ {"speed_slider_mask", "speed_slider_fraction"}
+ );
+
+.. note::
+ ``timestamp`` will always be a part of the output recipe and will be added afterwards, if not defined. As the ``timestamp`` used for verifying the connectivity.
- .. code-block:: c++
+Reading data
+------------
- rtde_interface::RTDEClient my_client(
- ROBOT_IP,
- notifier,
- {"timestamp", "actual_q"},
- {"speed_slider_mask", "speed_slider_fraction"}
- );
+After calling ``my_client.start()``, data can be read from the
+``RTDEClient`` by calling ``getDataPackage()`` (with background thread running) or ``getDataPackageBlocking()`` (without background thread running) respectively.
-Inside the ``RTDEclient`` data is received in a separate thread, parsed by the ``RTDEParser`` and
-added to a pipeline queue.
+Remember that, when not using a background thread, data has to be polled regularly, as the robot
+will shutdown RTDE communication if the receiving side doesn't empty its buffer.
-Right after calling ``my_client.start()``, it should be made sure to read the buffer from the
-``RTDEClient`` by calling ``getDataPackage()`` frequently. The Client's queue can only contain a
-restricted number of items at a time, so a ``Pipeline producer overflowed!`` error will be raised
-if the buffer isn't read frequently enough.
+Writing data
+------------
For writing data to the RTDE interface, use the ``RTDEWriter`` member of the ``RTDEClient``. It can be
retrieved by calling ``getWriter()`` method. The ``RTDEWriter`` provides convenience methods to write
@@ -83,11 +105,11 @@ an empty input recipe, like this:
// Alternatively, pass an empty filename when using recipe files
// rtde_interface::RTDEClient my_client(ROBOT_IP, notifier, OUTPUT_RECIPE_FILE, "");
my_client.init();
+ auto data_pkg = std::make_unique(my_client->getOutputRecipe());
my_client.start();
while (true)
{
- std::unique_ptr data_pkg = my_client.getDataPackage(READ_TIMEOUT);
- if (data_pkg)
+ if (my_client.getDataPackage(data_package, READ_TIMEOUT))
{
std::cout << data_pkg->toString() << std::endl;
}
diff --git a/doc/examples/direct_torque_control.rst b/doc/examples/direct_torque_control.rst
index f4d385d4..10e52950 100644
--- a/doc/examples/direct_torque_control.rst
+++ b/doc/examples/direct_torque_control.rst
@@ -77,4 +77,4 @@ To send the control command, the robot's :ref:`reverse_interface` is used via th
:linenos:
:lineno-match:
:start-at: // Setting the RobotReceiveTimeout
- :end-before: URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg->toString().c_str());
+ :end-before: URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg.toString().c_str());
diff --git a/doc/examples/rtde_client.rst b/doc/examples/rtde_client.rst
index bf47d97b..5339e719 100644
--- a/doc/examples/rtde_client.rst
+++ b/doc/examples/rtde_client.rst
@@ -25,11 +25,6 @@ to initialize the RTDE client.
: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
-----------------------
@@ -45,25 +40,32 @@ omitted, RTDE communication will be established at the robot's control frequency
: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.
+To read data received by the RTDE client, it has to be polled. See the :ref:`rtde_client` section
+for details on two possible strategies. In this example, we do not use background read and instead
+fetch data synchronously. Hence, we pass ``false`` to the ``start()`` method.
.. literalinclude:: ../../examples/rtde_client.cpp
:language: c++
:caption: examples/rtde_client.cpp
:linenos:
:lineno-match:
- :start-at: // Once RTDE communication is started
+ :start-at: auto data_pkg = std::make_unique(my_client.getOutputRecipe());
:end-before: // Change the speed slider
+In our main loop, we wait for a new data package to arrive using the blocking read method. Once
+received, data from the received package can be accessed using the ``getData()`` method of the
+``DataPackage`` object. This method takes the key of the data to be accessed as a parameter and
+returns the corresponding value.
+
+.. note:: The key used to access data has to be part of the output recipe used to initialize the RTDE
+ client. Passing a string literal, e.g. ``"actual_q"``, is possible but not recommended as it is
+ converted to an ``std::string`` automatically, causing heap allocations which should be avoided
+ in Real-Time contexts.
+
Writing Data to the RTDE client
-------------------------------
@@ -84,6 +86,12 @@ initialize the RTDE client has to contain the keys necessary to send that specif
:end-at: }
-.. note:: Many RTDE inputs require setting up the data key and a mask key. See the `RTDE guide
+.. note:: Many RTDE inputs require setting up the data key and a mask key. That is done
+ internally, but the mask keys have to be part of the input recipe, as well. See the `RTDE guide
`_
for more information.
+
+.. note:: Every ``send...`` call to the RTDEWriter triggers a package sent to the robot. If you
+ want to modify more than one input at a time, it is recommended to use the ``sendPackage()``
+ method. That allows setting up the complete data package with its input recipe and sending that
+ to the robot at once.
diff --git a/doc/examples/ur_driver.rst b/doc/examples/ur_driver.rst
index 874c4922..0942295c 100644
--- a/doc/examples/ur_driver.rst
+++ b/doc/examples/ur_driver.rst
@@ -70,4 +70,4 @@ To send the control command, the robot's :ref:`reverse_interface` is used via th
:linenos:
:lineno-match:
:start-at: // Setting the RobotReceiveTimeout
- :end-before: URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg->toString().c_str());
+ :end-before: URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg.toString().c_str());
diff --git a/examples/direct_torque_control.cpp b/examples/direct_torque_control.cpp
index f50ce4cb..d213cc97 100644
--- a/examples/direct_torque_control.cpp
+++ b/examples/direct_torque_control.cpp
@@ -96,20 +96,22 @@ int main(int argc, char* argv[])
// loop.
g_my_robot->getUrDriver()->startRTDECommunication();
auto start_time = std::chrono::system_clock::now();
+
+ urcl::rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe());
+
while (!(passed_positive_part && passed_negative_part))
{
// Read latest RTDE package. This will block for a hard-coded timeout (see UrDriver), so the
// robot will effectively be in charge of setting the frequency of this loop.
// In a real-world application this thread should be scheduled with real-time priority in order
// to ensure that this is called in time.
- std::unique_ptr data_pkg = g_my_robot->getUrDriver()->getDataPackage();
- if (!data_pkg)
+ if (!g_my_robot->getUrDriver()->getDataPackage(data_pkg))
{
URCL_LOG_WARN("Could not get fresh data package from robot");
return 1;
}
// Read current joint positions from robot data
- if (!data_pkg->getData("actual_q", g_joint_positions))
+ if (!data_pkg.getData("actual_q", g_joint_positions))
{
// This throwing should never happen unless misconfigured
std::string error_msg = "Did not find 'actual_q' in data sent from robot. This should not happen!";
@@ -146,7 +148,7 @@ int main(int argc, char* argv[])
URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?");
return 1;
}
- URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg->toString().c_str());
+ URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg.toString().c_str());
if (second_to_run.count() > 0 && (std::chrono::system_clock::now() - start_time) > second_to_run)
{
URCL_LOG_WARN("Time limit reached, stopping movement. This is expected on a simualted robot, as it doesn't move "
diff --git a/examples/external_fts_through_rtde.cpp b/examples/external_fts_through_rtde.cpp
index f8838f09..dfd22cbb 100644
--- a/examples/external_fts_through_rtde.cpp
+++ b/examples/external_fts_through_rtde.cpp
@@ -211,11 +211,12 @@ void rtdeWorker(const int second_to_run)
vector6d_t actual_tcp_force;
auto start_time = std::chrono::steady_clock::now();
+ std::unique_ptr data_pkg =
+ std::make_unique(g_my_robot->getUrDriver()->getRTDEOutputRecipe());
while (g_RUNNING)
{
urcl::vector6d_t local_ft_vec = g_FT_VEC;
- std::unique_ptr data_pkg = g_my_robot->getUrDriver()->getDataPackage();
- if (data_pkg)
+ if (g_my_robot->getUrDriver()->getDataPackageBlocking(data_pkg))
{
// 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.
diff --git a/examples/full_driver.cpp b/examples/full_driver.cpp
index d123b523..1af6ffb6 100644
--- a/examples/full_driver.cpp
+++ b/examples/full_driver.cpp
@@ -87,20 +87,20 @@ int main(int argc, char* argv[])
// otherwise we will get pipeline overflows. Therefor, do this directly before starting your main
// loop.
g_my_robot->getUrDriver()->startRTDECommunication();
+ rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe());
while (!(passed_positive_part && passed_negative_part))
{
// Read latest RTDE package. This will block for a hard-coded timeout (see UrDriver), so the
// robot will effectively be in charge of setting the frequency of this loop.
// In a real-world application this thread should be scheduled with real-time priority in order
// to ensure that this is called in time.
- std::unique_ptr data_pkg = g_my_robot->getUrDriver()->getDataPackage();
- if (!data_pkg)
+ if (!g_my_robot->getUrDriver()->getDataPackage(data_pkg))
{
URCL_LOG_WARN("Could not get fresh data package from robot");
return 1;
}
// Read current joint positions from robot data
- if (!data_pkg->getData("actual_q", g_joint_positions))
+ if (!data_pkg.getData("actual_q", g_joint_positions))
{
// This throwing should never happen unless misconfigured
std::string error_msg = "Did not find 'actual_q' in data sent from robot. This should not happen!";
@@ -140,7 +140,7 @@ int main(int argc, char* argv[])
URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?");
return 1;
}
- URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg->toString().c_str());
+ URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg.toString().c_str());
}
g_my_robot->getUrDriver()->stopControl();
URCL_LOG_INFO("Movement done");
diff --git a/examples/rtde_client.cpp b/examples/rtde_client.cpp
index 0972aafc..d92605c5 100644
--- a/examples/rtde_client.cpp
+++ b/examples/rtde_client.cpp
@@ -39,7 +39,9 @@ using namespace urcl;
const std::string DEFAULT_ROBOT_IP = "192.168.56.101";
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 };
+
+// Preallocation of string to avoid allocation in main loop
+const std::string TARGET_SPEED_FRACTION = "target_speed_fraction";
void printFraction(const double fraction, const std::string& label, const size_t width = 20)
{
@@ -81,28 +83,27 @@ int main(int argc, char* argv[])
double target_speed_fraction = 1.0;
double speed_slider_increment = 0.01;
+ auto data_pkg = std::make_unique(my_client.getOutputRecipe());
// 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();
+ my_client.start(false); // false -> do not start background read thread.
auto start_time = std::chrono::steady_clock::now();
while (second_to_run <= 0 ||
std::chrono::duration_cast(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
- // communication doesn't work in which case the user will be notified.
- // In a real-world application this thread should be scheduled with real-time priority in order
- // to ensure that this is called in time.
- std::unique_ptr data_pkg = my_client.getDataPackage(READ_TIMEOUT);
- if (data_pkg)
+ // Wait for a DataPackage. In a real-world application this thread should be scheduled with real-time priority in
+ // order to ensure that this is called in time.
+ bool success = my_client.getDataPackageBlocking(data_pkg);
+ if (success)
{
// 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");
+ // We preallocated the string TARGET_SPEED_FRACTION to avoid allocations in the main loop.
+ data_pkg->getData(TARGET_SPEED_FRACTION, target_speed_fraction);
+ printFraction(target_speed_fraction, TARGET_SPEED_FRACTION);
}
else
{
@@ -139,5 +140,7 @@ int main(int argc, char* argv[])
// Resetting the speedslider back to 100%
my_client.getWriter().sendSpeedSlider(1);
+ URCL_LOG_INFO("Exiting RTDE read/write example.");
+
return 0;
}
diff --git a/examples/spline_example.cpp b/examples/spline_example.cpp
index a29b2677..c9303ffd 100644
--- a/examples/spline_example.cpp
+++ b/examples/spline_example.cpp
@@ -139,12 +139,12 @@ int main(int argc, char* argv[])
g_my_robot->getUrDriver()->startRTDECommunication();
- std::unique_ptr data_pkg = g_my_robot->getUrDriver()->getDataPackage();
+ rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe());
+ if (g_my_robot->getUrDriver()->getDataPackage(data_pkg))
- if (data_pkg)
{
// Read current joint positions from robot data
- if (!data_pkg->getData("actual_q", g_joint_positions))
+ if (!data_pkg.getData("actual_q", g_joint_positions))
{
// This throwing should never happen unless misconfigured
std::string error_msg = "Did not find 'actual_q' in data sent from robot. This should not happen!";
@@ -199,11 +199,10 @@ int main(int argc, char* argv[])
g_trajectory_running = true;
while (g_trajectory_running)
{
- std::unique_ptr data_pkg = g_my_robot->getUrDriver()->getDataPackage();
- if (data_pkg)
+ if (g_my_robot->getUrDriver()->getDataPackage(data_pkg))
{
// Read current joint positions from robot data
- if (!data_pkg->getData("actual_q", g_joint_positions))
+ if (!data_pkg.getData("actual_q", g_joint_positions))
{
// This throwing should never happen unless misconfigured
std::string error_msg = "Did not find 'actual_q' in data sent from robot. This should not happen!";
@@ -231,11 +230,10 @@ int main(int argc, char* argv[])
g_trajectory_running = true;
while (g_trajectory_running)
{
- std::unique_ptr data_pkg = g_my_robot->getUrDriver()->getDataPackage();
- if (data_pkg)
+ if (g_my_robot->getUrDriver()->getDataPackage(data_pkg))
{
// Read current joint positions from robot data
- if (!data_pkg->getData("actual_q", g_joint_positions))
+ if (!data_pkg.getData("actual_q", g_joint_positions))
{
// This throwing should never happen unless misconfigured
std::string error_msg = "Did not find 'actual_q' in data sent from robot. This should not happen!";
diff --git a/include/ur_client_library/comm/package.h b/include/ur_client_library/comm/package.h
index f014a37e..8668acf1 100644
--- a/include/ur_client_library/comm/package.h
+++ b/include/ur_client_library/comm/package.h
@@ -35,9 +35,9 @@ namespace urcl
namespace comm
{
/*!
- * \brief The URPackage a parent class. From that two implementations are inherited,
- * one for the primary, one for the rtde interface (primary_interface::primaryPackage;
- * rtde_interface::rtdePackage). The URPackage makes use of the template HeaderT.
+ * \brief The URPackage is an abstract class. From that two implementations are inherited,
+ * one for the primary, one for the RTDE interface (primary_interface::PrimaryPackage;
+ * rtde_interface::RTDEPackage). The URPackage makes use of the template HeaderT.
*/
template
class URPackage
diff --git a/include/ur_client_library/comm/parser.h b/include/ur_client_library/comm/parser.h
index 66d98d19..08a86894 100644
--- a/include/ur_client_library/comm/parser.h
+++ b/include/ur_client_library/comm/parser.h
@@ -41,13 +41,28 @@ class Parser
virtual ~Parser() = default;
/*!
- * \brief Declares the parse function.
+ * \brief Parses data from a binary parser into a vector of packages.
+ *
+ * This will create new packages and push them to the results vector, thus new memory will be
+ * allocated for each package. Do not use this in a real-time context.
*
* \param bp Instance of class binaryParser
- * \param results A unique pointer
+ * \param results A vector of unique pointers to packages
*/
virtual bool parse(BinParser& bp, std::vector>& results) = 0;
+ /*!
+ * \brief Parses data from a binary parser into a single package.
+ *
+ * This implementation may try to store data in the passed package instance to avoid memory
+ * allocations. Thus, this function may be used in a real-time context. Refer to the specific
+ * implementation and parser for details.
+ *
+ * \param bp Instance of class binaryParser
+ * \param result A unique pointer to a package where parsed data should be stored.
+ */
+ virtual bool parse(BinParser& bp, std::unique_ptr& result) = 0;
+
private:
typename T::HeaderType header_;
// URProducer producer_;
diff --git a/include/ur_client_library/comm/pipeline.h b/include/ur_client_library/comm/pipeline.h
index aafd0f75..3b55faab 100644
--- a/include/ur_client_library/comm/pipeline.h
+++ b/include/ur_client_library/comm/pipeline.h
@@ -245,6 +245,17 @@ class IProducer
* \returns Success of the package production.
*/
virtual bool tryGet(std::vector>& products) = 0;
+
+ /*!
+ * \brief Reads a single package from some source and produces the corresponding object.
+ *
+ * If possible this function should try to reuse existing memory in the passed unique pointer.
+ *
+ * \param product Unique pointer to be set to the produced package.
+ *
+ * \returns Success of the package production.
+ */
+ virtual bool tryGet(std::unique_ptr& product) = 0;
};
/*!
diff --git a/include/ur_client_library/comm/producer.h b/include/ur_client_library/comm/producer.h
index 19a929a7..e509d901 100644
--- a/include/ur_client_library/comm/producer.h
+++ b/include/ur_client_library/comm/producer.h
@@ -47,6 +47,57 @@ class URProducer : public IProducer
bool running_;
+ template
+ bool tryGetImpl(ProductT& product)
+ {
+ // TODO This function has become really ugly! That should be refactored!
+
+ // 4KB should be enough to hold any packet received from UR
+ uint8_t buf[4096];
+ size_t read = 0;
+ // expoential backoff reconnects
+ while (true)
+ {
+ if (stream_.read(buf, sizeof(buf), read))
+ {
+ // reset sleep amount
+ timeout_ = std::chrono::seconds(1);
+ BinParser bp(buf, read);
+ return parser_.parse(bp, product);
+ }
+
+ if (!running_)
+ return false;
+
+ if (stream_.getState() == SocketState::Connected)
+ {
+ continue;
+ }
+
+ if (stream_.closed())
+ return false;
+
+ if (on_reconnect_cb_)
+ {
+ URCL_LOG_WARN("Failed to read from stream, invoking on reconnect callback and stopping the producer");
+ on_reconnect_cb_();
+ return false;
+ }
+
+ URCL_LOG_WARN("Failed to read from stream, reconnecting in %ld seconds...", timeout_.count());
+ std::this_thread::sleep_for(timeout_);
+
+ if (stream_.connect())
+ continue;
+
+ auto next = timeout_ * 2;
+ if (next <= std::chrono::seconds(120))
+ timeout_ = next;
+ }
+
+ return false;
+ }
+
public:
/*!
* \brief Creates a URProducer object, registering a stream and a parser.
@@ -106,52 +157,22 @@ class URProducer : public IProducer
*/
bool tryGet(std::vector>& products) override
{
- // TODO This function has become really ugly! That should be refactored!
-
- // 4KB should be enough to hold any packet received from UR
- uint8_t buf[4096];
- size_t read = 0;
- // expoential backoff reconnects
- while (true)
- {
- if (stream_.read(buf, sizeof(buf), read))
- {
- // reset sleep amount
- timeout_ = std::chrono::seconds(1);
- BinParser bp(buf, read);
- return parser_.parse(bp, products);
- }
-
- if (!running_)
- return true;
-
- if (stream_.getState() == SocketState::Connected)
- {
- continue;
- }
-
- if (stream_.closed())
- return false;
-
- if (on_reconnect_cb_)
- {
- URCL_LOG_WARN("Failed to read from stream, invoking on reconnect callback and stopping the producer");
- on_reconnect_cb_();
- return false;
- }
-
- URCL_LOG_WARN("Failed to read from stream, reconnecting in %ld seconds...", timeout_.count());
- std::this_thread::sleep_for(timeout_);
-
- if (stream_.connect())
- continue;
-
- auto next = timeout_ * 2;
- if (next <= std::chrono::seconds(120))
- timeout_ = next;
- }
+ return tryGetImpl(products);
+ }
- return false;
+ /*!
+ * \brief Attempts to read byte stream from the robot and parse it as a URPackage.
+ *
+ * If supported by the parser, this function will try to reuse existing memory in the passed
+ * unique pointer.
+ *
+ * \param product Unique pointer to hold the produced package
+ *
+ * \returns Success of reading and parsing the package
+ */
+ bool tryGet(std::unique_ptr& product) override
+ {
+ return tryGetImpl(product);
}
/*!
diff --git a/include/ur_client_library/example_robot_wrapper.h b/include/ur_client_library/example_robot_wrapper.h
index 38b422e9..06c31c20 100644
--- a/include/ur_client_library/example_robot_wrapper.h
+++ b/include/ur_client_library/example_robot_wrapper.h
@@ -112,34 +112,10 @@ class ExampleRobotWrapper
*
* @param consume_data Once the RTDE client is started, it's data has to be consumed. If you
* don't actually care about that data, this class can silently consume RTDE data when `true` is
- * passed. This can be stopped and started at any time using the startConsumingRTDEData() and
- * stopConsumingRTDEData() methods.
+ * passed.
*/
void startRTDECommununication(const bool consume_data = false);
- /**
- * @brief Start consuming RTDE data in the background.
- */
- void startConsumingRTDEData();
-
- /**
- * @brief Stop consuming RTDE data in the background. Note that data has to be consumed manually
- * using readDataPackage().
- */
- void stopConsumingRTDEData();
-
- /**
- * @brief Get the latest RTDE package.
- *
- * Do not call this, while RTDE data is being consumed in the background. In doubt, call
- * stopConsumingRTDEData() before calling this function.
- *
- * @param[out] data_pkg The data package will be stored in that object
- * @return true on a successful read, false if no package can be read or when RTDE data is
- * already being consumed in the background.
- */
- bool readDataPackage(std::unique_ptr& data_pkg);
-
/**
* @brief Blocks until there is a robot program connected to the driver's reverse interface or
* until the timeout is hit.
@@ -217,9 +193,6 @@ class ExampleRobotWrapper
comm::INotifier notifier_;
std::atomic rtde_communication_started_ = false;
- std::atomic consume_rtde_packages_ = false;
- std::mutex read_package_mutex_;
- std::unique_ptr data_pkg_;
bool robot_initialized_ = false;
@@ -229,8 +202,6 @@ class ExampleRobotWrapper
std::mutex program_running_mutex_;
std::mutex program_not_running_mutex_;
- std::thread rtde_consumer_thread_;
-
bool headless_mode_;
std::string autostart_program_;
};
diff --git a/include/ur_client_library/exceptions.h b/include/ur_client_library/exceptions.h
index 2f70aa85..9554a80c 100644
--- a/include/ur_client_library/exceptions.h
+++ b/include/ur_client_library/exceptions.h
@@ -272,5 +272,52 @@ class UnexpectedResponse : public UrException
private:
std::string text_;
};
+
+class RTDEInvalidKeyException : public UrException
+{
+public:
+ explicit RTDEInvalidKeyException() : std::runtime_error("RTDE Invalid Key Exception")
+ {
+ }
+
+ explicit RTDEInvalidKeyException(const std::string& text) : std::runtime_error(text)
+ {
+ }
+
+ virtual ~RTDEInvalidKeyException() = default;
+
+ virtual const char* what() const noexcept override
+ {
+ return std::runtime_error::what();
+ }
+};
+
+class RTDEInputConflictException : public UrException
+{
+public:
+ explicit RTDEInputConflictException() : std::runtime_error("RTDE Output Conflict Exception")
+ {
+ }
+
+ explicit RTDEInputConflictException(const std::string& key)
+ : std::runtime_error("RTDE Input Conflict for key: " + key), key_(key)
+ {
+ message_ = "Variable '" + key_ +
+ "' is currently controlled by another RTDE client. The input recipe can't be used as "
+ "configured. Note: when using a fieldbus (e.g. Ethernet/IP or Modbus), all outputs are claimed by "
+ "those.";
+ }
+
+ virtual ~RTDEInputConflictException() = default;
+
+ virtual const char* what() const noexcept override
+ {
+ return message_.c_str();
+ }
+
+private:
+ std::string key_;
+ std::string message_;
+};
} // namespace urcl
#endif // ifndef UR_CLIENT_LIBRARY_EXCEPTIONS_H_INCLUDED
diff --git a/include/ur_client_library/primary/primary_parser.h b/include/ur_client_library/primary/primary_parser.h
index 8f3fbba9..e9ec70c3 100644
--- a/include/ur_client_library/primary/primary_parser.h
+++ b/include/ur_client_library/primary/primary_parser.h
@@ -58,7 +58,7 @@ class PrimaryParser : public comm::Parser
* \returns True, if the byte stream could successfully be parsed as primary packages, false
* otherwise
*/
- bool parse(comm::BinParser& bp, std::vector>& results)
+ bool parse(comm::BinParser& bp, std::vector>& results) override
{
int32_t packet_size;
RobotPackageType type;
@@ -150,6 +150,36 @@ class PrimaryParser : public comm::Parser
return true;
}
+ /**
+ * \brief Uses the given BinParser to create a single package object from the contained serialization.
+ *
+ * Note: This function assumes that the byte stream contains exactly one primary package. For
+ * packages with sub-packages this will return false.
+ *
+ * \param bp A BinParser holding one serialized primary package
+ * \param results A unique pointer to hold the created primary package object
+ *
+ * \returns True, if the byte stream could successfully be parsed as a primary package, false
+ * otherwise
+ */
+ bool parse(comm::BinParser& bp, std::unique_ptr& results) override
+ {
+ std::vector> packages;
+ if (!parse(bp, packages))
+ {
+ return false;
+ }
+
+ if (packages.size() != 1)
+ {
+ URCL_LOG_ERROR("Expected exactly one primary package, but received %zu", packages.size());
+ return false;
+ }
+
+ results = std::move(packages[0]);
+ return true;
+ }
+
private:
RobotState* stateFromType(RobotStateType type)
{
diff --git a/include/ur_client_library/rtde/data_package.h b/include/ur_client_library/rtde/data_package.h
index ce2b991d..ffd9cc7a 100644
--- a/include/ur_client_library/rtde/data_package.h
+++ b/include/ur_client_library/rtde/data_package.h
@@ -97,6 +97,7 @@ class DataPackage : public RTDEPackage
DataPackage(const std::vector& recipe, const uint16_t& protocol_version = 2)
: RTDEPackage(PackageType::RTDE_DATA_PACKAGE), recipe_(recipe), protocol_version_(protocol_version)
{
+ initEmpty();
}
virtual ~DataPackage() = default;
@@ -144,9 +145,13 @@ class DataPackage : public RTDEPackage
template
bool getData(const std::string& name, T& val) const
{
- if (data_.find(name) != data_.end())
+ const auto it =
+ std::find_if(data_.begin(), data_.end(), [&name](const std::pair& element) {
+ return element.first == name;
+ });
+ if (it != data_.end())
{
- val = std::get(data_.at(name));
+ val = std::get(it->second);
}
else
{
@@ -170,9 +175,13 @@ class DataPackage : public RTDEPackage
{
static_assert(sizeof(T) * 8 >= N, "Bitset is too large for underlying variable");
- if (data_.find(name) != data_.end())
+ const auto it =
+ std::find_if(data_.begin(), data_.end(), [&name](const std::pair& element) {
+ return element.first == name;
+ });
+ if (it != data_.end())
{
- val = std::bitset(std::get(data_.at(name)));
+ val = std::bitset(std::get(it->second));
}
else
{
@@ -192,11 +201,22 @@ class DataPackage : public RTDEPackage
* \returns True on success, false if the field cannot be found inside the package.
*/
template
- bool setData(const std::string& name, T& val)
+ bool setData(const std::string& name, const T& val)
{
- if (data_.find(name) != data_.end())
+ const auto it =
+ std::find_if(data_.begin(), data_.end(), [&name](const std::pair& element) {
+ return element.first == name;
+ });
+ if (it != data_.end())
{
- data_.at(name) = val;
+ if (!std::holds_alternative(it->second))
+ {
+ // TODO: It might be better to replace the return type by void and use exceptions for the
+ // error case.
+ URCL_LOG_ERROR("Type of passed data doesn't match type of existing field for index '%s'", name.c_str());
+ return false;
+ }
+ it->second = val;
}
else
{
@@ -219,7 +239,7 @@ class DataPackage : public RTDEPackage
// Const would be better here
static std::unordered_map g_type_list;
uint8_t recipe_id_;
- std::unordered_map data_;
+ std::vector> data_;
std::vector recipe_;
uint16_t protocol_version_;
};
diff --git a/include/ur_client_library/rtde/rtde_client.h b/include/ur_client_library/rtde/rtde_client.h
index cad07618..92d3693a 100644
--- a/include/ur_client_library/rtde/rtde_client.h
+++ b/include/ur_client_library/rtde/rtde_client.h
@@ -31,21 +31,14 @@
#include
-#include "ur_client_library/comm/pipeline.h"
-#include "ur_client_library/rtde/package_header.h"
-#include "ur_client_library/rtde/rtde_package.h"
-#include "ur_client_library/comm/stream.h"
-#include "ur_client_library/rtde/rtde_parser.h"
#include "ur_client_library/comm/producer.h"
+#include "ur_client_library/comm/stream.h"
#include "ur_client_library/rtde/data_package.h"
-#include "ur_client_library/rtde/request_protocol_version.h"
-#include "ur_client_library/rtde/control_package_setup_outputs.h"
-#include "ur_client_library/rtde/control_package_start.h"
-#include "ur_client_library/log.h"
+#include "ur_client_library/rtde/rtde_package.h"
+#include "ur_client_library/rtde/rtde_parser.h"
#include "ur_client_library/rtde/rtde_writer.h"
static const int UR_RTDE_PORT = 30004;
-static const std::string PIPELINE_NAME = "RTDE Data Pipeline";
namespace urcl
{
@@ -97,11 +90,11 @@ class RTDEClient
public:
RTDEClient() = delete;
/*!
- * \brief Creates a new RTDEClient object, including a used URStream and Pipeline to handle the
+ * \brief Creates a new RTDEClient object, including a used URStream and Producer to handle the
* communication with the robot.
*
* \param robot_ip The IP of the robot
- * \param notifier The notifier to use in the pipeline
+ * \param notifier The notifier to notify of start and stop events
* \param output_recipe_file Path to the file containing the output recipe
* \param input_recipe_file Path to the file containing the input recipe
* \param target_frequency Frequency to run at. Defaults to 0.0 which means maximum frequency.
@@ -114,11 +107,11 @@ class RTDEClient
bool ignore_unavailable_outputs = false, const uint32_t port = UR_RTDE_PORT);
/*!
- * \brief Creates a new RTDEClient object, including a used URStream and Pipeline to handle the
+ * \brief Creates a new RTDEClient object, including a used URStream and Producer to handle the
* communication with the robot.
*
* \param robot_ip The IP of the robot
- * \param notifier The notifier to use in the pipeline
+ * \param notifier The notifier to notify of start and stop events
* \param output_recipe Vector containing the output recipe
* \param input_recipe Vector containing the input recipe
* \param target_frequency Frequency to run at. Defaults to 0.0 which means maximum frequency.
@@ -130,6 +123,7 @@ class RTDEClient
const std::vector& input_recipe, double target_frequency = 0.0,
bool ignore_unavailable_outputs = false, const uint32_t port = UR_RTDE_PORT);
~RTDEClient();
+
/*!
* \brief Sets up RTDE communication with the robot. The handshake includes negotiation of the
* used protocol version and setting of input and output recipes.
@@ -152,24 +146,77 @@ class RTDEClient
/*!
* \brief Triggers the robot to start sending RTDE data packages in the negotiated format.
*
+ * \param read_packages_in_background Whether to start a background thread to read packages from.
+ * If packages are read in the background, getDataPackage() will return the latest one received.
+ * If packages aren't read in the background, the application is required to call
+ * getDataPackageBlocking() frequently.
+ *
* \returns Success of the requested start
*/
- bool start();
+ bool start(const bool read_packages_in_background = true);
+
/*!
* \brief Pauses RTDE data package communication
*
* \returns Whether the RTDE data package communication was paused successfully
*/
bool pause();
+
/*!
- * \brief Reads the pipeline to fetch the next data package.
+ * \brief Return the latest data package received
+ *
+ * \deprecated This method allocates memory on each call. Please use the overload which takes a
+ * reference to an existing DataPackage instead. This function will be removed in May 2027.
+ *
+ * When packages are read from the background thread, this will return the latest data package
+ * received from the robot. When no new data has been received since the last call to this
+ * function, it will wait for the time specified in the \p timeout parameter.
+ *
+ * When packages are not read from the background thread, this function will return nullptr and
+ * print an error message.
*
* \param timeout Time to wait if no data package is currently in the queue
*
* \returns Unique ptr to the package, if a package was fetched successfully, nullptr otherwise
*/
+ [[deprecated("This method allocates memory on each call. Please use the overload which takes a reference to an "
+ "existing DataPackage instead. This function will be removed in May 2027.")]]
std::unique_ptr getDataPackage(std::chrono::milliseconds timeout);
+ /*!
+ * \brief Return the latest data package received
+ *
+ * When packages are read from the background thread, the latest data package
+ * received from the robot can be fetched with this. When no new data has been received since the last call to this
+ * function, it will wait for the time specified in the \p timeout parameter.
+ *
+ * When packages are not read from the background thread, this function will return false and
+ * print an error message.
+ *
+ * \param data_package Reference to a DataPackage where the received data package will be stored
+ * if a package was fetched successfully.
+ * \param timeout Time to wait if no data package is currently in the queue
+ *
+ * \returns Whether a data package was received successfully
+ */
+ bool getDataPackage(DataPackage& data_package, std::chrono::milliseconds timeout);
+ bool getDataPackage(std::unique_ptr& data_package, std::chrono::milliseconds timeout);
+
+ /*!
+ * \brief Blocking call to get the next data package received from the robot.
+ *
+ * This function will block until a new data package is received from the robot and return it.
+ *
+ * \param data_package Reference to a unique ptr where the received data package will be stored.
+ * For optimal performance, the data package pointer should contain a pre-allocated data package
+ * that was initialized with the same output recipe as used in this RTDEClient. If it is not an
+ * initialized data package, a new one will be allocated internally which will have a negative
+ * performance impact and print a warning.
+ *
+ * \returns Whether a data package was received successfully
+ */
+ bool getDataPackageBlocking(std::unique_ptr& data_package);
+
/*!
* \brief Getter for the maximum frequency the robot can publish RTDE data packages with.
*
@@ -235,7 +282,8 @@ class RTDEClient
return input_recipe_;
}
- // Reads output or input recipe from a file
+ /// Reads output or input recipe from a file and parses it into a vector of strings where each
+ /// string is a line from the file.
static std::vector readRecipe(const std::string& recipe_file);
ClientState getClientState() const
@@ -243,7 +291,24 @@ class RTDEClient
return client_state_;
}
-private:
+ /*! \brief Starts a background thread to read data packages from the robot.
+ *
+ * After calling this function, getDataPackage() can be used to get the latest data package
+ * received from the robot.
+ */
+ void startBackgroundRead();
+
+ /*! \brief Stops the background thread reading data packages from the robot.
+ *
+ * After calling this function, getDataPackageBlocking() must be used to get data packages
+ * from the robot.
+ *
+ * \note When getDataPackageBlocking() is not called frequently enough, the internal buffer
+ * of received data packages might fill up, causing the robot to shutdown the RTDE connection.
+ */
+ void stopBackgroundRead();
+
+protected:
comm::URStream stream_;
std::vector output_recipe_;
bool ignore_unavailable_outputs_;
@@ -251,7 +316,6 @@ class RTDEClient
RTDEParser parser_;
std::unique_ptr> prod_;
comm::INotifier notifier_;
- std::unique_ptr> pipeline_;
RTDEWriter writer_;
std::atomic reconnecting_;
std::atomic stop_reconnection_;
@@ -263,8 +327,21 @@ class RTDEClient
double max_frequency_;
double target_frequency_;
+ std::unique_ptr data_buffer0_;
+ std::unique_ptr data_buffer1_;
+ std::mutex read_mutex_;
+ std::mutex write_mutex_;
+ std::atomic new_data_ = false;
+ std::atomic background_read_running_ = false;
+ std::thread background_read_thread_;
+ std::condition_variable background_read_cv_;
+
+ DataPackage preallocated_data_pkg_;
+
ClientState client_state_;
+ uint16_t protocol_version_;
+
constexpr static const double CB3_MAX_FREQUENCY = 125.0;
constexpr static const double URE_MAX_FREQUENCY = 500.0;
@@ -277,7 +354,7 @@ class RTDEClient
uint16_t negotiateProtocolVersion();
bool queryURControlVersion();
void setTargetFrequency();
- bool setupOutputs(const uint16_t protocol_version);
+ bool setupOutputs();
bool setupInputs();
void disconnect();
@@ -313,6 +390,8 @@ class RTDEClient
std::chrono::milliseconds reconnection_timeout_;
size_t max_initialization_attempts_;
std::chrono::milliseconds initialization_timeout_;
+
+ void backgroundReadThreadFunc();
};
} // namespace rtde_interface
diff --git a/include/ur_client_library/rtde/rtde_package.h b/include/ur_client_library/rtde/rtde_package.h
index 23156b99..08019aa1 100644
--- a/include/ur_client_library/rtde/rtde_package.h
+++ b/include/ur_client_library/rtde/rtde_package.h
@@ -61,6 +61,7 @@ class RTDEPackage : public comm::URPackage
* \returns True, if the package was parsed successfully, false otherwise
*/
virtual bool parseWith(comm::BinParser& bp);
+
/*!
* \brief Produces a human readable representation of the package object.
*
@@ -68,6 +69,14 @@ class RTDEPackage : public comm::URPackage
*/
virtual std::string toString() const;
+ /*!
+ * \brief Returns the type of the RTDE package.
+ */
+ PackageType getType() const
+ {
+ return type_;
+ }
+
protected:
std::unique_ptr buffer_;
size_t buffer_length_;
diff --git a/include/ur_client_library/rtde/rtde_parser.h b/include/ur_client_library/rtde/rtde_parser.h
index 7ea4d131..9c97827b 100644
--- a/include/ur_client_library/rtde/rtde_parser.h
+++ b/include/ur_client_library/rtde/rtde_parser.h
@@ -56,6 +56,21 @@ class RTDEParser : public comm::Parser
}
virtual ~RTDEParser() = default;
+ /*!
+ * \brief Uses the given BinParser to fill single package object from the contained serialization.
+ *
+ *
+ * \param bp A BinParser holding a serialized RTDE package
+ * \param result A pointer to the created RTDE package object. Ideally, the passed \p result is a pre-allocated
+ * package of the type expected to be read. For example, when RTDE communication has been setup it enters the data
+ * communication phase, where the expected package is a DataPackage. If the package content inside the \p bp object
+ * being doesn't match the result package's type or if the \p result is a nullptr, a new package will be allocated.
+ *
+ * \returns True, if the byte stream could successfully be parsed as an RTDE package, false
+ * otherwise
+ */
+ bool parse(comm::BinParser& bp, std::unique_ptr& result) override;
+
/*!
* \brief Uses the given BinParser to create package objects from the contained serialization.
*
@@ -65,93 +80,25 @@ class RTDEParser : public comm::Parser
* \returns True, if the byte stream could successfully be parsed as RTDE packages, false
* otherwise
*/
- bool parse(comm::BinParser& bp, std::vector>& results)
+ [[deprecated("This method allocates memory on each call. Please use the overload which takes a single unique ptr to "
+ "a pre-allocated package. This function will be removed in May 2027.")]]
+ bool parse(comm::BinParser& bp, std::vector>& results) override;
+ void setProtocolVersion(uint16_t protocol_version)
{
- PackageHeader::_package_size_type size;
- PackageType type;
- bp.parse(size);
- bp.parse(type);
-
- if (!bp.checkSize(size - sizeof(size) - sizeof(type)))
- {
- URCL_LOG_ERROR("Buffer len shorter than expected packet length");
- return false;
- }
-
- switch (type)
- {
- case PackageType::RTDE_DATA_PACKAGE:
- {
- std::unique_ptr package(new DataPackage(recipe_, protocol_version_));
-
- if (!package->parseWith(bp))
- {
- URCL_LOG_ERROR("Package parsing of type %d failed!", static_cast(type));
- return false;
- }
- results.push_back(std::move(package));
- break;
- }
- default:
- {
- std::unique_ptr package(packageFromType(type));
- if (!package->parseWith(bp))
- {
- URCL_LOG_ERROR("Package parsing of type %d failed!", static_cast(type));
- return false;
- }
-
- results.push_back(std::move(package));
- break;
- }
- }
- if (!bp.empty())
- {
- URCL_LOG_ERROR("Package of type %d was not parsed completely!", static_cast(type));
- bp.debug();
- return false;
- }
-
- return true;
+ protocol_version_ = protocol_version;
}
- void setProtocolVersion(uint16_t protocol_version)
+ uint16_t getProtocolVersion() const
{
- protocol_version_ = protocol_version;
+ return protocol_version_;
}
private:
std::vector recipe_;
- RTDEPackage* packageFromType(PackageType type)
- {
- switch (type)
- {
- case PackageType::RTDE_TEXT_MESSAGE:
- return new TextMessage(protocol_version_);
- break;
- case PackageType::RTDE_GET_URCONTROL_VERSION:
- return new GetUrcontrolVersion;
- break;
- case PackageType::RTDE_REQUEST_PROTOCOL_VERSION:
- return new RequestProtocolVersion;
- break;
- case PackageType::RTDE_CONTROL_PACKAGE_PAUSE:
- return new ControlPackagePause;
- break;
- case PackageType::RTDE_CONTROL_PACKAGE_SETUP_INPUTS:
- return new ControlPackageSetupInputs;
- break;
- case PackageType::RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS:
- return new ControlPackageSetupOutputs(protocol_version_);
- break;
- case PackageType::RTDE_CONTROL_PACKAGE_START:
- return new ControlPackageStart;
- break;
- default:
- return new RTDEPackage(type);
- }
- }
+ PackageType getPackageTypeFromHeader(comm::BinParser& bp) const;
+ RTDEPackage* createNewPackageFromType(PackageType type) const;
+
uint16_t protocol_version_;
};
diff --git a/include/ur_client_library/rtde/rtde_writer.h b/include/ur_client_library/rtde/rtde_writer.h
index d5af8e5d..8b482454 100644
--- a/include/ur_client_library/rtde/rtde_writer.h
+++ b/include/ur_client_library/rtde/rtde_writer.h
@@ -33,8 +33,8 @@
#include "ur_client_library/rtde/rtde_package.h"
#include "ur_client_library/rtde/data_package.h"
#include "ur_client_library/comm/stream.h"
-#include "ur_client_library/queue/readerwriterqueue.h"
#include "ur_client_library/ur/datatypes.h"
+#include
#include
#include
@@ -89,6 +89,18 @@ class RTDEWriter
*/
void stop();
+ /*!
+ * \brief Sends a complete RTDEPackage to the robot.
+ *
+ * Use this if multiple values need to be sent at once. When using the other provided functions,
+ * an RTDE data package will be sent each time.
+ *
+ * \param package The package to send
+ *
+ * \returns Success of the package creation
+ */
+ bool sendPackage(const DataPackage& package);
+
/*!
* \brief Creates a package to request setting a new value for the speed slider.
*
@@ -181,15 +193,28 @@ class RTDEWriter
bool sendExternalForceTorque(const vector6d_t& external_force_torque);
private:
+ void resetMasks(const std::shared_ptr& buffer);
+ void markStorageToBeSent();
+
uint8_t pinToMask(uint8_t pin);
comm::URStream* stream_;
std::vector recipe_;
uint8_t recipe_id_;
- moodycamel::BlockingReaderWriterQueue> queue_;
+ std::shared_ptr data_buffer0_;
+ std::shared_ptr data_buffer1_;
+ std::shared_ptr current_store_buffer_;
+ std::shared_ptr current_send_buffer_;
+ std::vector used_masks_;
std::thread writer_thread_;
- bool running_;
- DataPackage package_;
- std::mutex package_mutex_;
+ std::atomic running_;
+ std::mutex store_mutex_;
+ std::atomic new_data_available_;
+ std::condition_variable data_available_cv_;
+
+ static const std::vector g_preallocated_input_bit_register_keys;
+ static const std::vector g_preallocated_input_int_register_keys;
+ static const std::vector g_preallocated_input_double_register_keys;
+ static const std::vector initializePreallocatedKeys(const std::string& prefix, const size_t size);
};
} // namespace rtde_interface
diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h
index e61a8e07..5d4c0425 100644
--- a/include/ur_client_library/ur/ur_driver.h
+++ b/include/ur_client_library/ur/ur_driver.h
@@ -444,11 +444,46 @@ class UrDriver
* \brief Access function to receive the latest data package sent from the robot through RTDE
* interface.
*
+ * \deprecated This method allocates memory on each call. Please use the overload which takes a
+ * reference to an existing DataPackage instead. This function will be removed in May 2027.
+ *
* \returns The latest data package on success, a nullptr if no package can be found inside a preconfigured time
* window.
*/
+ [[deprecated("This method allocates memory on each call. Please use the overload which takes a reference to an "
+ "existing DataPackage instead. This function will be removed in May 2027.")]]
std::unique_ptr getDataPackage();
+ /*!
+ * \brief Return the latest RTDE data package received
+ *
+ * When packages are read from the background thread, the latest data package
+ * received from the robot can be fetched with this. *
+ * When packages are not read from the background thread, this function will return false and
+ * print an error message.
+ *
+ * \param data_package Reference to a DataPackage where the received data package will be stored
+ * if a package was fetched successfully.
+ *
+ * \returns Whether a data package was received successfully
+ */
+ bool getDataPackage(rtde_interface::DataPackage& data_package);
+
+ /*!
+ * \brief Blocking call to get the next data package received from the robot.
+ *
+ * This function will block until a new data package is received from the robot and return it.
+ *
+ * \param data_package Reference to a unique ptr where the received data package will be stored.
+ * For optimal performance, the data package pointer should contain a pre-allocated data package
+ * that was initialized with the same output recipe as used in this RTDEClient. If it is not an
+ * initialized data package, a new one will be allocated internally which will have a negative
+ * performance impact and print a warning.
+ *
+ * \returns Whether a data package was received successfully
+ */
+ bool getDataPackageBlocking(std::unique_ptr& data_package);
+
uint32_t getControlFrequency() const
{
return rtde_client_->getTargetFrequency();
@@ -778,8 +813,12 @@ class UrDriver
*
* After initialization, the cyclic RTDE communication is not started automatically, so that data
* consumers can be started also at a later point.
+ *
+ * \param read_packages_in_background If true, RTDE packages will be read in a background thread.
+ * In this case use getDataPackage() to receive the latest package. If set to false,
+ * getDataPackageBlocking() has to be called frequently.
*/
- void startRTDECommunication();
+ void startRTDECommunication(const bool read_packages_in_background = true);
/*!
* \brief Sends a stop command to the socket interface which will signal the program running on
@@ -1004,6 +1043,26 @@ class UrDriver
return primary_client_;
}
+ /*! \brief Enable or disable background reading of RTDE packages.
+ *
+ * When enabled, RTDE packages will be read in a background thread.
+ * In this case use getDataPackage() to receive the latest package. If set to false,
+ * getDataPackageBlocking() has to be called frequently if RTDE communication is active.
+ *
+ * \param enabled Whether background reading should be enabled or disabled.
+ */
+ void setRTDEBackgroundReadEnabled(const bool enabled)
+ {
+ if (enabled)
+ {
+ rtde_client_->startBackgroundRead();
+ }
+ else
+ {
+ rtde_client_->stopBackgroundRead();
+ }
+ }
+
private:
void init(const UrDriverConfiguration& config);
@@ -1044,4 +1103,4 @@ class UrDriver
VersionInformation robot_version_;
};
} // namespace urcl
-#endif // ifndef UR_CLIENT_LIBRARY_UR_UR_DRIVER_H_INCLUDED
\ No newline at end of file
+#endif // ifndef UR_CLIENT_LIBRARY_UR_UR_DRIVER_H_INCLUDED
diff --git a/run_examples.sh b/run_examples.sh
index 2b91e3de..91d53786 100755
--- a/run_examples.sh
+++ b/run_examples.sh
@@ -23,7 +23,7 @@ if [[ -f "$file" && -x "$file" ]]; then
exit 1
fi
# Delay for 10 seconds to avoid too fast reconnects
- echo "Sleep 10"
- sleep 10
+ echo "Sleep 1"
+ sleep 1
fi
done
diff --git a/scripts/start_ursim.sh b/scripts/start_ursim.sh
index c0d38583..d7f49f94 100755
--- a/scripts/start_ursim.sh
+++ b/scripts/start_ursim.sh
@@ -54,7 +54,7 @@ help()
echo " -n Name of the docker container. Defaults to '$CONTAINER_NAME'"
echo " -i IP address the container should get. Defaults to $IP_ADDRESS"
echo " -d Detached mode - start in background"
- echo " -f Specify port forwarding to use. Defaults to '$PORT_FORWARDING'. Set to empty string to disable port forwarding."
+ echo " -f Specify port forwarding to use. Defaults to '$PORT_FORWARDING'. Set to \"DISABLED\" to disable port forwarding."
echo " -h Print this Help."
echo
}
@@ -431,6 +431,11 @@ main() {
fi
fi
+ if [ "$PORT_FORWARDING" == "DISABLED" ]; then
+ echo "Port forwarding disabled"
+ PORT_FORWARDING=""
+ fi
+
DOCKER_ARGS=""
if [ "$ROBOT_SERIES" == "polyscopex" ]; then
diff --git a/src/example_robot_wrapper.cpp b/src/example_robot_wrapper.cpp
index d5ba33d9..fed64779 100644
--- a/src/example_robot_wrapper.cpp
+++ b/src/example_robot_wrapper.cpp
@@ -98,10 +98,6 @@ ExampleRobotWrapper::ExampleRobotWrapper(const std::string& robot_ip, const std:
ExampleRobotWrapper::~ExampleRobotWrapper()
{
- if (rtde_communication_started_)
- {
- stopConsumingRTDEData();
- }
}
bool ExampleRobotWrapper::clearProtectiveStop()
@@ -236,55 +232,9 @@ void ExampleRobotWrapper::startRTDECommununication(const bool consume_data)
{
if (!rtde_communication_started_)
{
- ur_driver_->startRTDECommunication();
+ ur_driver_->startRTDECommunication(consume_data);
rtde_communication_started_ = true;
}
- if (consume_data)
- {
- startConsumingRTDEData();
- }
-}
-
-void ExampleRobotWrapper::startConsumingRTDEData()
-{
- consume_rtde_packages_ = true;
- rtde_consumer_thread_ = std::thread([this]() {
- while (consume_rtde_packages_)
- {
- // Consume package to prevent pipeline overflow
- std::lock_guard lk(read_package_mutex_);
- data_pkg_ = ur_driver_->getDataPackage();
- }
- });
-}
-
-void ExampleRobotWrapper::stopConsumingRTDEData()
-{
- if (consume_rtde_packages_)
- {
- consume_rtde_packages_ = false;
- if (rtde_consumer_thread_.joinable())
- {
- rtde_consumer_thread_.join();
- }
- }
-}
-
-bool ExampleRobotWrapper::readDataPackage(std::unique_ptr& data_pkg)
-{
- if (consume_rtde_packages_ == true)
- {
- URCL_LOG_ERROR("Unable to read packages while consuming, this should not happen!");
- return false;
- }
- std::lock_guard lk(read_package_mutex_);
- data_pkg = ur_driver_->getDataPackage();
- if (data_pkg == nullptr)
- {
- URCL_LOG_ERROR("Timed out waiting for a new package from the robot");
- return false;
- }
- return true;
}
bool ExampleRobotWrapper::waitForProgramRunning(int milliseconds)
diff --git a/src/primary/robot_message.cpp b/src/primary/robot_message.cpp
index 33a3ab96..0e3fb2ce 100644
--- a/src/primary/robot_message.cpp
+++ b/src/primary/robot_message.cpp
@@ -46,6 +46,7 @@ bool RobotMessage::consumeWith(AbstractPrimaryConsumer& consumer)
std::string RobotMessage::toString() const
{
std::stringstream ss;
+ ss << "RobotMessage: " << std::endl;
ss << "timestamp: " << timestamp_ << std::endl;
ss << "source: " << static_cast(source_) << std::endl;
ss << "message_type: " << static_cast(message_type_) << std::endl;
diff --git a/src/rtde/data_package.cpp b/src/rtde/data_package.cpp
index 055c0fd5..4d6b8112 100644
--- a/src/rtde/data_package.cpp
+++ b/src/rtde/data_package.cpp
@@ -462,13 +462,16 @@ std::unordered_map DataPackage::g_
void rtde_interface::DataPackage::initEmpty()
{
+ data_.clear();
+ data_.reserve(recipe_.size());
for (auto& item : recipe_)
{
- if (g_type_list.find(item) != g_type_list.end())
+ if (g_type_list.find(item) == g_type_list.end())
{
- _rtde_type_variant entry = g_type_list[item];
- data_[item] = entry;
+ throw RTDEInvalidKeyException("Unknown item in recipe: " + item);
}
+ _rtde_type_variant entry = g_type_list[item];
+ data_.push_back({ item, entry });
}
}
@@ -478,18 +481,9 @@ bool rtde_interface::DataPackage::parseWith(comm::BinParser& bp)
{
bp.parse(recipe_id_);
}
- for (auto& item : recipe_)
+ for (size_t i = 0; i < recipe_.size(); ++i)
{
- if (g_type_list.find(item) != g_type_list.end())
- {
- _rtde_type_variant entry = g_type_list[item];
- std::visit([&bp](auto&& arg) { bp.parse(arg); }, entry);
- data_[item] = entry;
- }
- else
- {
- return false;
- }
+ std::visit([&bp](auto&& arg) { bp.parse(arg); }, data_[i].second);
}
return true;
}
@@ -500,7 +494,14 @@ std::string rtde_interface::DataPackage::toString() const
for (auto& item : data_)
{
ss << item.first << ": ";
- std::visit([&ss](auto&& arg) { ss << arg; }, item.second);
+ if (std::holds_alternative(item.second))
+ {
+ ss << int(std::get(item.second));
+ }
+ else
+ {
+ std::visit([&ss](auto&& arg) { ss << arg; }, item.second);
+ }
ss << std::endl;
}
return ss.str();
@@ -517,11 +518,11 @@ size_t rtde_interface::DataPackage::serializePackage(uint8_t* buffer)
size_t size = 0;
size += PackageHeader::serializeHeader(buffer, PackageType::RTDE_DATA_PACKAGE, payload_size);
size += comm::PackageSerializer::serialize(buffer + size, recipe_id_);
- for (auto& item : recipe_)
+ for (size_t i = 0; i < data_.size(); ++i)
{
size += std::visit(
[&buffer, &size](auto&& arg) -> size_t { return comm::PackageSerializer::serialize(buffer + size, arg); },
- data_[item]);
+ data_[i].second);
}
return size;
diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp
index ab8e0d7e..9adeccd3 100644
--- a/src/rtde/rtde_client.cpp
+++ b/src/rtde/rtde_client.cpp
@@ -29,6 +29,7 @@
#include "ur_client_library/rtde/rtde_client.h"
#include "ur_client_library/exceptions.h"
#include "ur_client_library/log.h"
+#include "ur_client_library/rtde/data_package.h"
#include
#include
#include
@@ -46,12 +47,12 @@ RTDEClient::RTDEClient(std::string robot_ip, comm::INotifier& notifier, const st
, parser_(output_recipe_)
, prod_(std::make_unique>(stream_, parser_))
, notifier_(notifier)
- , pipeline_(std::make_unique>(*prod_, PIPELINE_NAME, notifier, true))
, writer_(&stream_, input_recipe_)
, reconnecting_(false)
, stop_reconnection_(false)
, max_frequency_(URE_MAX_FREQUENCY)
, target_frequency_(target_frequency)
+ , preallocated_data_pkg_(output_recipe_)
, client_state_(ClientState::UNINITIALIZED)
{
if (!input_recipe_file.empty())
@@ -71,12 +72,12 @@ RTDEClient::RTDEClient(std::string robot_ip, comm::INotifier& notifier, const st
, parser_(output_recipe_)
, prod_(std::make_unique>(stream_, parser_))
, notifier_(notifier)
- , pipeline_(std::make_unique>(*prod_, PIPELINE_NAME, notifier, true))
, writer_(&stream_, input_recipe_)
, reconnecting_(false)
, stop_reconnection_(false)
, max_frequency_(URE_MAX_FREQUENCY)
, target_frequency_(target_frequency)
+ , preallocated_data_pkg_(output_recipe_)
, client_state_(ClientState::UNINITIALIZED)
{
}
@@ -89,6 +90,7 @@ RTDEClient::~RTDEClient()
{
reconnecting_thread_.join();
}
+ stopBackgroundRead();
disconnect();
}
@@ -128,8 +130,6 @@ bool RTDEClient::init(const size_t max_connection_attempts, const std::chrono::m
URCL_LOG_ERROR("Failed to initialize RTDE client, retrying in %d seconds", initialization_timeout.count() / 1000);
std::this_thread::sleep_for(initialization_timeout);
}
- // Stop pipeline again
- pipeline_->stop();
client_state_ = ClientState::INITIALIZED;
// Set reconnection callback after we are initialized to ensure that a disconnect during initialization doesn't
// trigger a reconnect
@@ -140,24 +140,12 @@ bool RTDEClient::init(const size_t max_connection_attempts, const std::chrono::m
bool RTDEClient::setupCommunication(const size_t max_num_tries, const std::chrono::milliseconds reconnection_time)
{
client_state_ = ClientState::UNINITIALIZED;
- try
- {
- pipeline_->init(max_num_tries, reconnection_time);
- }
- catch (const UrException& exc)
- {
- URCL_LOG_ERROR("Caught exception '%s', while trying to initialize pipeline", exc.what());
- return false;
- }
- // The state initializing is used inside disconnect to stop the pipeline again.
- // A running pipeline is needed inside setup.
+ prod_->setupProducer(max_num_tries, reconnection_time);
client_state_ = ClientState::INITIALIZING;
- pipeline_->run();
-
- uint16_t protocol_version = negotiateProtocolVersion();
+ protocol_version_ = negotiateProtocolVersion();
// Protocol version must be above zero
- if (protocol_version == 0)
+ if (protocol_version_ == 0)
{
client_state_ = ClientState::UNINITIALIZED;
return false;
@@ -171,13 +159,29 @@ bool RTDEClient::setupCommunication(const size_t max_num_tries, const std::chron
setTargetFrequency();
}
- is_rtde_comm_setup = is_rtde_comm_setup && setupOutputs(protocol_version);
+ prod_->startProducer();
+
+ is_rtde_comm_setup = is_rtde_comm_setup && setupOutputs();
is_rtde_comm_setup = is_rtde_comm_setup && isRobotBooted();
if (input_recipe_.size() > 0)
{
- is_rtde_comm_setup = is_rtde_comm_setup && setupInputs();
+ try
+ {
+ is_rtde_comm_setup = is_rtde_comm_setup && setupInputs();
+ }
+ catch (const RTDEInputConflictException& exc)
+ {
+ /*
+ * If we are starting and shutting down the driver in quick succession, the robot might still
+ * have some old RTDE connections open. In this case conflicts might occur when we try reserve
+ * the same input fields again. To mitigate this, we try to setup communication again if
+ * that error occurs.
+ */
+ URCL_LOG_ERROR("Caught exception %s, while trying to setup RTDE inputs.", exc.what());
+ return false;
+ }
}
return is_rtde_comm_setup;
}
@@ -203,7 +207,7 @@ uint16_t RTDEClient::negotiateProtocolVersion()
while (num_retries < MAX_REQUEST_RETRIES)
{
std::unique_ptr package;
- if (!pipeline_->getLatestProduct(package, std::chrono::milliseconds(1000)))
+ if (!prod_->tryGet(package))
{
URCL_LOG_ERROR("failed to get package from RTDE interface");
return 0;
@@ -253,7 +257,7 @@ bool RTDEClient::queryURControlVersion()
std::unique_ptr package;
while (num_retries < MAX_REQUEST_RETRIES)
{
- if (!pipeline_->getLatestProduct(package, std::chrono::milliseconds(1000)))
+ if (!prod_->tryGet(package))
{
URCL_LOG_ERROR("No answer to urcontrol version query was received from robot");
return false;
@@ -263,13 +267,30 @@ bool RTDEClient::queryURControlVersion()
dynamic_cast(package.get()))
{
urcontrol_version_ = tmp_urcontrol_version->version_information_;
+ URCL_LOG_INFO("Received URControl version %s", urcontrol_version_.toString().c_str());
return true;
}
+ else if (rtde_interface::TextMessage* tmp_text_msg = dynamic_cast(package.get()))
+ {
+ // PolyScope X simulators seem to send a text message on every connect until they have been
+ // switched on.
+ if (tmp_text_msg->message_.find("SafetySetup has not been confirmed yet") != std::string::npos)
+ {
+ // silently retry
+ }
+ else
+ {
+ URCL_LOG_WARN("Received unexpected message from robot while querying URControl version. "
+ "Message:\n%s\nRetrying...",
+ tmp_text_msg->message_.c_str());
+ }
+ num_retries++;
+ }
else
{
std::stringstream ss;
ss << "Did not receive URControl version from robot. Message received instead: " << std::endl
- << package->toString() << ". Retrying...";
+ << package->toString() << "\n Retrying...";
num_retries++;
URCL_LOG_WARN("%s", ss.str().c_str());
}
@@ -306,17 +327,13 @@ void RTDEClient::resetOutputRecipe(const std::vector new_recipe)
disconnect();
output_recipe_.assign(new_recipe.begin(), new_recipe.end());
-
- // Reset pipeline first otherwise we will segfault, if the producer object no longer exists, when destroying the
- // pipeline
- pipeline_.reset();
+ preallocated_data_pkg_ = DataPackage(output_recipe_, protocol_version_);
parser_ = RTDEParser(output_recipe_);
prod_ = std::make_unique>(stream_, parser_);
- pipeline_ = std::make_unique>(*prod_, PIPELINE_NAME, notifier_, true);
}
-bool RTDEClient::setupOutputs(const uint16_t protocol_version)
+bool RTDEClient::setupOutputs()
{
unsigned int num_retries = 0;
size_t size;
@@ -327,7 +344,7 @@ bool RTDEClient::setupOutputs(const uint16_t protocol_version)
while (num_retries < MAX_REQUEST_RETRIES)
{
URCL_LOG_DEBUG("Sending output recipe");
- if (protocol_version == 2)
+ if (protocol_version_ == 2)
{
size = ControlPackageSetupOutputsRequest::generateSerializedRequest(buffer, target_frequency_, output_recipe_);
}
@@ -349,7 +366,7 @@ bool RTDEClient::setupOutputs(const uint16_t protocol_version)
}
std::unique_ptr package;
- if (!pipeline_->getLatestProduct(package, std::chrono::milliseconds(1000)))
+ if (!prod_->tryGet(package))
{
URCL_LOG_ERROR("Did not receive confirmation on RTDE output recipe");
return false;
@@ -443,7 +460,7 @@ bool RTDEClient::setupInputs()
while (num_retries < MAX_REQUEST_RETRIES)
{
std::unique_ptr package;
- if (!pipeline_->getLatestProduct(package, std::chrono::milliseconds(1000)))
+ if (!prod_->tryGet(package))
{
URCL_LOG_ERROR("Did not receive confirmation on RTDE input recipe");
return false;
@@ -461,14 +478,11 @@ bool RTDEClient::setupInputs()
{
std::string message = "Variable '" + input_recipe_[i] +
"' not recognized by the robot. Probably your input recipe contains errors";
- throw UrException(message);
+ throw RTDEInvalidKeyException(message);
}
else if (variable_types[i] == "IN_USE")
{
- std::string message = "Variable '" + input_recipe_[i] +
- "' is currently controlled by another RTDE client. The input recipe can't be used as "
- "configured";
- throw UrException(message);
+ throw RTDEInputConflictException(input_recipe_[i]);
}
}
writer_.init(tmp_input->input_recipe_id_);
@@ -501,7 +515,6 @@ void RTDEClient::disconnect()
}
if (client_state_ >= ClientState::INITIALIZING)
{
- pipeline_->stop();
}
if (client_state_ > ClientState::UNINITIALIZED)
{
@@ -509,6 +522,9 @@ void RTDEClient::disconnect()
writer_.stop();
}
client_state_ = ClientState::UNINITIALIZED;
+ prod_->stopProducer();
+ stopBackgroundRead();
+ notifier_.stopped("RTDE communication stopped");
}
bool RTDEClient::isRobotBooted()
@@ -518,7 +534,8 @@ bool RTDEClient::isRobotBooted()
if (!sendStart())
return false;
- std::unique_ptr package;
+ std::unique_ptr package = std::make_unique(output_recipe_, protocol_version_);
+
double timestamp = 0;
int reading_count = 0;
// During bootup the RTDE interface gets restarted once. If we connect to the RTDE interface before that happens, we
@@ -530,8 +547,7 @@ bool RTDEClient::isRobotBooted()
while (timestamp < 40 && reading_count < target_frequency_ * 2)
{
// Set timeout based on target frequency, to make sure that reading doesn't timeout
- int timeout = static_cast((1 / target_frequency_) * 1000) * 10;
- if (pipeline_->getLatestProduct(package, std::chrono::milliseconds(timeout)))
+ if (prod_->tryGet(package))
{
rtde_interface::DataPackage* tmp_input = dynamic_cast(package.get());
tmp_input->getData("timestamp", timestamp);
@@ -550,7 +566,7 @@ bool RTDEClient::isRobotBooted()
return true;
}
-bool RTDEClient::start()
+bool RTDEClient::start(const bool read_packages_in_background)
{
if (client_state_ == ClientState::RUNNING)
return true;
@@ -561,11 +577,14 @@ bool RTDEClient::start()
return false;
}
- pipeline_->run();
-
if (sendStart())
{
+ if (read_packages_in_background)
+ {
+ startBackgroundRead();
+ }
client_state_ = ClientState::RUNNING;
+ notifier_.started("RTDE communication started");
return true;
}
else
@@ -584,6 +603,7 @@ bool RTDEClient::pause()
return false;
}
+ stopBackgroundRead();
if (sendPause())
{
client_state_ = ClientState::PAUSED;
@@ -607,11 +627,13 @@ bool RTDEClient::sendStart()
return false;
}
- std::unique_ptr package;
+ // Worst case we get a data package as part of a race condition in the communication. If we
+ // didn't preallocate that, it might print a warning.
+ std::unique_ptr package = std::make_unique(output_recipe_, protocol_version_);
unsigned int num_retries = 0;
while (num_retries < MAX_REQUEST_RETRIES)
{
- if (!pipeline_->getLatestProduct(package, std::chrono::milliseconds(1000)))
+ if (!prod_->tryGet(package))
{
URCL_LOG_ERROR("Could not get response to RTDE communication start request from robot");
return false;
@@ -655,12 +677,14 @@ bool RTDEClient::sendPause()
URCL_LOG_ERROR("Sending RTDE pause command failed!");
return false;
}
- std::unique_ptr package;
+ // Worst case we get a data package as part of a race condition in the communication. If we
+ // didn't preallocate that, it might print a warning.
+ std::unique_ptr package = std::make_unique(output_recipe_, protocol_version_);
std::chrono::time_point start = std::chrono::steady_clock::now();
int seconds = 5;
while (std::chrono::steady_clock::now() - start < std::chrono::seconds(seconds))
{
- if (!pipeline_->getLatestProduct(package, std::chrono::milliseconds(1000)))
+ if (!prod_->tryGet(package))
{
URCL_LOG_ERROR("Could not get response to RTDE communication pause request from robot");
return false;
@@ -720,21 +744,85 @@ std::vector RTDEClient::ensureTimestampIsPresent(const std::vector<
std::unique_ptr RTDEClient::getDataPackage(std::chrono::milliseconds timeout)
{
+ if (getDataPackage(preallocated_data_pkg_, timeout))
+ {
+ // Return a copy of the cached one
+ return std::make_unique(preallocated_data_pkg_);
+ }
+
+ return std::unique_ptr(nullptr);
+}
+
+bool RTDEClient::getDataPackage(std::unique_ptr& data_package,
+ std::chrono::milliseconds timeout)
+{
+ return getDataPackage(*data_package, timeout);
+}
+
+bool RTDEClient::getDataPackage(DataPackage& data_package, std::chrono::milliseconds timeout)
+{
+ if (reconnecting_)
+ {
+ URCL_LOG_WARN("Currently reconnecting to the RTDE interface, unable to get data package");
+ return false;
+ }
+ if (!background_read_running_)
+ {
+ URCL_LOG_ERROR("Background reading is not running, cannot get data package. Please either start background "
+ "reading or use getDataPackageBlocking(...).");
+ return false;
+ }
+
+ if (new_data_.load())
+ {
+ std::lock_guard guard(read_mutex_);
+ data_package = *dynamic_cast(data_buffer0_.get());
+ new_data_.store(false);
+ }
+ else
+ {
+ std::unique_lock lock(read_mutex_);
+ auto wait_result = background_read_cv_.wait_for(lock, timeout);
+ if (wait_result == std::cv_status::timeout)
+ {
+ return false;
+ }
+ if (new_data_.load())
+ {
+ data_package = *dynamic_cast(data_buffer0_.get());
+ new_data_.store(false);
+ }
+ }
+ return true;
+}
+
+bool RTDEClient::getDataPackageBlocking(std::unique_ptr& data_package)
+{
+ if (background_read_running_)
+ {
+ URCL_LOG_ERROR("Background reading is running, cannot get data package in blocking mode. Please either stop "
+ "background reading or use getDataPackage(...).");
+ return false;
+ }
+
// Cannot get data packages while reconnecting as we could end up getting some of the configuration packages
- if (reconnect_mutex_.try_lock())
+ std::unique_ptr base_package(data_package.release());
+ std::unique_lock lock(reconnect_mutex_, std::defer_lock);
+ if (lock.try_lock())
{
- std::unique_ptr urpackage;
- if (pipeline_->getLatestProduct(urpackage, timeout))
+ if (prod_->tryGet(base_package))
{
- rtde_interface::DataPackage* tmp = dynamic_cast(urpackage.get());
- if (tmp != nullptr)
+ lock.unlock();
+ auto package_type = base_package->getType();
+ if (package_type != PackageType::RTDE_DATA_PACKAGE)
{
- urpackage.release();
- reconnect_mutex_.unlock();
- return std::unique_ptr(tmp);
+ URCL_LOG_ERROR("Received package from RTDE interface is not a data package, but of type %d", package_type);
+ return false;
}
+ data_package.reset(dynamic_cast(base_package.release()));
+ return true;
}
- reconnect_mutex_.unlock();
+ lock.unlock();
}
else
{
@@ -743,7 +831,8 @@ std::unique_ptr RTDEClient::getDataPackage(std::chr
std::this_thread::sleep_for(period);
}
- return std::unique_ptr(nullptr);
+ data_package.reset(dynamic_cast(base_package.release()));
+ return false;
}
std::string RTDEClient::getIP() const
@@ -764,6 +853,7 @@ void RTDEClient::reconnect()
std::lock_guard lock(reconnect_mutex_);
ClientState cur_client_state = client_state_;
client_state_ = ClientState::CONNECTION_LOST;
+ bool background_read_was_running = background_read_running_;
disconnect();
size_t cur_initialization_attempt = 0;
@@ -833,7 +923,7 @@ void RTDEClient::reconnect()
URCL_LOG_INFO("Successfully reconnected to the RTDE interface, starting communication again");
- start();
+ start(background_read_was_running);
if (cur_client_state == ClientState::PAUSED)
{
pause();
@@ -856,5 +946,73 @@ void RTDEClient::reconnectCallback()
reconnecting_thread_ = std::thread(&RTDEClient::reconnect, this);
}
+void RTDEClient::startBackgroundRead()
+{
+ if (background_read_running_)
+ {
+ URCL_LOG_WARN("Requested to start RTDEClient's background read, while it is already running. Doing nothing.");
+ return;
+ }
+ background_read_running_ = true;
+ data_buffer0_ = std::make_unique(output_recipe_, protocol_version_);
+ data_buffer1_ = std::make_unique(output_recipe_, protocol_version_);
+
+ background_read_thread_ = std::thread(&RTDEClient::backgroundReadThreadFunc, this);
+}
+
+void RTDEClient::stopBackgroundRead()
+{
+ background_read_running_ = false;
+ background_read_cv_.notify_one();
+ if (background_read_thread_.joinable())
+ {
+ background_read_thread_.join();
+ }
+}
+
+void RTDEClient::backgroundReadThreadFunc()
+{
+ while (background_read_running_)
+ {
+ std::unique_lock lock(reconnect_mutex_, std::defer_lock);
+ if (lock.try_lock())
+ {
+ if (prod_->tryGet(data_buffer1_))
+ {
+ lock.unlock();
+ rtde_interface::DataPackage* data_pkg = dynamic_cast(data_buffer1_.get());
+ if (data_pkg != nullptr)
+ {
+ {
+ std::scoped_lock lock(read_mutex_, write_mutex_);
+ std::swap(data_buffer0_, data_buffer1_);
+ }
+
+ new_data_.store(true);
+ background_read_cv_.notify_one();
+ }
+ else if (data_buffer1_->getType() == PackageType::RTDE_TEXT_MESSAGE)
+ {
+ URCL_LOG_INFO(data_buffer1_->toString().c_str());
+ }
+ }
+ else
+ {
+ lock.unlock();
+ auto period = std::chrono::duration(1.0 / target_frequency_);
+ std::this_thread::sleep_for(period);
+ }
+ }
+ else
+ {
+ URCL_LOG_WARN("Unable to get data package while reconnecting to the RTDE interface");
+ auto period = std::chrono::duration(2.0 / target_frequency_);
+ std::this_thread::sleep_for(period);
+ }
+ }
+ new_data_.store(false);
+ URCL_LOG_INFO("RTDE background read thread stopped");
+}
+
} // namespace rtde_interface
} // namespace urcl
diff --git a/src/rtde/rtde_parser.cpp b/src/rtde/rtde_parser.cpp
new file mode 100644
index 00000000..48bd60c2
--- /dev/null
+++ b/src/rtde/rtde_parser.cpp
@@ -0,0 +1,204 @@
+/*
+ * Copyright 2025, Universal Robots A/S (refactor)
+ *
+ * Copyright 2019, FZI Forschungszentrum Informatik (refactor)
+ *
+ * Copyright 2017, 2018 Simon Rasmussen (refactor)
+ *
+ * Copyright 2015, 2016 Thomas Timm Andersen (original version)
+ *
+ * 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.
+ */
+#include "ur_client_library/rtde/rtde_parser.h"
+#include "ur_client_library/rtde/package_header.h"
+#include "ur_client_library/rtde/rtde_package.h"
+
+namespace urcl
+{
+namespace rtde_interface
+{
+
+bool RTDEParser::parse(comm::BinParser& bp, std::vector>& results)
+{
+ static bool warning_printed = false;
+ if (!warning_printed)
+ {
+ URCL_LOG_WARN("Calling RTDEParser::parse(...) with a vector of products is deprecated. It will allocate memory in "
+ "each cycle in order to create parsed packages. Please use the overloaded function that accepts a "
+ "pre-allocated unique pointer to a package. This message is for application developers using this "
+ "function.");
+ warning_printed = true;
+ }
+ PackageType type;
+ try
+ {
+ type = getPackageTypeFromHeader(bp);
+ }
+ catch (const UrException& e)
+ {
+ URCL_LOG_ERROR("Exception during RTDE package parsing: %s", e.what());
+ return false;
+ }
+
+ switch (type)
+ {
+ case PackageType::RTDE_DATA_PACKAGE:
+ {
+ std::unique_ptr package(new DataPackage(recipe_, protocol_version_));
+
+ if (!package->parseWith(bp))
+ {
+ URCL_LOG_ERROR("Package parsing of type %d failed!", static_cast(type));
+ return false;
+ }
+ results.push_back(std::move(package));
+ break;
+ }
+ default:
+ {
+ std::unique_ptr package(createNewPackageFromType(type));
+ if (!package->parseWith(bp))
+ {
+ URCL_LOG_ERROR("Package parsing of type %d failed!", static_cast(type));
+ return false;
+ }
+
+ results.push_back(std::move(package));
+ break;
+ }
+ }
+ if (!bp.empty())
+ {
+ URCL_LOG_ERROR("Package of type %d was not parsed completely!", static_cast(type));
+ bp.debug();
+ return false;
+ }
+
+ return true;
+}
+
+bool RTDEParser::parse(comm::BinParser& bp, std::unique_ptr& result)
+{
+ PackageType type;
+ try
+ {
+ type = getPackageTypeFromHeader(bp);
+ }
+ catch (const UrException& e)
+ {
+ URCL_LOG_ERROR("Exception during RTDE package parsing: %s", e.what());
+ return false;
+ }
+
+ switch (type)
+ {
+ case PackageType::RTDE_DATA_PACKAGE:
+ {
+ if (result == nullptr || result->getType() != PackageType::RTDE_DATA_PACKAGE)
+ {
+ if (result == nullptr)
+ {
+ URCL_LOG_WARN("The passed result pointer is empty. A new DataPackage will "
+ "have to be allocated. Please pass a pre-allocated DataPackage if you expect a DataPackage "
+ "would be sent.");
+ }
+ else
+ {
+ URCL_LOG_WARN("Passed a pre-allocated RTDE package of type %u while a DataPackage was received. A new "
+ "DataPackage will have to be allocated. Please pass a pre-allocated DataPackage if you expect "
+ "a DataPackage would be sent.",
+ result->getType());
+ }
+ result = std::make_unique(recipe_, protocol_version_);
+ }
+
+ if (!dynamic_cast(result.get())->parseWith(bp))
+ {
+ URCL_LOG_ERROR("Package parsing of type %d failed!", static_cast(type));
+ return false;
+ }
+ break;
+ }
+ default:
+ {
+ if (result == nullptr || result->getType() != type)
+ {
+ // For all non data-packages real-time communication isn't critical. Hence, we silently
+ // allocate a new package if preallocation isn't given.
+ result = std::unique_ptr(createNewPackageFromType(type));
+ }
+ if (!result->parseWith(bp))
+ {
+ URCL_LOG_ERROR("Package parsing of type %d failed!", static_cast(type));
+ return false;
+ }
+
+ break;
+ }
+ }
+ if (!bp.empty())
+ {
+ URCL_LOG_ERROR("Package of type %d was not parsed completely!", static_cast(type));
+ bp.debug();
+ return false;
+ }
+
+ return true;
+}
+
+PackageType RTDEParser::getPackageTypeFromHeader(comm::BinParser& bp) const
+{
+ PackageHeader::_package_size_type size;
+ PackageType type;
+ bp.parse(size);
+ bp.parse(type);
+
+ if (!bp.checkSize(size - sizeof(size) - sizeof(type)))
+ {
+ throw UrException("Buffer len shorter than expected packet length");
+ }
+ return type;
+}
+
+RTDEPackage* RTDEParser::createNewPackageFromType(PackageType type) const
+{
+ switch (type)
+ {
+ case PackageType::RTDE_TEXT_MESSAGE:
+ return new TextMessage(protocol_version_);
+ break;
+ case PackageType::RTDE_GET_URCONTROL_VERSION:
+ return new GetUrcontrolVersion;
+ break;
+ case PackageType::RTDE_REQUEST_PROTOCOL_VERSION:
+ return new RequestProtocolVersion;
+ break;
+ case PackageType::RTDE_CONTROL_PACKAGE_PAUSE:
+ return new ControlPackagePause;
+ break;
+ case PackageType::RTDE_CONTROL_PACKAGE_SETUP_INPUTS:
+ return new ControlPackageSetupInputs;
+ break;
+ case PackageType::RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS:
+ return new ControlPackageSetupOutputs(protocol_version_);
+ break;
+ case PackageType::RTDE_CONTROL_PACKAGE_START:
+ return new ControlPackageStart;
+ break;
+ default:
+ return new RTDEPackage(type);
+ }
+}
+
+} // namespace rtde_interface
+} // namespace urcl
diff --git a/src/rtde/rtde_writer.cpp b/src/rtde/rtde_writer.cpp
index 2df14950..425ec091 100644
--- a/src/rtde/rtde_writer.cpp
+++ b/src/rtde/rtde_writer.cpp
@@ -27,32 +27,74 @@
//----------------------------------------------------------------------
#include "ur_client_library/rtde/rtde_writer.h"
+#include
+#include "ur_client_library/log.h"
namespace urcl
{
namespace rtde_interface
{
+const std::vector RTDEWriter::g_preallocated_input_bit_register_keys =
+ RTDEWriter::initializePreallocatedKeys("input_bit_register_", 128);
+const std::vector RTDEWriter::g_preallocated_input_int_register_keys =
+ RTDEWriter::initializePreallocatedKeys("input_int_register_", 48);
+const std::vector RTDEWriter::g_preallocated_input_double_register_keys =
+ RTDEWriter::initializePreallocatedKeys("input_double_register_", 48);
+
+const std::vector RTDEWriter::initializePreallocatedKeys(const std::string& prefix, const size_t size)
+{
+ std::vector keys(size);
+ for (size_t i = 0; i < size; ++i)
+ {
+ keys[i] = prefix + std::to_string(i);
+ }
+ return keys;
+}
+
RTDEWriter::RTDEWriter(comm::URStream* stream, const std::vector& recipe)
- : stream_(stream), recipe_(recipe), recipe_id_(0), queue_{ 32 }, running_(false), package_(recipe_)
+ : stream_(stream), recipe_id_(0), running_(false)
{
+ setInputRecipe(recipe);
}
void RTDEWriter::setInputRecipe(const std::vector& recipe)
{
- std::lock_guard guard(package_mutex_);
+ if (running_)
+ {
+ throw UrException("Requesting to change the input recipe while the RTDEWriter is running. The writer has to be "
+ "stopped before setting the recipe.");
+ }
+ std::lock_guard lock_guard(store_mutex_);
recipe_ = recipe;
- package_ = DataPackage(recipe_);
- package_.initEmpty();
+ used_masks_.clear();
+ for (const auto& field : recipe)
+ {
+ if (field.size() >= 5 && field.substr(field.size() - 5) == "_mask")
+ {
+ used_masks_.push_back(field);
+ }
+ }
+ data_buffer0_ = std::make_shared(recipe_);
+ data_buffer1_ = std::make_shared(recipe_);
+
+ current_store_buffer_ = data_buffer0_;
+ current_send_buffer_ = data_buffer1_;
}
void RTDEWriter::init(uint8_t recipe_id)
{
if (running_)
{
- return;
+ throw UrException("Requesting to init a RTDEWriter while it is running. The writer has to be "
+ "stopped before initializing it.");
+ }
+ {
+ std::lock_guard lock_guard(store_mutex_);
+ data_buffer0_->setRecipeID(recipe_id);
+ data_buffer1_->setRecipeID(recipe_id);
}
recipe_id_ = recipe_id;
- package_.initEmpty();
+ new_data_available_ = false;
running_ = true;
writer_thread_ = std::thread(&RTDEWriter::run, this);
}
@@ -62,14 +104,24 @@ void RTDEWriter::run()
uint8_t buffer[4096];
size_t size;
size_t written;
- std::unique_ptr package;
while (running_)
{
- if (queue_.waitDequeTimed(package, 1000000))
+ std::unique_lock lock(store_mutex_);
+ data_available_cv_.wait(lock, [this] { return new_data_available_.load() || !running_.load(); });
{
- package->setRecipeID(recipe_id_);
- size = package->serializePackage(buffer);
- stream_->write(buffer, size, written);
+ if (new_data_available_.load() && running_.load())
+ {
+ std::swap(current_store_buffer_, current_send_buffer_);
+ new_data_available_ = false;
+ lock.unlock();
+ size = current_send_buffer_->serializePackage(buffer);
+ stream_->write(buffer, size, written);
+ resetMasks(current_send_buffer_);
+ }
+ else
+ {
+ lock.unlock();
+ }
}
}
URCL_LOG_DEBUG("Write thread ended.");
@@ -77,13 +129,25 @@ void RTDEWriter::run()
void RTDEWriter::stop()
{
- running_ = false;
+ {
+ std::lock_guard lock(store_mutex_);
+ running_ = false;
+ data_available_cv_.notify_one();
+ }
if (writer_thread_.joinable())
{
writer_thread_.join();
}
}
+bool RTDEWriter::sendPackage(const DataPackage& package)
+{
+ std::lock_guard guard(store_mutex_);
+ *current_store_buffer_ = package;
+ markStorageToBeSent();
+ return true;
+}
+
bool RTDEWriter::sendSpeedSlider(double speed_slider_fraction)
{
if (speed_slider_fraction > 1.0 || speed_slider_fraction < 0.0)
@@ -95,21 +159,18 @@ bool RTDEWriter::sendSpeedSlider(double speed_slider_fraction)
return false;
}
- std::lock_guard guard(package_mutex_);
+ static const std::string key = "speed_slider_fraction";
+ static const std::string mask_key = "speed_slider_mask";
+ std::lock_guard guard(store_mutex_);
uint32_t mask = 1;
bool success = true;
- success = package_.setData("speed_slider_mask", mask);
- success = success && package_.setData("speed_slider_fraction", speed_slider_fraction);
-
+ success = current_store_buffer_->setData(mask_key, mask);
+ success = success && current_store_buffer_->setData(key, speed_slider_fraction);
if (success)
{
- if (!queue_.tryEnqueue(std::unique_ptr(new DataPackage(package_))))
- {
- return false;
- }
+ markStorageToBeSent();
}
- mask = 0;
- success = package_.setData("speed_slider_mask", mask);
+
return success;
}
@@ -123,7 +184,9 @@ bool RTDEWriter::sendStandardDigitalOutput(uint8_t output_pin, bool value)
return false;
}
- std::lock_guard guard(package_mutex_);
+ static const std::string key_mask = "standard_digital_output_mask";
+ static const std::string key_output = "standard_digital_output";
+ std::lock_guard guard(store_mutex_);
uint8_t mask = pinToMask(output_pin);
bool success = true;
uint8_t digital_output;
@@ -135,18 +198,13 @@ bool RTDEWriter::sendStandardDigitalOutput(uint8_t output_pin, bool value)
{
digital_output = 0;
}
- success = package_.setData("standard_digital_output_mask", mask);
- success = success && package_.setData("standard_digital_output", digital_output);
-
+ success = current_store_buffer_->setData(key_mask, mask);
+ success = success && current_store_buffer_->setData(key_output, digital_output);
if (success)
{
- if (!queue_.tryEnqueue(std::unique_ptr(new DataPackage(package_))))
- {
- return false;
- }
+ markStorageToBeSent();
}
- mask = 0;
- success = package_.setData("standard_digital_output_mask", mask);
+
return success;
}
@@ -161,7 +219,9 @@ bool RTDEWriter::sendConfigurableDigitalOutput(uint8_t output_pin, bool value)
return false;
}
- std::lock_guard guard(package_mutex_);
+ static const std::string key_mask = "configurable_digital_output_mask";
+ static const std::string key_output = "configurable_digital_output";
+ std::lock_guard guard(store_mutex_);
uint8_t mask = pinToMask(output_pin);
bool success = true;
uint8_t digital_output;
@@ -173,18 +233,13 @@ bool RTDEWriter::sendConfigurableDigitalOutput(uint8_t output_pin, bool value)
{
digital_output = 0;
}
- success = package_.setData("configurable_digital_output_mask", mask);
- success = success && package_.setData("configurable_digital_output", digital_output);
-
+ success = current_store_buffer_->setData(key_mask, mask);
+ success = success && current_store_buffer_->setData(key_output, digital_output);
if (success)
{
- if (!queue_.tryEnqueue(std::unique_ptr(new DataPackage(package_))))
- {
- return false;
- }
+ markStorageToBeSent();
}
- mask = 0;
- success = package_.setData("configurable_digital_output_mask", mask);
+
return success;
}
@@ -198,7 +253,9 @@ bool RTDEWriter::sendToolDigitalOutput(uint8_t output_pin, bool value)
return false;
}
- std::lock_guard guard(package_mutex_);
+ static const std::string key_mask = "tool_digital_output_mask";
+ static const std::string key_output = "tool_digital_output";
+ std::lock_guard guard(store_mutex_);
uint8_t mask = pinToMask(output_pin);
bool success = true;
uint8_t digital_output;
@@ -210,18 +267,13 @@ bool RTDEWriter::sendToolDigitalOutput(uint8_t output_pin, bool value)
{
digital_output = 0;
}
- success = package_.setData("tool_digital_output_mask", mask);
- success = success && package_.setData("tool_digital_output", digital_output);
-
+ success = current_store_buffer_->setData(key_mask, mask);
+ success = success && current_store_buffer_->setData(key_output, digital_output);
if (success)
{
- if (!queue_.tryEnqueue(std::unique_ptr(new DataPackage(package_))))
- {
- return false;
- }
+ markStorageToBeSent();
}
- mask = 0;
- success = package_.setData("tool_digital_output_mask", mask);
+
return success;
}
@@ -242,29 +294,28 @@ bool RTDEWriter::sendStandardAnalogOutput(uint8_t output_pin, double value, cons
return false;
}
- std::lock_guard guard(package_mutex_);
+ std::lock_guard guard(store_mutex_);
uint8_t mask = pinToMask(output_pin);
bool success = true;
- success = package_.setData("standard_analog_output_mask", mask);
+ static const std::string key_mask = "standard_analog_output_mask";
+ success = current_store_buffer_->setData(key_mask, mask);
if (type != AnalogOutputType::SET_ON_TEACH_PENDANT)
{
+ static const std::string key_type = "standard_analog_output_type";
auto output_type_bits = [](const uint8_t pin, const uint8_t type) { return type << pin; };
uint8_t output_type = output_type_bits(output_pin, toUnderlying(type));
- success = success && package_.setData("standard_analog_output_type", output_type);
+ success = success && current_store_buffer_->setData(key_type, output_type);
}
- success = success && package_.setData("standard_analog_output_0", value);
- success = success && package_.setData("standard_analog_output_1", value);
-
+ static const std::string key_output_0 = "standard_analog_output_0";
+ success = success && current_store_buffer_->setData(key_output_0, value);
+ static const std::string key_output_1 = "standard_analog_output_1";
+ success = success && current_store_buffer_->setData(key_output_1, value);
if (success)
{
- if (!queue_.tryEnqueue(std::unique_ptr(new DataPackage(package_))))
- {
- return false;
- }
+ markStorageToBeSent();
}
- mask = 0;
- success = package_.setData("standard_analog_output_mask", mask);
+
return success;
}
@@ -288,19 +339,13 @@ bool RTDEWriter::sendInputBitRegister(uint32_t register_id, bool value)
return false;
}
- std::lock_guard guard(package_mutex_);
- std::stringstream ss;
- ss << "input_bit_register_" << register_id;
-
- bool success = package_.setData(ss.str(), value);
-
+ std::lock_guard guard(store_mutex_);
+ bool success = current_store_buffer_->setData(g_preallocated_input_bit_register_keys[register_id], value);
if (success)
{
- if (!queue_.tryEnqueue(std::unique_ptr(new DataPackage(package_))))
- {
- return false;
- }
+ markStorageToBeSent();
}
+
return success;
}
@@ -314,19 +359,13 @@ bool RTDEWriter::sendInputIntRegister(uint32_t register_id, int32_t value)
return false;
}
- std::lock_guard guard(package_mutex_);
- std::stringstream ss;
- ss << "input_int_register_" << register_id;
-
- bool success = package_.setData(ss.str(), value);
-
+ std::lock_guard guard(store_mutex_);
+ bool success = current_store_buffer_->setData(g_preallocated_input_int_register_keys[register_id], value);
if (success)
{
- if (!queue_.tryEnqueue(std::unique_ptr(new DataPackage(package_))))
- {
- return false;
- }
+ markStorageToBeSent();
}
+
return success;
}
@@ -340,34 +379,52 @@ bool RTDEWriter::sendInputDoubleRegister(uint32_t register_id, double value)
return false;
}
- std::lock_guard guard(package_mutex_);
- std::stringstream ss;
- ss << "input_double_register_" << register_id;
-
- bool success = package_.setData(ss.str(), value);
-
+ std::lock_guard guard(store_mutex_);
+ bool success = current_store_buffer_->setData(g_preallocated_input_double_register_keys[register_id], value);
if (success)
{
- if (!queue_.tryEnqueue(std::unique_ptr(new DataPackage(package_))))
- {
- return false;
- }
+ markStorageToBeSent();
}
+
return success;
}
bool RTDEWriter::sendExternalForceTorque(const vector6d_t& external_force_torque)
{
- std::lock_guard guard(package_mutex_);
- bool success = package_.setData("external_force_torque", external_force_torque);
+ static const std::string key = "external_force_torque";
+ std::lock_guard guard(store_mutex_);
+ bool success = current_store_buffer_->setData(key, external_force_torque);
if (success)
{
- if (!queue_.tryEnqueue(std::unique_ptr(new DataPackage(package_))))
+ markStorageToBeSent();
+ }
+ return success;
+}
+
+void RTDEWriter::resetMasks(const std::shared_ptr& buffer)
+{
+ for (const auto& mask_name : used_masks_)
+ {
+ // "speed_slider_mask" is uint32_t, all others are uint8_t
+ // If we reset it to the wrong type, serialization will be wrong
+ if (mask_name == "speed_slider_mask")
+
+ {
+ uint32_t mask = 0;
+ buffer->setData(mask_name, mask);
+ }
+ else
{
- return false;
+ uint8_t mask = 0;
+ buffer->setData(mask_name, mask);
}
}
- return success;
+}
+
+void RTDEWriter::markStorageToBeSent()
+{
+ new_data_available_ = true;
+ data_available_cv_.notify_one();
}
} // namespace rtde_interface
diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp
index 3f5271f5..6e0a78c7 100644
--- a/src/ur/ur_driver.cpp
+++ b/src/ur/ur_driver.cpp
@@ -188,6 +188,8 @@ void UrDriver::init(const UrDriverConfiguration& config)
URCL_LOG_DEBUG("Initialization done");
}
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
std::unique_ptr urcl::UrDriver::getDataPackage()
{
// This can take one of two values, 0ms or 100ms. The large timeout is for when the robot is commanding the control
@@ -197,6 +199,21 @@ std::unique_ptr urcl::UrDriver::getDataPackage()
return rtde_client_->getDataPackage(timeout);
}
+#pragma GCC diagnostic pop
+
+bool UrDriver::getDataPackage(rtde_interface::DataPackage& data_package)
+{
+ // This can take one of two values, 0ms or 100ms. The large timeout is for when the robot is commanding the control
+ // loop's timing (read is blocking). The zero timeout is for when the robot is sharing a control loop with
+ // something else (combined_robot_hw)
+ std::chrono::milliseconds timeout(get_packet_timeout_);
+ return rtde_client_->getDataPackage(data_package, timeout);
+}
+
+bool UrDriver::getDataPackageBlocking(std::unique_ptr& data_package)
+{
+ return rtde_client_->getDataPackageBlocking(data_package);
+}
bool UrDriver::writeJointCommand(const vector6d_t& values, const comm::ControlMode control_mode,
const RobotReceiveTimeout& robot_receive_timeout)
@@ -580,9 +597,9 @@ bool UrDriver::writeKeepalive(const RobotReceiveTimeout& robot_receive_timeout)
return reverse_interface_->write(fake, comm::ControlMode::MODE_IDLE, robot_receive_timeout);
}
-void UrDriver::startRTDECommunication()
+void UrDriver::startRTDECommunication(const bool read_packages_in_background)
{
- rtde_client_->start();
+ rtde_client_->start(read_packages_in_background);
}
bool UrDriver::stopControl()
diff --git a/tests/fake_rtde_server.cpp b/tests/fake_rtde_server.cpp
index f7473517..72e23a63 100644
--- a/tests/fake_rtde_server.cpp
+++ b/tests/fake_rtde_server.cpp
@@ -192,10 +192,11 @@ void RTDEServer::startSendingDataPackages()
void RTDEServer::stopSendingDataPackages()
{
- URCL_LOG_INFO("Stop sending data.");
+ std::lock_guard thread_lock(thread_control_mutex_);
send_loop_running_ = false;
if (send_thread_.joinable())
{
+ URCL_LOG_INFO("Stop sending data.");
send_thread_.join();
}
}
diff --git a/tests/fake_rtde_server.h b/tests/fake_rtde_server.h
index 72be6d59..b34eec51 100644
--- a/tests/fake_rtde_server.h
+++ b/tests/fake_rtde_server.h
@@ -51,6 +51,7 @@ class RTDEServer
void actOnInput();
std::mutex output_data_mutex_;
+ std::mutex thread_control_mutex_;
};
} // namespace urcl
diff --git a/tests/test_deprecated_ur_driver_construction.cpp b/tests/test_deprecated_ur_driver_construction.cpp
index 08b55e65..3f6ff293 100644
--- a/tests/test_deprecated_ur_driver_construction.cpp
+++ b/tests/test_deprecated_ur_driver_construction.cpp
@@ -30,6 +30,7 @@
#include
#include
+#include
#include "ur_client_library/ur/ur_driver.h"
const std::string SCRIPT_FILE = "../resources/external_control.urscript";
@@ -44,46 +45,53 @@ void handleRobotProgramState(bool program_running)
std::cout << "\033[1;32mProgram running: " << std::boolalpha << program_running << "\033[0m\n" << std::endl;
}
-TEST(UrDriverTestDeprecatedConstructor, sigA)
+void startDriver(std::function()> constructor_fun)
{
- std::unique_ptr tool_comm_setup;
- auto driver = std::make_shared(g_ROBOT_IP, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE,
- std::bind(&handleRobotProgramState, std::placeholders::_1), g_HEADLESS,
- std::move(tool_comm_setup));
+ auto driver = constructor_fun();
driver->checkCalibration(CALIBRATION_CHECKSUM);
auto version = driver->getVersion();
ASSERT_TRUE(version.major > 0);
}
+TEST(UrDriverTestDeprecatedConstructor, sigA)
+{
+ std::unique_ptr tool_comm_setup;
+ startDriver([&tool_comm_setup]() {
+ return std::make_shared(g_ROBOT_IP, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE,
+ std::bind(&handleRobotProgramState, std::placeholders::_1), g_HEADLESS,
+ std::move(tool_comm_setup));
+ });
+}
+
TEST(UrDriverTestDeprecatedConstructor, sigB)
{
std::unique_ptr tool_comm_setup;
- auto driver = std::make_shared(
- g_ROBOT_IP, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, std::bind(&handleRobotProgramState, std::placeholders::_1),
- g_HEADLESS, std::move(tool_comm_setup), 50001, 50002, 2000, 0.03, false, "", 50003, 50004, 0.025, 0.5);
- driver->checkCalibration(CALIBRATION_CHECKSUM);
- auto version = driver->getVersion();
- ASSERT_TRUE(version.major > 0);
+ startDriver([&tool_comm_setup]() {
+ return std::make_shared(g_ROBOT_IP, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE,
+ std::bind(&handleRobotProgramState, std::placeholders::_1), g_HEADLESS,
+ std::move(tool_comm_setup), 50001, 50002, 2000, 0.03, false, "", 50003,
+ 50004, 0.025, 0.5);
+ });
}
TEST(UrDriverTestDeprecatedConstructor, sigC)
{
std::unique_ptr tool_comm_setup;
- auto driver = std::make_shared(g_ROBOT_IP, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE,
- std::bind(&handleRobotProgramState, std::placeholders::_1), g_HEADLESS,
- std::move(tool_comm_setup), CALIBRATION_CHECKSUM);
- auto version = driver->getVersion();
- ASSERT_TRUE(version.major > 0);
+ startDriver([&tool_comm_setup]() {
+ return std::make_shared(g_ROBOT_IP, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE,
+ std::bind(&handleRobotProgramState, std::placeholders::_1), g_HEADLESS,
+ std::move(tool_comm_setup), CALIBRATION_CHECKSUM);
+ });
}
TEST(UrDriverTestDeprecatedConstructor, sigD)
{
std::unique_ptr tool_comm_setup;
- auto driver = std::make_shared(g_ROBOT_IP, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE,
- std::bind(&handleRobotProgramState, std::placeholders::_1), g_HEADLESS,
- CALIBRATION_CHECKSUM);
- auto version = driver->getVersion();
- ASSERT_TRUE(version.major > 0);
+ startDriver([]() {
+ return std::make_shared(g_ROBOT_IP, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE,
+ std::bind(&handleRobotProgramState, std::placeholders::_1), g_HEADLESS,
+ CALIBRATION_CHECKSUM);
+ });
}
int main(int argc, char* argv[])
diff --git a/tests/test_primary_parser.cpp b/tests/test_primary_parser.cpp
index 19429e78..c076ebd8 100644
--- a/tests/test_primary_parser.cpp
+++ b/tests/test_primary_parser.cpp
@@ -205,19 +205,29 @@ TEST(primary_parser, parse_calibration_data)
}
}
+TEST(primary_parser, parse_robot_state_with_single_parser)
+{
+ unsigned char raw_data[sizeof(ROBOT_STATE)];
+ memcpy(raw_data, ROBOT_STATE, sizeof(ROBOT_STATE));
+ comm::BinParser bp(raw_data, sizeof(raw_data));
+
+ std::unique_ptr product;
+ primary_interface::PrimaryParser parser;
+ ASSERT_FALSE(parser.parse(bp, product));
+};
+
TEST(primary_parser, parse_version_message)
{
unsigned char raw_data[sizeof(VERSION_MESSAGE)];
memcpy(raw_data, VERSION_MESSAGE, sizeof(VERSION_MESSAGE));
comm::BinParser bp(raw_data, sizeof(raw_data));
- std::vector> products;
+ std::unique_ptr product;
primary_interface::PrimaryParser parser;
- ASSERT_TRUE(parser.parse(bp, products));
-
- EXPECT_EQ(products.size(), 1);
+ ASSERT_TRUE(parser.parse(bp, product));
- if (primary_interface::VersionMessage* data = dynamic_cast(products[0].get()))
+ EXPECT_NE(product, nullptr);
+ if (primary_interface::VersionMessage* data = dynamic_cast(product.get()))
{
EXPECT_EQ(data->major_version_, 5);
EXPECT_EQ(data->minor_version_, 24);
diff --git a/tests/test_rtde_client.cpp b/tests/test_rtde_client.cpp
index 3b77f2db..a6437cca 100644
--- a/tests/test_rtde_client.cpp
+++ b/tests/test_rtde_client.cpp
@@ -35,12 +35,14 @@
#include
#include
#include
+#include "ur_client_library/comm/tcp_server.h"
#include "ur_client_library/exceptions.h"
#include