diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 860384b24..e56633ac2 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 ab44d1cb3..50eab4edb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -40,6 +40,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 497906471..0469bc223 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 + +.. code-block:: c++ - The recipes can be either passed as a filename or as a list of strings directly. E.g. the - following will work + rtde_interface::RTDEClient my_client( + ROBOT_IP, + notifier, + {"timestamp", "actual_q"}, + {"speed_slider_mask", "speed_slider_fraction"} + ); + +.. note:: + Remember, that ``timestamp`` always has to be part of the output recipe. - .. 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,7 +105,7 @@ 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(); - my_client.start(); + my_client.start(false); while (true) { std::unique_ptr data_pkg = my_client.getDataPackage(READ_TIMEOUT); diff --git a/doc/examples/direct_torque_control.rst b/doc/examples/direct_torque_control.rst index f4d385d4d..10e52950a 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 bf47d97bf..9e5cc4f6e 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: std::unique_ptr data_pkg = :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 874c49220..0942295c6 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 f50ce4cbf..d213cc97f 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 f8838f092..dfd22cbb2 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 d123b5237..1af6ffb61 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 0972aafc7..3c3eb27a5 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,29 @@ int main(int argc, char* argv[]) double target_speed_fraction = 1.0; double speed_slider_increment = 0.01; + // std::unique_ptr data_pkg = + // std::make_unique(my_client.getOutputRecipe()); + rtde_interface::DataPackage data_pkg(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(true); // 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.getDataPackage(data_pkg, std::chrono::milliseconds(100)); + 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 +142,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 a29b26772..c9303ffde 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 f014a37e8..8668acf1f 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 66d98d19a..08a868948 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 aafd0f754..3b55faab0 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 19a929a70..e509d9017 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 38b422e95..516b0bffc 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,7 +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_; @@ -229,8 +204,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 2f70aa85a..72b10bc49 100644 --- a/include/ur_client_library/exceptions.h +++ b/include/ur_client_library/exceptions.h @@ -272,5 +272,51 @@ 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 Output 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"; + } + + 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 d707f7588..47722e948 100644 --- a/include/ur_client_library/primary/primary_parser.h +++ b/include/ur_client_library/primary/primary_parser.h @@ -55,7 +55,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; @@ -147,6 +147,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) { @@ -182,7 +212,7 @@ class PrimaryParser : public comm::Parser case RobotMessagePackageType::ROBOT_MESSAGE_ERROR_CODE: return new ErrorCodeMessage(timestamp, source); default: - return new RobotMessage(timestamp, source); + return new RobotMessage(timestamp, source, type); } } }; diff --git a/include/ur_client_library/rtde/data_package.h b/include/ur_client_library/rtde/data_package.h index ce2b991d2..ffd9cc7a6 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 5ebc76ce3..39085a1e6 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 { @@ -96,11 +89,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. @@ -112,11 +105,11 @@ class RTDEClient bool ignore_unavailable_outputs = false); /*! - * \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. @@ -127,6 +120,7 @@ class RTDEClient const std::vector& input_recipe, double target_frequency = 0.0, bool ignore_unavailable_outputs = false); ~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. @@ -149,24 +143,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. * @@ -232,10 +279,28 @@ 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); -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_; @@ -243,7 +308,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_; @@ -255,6 +319,15 @@ 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_; + ClientState client_state_; constexpr static const double CB3_MAX_FREQUENCY = 125.0; @@ -310,6 +383,8 @@ class RTDEClient */ void reconnect(); void reconnectCallback(); + + 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 23156b990..08019aa13 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 7ea4d131b..416dddd4a 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,23 @@ 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) + 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 d5af8e5df..950e116fd 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,27 @@ 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 std::vector g_preallocated_input_bit_register_keys; + static std::vector g_preallocated_input_int_register_keys; + static std::vector g_preallocated_input_double_register_keys; }; } // namespace rtde_interface diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index e61a8e077..5d4c0425d 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 2b91e3dea..91d537860 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 c0d38583e..d7f49f944 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 d5ba33d98..fed647794 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 191a730ce..89c3d649b 100644 --- a/src/primary/robot_message.cpp +++ b/src/primary/robot_message.cpp @@ -45,6 +45,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/primary/robot_message/version_message.cpp b/src/primary/robot_message/version_message.cpp index f18e07ab3..0a5138210 100644 --- a/src/primary/robot_message/version_message.cpp +++ b/src/primary/robot_message/version_message.cpp @@ -57,9 +57,11 @@ bool VersionMessage ::consumeWith(AbstractPrimaryConsumer& consumer) std::string VersionMessage::toString() const { std::stringstream ss; + ss << "VersionMessage:" << std::endl; ss << "project name: " << project_name_ << std::endl; ss << "version: " << unsigned(major_version_) << "." << unsigned(minor_version_) << "." << svn_version_ << std::endl; - ss << "build date: " << build_date_; + ss << "build date: " << build_date_ << std::endl; + ss << "build number: " << build_number_ << std::endl; return ss.str(); } diff --git a/src/primary/robot_state.cpp b/src/primary/robot_state.cpp index 15d8a2763..4b6ca3db6 100644 --- a/src/primary/robot_state.cpp +++ b/src/primary/robot_state.cpp @@ -47,7 +47,7 @@ bool RobotState::consumeWith(AbstractPrimaryConsumer& consumer) std::string RobotState::toString() const { std::stringstream ss; - ss << "Type: " << static_cast(state_type_) << std::endl; + ss << "RobotStateType: " << static_cast(state_type_) << std::endl; ss << PrimaryPackage::toString(); return ss.str(); } diff --git a/src/rtde/data_package.cpp b/src/rtde/data_package.cpp index 055c0fd5d..9f26ec2c6 100644 --- a/src/rtde/data_package.cpp +++ b/src/rtde/data_package.cpp @@ -462,13 +462,12 @@ 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()) - { - _rtde_type_variant entry = g_type_list[item]; - data_[item] = entry; - } + _rtde_type_variant entry = g_type_list[item]; + data_.push_back({ item, entry }); } } @@ -478,18 +477,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 +490,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 +514,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 4ed2f3cd6..ac24354c7 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 @@ -44,7 +45,6 @@ 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) @@ -69,7 +69,6 @@ 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) @@ -87,6 +86,7 @@ RTDEClient::~RTDEClient() { reconnecting_thread_.join(); } + stopBackgroundRead(); disconnect(); } @@ -121,8 +121,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 @@ -132,19 +130,8 @@ 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) { - // The state initializing is used inside disconnect to stop the pipeline again. client_state_ = ClientState::INITIALIZING; - // A running pipeline is needed inside setup. - 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; - } - pipeline_->run(); + prod_->setupProducer(max_num_tries, reconnection_time); uint16_t protocol_version = negotiateProtocolVersion(); // Protocol version must be above zero @@ -161,13 +148,29 @@ bool RTDEClient::setupCommunication(const size_t max_num_tries, const std::chron setTargetFrequency(); } + prod_->startProducer(); + is_rtde_comm_setup = is_rtde_comm_setup && setupOutputs(protocol_version); 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; } @@ -193,7 +196,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; @@ -243,7 +246,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; @@ -253,13 +256,14 @@ 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 { 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()); } @@ -296,13 +300,8 @@ void RTDEClient::resetOutputRecipe(const std::vector new_recipe) 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(); - 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) @@ -338,7 +337,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; @@ -432,7 +431,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; @@ -450,14 +449,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_); @@ -490,7 +486,6 @@ void RTDEClient::disconnect() } if (client_state_ >= ClientState::INITIALIZING) { - pipeline_->stop(); } if (client_state_ > ClientState::UNINITIALIZED) { @@ -498,6 +493,9 @@ void RTDEClient::disconnect() writer_.stop(); } client_state_ = ClientState::UNINITIALIZED; + prod_->stopProducer(); + stopBackgroundRead(); + notifier_.stopped("RTDE communication stopped"); } bool RTDEClient::isRobotBooted() @@ -507,7 +505,8 @@ bool RTDEClient::isRobotBooted() if (!sendStart()) return false; - std::unique_ptr package; + std::unique_ptr package = std::make_unique(output_recipe_); + 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 @@ -519,8 +518,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); @@ -539,7 +537,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; @@ -550,11 +548,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 @@ -573,6 +574,7 @@ bool RTDEClient::pause() return false; } + stopBackgroundRead(); if (sendPause()) { client_state_ = ClientState::PAUSED; @@ -600,7 +602,7 @@ bool RTDEClient::sendStart() 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; @@ -649,7 +651,7 @@ bool RTDEClient::sendPause() 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; @@ -709,19 +711,49 @@ std::vector RTDEClient::ensureTimestampIsPresent(const std::vector< std::unique_ptr RTDEClient::getDataPackage(std::chrono::milliseconds timeout) { - // Cannot get data packages while reconnecting as we could end up getting some of the configuration packages + static DataPackage data_pkg(getOutputRecipe()); + if (getDataPackage(data_pkg, timeout)) + { + return std::make_unique(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 (!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 (reconnect_mutex_.try_lock()) { - std::unique_ptr urpackage; - if (pipeline_->getLatestProduct(urpackage, timeout)) + if (new_data_.load()) { - rtde_interface::DataPackage* tmp = dynamic_cast(urpackage.get()); - if (tmp != nullptr) + 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) { - urpackage.release(); reconnect_mutex_.unlock(); - return std::unique_ptr(tmp); + return false; } + data_package = *dynamic_cast(data_buffer0_.get()); + new_data_.store(false); } reconnect_mutex_.unlock(); } @@ -730,9 +762,40 @@ std::unique_ptr RTDEClient::getDataPackage(std::chr URCL_LOG_WARN("Unable to get data package while reconnecting to the RTDE interface"); auto period = std::chrono::duration(1.0 / target_frequency_); std::this_thread::sleep_for(period); + return false; } + return true; +} - return std::unique_ptr(nullptr); +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 + std::unique_ptr base_package(data_package.release()); + if (reconnect_mutex_.try_lock()) + { + if (prod_->tryGet(base_package)) + { + reconnect_mutex_.unlock(); + data_package.reset(dynamic_cast(base_package.release())); + return true; + } + reconnect_mutex_.unlock(); + } + else + { + URCL_LOG_WARN("Unable to get data package while reconnecting to the RTDE interface"); + auto period = std::chrono::duration(1.0 / target_frequency_); + std::this_thread::sleep_for(period); + } + + return false; } std::string RTDEClient::getIP() const @@ -764,6 +827,7 @@ void RTDEClient::reconnect() // the RTDE connection std::lock_guard lock(reconnect_mutex_); ClientState cur_client_state = client_state_; + bool background_read_was_running = background_read_running_; disconnect(); const size_t max_initialization_attempts = 3; @@ -834,7 +898,7 @@ void RTDEClient::reconnect() return; } - start(); + start(background_read_was_running); if (cur_client_state == ClientState::PAUSED) { pause(); @@ -857,5 +921,61 @@ 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_); + data_buffer1_ = std::make_unique(output_recipe_); + + 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_) + { + if (prod_->tryGet(data_buffer1_)) + { + 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 + { + auto period = std::chrono::duration(1.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 000000000..5a6257075 --- /dev/null +++ b/src/rtde/rtde_parser.cpp @@ -0,0 +1,174 @@ +/* + * 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) +{ + 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 = getPackageTypeFromHeader(bp); + + switch (type) + { + case PackageType::RTDE_DATA_PACKAGE: + { + if (result == nullptr || result->getType() != PackageType::RTDE_DATA_PACKAGE) + { + result = std::make_unique(recipe_, protocol_version_); + URCL_LOG_WARN("The passed result pointer is empty or does not contain a DataPackage. A new DataPackage will " + "have to be allocated. Please pass a pre-allocated DataPackage if you expect a DataPackage " + "would be sent."); + } + + 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) + { + 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 2df149505..1937933c7 100644 --- a/src/rtde/rtde_writer.cpp +++ b/src/rtde/rtde_writer.cpp @@ -27,32 +27,79 @@ //---------------------------------------------------------------------- #include "ur_client_library/rtde/rtde_writer.h" +#include +#include "ur_client_library/log.h" namespace urcl { namespace rtde_interface { +std::vector RTDEWriter::g_preallocated_input_bit_register_keys; +std::vector RTDEWriter::g_preallocated_input_int_register_keys; +std::vector RTDEWriter::g_preallocated_input_double_register_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); + + g_preallocated_input_bit_register_keys.resize(128); + for (size_t i = 0; i < 128; ++i) + { + g_preallocated_input_bit_register_keys[i] = "input_bit_register_" + std::to_string(i); + } + + g_preallocated_input_int_register_keys.resize(48); + for (size_t i = 0; i < 48; ++i) + { + g_preallocated_input_int_register_keys[i] = "input_int_register_" + std::to_string(i); + } + + g_preallocated_input_double_register_keys.resize(48); + for (size_t i = 0; i < 48; ++i) + { + g_preallocated_input_double_register_keys[i] = "input_double_register_" + std::to_string(i); + } + + for (const auto& field : recipe) + { + if (field.size() >= 5 && field.substr(field.size() - 5) == "_mask") + { + used_masks_.push_back(field); + } + } } 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(); + 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 +109,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 +134,24 @@ 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) +{ + *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 +163,15 @@ 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); + markStorageToBeSent(); - if (success) - { - if (!queue_.tryEnqueue(std::unique_ptr(new DataPackage(package_)))) - { - return false; - } - } - mask = 0; - success = package_.setData("speed_slider_mask", mask); return success; } @@ -123,7 +185,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 +199,10 @@ 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); + markStorageToBeSent(); - if (success) - { - if (!queue_.tryEnqueue(std::unique_ptr(new DataPackage(package_)))) - { - return false; - } - } - mask = 0; - success = package_.setData("standard_digital_output_mask", mask); return success; } @@ -161,7 +217,10 @@ bool RTDEWriter::sendConfigurableDigitalOutput(uint8_t output_pin, bool value) return false; } - std::lock_guard guard(package_mutex_); + URCL_LOG_INFO("Write thread started."); + 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 +232,10 @@ 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); + markStorageToBeSent(); - if (success) - { - if (!queue_.tryEnqueue(std::unique_ptr(new DataPackage(package_)))) - { - return false; - } - } - mask = 0; - success = package_.setData("configurable_digital_output_mask", mask); return success; } @@ -198,7 +249,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 +263,10 @@ 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); + markStorageToBeSent(); - if (success) - { - if (!queue_.tryEnqueue(std::unique_ptr(new DataPackage(package_)))) - { - return false; - } - } - mask = 0; - success = package_.setData("tool_digital_output_mask", mask); return success; } @@ -242,29 +287,25 @@ 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); + markStorageToBeSent(); - if (success) - { - if (!queue_.tryEnqueue(std::unique_ptr(new DataPackage(package_)))) - { - return false; - } - } - mask = 0; - success = package_.setData("standard_analog_output_mask", mask); return success; } @@ -288,19 +329,10 @@ 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); + markStorageToBeSent(); - if (success) - { - if (!queue_.tryEnqueue(std::unique_ptr(new DataPackage(package_)))) - { - return false; - } - } return success; } @@ -314,19 +346,10 @@ 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); + markStorageToBeSent(); - if (success) - { - if (!queue_.tryEnqueue(std::unique_ptr(new DataPackage(package_)))) - { - return false; - } - } return success; } @@ -340,34 +363,46 @@ 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); + markStorageToBeSent(); - if (success) - { - if (!queue_.tryEnqueue(std::unique_ptr(new DataPackage(package_)))) - { - return false; - } - } 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); - if (success) + static const std::string key = "external_force_torque"; + std::lock_guard guard(store_mutex_); + bool success = current_store_buffer_->setData(key, external_force_torque); + markStorageToBeSent(); + return success; +} + +void RTDEWriter::resetMasks(const std::shared_ptr& buffer) +{ + for (const auto& mask_name : used_masks_) { - if (!queue_.tryEnqueue(std::unique_ptr(new DataPackage(package_)))) + // "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 3f5271f52..6e0a78c78 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/test_deprecated_ur_driver_construction.cpp b/tests/test_deprecated_ur_driver_construction.cpp index 08b55e65f..3f6ff2936 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 cba6d99ae..dd07fd7fa 100644 --- a/tests/test_primary_parser.cpp +++ b/tests/test_primary_parser.cpp @@ -27,106 +27,126 @@ //---------------------------------------------------------------------- #include +#include #include #include using namespace urcl; +/* First RobotState of UR5e from URSim v5.8 + * + * This package contains: + * - ROBOT_MODE_DATA + * - JOINT_DATA + * - CARTESIAN_INFO + * - KINEMATICS_INFO + * - NEEDED_FOR_CALIB_DATA + * - MASTERBOARD_DATA + * - TOOL_DATA + * - CONFIGURATION_DATA + * - FORCE_MODE_DATA + * - ADDITIONAL_INFO + * - SAFETY_DATA + * - TOOL_COMM_INFO + * - TOOL_MODE_INFO + */ +const unsigned char ROBOT_STATE[] = { + 0x00, 0x00, 0x05, 0x6a, 0x10, 0x00, 0x00, 0x00, 0x2f, 0x00, 0x00, 0x00, 0x00, 0x05, 0xf8, 0x7d, 0x17, 0x40, 0x01, + 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x07, 0x00, 0x3f, 0xef, 0x0a, 0x3d, 0x70, 0xa3, 0xd7, 0x0a, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfb, 0x01, + 0xbf, 0xf9, 0x9c, 0x77, 0x9a, 0x6b, 0x50, 0xb0, 0xbf, 0xf9, 0x9c, 0x77, 0x9a, 0x6b, 0x50, 0xb0, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0xa3, 0xa8, 0x79, 0x38, 0x00, 0x00, 0x00, 0x00, 0x41, 0xcc, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0xfd, 0xbf, 0xfb, 0xa2, 0x33, 0x9c, 0x0e, 0xbe, 0xe0, 0xbf, 0xfb, 0xa2, 0x33, 0x9c, 0x0e, 0xbe, 0xe0, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x09, 0x06, 0x5a, 0x00, 0x00, 0x00, 0x00, 0x41, 0xc8, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0xc0, 0x01, 0x9f, 0xbe, 0x76, 0xc8, 0xb4, 0x38, 0xc0, 0x01, 0x9f, 0xbe, 0x76, + 0xc8, 0xb4, 0x38, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xbf, 0x0f, 0xf7, 0x00, 0x00, 0x00, 0x00, + 0x41, 0xc4, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0xbf, 0xe9, 0xdb, 0x22, 0xd0, 0xe5, 0x60, 0x40, 0xbf, 0xe9, + 0xdb, 0x22, 0xd0, 0xe5, 0x60, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3e, 0x6e, 0xbb, 0xe2, 0x00, + 0x00, 0x00, 0x00, 0x41, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0x3f, 0xf9, 0x85, 0x87, 0x93, 0xdd, 0x97, + 0xf6, 0x3f, 0xf9, 0x85, 0x87, 0x93, 0xdd, 0x97, 0xf6, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3a, 0xe6, + 0x05, 0x69, 0x00, 0x00, 0x00, 0x00, 0x41, 0xbc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0xbf, 0x9f, 0xbe, 0x76, + 0xc8, 0xb4, 0x39, 0x00, 0xbf, 0x9f, 0xbe, 0x76, 0xc8, 0xb4, 0x39, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x41, 0xbc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0x00, + 0x00, 0x00, 0x65, 0x04, 0xbf, 0xc2, 0x6d, 0x90, 0xa0, 0x1d, 0xe7, 0x77, 0xbf, 0xdb, 0xe1, 0x32, 0xf6, 0xa8, 0x66, + 0x44, 0x3f, 0xc9, 0xdc, 0x1e, 0xb0, 0x03, 0x31, 0xe6, 0xbf, 0x54, 0x02, 0xc0, 0xf8, 0xe6, 0xe6, 0x79, 0x40, 0x08, + 0xee, 0x22, 0x63, 0x78, 0xfa, 0xe0, 0x3f, 0xa3, 0xe9, 0xa4, 0x23, 0x7a, 0x7b, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xe1, 0x05, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xbf, 0xdb, 0x33, 0x33, 0x33, + 0x33, 0x33, 0x33, 0xbf, 0xd9, 0x19, 0xce, 0x07, 0x5f, 0x6f, 0xd2, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xc4, 0xcc, + 0xcc, 0xcc, 0xcc, 0xcc, 0xcd, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x3f, 0xc1, 0x0f, 0xf9, 0x72, 0x47, 0x45, 0x39, 0x3f, 0xb9, 0x85, 0xf0, 0x6f, 0x69, 0x44, 0x67, 0x3f, + 0xb9, 0x7f, 0x62, 0xb6, 0xae, 0x7d, 0x56, 0x3f, 0xf9, 0x21, 0xfb, 0x54, 0x52, 0x45, 0x50, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xf9, 0x21, 0xfb, 0x54, 0x52, 0x45, + 0x50, 0xbf, 0xf9, 0x21, 0xfb, 0x54, 0x52, 0x45, 0x50, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x35, 0x09, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x4b, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0x70, 0x62, 0x4d, 0xe0, 0x00, 0x00, + 0x00, 0x3f, 0x70, 0x62, 0x4d, 0xe0, 0x00, 0x00, 0x00, 0x41, 0xc0, 0x00, 0x00, 0x42, 0x40, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x57, 0xf4, 0x28, 0x5b, 0x01, 0x01, 0x01, 0x00, 0x00, 0x00, + 0x25, 0x02, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0x00, 0x00, 0x01, 0xbd, + 0x06, 0xc0, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0x40, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0xc0, 0x19, + 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0x40, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0xc0, 0x19, 0x57, 0x99, 0x28, + 0x2c, 0x2f, 0x63, 0x40, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0xc0, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, + 0x40, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0xc0, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0x40, 0x19, 0x57, + 0x99, 0x28, 0x2c, 0x2f, 0x63, 0xc0, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0x40, 0x19, 0x57, 0x99, 0x28, 0x2c, + 0x2f, 0x63, 0x40, 0x0a, 0xbb, 0x94, 0xed, 0xdd, 0xc6, 0xb1, 0x40, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, + 0x0a, 0xbb, 0x94, 0xed, 0xdd, 0xc6, 0xb1, 0x40, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x0a, 0xbb, 0x94, + 0xed, 0xdd, 0xc6, 0xb1, 0x40, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x0a, 0xbb, 0x94, 0xed, 0xdd, 0xc6, + 0xb1, 0x40, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x0a, 0xbb, 0x94, 0xed, 0xdd, 0xc6, 0xb1, 0x40, 0x44, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x0a, 0xbb, 0x94, 0xed, 0xdd, 0xc6, 0xb1, 0x40, 0x44, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x3f, 0xf0, 0xc1, 0x52, 0x38, 0x2d, 0x73, 0x65, 0x3f, 0xf6, 0x57, 0x18, 0x4a, 0xe7, 0x44, 0x87, + 0x3f, 0xd0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xf3, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x3f, 0xd0, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xbf, 0xdb, 0x33, 0x33, 0x33, 0x33, + 0x33, 0x33, 0xbf, 0xd9, 0x19, 0xce, 0x07, 0x5f, 0x6f, 0xd2, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xc4, 0xcc, 0xcc, + 0xcc, 0xcc, 0xcc, 0xcd, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x3f, 0xc1, 0x0f, 0xf9, 0x72, 0x47, 0x45, 0x39, 0x3f, 0xb9, 0x85, 0xf0, 0x6f, 0x69, 0x44, 0x67, 0x3f, 0xb9, + 0x7f, 0x62, 0xb6, 0xae, 0x7d, 0x56, 0x3f, 0xf9, 0x21, 0xfb, 0x54, 0x52, 0x45, 0x50, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xf9, 0x21, 0xfb, 0x54, 0x52, 0x45, 0x50, + 0xbf, 0xf9, 0x21, 0xfb, 0x54, 0x52, 0x45, 0x50, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00, 0x01, + 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x3d, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x3f, 0x6c, 0xf5, 0xac, 0x1d, 0xb9, 0xa1, 0x08, 0x00, 0x00, 0x00, 0x09, 0x08, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, + 0x00, 0x2b, 0x0a, 0x57, 0xf4, 0x28, 0x5b, 0x01, 0x01, 0xbf, 0xb8, 0x4d, 0xc2, 0x84, 0x9f, 0xed, 0xcf, 0xbf, 0xb0, + 0x37, 0x9e, 0xd0, 0x87, 0xba, 0x97, 0x3f, 0xe2, 0xa2, 0x5b, 0x78, 0xc3, 0x9f, 0x6c, 0x3f, 0xc0, 0xa3, 0xd7, 0x0a, + 0x3d, 0x70, 0xa4, 0x00, 0x00, 0x00, 0x1a, 0x0b, 0x00, 0x00, 0x01, 0xc2, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x01, 0x3f, 0xc0, 0x00, 0x00, 0x40, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x08, 0x0c, 0x00, 0x01, 0x01 +}; +// Create a VersionMessage +const unsigned char VERSION_MESSAGE[] = { // message size + 0x00, 0x00, 0x00, 0x37, + // message type + 0x14, + // timestamp + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + // source + 0xfe, + // robot_message_type + 0x03, + // payload + 0x09, 0x55, 0x52, 0x43, 0x6f, 0x6e, 0x74, 0x72, 0x6f, 0x6c, 0x05, 0x18, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x30, 0x34, 0x2d, 0x31, 0x30, 0x2d, 0x32, 0x30, 0x32, 0x35, 0x2c, 0x20, 0x30, 0x30, 0x3a, 0x32, 0x35, 0x3a, + 0x33, 0x33 +}; + TEST(primary_parser, parse_calibration_data) { - /* First RobotState of UR5e from URSim v5.8 - * - * This package contains: - * - ROBOT_MODE_DATA - * - JOINT_DATA - * - CARTESIAN_INFO - * - KINEMATICS_INFO - * - NEEDED_FOR_CALIB_DATA - * - MASTERBOARD_DATA - * - TOOL_DATA - * - CONFIGURATION_DATA - * - FORCE_MODE_DATA - * - ADDITIONAL_INFO - * - SAFETY_DATA - * - TOOL_COMM_INFO - * - TOOL_MODE_INFO - */ - unsigned char raw_data[] = { - 0x00, 0x00, 0x05, 0x6a, 0x10, 0x00, 0x00, 0x00, 0x2f, 0x00, 0x00, 0x00, 0x00, 0x05, 0xf8, 0x7d, 0x17, 0x40, 0x01, - 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x07, 0x00, 0x3f, 0xef, 0x0a, 0x3d, 0x70, 0xa3, 0xd7, 0x0a, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfb, 0x01, - 0xbf, 0xf9, 0x9c, 0x77, 0x9a, 0x6b, 0x50, 0xb0, 0xbf, 0xf9, 0x9c, 0x77, 0x9a, 0x6b, 0x50, 0xb0, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0xa3, 0xa8, 0x79, 0x38, 0x00, 0x00, 0x00, 0x00, 0x41, 0xcc, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0xfd, 0xbf, 0xfb, 0xa2, 0x33, 0x9c, 0x0e, 0xbe, 0xe0, 0xbf, 0xfb, 0xa2, 0x33, 0x9c, 0x0e, 0xbe, 0xe0, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x09, 0x06, 0x5a, 0x00, 0x00, 0x00, 0x00, 0x41, 0xc8, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0xc0, 0x01, 0x9f, 0xbe, 0x76, 0xc8, 0xb4, 0x38, 0xc0, 0x01, 0x9f, 0xbe, 0x76, - 0xc8, 0xb4, 0x38, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xbf, 0x0f, 0xf7, 0x00, 0x00, 0x00, 0x00, - 0x41, 0xc4, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0xbf, 0xe9, 0xdb, 0x22, 0xd0, 0xe5, 0x60, 0x40, 0xbf, 0xe9, - 0xdb, 0x22, 0xd0, 0xe5, 0x60, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3e, 0x6e, 0xbb, 0xe2, 0x00, - 0x00, 0x00, 0x00, 0x41, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0x3f, 0xf9, 0x85, 0x87, 0x93, 0xdd, 0x97, - 0xf6, 0x3f, 0xf9, 0x85, 0x87, 0x93, 0xdd, 0x97, 0xf6, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3a, 0xe6, - 0x05, 0x69, 0x00, 0x00, 0x00, 0x00, 0x41, 0xbc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0xbf, 0x9f, 0xbe, 0x76, - 0xc8, 0xb4, 0x39, 0x00, 0xbf, 0x9f, 0xbe, 0x76, 0xc8, 0xb4, 0x39, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x41, 0xbc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0x00, - 0x00, 0x00, 0x65, 0x04, 0xbf, 0xc2, 0x6d, 0x90, 0xa0, 0x1d, 0xe7, 0x77, 0xbf, 0xdb, 0xe1, 0x32, 0xf6, 0xa8, 0x66, - 0x44, 0x3f, 0xc9, 0xdc, 0x1e, 0xb0, 0x03, 0x31, 0xe6, 0xbf, 0x54, 0x02, 0xc0, 0xf8, 0xe6, 0xe6, 0x79, 0x40, 0x08, - 0xee, 0x22, 0x63, 0x78, 0xfa, 0xe0, 0x3f, 0xa3, 0xe9, 0xa4, 0x23, 0x7a, 0x7b, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xe1, 0x05, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xbf, 0xdb, 0x33, 0x33, 0x33, - 0x33, 0x33, 0x33, 0xbf, 0xd9, 0x19, 0xce, 0x07, 0x5f, 0x6f, 0xd2, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xc4, 0xcc, - 0xcc, 0xcc, 0xcc, 0xcc, 0xcd, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x3f, 0xc1, 0x0f, 0xf9, 0x72, 0x47, 0x45, 0x39, 0x3f, 0xb9, 0x85, 0xf0, 0x6f, 0x69, 0x44, 0x67, 0x3f, - 0xb9, 0x7f, 0x62, 0xb6, 0xae, 0x7d, 0x56, 0x3f, 0xf9, 0x21, 0xfb, 0x54, 0x52, 0x45, 0x50, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xf9, 0x21, 0xfb, 0x54, 0x52, 0x45, - 0x50, 0xbf, 0xf9, 0x21, 0xfb, 0x54, 0x52, 0x45, 0x50, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x35, 0x09, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x4b, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0x70, 0x62, 0x4d, 0xe0, 0x00, 0x00, - 0x00, 0x3f, 0x70, 0x62, 0x4d, 0xe0, 0x00, 0x00, 0x00, 0x41, 0xc0, 0x00, 0x00, 0x42, 0x40, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x57, 0xf4, 0x28, 0x5b, 0x01, 0x01, 0x01, 0x00, 0x00, 0x00, - 0x25, 0x02, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0x00, 0x00, 0x01, 0xbd, - 0x06, 0xc0, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0x40, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0xc0, 0x19, - 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0x40, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0xc0, 0x19, 0x57, 0x99, 0x28, - 0x2c, 0x2f, 0x63, 0x40, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0xc0, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, - 0x40, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0xc0, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0x40, 0x19, 0x57, - 0x99, 0x28, 0x2c, 0x2f, 0x63, 0xc0, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0x40, 0x19, 0x57, 0x99, 0x28, 0x2c, - 0x2f, 0x63, 0x40, 0x0a, 0xbb, 0x94, 0xed, 0xdd, 0xc6, 0xb1, 0x40, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, - 0x0a, 0xbb, 0x94, 0xed, 0xdd, 0xc6, 0xb1, 0x40, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x0a, 0xbb, 0x94, - 0xed, 0xdd, 0xc6, 0xb1, 0x40, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x0a, 0xbb, 0x94, 0xed, 0xdd, 0xc6, - 0xb1, 0x40, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x0a, 0xbb, 0x94, 0xed, 0xdd, 0xc6, 0xb1, 0x40, 0x44, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x0a, 0xbb, 0x94, 0xed, 0xdd, 0xc6, 0xb1, 0x40, 0x44, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x3f, 0xf0, 0xc1, 0x52, 0x38, 0x2d, 0x73, 0x65, 0x3f, 0xf6, 0x57, 0x18, 0x4a, 0xe7, 0x44, 0x87, - 0x3f, 0xd0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xf3, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x3f, 0xd0, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xbf, 0xdb, 0x33, 0x33, 0x33, 0x33, - 0x33, 0x33, 0xbf, 0xd9, 0x19, 0xce, 0x07, 0x5f, 0x6f, 0xd2, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xc4, 0xcc, 0xcc, - 0xcc, 0xcc, 0xcc, 0xcd, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x3f, 0xc1, 0x0f, 0xf9, 0x72, 0x47, 0x45, 0x39, 0x3f, 0xb9, 0x85, 0xf0, 0x6f, 0x69, 0x44, 0x67, 0x3f, 0xb9, - 0x7f, 0x62, 0xb6, 0xae, 0x7d, 0x56, 0x3f, 0xf9, 0x21, 0xfb, 0x54, 0x52, 0x45, 0x50, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xf9, 0x21, 0xfb, 0x54, 0x52, 0x45, 0x50, - 0xbf, 0xf9, 0x21, 0xfb, 0x54, 0x52, 0x45, 0x50, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00, 0x01, - 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x3d, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x3f, 0x6c, 0xf5, 0xac, 0x1d, 0xb9, 0xa1, 0x08, 0x00, 0x00, 0x00, 0x09, 0x08, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, - 0x00, 0x2b, 0x0a, 0x57, 0xf4, 0x28, 0x5b, 0x01, 0x01, 0xbf, 0xb8, 0x4d, 0xc2, 0x84, 0x9f, 0xed, 0xcf, 0xbf, 0xb0, - 0x37, 0x9e, 0xd0, 0x87, 0xba, 0x97, 0x3f, 0xe2, 0xa2, 0x5b, 0x78, 0xc3, 0x9f, 0x6c, 0x3f, 0xc0, 0xa3, 0xd7, 0x0a, - 0x3d, 0x70, 0xa4, 0x00, 0x00, 0x00, 0x1a, 0x0b, 0x00, 0x00, 0x01, 0xc2, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x01, 0x3f, 0xc0, 0x00, 0x00, 0x40, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x08, 0x0c, 0x00, 0x01, 0x01 - }; + unsigned char raw_data[sizeof(ROBOT_STATE)]; + memcpy(raw_data, ROBOT_STATE, sizeof(ROBOT_STATE)); comm::BinParser bp(raw_data, sizeof(raw_data)); std::vector> products; @@ -146,6 +166,42 @@ 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::unique_ptr product; + primary_interface::PrimaryParser parser; + ASSERT_TRUE(parser.parse(bp, product)); + + 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); + EXPECT_EQ(data->svn_version_, 0); + EXPECT_EQ(data->build_date_, "04-10-2025, 00:25:33"); + EXPECT_EQ(data->project_name_, "URControl"); + } + else + { + FAIL() << "Parsed package is not of type VersionMessage"; + } +} + int main(int argc, char* argv[]) { ::testing::InitGoogleTest(&argc, argv); diff --git a/tests/test_rtde_client.cpp b/tests/test_rtde_client.cpp index 37d1296bf..ba67c5aaa 100644 --- a/tests/test_rtde_client.cpp +++ b/tests/test_rtde_client.cpp @@ -30,10 +30,10 @@ #include #include #include -#include #include #include #include +#include "ur_client_library/comm/tcp_server.h" #include "ur_client_library/exceptions.h" #include @@ -43,19 +43,30 @@ using namespace urcl; std::string g_ROBOT_IP = "192.168.56.101"; +class TestableRTDEClient : public rtde_interface::RTDEClient +{ +public: + using RTDEClient::RTDEClient; + + void triggerReconnect() + { + this->reconnect(); + } +}; + class RTDEClientTest : public ::testing::Test { protected: void SetUp() { - client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file_)); + client_.reset(new TestableRTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file_)); } void TearDown() { client_.reset(); // If we don't sleep we can get a conflict between two tests controlling the same rtde inputs - std::this_thread::sleep_for(std::chrono::seconds(1)); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); } std::string output_recipe_file_ = "resources/rtde_output_recipe.txt"; @@ -63,7 +74,7 @@ class RTDEClientTest : public ::testing::Test std::string docs_output_recipe_file_ = "resources/docs_rtde_output_recipe.txt"; std::string input_recipe_file_ = "resources/rtde_input_recipe.txt"; comm::INotifier notifier_; - std::unique_ptr client_; + std::unique_ptr client_; std::vector resources_output_recipe_ = { "timestamp", "actual_q", @@ -117,16 +128,14 @@ TEST_F(RTDEClientTest, no_recipe) { std::string output_recipe_file = ""; std::string input_recipe_file = ""; - EXPECT_THROW( - client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, output_recipe_file, input_recipe_file)), - UrException); + EXPECT_THROW(client_.reset(new TestableRTDEClient(g_ROBOT_IP, notifier_, output_recipe_file, input_recipe_file)), + UrException); // Only input recipe is unconfigured - EXPECT_NO_THROW( - client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file))); + EXPECT_NO_THROW(client_.reset(new TestableRTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file))); - EXPECT_THROW(client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, - "/i/do/not/exist/urclrtdetest.txt")), + EXPECT_THROW(client_.reset(new TestableRTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, + "/i/do/not/exist/urclrtdetest.txt")), UrException); } @@ -134,26 +143,22 @@ TEST_F(RTDEClientTest, empty_recipe_file) { std::string output_recipe_file = "resources/empty.txt"; std::string input_recipe_file = "resources/empty.txt"; - EXPECT_THROW( - client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, output_recipe_file, input_recipe_file)), - UrException); + EXPECT_THROW(client_.reset(new TestableRTDEClient(g_ROBOT_IP, notifier_, output_recipe_file, input_recipe_file)), + UrException); // Only input recipe is empty - EXPECT_THROW( - client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file)), - UrException); + EXPECT_THROW(client_.reset(new TestableRTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file)), + UrException); } TEST_F(RTDEClientTest, invalid_target_frequency) { // Setting target frequency below 0 or above 500, should throw an exception - client_.reset( - new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file_, -1.0, false)); + client_.reset(new TestableRTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file_, -1.0, false)); EXPECT_THROW(client_->init(), UrException); - client_.reset( - new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file_, 1000, false)); + client_.reset(new TestableRTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file_, 1000, false)); EXPECT_THROW(client_->init(), UrException); } @@ -172,8 +177,12 @@ TEST_F(RTDEClientTest, unconfigured_target_frequency) TEST_F(RTDEClientTest, set_target_frequency) { + // Set a target frequency that is different from the maximum frequency but a factor of it. + // Since we check timestamp differences, we need to make sure that the target frequency is + // achievable. 25 Hz is a factor of both 125 Hz and 500 Hz. + const double target_frequency = 25.0; client_.reset( - new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file_, 1, false)); + new TestableRTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file_, target_frequency, false)); client_->init(); // Maximum frequency should still be equal to the robot's maximum frequency @@ -188,35 +197,23 @@ TEST_F(RTDEClientTest, set_target_frequency) EXPECT_EQ(client_->getMaxFrequency(), expected_max_frequency); } - double expected_target_frequency = 1; - EXPECT_EQ(client_->getTargetFrequency(), expected_target_frequency); + EXPECT_EQ(client_->getTargetFrequency(), target_frequency); - EXPECT_TRUE(client_->start()); + EXPECT_TRUE(client_->start(false)); - // Test that we receive packages with a frequency of 2 Hz - const std::chrono::milliseconds read_timeout{ 10000 }; - std::unique_ptr data_pkg = client_->getDataPackage(read_timeout); - if (data_pkg == nullptr) - { - std::cout << "Failed to get data package from robot" << std::endl; - GTEST_FAIL(); - } + // Test that we receive packages with a frequency of `target_frequency` Hz + auto data_pkg = std::make_unique(client_->getOutputRecipe()); + ASSERT_TRUE(client_->getDataPackageBlocking(data_pkg)); double first_time_stamp = 0.0; data_pkg->getData("timestamp", first_time_stamp); - data_pkg = client_->getDataPackage(read_timeout); - if (data_pkg == nullptr) - { - std::cout << "Failed to get data package from robot" << std::endl; - GTEST_FAIL(); - } - + ASSERT_TRUE(client_->getDataPackageBlocking(data_pkg)); double second_time_stamp = 0.0; data_pkg->getData("timestamp", second_time_stamp); - // There should be 1 second between each timestamp - EXPECT_NEAR(second_time_stamp - first_time_stamp, 1, 1e-6); + // There should be 0.1 second between each timestamp + EXPECT_NEAR(second_time_stamp - first_time_stamp, 1.0 / target_frequency, 1e-6); client_->pause(); } @@ -271,7 +268,16 @@ TEST_F(RTDEClientTest, output_recipe_file) } } -TEST_F(RTDEClientTest, recipe_compairson) +TEST_F(RTDEClientTest, input_recipe_with_invalid_key) +{ + std::vector actual_input_recipe = resources_input_recipe_; + actual_input_recipe.push_back("i_do_not_exist"); + + client_.reset(new TestableRTDEClient(g_ROBOT_IP, notifier_, resources_output_recipe_, actual_input_recipe)); + EXPECT_THROW(client_->init(), RTDEInvalidKeyException); +} + +TEST_F(RTDEClientTest, recipe_comparison) { // Check that vectorized constructor provides same recipes as from file auto client = rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, resources_output_recipe_, resources_input_recipe_); @@ -284,14 +290,17 @@ TEST_F(RTDEClientTest, recipe_compairson) } } -TEST_F(RTDEClientTest, get_data_package) +TEST_F(RTDEClientTest, get_data_package_w_background_deprecated) { client_->init(); client_->start(); // Test that we can receive a package and extract data from the received package const std::chrono::milliseconds read_timeout{ 100 }; +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" std::unique_ptr data_pkg = client_->getDataPackage(read_timeout); +#pragma GCC diagnostic pop if (data_pkg == nullptr) { std::cout << "Failed to get data package from robot" << std::endl; @@ -304,6 +313,115 @@ TEST_F(RTDEClientTest, get_data_package) client_->pause(); } +TEST_F(RTDEClientTest, get_data_package_w_background) +{ + client_->init(); + client_->start(); + + // Test that we can receive a package and extract data from the received package + const std::chrono::milliseconds read_timeout{ 100 }; + + // Create an empty data package. Its timestamp should be 0.0 + rtde_interface::DataPackage data_pkg(client_->getOutputRecipe()); + double timestamp; + EXPECT_TRUE(data_pkg.getData("timestamp", timestamp)); + ASSERT_TRUE(data_pkg.setData("timestamp", 0.0)); + + ASSERT_TRUE(client_->getDataPackage(data_pkg, read_timeout)); + + // Verify that we actually got data from the robot + EXPECT_TRUE(data_pkg.getData("timestamp", timestamp)); + EXPECT_GT(timestamp, 0.0); + + ASSERT_TRUE(client_->getDataPackage(data_pkg, read_timeout)); + // Trying to get data with a very short timeout should fail + ASSERT_FALSE(client_->getDataPackage(data_pkg, std::chrono::milliseconds(1))); + + // Check the second signature + auto data_pkg_ptr = std::make_unique(client_->getOutputRecipe()); + ASSERT_TRUE(data_pkg_ptr->setData("timestamp", 0.0)); + ASSERT_TRUE(client_->getDataPackage(data_pkg_ptr, std::chrono::milliseconds(100))); + EXPECT_TRUE(data_pkg_ptr->getData("timestamp", timestamp)); + EXPECT_GT(timestamp, 0.0); + + // Blocking call while packages are fetched in background should fail + ASSERT_FALSE(client_->getDataPackageBlocking(data_pkg_ptr)); + + // starting the background read twice should be fine (no effect) + ASSERT_NO_THROW(client_->startBackgroundRead()); + + ASSERT_NO_THROW(client_->stopBackgroundRead()); + EXPECT_FALSE(client_->getDataPackage(data_pkg_ptr, std::chrono::milliseconds(100))); + ASSERT_NO_THROW(client_->startBackgroundRead()); + EXPECT_TRUE(client_->getDataPackage(data_pkg_ptr, std::chrono::milliseconds(100))); + + client_->pause(); +} + +TEST_F(RTDEClientTest, get_data_package_wo_background) +{ + client_->init(); + client_->start(false); + + auto data_pkg = std::make_unique(client_->getOutputRecipe()); + ASSERT_TRUE(data_pkg->setData("timestamp", 0.0)); + ASSERT_TRUE(client_->getDataPackageBlocking(data_pkg)); + + urcl::vector6d_t actual_q; + EXPECT_TRUE(data_pkg->getData("actual_q", actual_q)); + double timestamp; + EXPECT_TRUE(data_pkg->getData("timestamp", timestamp)); + EXPECT_GT(timestamp, 0.0); + + // Non-blocking call should fail since we are not reading in background + ASSERT_FALSE(client_->getDataPackage(data_pkg, std::chrono::milliseconds(100))); + ASSERT_FALSE(client_->getDataPackage(*data_pkg, std::chrono::milliseconds(100))); + + // We should be able to start background reading while the client is started without background + // reading and then query packages using the non-blocking call + client_->startBackgroundRead(); + ASSERT_TRUE(client_->getDataPackage(data_pkg, std::chrono::milliseconds(100))); + double timestamp_2; + EXPECT_TRUE(data_pkg->getData("timestamp", timestamp_2)); + EXPECT_GT(timestamp_2, timestamp); + EXPECT_FALSE(client_->getDataPackageBlocking(data_pkg)); + + client_->pause(); +} + +TEST_F(RTDEClientTest, reconnect_rtde_client) +{ + client_->init(); + client_->start(); + + // Test that we can receive a package and extract data from the received package + const std::chrono::milliseconds read_timeout{ 100 }; + rtde_interface::DataPackage data_pkg(client_->getOutputRecipe()); + ASSERT_TRUE(data_pkg.setData("timestamp", 0.0)); + ASSERT_TRUE(client_->getDataPackage(data_pkg, read_timeout)); + + double timestamp; + EXPECT_TRUE(data_pkg.getData("timestamp", timestamp)); + EXPECT_GT(timestamp, 0.0); + + std::thread reconnection_thread([this]() { client_->triggerReconnect(); }); + + // Give some time to ensure that the reconnection has started. TODO: A proper thread sync + // mechanism would be better here. + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + // Trying to get data packages while disconnected should fail + ASSERT_FALSE(client_->getDataPackage(data_pkg, read_timeout)); + + reconnection_thread.join(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + ASSERT_TRUE(client_->getDataPackage(data_pkg, read_timeout)); + double timestamp_2; + EXPECT_TRUE(data_pkg.getData("timestamp", timestamp_2)); + EXPECT_GT(timestamp_2, timestamp); + + client_->pause(); +} + TEST_F(RTDEClientTest, write_rtde_data) { client_->init(); @@ -314,15 +432,12 @@ TEST_F(RTDEClientTest, write_rtde_data) // Make sure that the data has been written to the robot const std::chrono::milliseconds read_timeout{ 100 }; - std::unique_ptr data_pkg = client_->getDataPackage(read_timeout); - if (data_pkg == nullptr) - { - std::cout << "Failed to get data package from robot" << std::endl; - GTEST_FAIL(); - } + + rtde_interface::DataPackage data_pkg(client_->getOutputRecipe()); + ASSERT_TRUE(client_->getDataPackage(data_pkg, read_timeout)); std::bitset<18> actual_dig_out_bits; - data_pkg->getData("actual_digital_output_bits", actual_dig_out_bits); + data_pkg.getData("actual_digital_output_bits", actual_dig_out_bits); // If we get the data package to soon the digital output might not have been updated, therefore we get the package a // couple of times @@ -330,8 +445,8 @@ TEST_F(RTDEClientTest, write_rtde_data) int counter = 0; while (actual_dig_out_bits[0] != send_digital_output) { - data_pkg = client_->getDataPackage(read_timeout); - data_pkg->getData("actual_digital_output_bits", actual_dig_out_bits); + ASSERT_TRUE(client_->getDataPackage(data_pkg, read_timeout)); + data_pkg.getData("actual_digital_output_bits", actual_dig_out_bits); if (counter == max_tries) { break; @@ -347,7 +462,7 @@ TEST_F(RTDEClientTest, write_rtde_data) TEST_F(RTDEClientTest, output_recipe_without_timestamp) { std::string output_recipe_file = "resources/rtde_output_recipe_without_timestamp.txt"; - client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, output_recipe_file, input_recipe_file_)); + client_.reset(new TestableRTDEClient(g_ROBOT_IP, notifier_, output_recipe_file, input_recipe_file_)); std::vector actual_output_recipe_from_file = client_->getOutputRecipe(); const std::string timestamp = "timestamp"; @@ -365,10 +480,9 @@ TEST_F(RTDEClientTest, output_recipe_without_timestamp) TEST_F(RTDEClientTest, connect_non_running_robot) { // We use an IP address on the integration_test's subnet - client_.reset( - new rtde_interface::RTDEClient("192.168.56.123", notifier_, resources_output_recipe_, resources_input_recipe_)); + client_.reset(new TestableRTDEClient("127.0.0.1", notifier_, resources_output_recipe_, resources_input_recipe_)); auto start = std::chrono::system_clock::now(); - EXPECT_THROW(client_->init(2, std::chrono::milliseconds(500), 1), UrException); + EXPECT_THROW(client_->init(2, std::chrono::milliseconds(50), 1), UrException); auto end = std::chrono::system_clock::now(); auto elapsed = end - start; // This is only a rough estimate, obviously. @@ -398,24 +512,20 @@ TEST_F(RTDEClientTest, check_all_rtde_output_variables_exist) client_->init(); // Ignore unknown output variables to account for variables not available in old urcontrol versions. - client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, exhaustive_output_recipe_file_, - input_recipe_file_, 0.0, false)); + client_.reset( + new TestableRTDEClient(g_ROBOT_IP, notifier_, exhaustive_output_recipe_file_, input_recipe_file_, 0.0, false)); EXPECT_TRUE(client_->init()); client_->start(); // Test that we can receive and parse the timestamp from the received package to prove the setup was successful const std::chrono::milliseconds read_timeout{ 100 }; - std::unique_ptr data_pkg = client_->getDataPackage(read_timeout); - - if (data_pkg == nullptr) - { - std::cout << "Failed to get data package from robot" << std::endl; - GTEST_FAIL(); - } + rtde_interface::DataPackage data_pkg(client_->getOutputRecipe()); + ASSERT_TRUE(client_->getDataPackage(data_pkg, read_timeout)); double timestamp; - EXPECT_TRUE(data_pkg->getData("timestamp", timestamp)); + EXPECT_TRUE(data_pkg.getData("timestamp", timestamp)); + EXPECT_GT(timestamp, 0.0); client_->pause(); } @@ -472,54 +582,72 @@ TEST_F(RTDEClientTest, check_unknown_rtde_output_variable) std::vector incorrect_output_recipe = client_->getOutputRecipe(); incorrect_output_recipe.push_back("unknown_rtde_variable"); - client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, incorrect_output_recipe, resources_input_recipe_, - 0.0, false)); - + // If unknown variables are not ignored, initialization should fail + client_.reset( + new TestableRTDEClient(g_ROBOT_IP, notifier_, incorrect_output_recipe, resources_input_recipe_, 0.0, false)); EXPECT_THROW(client_->init(), UrException); + + // Unknown variables can be ignored, so initialization should succeed + client_.reset( + new TestableRTDEClient(g_ROBOT_IP, notifier_, incorrect_output_recipe, resources_input_recipe_, 0.0, true)); + EXPECT_TRUE(client_->init()); } TEST_F(RTDEClientTest, empty_input_recipe) { std::vector empty_input_recipe = {}; - client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, resources_output_recipe_, empty_input_recipe)); + client_.reset(new TestableRTDEClient(g_ROBOT_IP, notifier_, resources_output_recipe_, empty_input_recipe)); client_->init(); client_->start(); // Test that we can receive and parse the timestamp from the received package to prove the setup was successful const std::chrono::milliseconds read_timeout{ 100 }; - std::unique_ptr data_pkg = client_->getDataPackage(read_timeout); - - if (data_pkg == nullptr) - { - std::cout << "Failed to get data package from robot" << std::endl; - GTEST_FAIL(); - } + rtde_interface::DataPackage data_pkg(client_->getOutputRecipe()); + ASSERT_TRUE(client_->getDataPackage(data_pkg, read_timeout)); double timestamp; - EXPECT_TRUE(data_pkg->getData("timestamp", timestamp)); + EXPECT_TRUE(data_pkg.getData("timestamp", timestamp)); EXPECT_FALSE(client_->getWriter().sendStandardDigitalOutput(1, false)); client_->pause(); - client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, "")); + client_.reset(new TestableRTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, "")); client_->init(); client_->start(); - data_pkg = client_->getDataPackage(read_timeout); - - if (data_pkg == nullptr) - { - std::cout << "Failed to get data package from robot" << std::endl; - GTEST_FAIL(); - } - EXPECT_TRUE(data_pkg->getData("timestamp", timestamp)); + ASSERT_TRUE(client_->getDataPackage(data_pkg, read_timeout)); + EXPECT_TRUE(data_pkg.getData("timestamp", timestamp)); EXPECT_FALSE(client_->getWriter().sendStandardDigitalOutput(1, false)); client_->pause(); } +TEST_F(RTDEClientTest, test_initialization) +{ + // Test that initialization fails with 0 initialization attempts + EXPECT_THROW(client_->init(1, std::chrono::milliseconds(100), 0), UrException); + + comm::TCPServer dummy_server(UR_RTDE_PORT); + dummy_server.start(); + + // Test that initialization fails when no RTDE interface is available on the robot + // within the given initialization attempts + // We use a dummy server here that doesn't implement the RTDE interface + // to simulate this scenario. + // The total time should be at least (initialization attempts - 1) * initialization timeout + // since the last attempt doesn't wait after failing. + + URCL_LOG_INFO("Starting initialization timing test"); + client_.reset(new TestableRTDEClient("127.0.0.1", notifier_, resources_output_recipe_, {})); + auto start = std::chrono::system_clock::now(); + EXPECT_THROW(client_->init(2, std::chrono::milliseconds(10), 2, std::chrono::milliseconds(10)), UrException); + auto end = std::chrono::system_clock::now(); + auto elapsed = end - start; + EXPECT_GE(std::chrono::duration_cast(elapsed).count(), 20); +} + int main(int argc, char* argv[]) { ::testing::InitGoogleTest(&argc, argv); @@ -534,4 +662,4 @@ int main(int argc, char* argv[]) } return RUN_ALL_TESTS(); -} \ No newline at end of file +} diff --git a/tests/test_rtde_data_package.cpp b/tests/test_rtde_data_package.cpp index e1c34298e..f641fcf21 100644 --- a/tests/test_rtde_data_package.cpp +++ b/tests/test_rtde_data_package.cpp @@ -36,7 +36,6 @@ TEST(rtde_data_package, serialize_pkg) { std::vector recipe{ "speed_slider_mask" }; rtde_interface::DataPackage package(recipe); - package.initEmpty(); uint32_t value = 1; package.setData("speed_slider_mask", value); @@ -59,7 +58,6 @@ TEST(rtde_data_package, parse_pkg_protocolv2) { std::vector recipe{ "timestamp", "actual_q" }; rtde_interface::DataPackage package(recipe); - package.initEmpty(); uint8_t data_package[] = { 0x01, 0x40, 0xd0, 0x75, 0x8c, 0x49, 0xba, 0x5e, 0x35, 0xbf, 0xf9, 0x9c, 0x77, 0xd1, 0x10, 0xb4, 0x60, 0xbf, 0xfb, 0xa2, 0x33, 0xd1, 0x10, 0xb4, 0x60, 0xc0, 0x01, 0x9f, 0xbe, 0x68, @@ -93,7 +91,6 @@ TEST(rtde_data_package, parse_pkg_protocolv1) { std::vector recipe{ "timestamp", "actual_q" }; rtde_interface::DataPackage package(recipe, 1); - package.initEmpty(); uint8_t data_package[] = { 0x40, 0xd0, 0x75, 0x8c, 0x49, 0xba, 0x5e, 0x35, 0xbf, 0xf9, 0x9c, 0x77, 0xd1, 0x10, 0xb4, 0x60, 0xbf, 0xfb, 0xa2, 0x33, 0xd1, 0x10, 0xb4, 0x60, 0xc0, 0x01, 0x9f, 0xbe, @@ -126,7 +123,6 @@ TEST(rtde_data_package, get_data_not_part_of_recipe) { std::vector recipe{ "timestamp", "actual_q" }; rtde_interface::DataPackage package(recipe); - package.initEmpty(); uint32_t speed_slider_mask; EXPECT_FALSE(package.getData("speed_slider_mask", speed_slider_mask)); @@ -136,7 +132,6 @@ TEST(rtde_data_package, set_data_not_part_of_recipe) { std::vector recipe{ "timestamp", "actual_q" }; rtde_interface::DataPackage package(recipe); - package.initEmpty(); uint32_t speed_slider_mask = 1; EXPECT_FALSE(package.setData("speed_slider_mask", speed_slider_mask)); @@ -146,7 +141,6 @@ TEST(rtde_data_package, parse_and_get_bitset_data) { std::vector recipe{ "robot_status_bits" }; rtde_interface::DataPackage package(recipe); - package.initEmpty(); uint8_t data_package[] = { 0x01, 0x00, 0x00, 0x00, 0x00, 0x40, 0xb2, 0x3d, 0xa9, 0xfb, 0xe7, 0x6c, 0x8b }; comm::BinParser bp(data_package, sizeof(data_package)); @@ -160,6 +154,41 @@ TEST(rtde_data_package, parse_and_get_bitset_data) EXPECT_EQ(expected_robot_status_bits, actual_robot_status_bits); } +TEST(rtde_data_package, parse_incorrect_data_size) +{ + std::vector recipe{ "timestamp", "actual_q" }; + rtde_interface::DataPackage package(recipe); + + // Data package with incorrect size (should be 56 bytes for the given recipe) + uint8_t data_package[] = { 0x01, 0x40, 0xd0, 0x75, 0x8c, 0x49, 0xba, 0x5e, 0x35, 0xbf }; + + comm::BinParser bp(data_package, sizeof(data_package)); + + EXPECT_THROW(package.parseWith(bp), UrException); +} + +TEST(rtde_data_package, data_package_to_string) +{ + std::vector recipe{ "speed_slider_mask", "speed_slider_fraction", "external_force_torque", + "standard_digital_output_mask", "actual_digital_output_bits" }; + rtde_interface::DataPackage package(recipe); + ASSERT_TRUE(package.setData("speed_slider_mask", 1)); + ASSERT_TRUE(package.setData("speed_slider_fraction", 0.5)); + ASSERT_TRUE(package.setData("external_force_torque", vector6d_t{ -1.6007, -1.7271, -2.203, -0.808, 1.5951, -0.031 })); + ASSERT_TRUE(package.setData("standard_digital_output_mask", 1 << 7)); + ASSERT_TRUE(package.setData("actual_digital_output_bits", 1 << 7)); + + std::string pkg_str = package.toString(); + + std::string expected_str = "speed_slider_mask: 1\n" + "speed_slider_fraction: 0.5\n" + "external_force_torque: [-1.6007, -1.7271, -2.203, -0.808, 1.5951, -0.031]\n" + "standard_digital_output_mask: 128\n" + "actual_digital_output_bits: 128\n"; + std::cout << "Package string:\n" << pkg_str << std::endl; + EXPECT_EQ(expected_str, pkg_str); +} + int main(int argc, char* argv[]) { ::testing::InitGoogleTest(&argc, argv); diff --git a/tests/test_rtde_writer.cpp b/tests/test_rtde_writer.cpp index e0b7e76e6..37d450828 100644 --- a/tests/test_rtde_writer.cpp +++ b/tests/test_rtde_writer.cpp @@ -212,6 +212,15 @@ TEST_F(RTDEWriterTest, send_standard_digital_output) // Changing pins above 7, should return false. pin = 8; EXPECT_FALSE(writer_->sendStandardDigitalOutput(pin, send_pin_value)); + + // Set pin to value false + pin = 2; + EXPECT_TRUE(writer_->sendStandardDigitalOutput(pin, false)); + waitForMessageCallback(1000); + received_pin_value = std::get(parsed_data_["standard_digital_output"]) != 0; + received_standard_digital_output_mask = std::get(parsed_data_["standard_digital_output_mask"]); + EXPECT_EQ(received_pin_value, false); + EXPECT_EQ(expected_standard_digital_output_mask, received_standard_digital_output_mask); } TEST_F(RTDEWriterTest, send_configurable_digital_output) @@ -227,14 +236,25 @@ TEST_F(RTDEWriterTest, send_configurable_digital_output) ASSERT_TRUE(dataFieldExist("configurable_digital_output_mask")); bool received_pin_value = std::get(parsed_data_["configurable_digital_output"]) != 0; - uint8_t received_standard_digital_output_mask = std::get(parsed_data_["configurable_digital_output_mask"]); + uint8_t received_configurable_digital_output_mask = std::get(parsed_data_["configurable_digital_output_" + "mask"]); EXPECT_EQ(send_pin_value, received_pin_value); - EXPECT_EQ(expected_configurable_digital_output_mask, received_standard_digital_output_mask); + EXPECT_EQ(expected_configurable_digital_output_mask, received_configurable_digital_output_mask); // Changing pins above 7, should return false. pin = 8; - EXPECT_FALSE(writer_->sendStandardDigitalOutput(pin, send_pin_value)); + EXPECT_FALSE(writer_->sendConfigurableDigitalOutput(pin, send_pin_value)); + + // Set pin to value false + pin = 2; + expected_configurable_digital_output_mask = 0b0000100; + EXPECT_TRUE(writer_->sendConfigurableDigitalOutput(pin, false)); + waitForMessageCallback(1000); + received_pin_value = std::get(parsed_data_["configurable_digital_output"]) != 0; + received_configurable_digital_output_mask = std::get(parsed_data_["configurable_digital_output_mask"]); + EXPECT_EQ(received_pin_value, false); + EXPECT_EQ(expected_configurable_digital_output_mask, received_configurable_digital_output_mask); } TEST_F(RTDEWriterTest, send_tool_digital_output) @@ -258,12 +278,18 @@ TEST_F(RTDEWriterTest, send_tool_digital_output) // Changing pins above 1, should return false. pin = 2; EXPECT_FALSE(writer_->sendToolDigitalOutput(pin, send_pin_value)); + // Set pin to value false + pin = 0; + EXPECT_TRUE(writer_->sendToolDigitalOutput(pin, false)); + waitForMessageCallback(1000); + received_pin_value = std::get(parsed_data_["tool_digital_output"]) != 0; + received_tool_digital_output_mask = std::get(parsed_data_["tool_digital_output_mask"]); + EXPECT_EQ(received_pin_value, false); + EXPECT_EQ(expected_tool_digital_output_mask, received_tool_digital_output_mask); } TEST_F(RTDEWriterTest, send_standard_analog_output_unknown_domain) { - waitForMessageCallback(1000); - uint8_t expected_standard_analog_output_mask = 1; uint8_t pin = 0; @@ -460,6 +486,50 @@ TEST_F(RTDEWriterTest, send_external_force_torque) EXPECT_EQ(send_external_force_torque[5], received_external_force_torque[5]); } +TEST_F(RTDEWriterTest, send_data_package) +{ + const uint32_t send_speed_slider_mask = 1; + const double send_speed_slider_fraction = 0.7; + const bool standard_digital_output_value = true; + const uint8_t standard_digital_output_mask = 0b00000001; // pin 1 + + rtde_interface::DataPackage data_package(input_recipe_); + ASSERT_TRUE(data_package.setData("speed_slider_fraction", send_speed_slider_fraction)); + ASSERT_TRUE(data_package.setData("speed_slider_mask", send_speed_slider_mask)); + ASSERT_TRUE( + data_package.setData("standard_digital_output", static_cast(standard_digital_output_value ? 255 : 0))); + ASSERT_TRUE(data_package.setData("standard_digital_output_mask", standard_digital_output_mask)); + + EXPECT_TRUE(writer_->sendPackage(data_package)); + + waitForMessageCallback(1000); + + ASSERT_TRUE(dataFieldExist("speed_slider_fraction")); + ASSERT_TRUE(dataFieldExist("speed_slider_mask")); + ASSERT_TRUE(dataFieldExist("standard_digital_output")); + ASSERT_TRUE(dataFieldExist("standard_digital_output_mask")); + + double received_speed_slider_fraction = std::get(parsed_data_["speed_slider_fraction"]); + uint32_t received_speed_slider_mask = std::get(parsed_data_["speed_slider_mask"]); + bool received_standard_digital_output_value = std::get(parsed_data_["standard_digital_output"]) != 0; + uint8_t received_standard_digital_output_mask = std::get(parsed_data_["standard_digital_output_mask"]); + + EXPECT_EQ(send_speed_slider_fraction, received_speed_slider_fraction); + EXPECT_EQ(send_speed_slider_mask, received_speed_slider_mask); + EXPECT_EQ(standard_digital_output_value, received_standard_digital_output_value); + EXPECT_EQ(standard_digital_output_mask, received_standard_digital_output_mask); +} + +TEST_F(RTDEWriterTest, init_while_running_throws) +{ + EXPECT_THROW(writer_->init(1), UrException); +} + +TEST_F(RTDEWriterTest, set_recipe_while_running_throws) +{ + EXPECT_THROW(writer_->setInputRecipe(input_recipe_), UrException); +} + int main(int argc, char* argv[]) { ::testing::InitGoogleTest(&argc, argv); diff --git a/tests/test_spline_interpolation.cpp b/tests/test_spline_interpolation.cpp index 6c6630e66..52f859c6e 100644 --- a/tests/test_spline_interpolation.cpp +++ b/tests/test_spline_interpolation.cpp @@ -259,12 +259,11 @@ class SplineInterpolationTest : public ::testing::Test bool trajectory_started = false; double timeout = 1; double cur_time = 0.0; - while (trajectory_started == false) + rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe()); + while (trajectory_started == false && g_my_robot->getUrDriver()->getDataPackage(data_pkg)) { - std::unique_ptr data_pkg; - g_my_robot->readDataPackage(data_pkg); double spline_travel_time = 0.0; - data_pkg->getData("output_double_register_1", spline_travel_time); + data_pkg.getData("output_double_register_1", spline_travel_time); // Keep connection alive ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage( @@ -353,18 +352,16 @@ class SplineInterpolationTest : public ::testing::Test TEST_F(SplineInterpolationTest, cubic_spline_with_end_point_velocity) { - g_my_robot->stopConsumingRTDEData(); - std::unique_ptr data_pkg; - g_my_robot->readDataPackage(data_pkg); - ASSERT_TRUE(data_pkg != nullptr); + rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe()); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); urcl::vector6d_t joint_positions, joint_velocities, joint_acc; - ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); - ASSERT_TRUE(data_pkg->getData("target_qd", joint_velocities)); - ASSERT_TRUE(data_pkg->getData("target_qdd", joint_acc)); + ASSERT_TRUE(data_pkg.getData("target_q", joint_positions)); + ASSERT_TRUE(data_pkg.getData("target_qd", joint_velocities)); + ASSERT_TRUE(data_pkg.getData("target_qdd", joint_acc)); double speed_scaling; - ASSERT_TRUE(data_pkg->getData("speed_scaling", speed_scaling)); + ASSERT_TRUE(data_pkg.getData("speed_scaling", speed_scaling)); std::vector speed_scaling_vec; std::vector s_pos, s_vel; @@ -394,11 +391,11 @@ TEST_F(SplineInterpolationTest, cubic_spline_with_end_point_velocity) double plot_time = 0.0; while (g_trajectory_running) { - g_my_robot->readDataPackage(data_pkg); - ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); - ASSERT_TRUE(data_pkg->getData("target_qd", joint_velocities)); - ASSERT_TRUE(data_pkg->getData("target_qdd", joint_acc)); - ASSERT_TRUE(data_pkg->getData("speed_scaling", speed_scaling)); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); + ASSERT_TRUE(data_pkg.getData("target_q", joint_positions)); + ASSERT_TRUE(data_pkg.getData("target_qd", joint_velocities)); + ASSERT_TRUE(data_pkg.getData("target_qdd", joint_acc)); + ASSERT_TRUE(data_pkg.getData("speed_scaling", speed_scaling)); // Keep connection alive ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage( @@ -406,7 +403,7 @@ TEST_F(SplineInterpolationTest, cubic_spline_with_end_point_velocity) // Read spline travel time from the robot double spline_travel_time = 0.0; - data_pkg->getData("output_double_register_1", spline_travel_time); + data_pkg.getData("output_double_register_1", spline_travel_time); double spline_travel_step_time = spline_travel_time - old_spline_travel_time; old_spline_travel_time = spline_travel_time; @@ -448,8 +445,8 @@ TEST_F(SplineInterpolationTest, cubic_spline_with_end_point_velocity) EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, g_trajectory_result); // Make sure the velocity is zero when the trajectory has finished - g_my_robot->readDataPackage(data_pkg); - ASSERT_TRUE(data_pkg->getData("target_qd", joint_velocities)); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); + ASSERT_TRUE(data_pkg.getData("target_qd", joint_velocities)); for (unsigned int i = 0; i < 6; ++i) { EXPECT_FLOAT_EQ(joint_velocities[i], 0.0); @@ -457,7 +454,7 @@ TEST_F(SplineInterpolationTest, cubic_spline_with_end_point_velocity) // Verify that the full trajectory have been executed double spline_travel_time; - data_pkg->getData("output_double_register_1", spline_travel_time); + data_pkg.getData("output_double_register_1", spline_travel_time); ASSERT_NEAR(spline_travel_time, s_time.back(), 1e-5); writeTrajectoryToFile("../test_artifacts/cubic_spline_with_end_point_velocity.csv", time_vec, expected_positions, @@ -466,21 +463,20 @@ TEST_F(SplineInterpolationTest, cubic_spline_with_end_point_velocity) TEST_F(SplineInterpolationTest, quintic_spline_with_end_point_velocity_with_speedscaling) { - g_my_robot->stopConsumingRTDEData(); // Set speed scaling to 25% to test interpolation with speed scaling active const unsigned int reduse_factor(4); g_my_robot->getUrDriver()->getRTDEWriter().sendSpeedSlider(1.0 / reduse_factor); - std::unique_ptr data_pkg; - g_my_robot->readDataPackage(data_pkg); + rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe()); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); urcl::vector6d_t joint_positions, joint_velocities, joint_acc; - ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); - ASSERT_TRUE(data_pkg->getData("target_qd", joint_velocities)); - ASSERT_TRUE(data_pkg->getData("target_qdd", joint_acc)); + ASSERT_TRUE(data_pkg.getData("target_q", joint_positions)); + ASSERT_TRUE(data_pkg.getData("target_qd", joint_velocities)); + ASSERT_TRUE(data_pkg.getData("target_qdd", joint_acc)); double speed_scaling; - ASSERT_TRUE(data_pkg->getData("target_speed_fraction", speed_scaling)); + ASSERT_TRUE(data_pkg.getData("target_speed_fraction", speed_scaling)); std::vector speed_scaling_vec; std::vector s_pos, s_vel, s_acc; @@ -517,15 +513,15 @@ TEST_F(SplineInterpolationTest, quintic_spline_with_end_point_velocity_with_spee const double eps_acc_change(1e-15); while (g_trajectory_running) { - g_my_robot->readDataPackage(data_pkg); - ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); - ASSERT_TRUE(data_pkg->getData("target_qd", joint_velocities)); - ASSERT_TRUE(data_pkg->getData("target_qdd", joint_acc)); - ASSERT_TRUE(data_pkg->getData("target_speed_fraction", speed_scaling)); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); + ASSERT_TRUE(data_pkg.getData("target_q", joint_positions)); + ASSERT_TRUE(data_pkg.getData("target_qd", joint_velocities)); + ASSERT_TRUE(data_pkg.getData("target_qdd", joint_acc)); + ASSERT_TRUE(data_pkg.getData("target_speed_fraction", speed_scaling)); // Read spline travel time from the robot double spline_travel_time = 0.0; - data_pkg->getData("output_double_register_1", spline_travel_time); + data_pkg.getData("output_double_register_1", spline_travel_time); double spline_travel_step_time = spline_travel_time - old_spline_travel_time; old_spline_travel_time = spline_travel_time; @@ -610,8 +606,8 @@ TEST_F(SplineInterpolationTest, quintic_spline_with_end_point_velocity_with_spee EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, g_trajectory_result); // Make sure the velocity is zero when the trajectory has finished - g_my_robot->readDataPackage(data_pkg); - ASSERT_TRUE(data_pkg->getData("target_qd", joint_velocities)); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); + ASSERT_TRUE(data_pkg.getData("target_qd", joint_velocities)); for (unsigned int i = 0; i < 6; ++i) { EXPECT_FLOAT_EQ(joint_velocities[i], 0.0); @@ -619,7 +615,7 @@ TEST_F(SplineInterpolationTest, quintic_spline_with_end_point_velocity_with_spee // Verify that the full trajectory have been executed double spline_travel_time; - data_pkg->getData("output_double_register_1", spline_travel_time); + data_pkg.getData("output_double_register_1", spline_travel_time); ASSERT_NEAR(spline_travel_time, s_time.back(), 1e-5); writeTrajectoryToFile("../test_artifacts/quintic_spline_with_end_point_velocity_speedscaling.csv", time_vec, @@ -629,16 +625,15 @@ TEST_F(SplineInterpolationTest, quintic_spline_with_end_point_velocity_with_spee TEST_F(SplineInterpolationTest, spline_interpolation_cubic) { - g_my_robot->stopConsumingRTDEData(); - std::unique_ptr data_pkg; - g_my_robot->readDataPackage(data_pkg); + rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe()); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); urcl::vector6d_t joint_positions, joint_velocities, joint_acc; - ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); - ASSERT_TRUE(data_pkg->getData("target_qd", joint_velocities)); - ASSERT_TRUE(data_pkg->getData("target_qdd", joint_acc)); + ASSERT_TRUE(data_pkg.getData("target_q", joint_positions)); + ASSERT_TRUE(data_pkg.getData("target_qd", joint_velocities)); + ASSERT_TRUE(data_pkg.getData("target_qdd", joint_acc)); double speed_scaling; - ASSERT_TRUE(data_pkg->getData("speed_scaling", speed_scaling)); + ASSERT_TRUE(data_pkg.getData("speed_scaling", speed_scaling)); std::vector speed_scaling_vec; std::vector s_pos, s_vel; @@ -679,14 +674,14 @@ TEST_F(SplineInterpolationTest, spline_interpolation_cubic) double plot_time = 0.0; while (g_trajectory_running) { - g_my_robot->readDataPackage(data_pkg); - ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); - ASSERT_TRUE(data_pkg->getData("target_qd", joint_velocities)); - ASSERT_TRUE(data_pkg->getData("target_qdd", joint_acc)); - ASSERT_TRUE(data_pkg->getData("speed_scaling", speed_scaling)); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); + ASSERT_TRUE(data_pkg.getData("target_q", joint_positions)); + ASSERT_TRUE(data_pkg.getData("target_qd", joint_velocities)); + ASSERT_TRUE(data_pkg.getData("target_qdd", joint_acc)); + ASSERT_TRUE(data_pkg.getData("speed_scaling", speed_scaling)); double spline_travel_time = 0.0; - data_pkg->getData("output_double_register_1", spline_travel_time); + data_pkg.getData("output_double_register_1", spline_travel_time); // The data received from the robot is one time step behind where the robot is interpolating spline_travel_time = (spline_travel_time == 0 ? spline_travel_time : spline_travel_time - step_time_); @@ -725,7 +720,7 @@ TEST_F(SplineInterpolationTest, spline_interpolation_cubic) // Verify that the full trajectory have been executed double spline_travel_time; - data_pkg->getData("output_double_register_1", spline_travel_time); + data_pkg.getData("output_double_register_1", spline_travel_time); ASSERT_NEAR(spline_travel_time, s_time.back(), 1e-5); writeTrajectoryToFile("../test_artifacts/spline_interpolation_cubic.csv", time_vec, expected_positions, @@ -734,17 +729,16 @@ TEST_F(SplineInterpolationTest, spline_interpolation_cubic) TEST_F(SplineInterpolationTest, spline_interpolation_quintic) { - g_my_robot->stopConsumingRTDEData(); - std::unique_ptr data_pkg; - g_my_robot->readDataPackage(data_pkg); + rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe()); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); urcl::vector6d_t joint_positions, joint_velocities, joint_acc; - ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); - ASSERT_TRUE(data_pkg->getData("target_qd", joint_velocities)); - ASSERT_TRUE(data_pkg->getData("target_qdd", joint_acc)); + ASSERT_TRUE(data_pkg.getData("target_q", joint_positions)); + ASSERT_TRUE(data_pkg.getData("target_qd", joint_velocities)); + ASSERT_TRUE(data_pkg.getData("target_qdd", joint_acc)); double speed_scaling; - ASSERT_TRUE(data_pkg->getData("speed_scaling", speed_scaling)); + ASSERT_TRUE(data_pkg.getData("speed_scaling", speed_scaling)); std::vector speed_scaling_vec; std::vector s_pos, s_vel, s_acc; @@ -792,14 +786,14 @@ TEST_F(SplineInterpolationTest, spline_interpolation_quintic) g_trajectory_running = true; while (g_trajectory_running) { - g_my_robot->readDataPackage(data_pkg); - ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); - ASSERT_TRUE(data_pkg->getData("target_qd", joint_velocities)); - ASSERT_TRUE(data_pkg->getData("target_qdd", joint_acc)); - ASSERT_TRUE(data_pkg->getData("speed_scaling", speed_scaling)); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); + ASSERT_TRUE(data_pkg.getData("target_q", joint_positions)); + ASSERT_TRUE(data_pkg.getData("target_qd", joint_velocities)); + ASSERT_TRUE(data_pkg.getData("target_qdd", joint_acc)); + ASSERT_TRUE(data_pkg.getData("speed_scaling", speed_scaling)); double spline_travel_time = 0.0; - data_pkg->getData("output_double_register_1", spline_travel_time); + data_pkg.getData("output_double_register_1", spline_travel_time); // The data received from the robot is one time step behind where the robot is interpolating spline_travel_time = (spline_travel_time == 0 ? spline_travel_time : spline_travel_time - step_time_); @@ -837,7 +831,7 @@ TEST_F(SplineInterpolationTest, spline_interpolation_quintic) // Verify that the full trajectory have been executed double spline_travel_time; - data_pkg->getData("output_double_register_1", spline_travel_time); + data_pkg.getData("output_double_register_1", spline_travel_time); ASSERT_NEAR(spline_travel_time, s_time.back(), 1e-5); writeTrajectoryToFile("../test_artifacts/spline_interpolation_quintic.csv", time_vec, expected_positions, @@ -846,15 +840,11 @@ TEST_F(SplineInterpolationTest, spline_interpolation_quintic) TEST_F(SplineInterpolationTest, zero_time_trajectory_cubic_spline) { - g_my_robot->stopConsumingRTDEData(); - std::unique_ptr data_pkg; - g_my_robot->readDataPackage(data_pkg); + rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe()); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); urcl::vector6d_t joint_positions_before; - ASSERT_TRUE(data_pkg->getData("target_q", joint_positions_before)); - - // Start consuming rtde packages to avoid pipeline overflows while testing that the control script aborts correctly - g_my_robot->startConsumingRTDEData(); + ASSERT_TRUE(data_pkg.getData("target_q", joint_positions_before)); // Create illegal trajectory std::vector s_pos, s_vel; @@ -884,13 +874,10 @@ TEST_F(SplineInterpolationTest, zero_time_trajectory_cubic_spline) ASSERT_FALSE(g_my_robot->waitForProgramNotRunning(1000)); EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_CANCELED, g_trajectory_result); - // Stop consuming rtde packages - g_my_robot->stopConsumingRTDEData(); - // Ensure that the robot hasn't moved - g_my_robot->readDataPackage(data_pkg); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); urcl::vector6d_t joint_positions_after; - ASSERT_TRUE(data_pkg->getData("target_q", joint_positions_after)); + ASSERT_TRUE(data_pkg.getData("target_q", joint_positions_after)); for (unsigned int i = 0; i < 6; ++i) { EXPECT_FLOAT_EQ(joint_positions_before[i], joint_positions_after[i]); @@ -913,15 +900,15 @@ TEST_F(SplineInterpolationTest, zero_time_trajectory_cubic_spline) urcl::vector6d_t joint_positions; while (g_trajectory_running) { - g_my_robot->readDataPackage(data_pkg); - ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); + ASSERT_TRUE(data_pkg.getData("target_q", joint_positions)); // Keep connection alive ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage( urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); } EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, g_trajectory_result); - ASSERT_TRUE(data_pkg->getData("target_q", joint_positions_after)); + ASSERT_TRUE(data_pkg.getData("target_q", joint_positions_after)); for (unsigned int i = 0; i < 6; ++i) { EXPECT_NEAR(joint_positions[i], third_point[i], eps_); @@ -930,15 +917,11 @@ TEST_F(SplineInterpolationTest, zero_time_trajectory_cubic_spline) TEST_F(SplineInterpolationTest, zero_time_trajectory_quintic_spline) { - g_my_robot->stopConsumingRTDEData(); - std::unique_ptr data_pkg; - g_my_robot->readDataPackage(data_pkg); + rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe()); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); urcl::vector6d_t joint_positions_before; - ASSERT_TRUE(data_pkg->getData("target_q", joint_positions_before)); - - // Start consuming rtde packages to avoid pipeline overflows while testing - g_my_robot->startConsumingRTDEData(); + ASSERT_TRUE(data_pkg.getData("target_q", joint_positions_before)); // Create illegal trajectory std::vector s_pos, s_vel, s_acc; @@ -964,13 +947,10 @@ TEST_F(SplineInterpolationTest, zero_time_trajectory_quintic_spline) ASSERT_FALSE(g_my_robot->waitForProgramNotRunning(1000)); EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_CANCELED, g_trajectory_result); - // Stop consuming rtde packages - g_my_robot->stopConsumingRTDEData(); - // Ensure that the robot hasn't moved - g_my_robot->readDataPackage(data_pkg); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); urcl::vector6d_t joint_positions_after; - ASSERT_TRUE(data_pkg->getData("target_q", joint_positions_after)); + ASSERT_TRUE(data_pkg.getData("target_q", joint_positions_after)); for (unsigned int i = 0; i < 6; ++i) { EXPECT_FLOAT_EQ(joint_positions_before[i], joint_positions_after[i]); @@ -994,15 +974,15 @@ TEST_F(SplineInterpolationTest, zero_time_trajectory_quintic_spline) urcl::vector6d_t joint_positions; while (g_trajectory_running) { - g_my_robot->readDataPackage(data_pkg); - ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); + ASSERT_TRUE(data_pkg.getData("target_q", joint_positions)); // Keep connection alive ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage( urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); } EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, g_trajectory_result); - ASSERT_TRUE(data_pkg->getData("target_q", joint_positions_after)); + ASSERT_TRUE(data_pkg.getData("target_q", joint_positions_after)); for (unsigned int i = 0; i < 6; ++i) { EXPECT_NEAR(joint_positions[i], third_point[i], eps_); @@ -1011,15 +991,11 @@ TEST_F(SplineInterpolationTest, zero_time_trajectory_quintic_spline) TEST_F(SplineInterpolationTest, physically_unfeasible_trajectory_cubic_spline) { - g_my_robot->stopConsumingRTDEData(); - std::unique_ptr data_pkg; - g_my_robot->readDataPackage(data_pkg); + rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe()); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); urcl::vector6d_t joint_positions_before; - ASSERT_TRUE(data_pkg->getData("target_q", joint_positions_before)); - - // Start consuming rtde packages to avoid pipeline overflows while testing that the control script aborts correctly - g_my_robot->startConsumingRTDEData(); + ASSERT_TRUE(data_pkg.getData("target_q", joint_positions_before)); // Create a trajectory that cannot be executed within the robots limits std::vector s_pos, s_vel; @@ -1044,13 +1020,10 @@ TEST_F(SplineInterpolationTest, physically_unfeasible_trajectory_cubic_spline) ASSERT_FALSE(g_my_robot->waitForProgramNotRunning(1000)); EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_CANCELED, g_trajectory_result); - // Stop consuming rtde packages - g_my_robot->stopConsumingRTDEData(); - // Ensure that the robot hasn't moved - g_my_robot->readDataPackage(data_pkg); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); urcl::vector6d_t joint_positions_after; - ASSERT_TRUE(data_pkg->getData("target_q", joint_positions_after)); + ASSERT_TRUE(data_pkg.getData("target_q", joint_positions_after)); for (unsigned int i = 0; i < 6; ++i) { EXPECT_FLOAT_EQ(joint_positions_before[i], joint_positions_after[i]); @@ -1059,15 +1032,11 @@ TEST_F(SplineInterpolationTest, physically_unfeasible_trajectory_cubic_spline) TEST_F(SplineInterpolationTest, physically_unfeasible_trajectory_quintic_spline) { - g_my_robot->stopConsumingRTDEData(); - std::unique_ptr data_pkg; - g_my_robot->readDataPackage(data_pkg); + rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe()); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); urcl::vector6d_t joint_positions_before; - ASSERT_TRUE(data_pkg->getData("target_q", joint_positions_before)); - - // Start consuming rtde packages to avoid pipeline overflows while testing that the control script aborts correctly - g_my_robot->startConsumingRTDEData(); + ASSERT_TRUE(data_pkg.getData("target_q", joint_positions_before)); // Create a trajectory that cannot be executed within the robots limits std::vector s_pos, s_vel, s_acc; @@ -1093,13 +1062,10 @@ TEST_F(SplineInterpolationTest, physically_unfeasible_trajectory_quintic_spline) ASSERT_FALSE(g_my_robot->waitForProgramNotRunning(1000)); EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_CANCELED, g_trajectory_result); - // Stop consuming rtde packages - g_my_robot->stopConsumingRTDEData(); - // Ensure that the robot hasn't moved - g_my_robot->readDataPackage(data_pkg); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); urcl::vector6d_t joint_positions_after; - ASSERT_TRUE(data_pkg->getData("target_q", joint_positions_after)); + ASSERT_TRUE(data_pkg.getData("target_q", joint_positions_after)); for (unsigned int i = 0; i < 6; ++i) { EXPECT_FLOAT_EQ(joint_positions_before[i], joint_positions_after[i]); @@ -1120,15 +1086,12 @@ TEST_F(SplineInterpolationTest, switching_control_mode_without_trajectory_produc TEST_F(SplineInterpolationTest, switching_control_mode_with_trajectory_produces_result) { - g_my_robot->stopConsumingRTDEData(); - std::unique_ptr data_pkg; - g_my_robot->readDataPackage(data_pkg); + rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe()); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); urcl::vector6d_t joint_positions_before{ 0, 0, 0, 0, 0, 0 }; - ASSERT_TRUE(data_pkg->getData("target_q", joint_positions_before)); + ASSERT_TRUE(data_pkg.getData("target_q", joint_positions_before)); - // Start consuming rtde packages to avoid pipeline overflows while testing that the control script aborts correctly - g_my_robot->startConsumingRTDEData(); // Create a trajectory that cannot be executed within the robots limits std::vector s_pos, s_vel, s_acc; urcl::vector6d_t first_point = { diff --git a/tests/test_start_ursim.bats b/tests/test_start_ursim.bats index ab368f616..7feead249 100644 --- a/tests/test_start_ursim.bats +++ b/tests/test_start_ursim.bats @@ -516,6 +516,14 @@ setup() { [ "$port_forwarding" = "-p 1234:1234 -p 50001-50004:60001-60004" ] } +@test "disable_port_forwarding" { + run main -t -f "DISABLED" + echo "$output" + [ $status -eq 0 ] + docker_line=$(echo "$output" | tail -n -1) + grep -v -E "\-p\s*[0-9]+(\-[0-9]+)?:[0-9]+(\-[0-9]+)?" <<< "$docker_line" +} + @test "default_container_name" { run main -t echo "$output" diff --git a/tests/test_ur_driver.cpp b/tests/test_ur_driver.cpp index 4ea0d1d00..5892b89b4 100644 --- a/tests/test_ur_driver.cpp +++ b/tests/test_ur_driver.cpp @@ -217,66 +217,62 @@ TEST_F(UrDriverTest, stop_robot_control) TEST_F(UrDriverTest, target_outside_limits_servoj) { - g_my_robot->stopConsumingRTDEData(); - std::unique_ptr data_pkg; - g_my_robot->readDataPackage(data_pkg); + rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe()); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); urcl::vector6d_t joint_positions_before; - ASSERT_TRUE(data_pkg->getData("actual_q", joint_positions_before)); + ASSERT_TRUE(data_pkg.getData("actual_q", joint_positions_before)); // Create physically unfeasible target urcl::vector6d_t joint_target = joint_positions_before; joint_target[5] -= 2.5; // Send unfeasible targets to the robot - g_my_robot->readDataPackage(data_pkg); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); g_my_robot->getUrDriver()->writeJointCommand(joint_target, comm::ControlMode::MODE_SERVOJ, RobotReceiveTimeout::millisec(200)); // Ensure that the robot didn't move - g_my_robot->readDataPackage(data_pkg); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); urcl::vector6d_t joint_positions; - ASSERT_TRUE(data_pkg->getData("actual_q", joint_positions)); + ASSERT_TRUE(data_pkg.getData("actual_q", joint_positions)); for (unsigned int i = 0; i < 6; ++i) { EXPECT_FLOAT_EQ(joint_positions_before[i], joint_positions[i]); } // Make sure the program is stopped - g_my_robot->startConsumingRTDEData(); g_my_robot->getUrDriver()->stopControl(); g_my_robot->waitForProgramNotRunning(1000); } TEST_F(UrDriverTest, target_outside_limits_pose) { - g_my_robot->stopConsumingRTDEData(); - std::unique_ptr data_pkg; - g_my_robot->readDataPackage(data_pkg); + rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe()); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); urcl::vector6d_t tcp_pose_before; - ASSERT_TRUE(data_pkg->getData("actual_TCP_pose", tcp_pose_before)); + ASSERT_TRUE(data_pkg.getData("actual_TCP_pose", tcp_pose_before)); // Create physically unfeasible target urcl::vector6d_t tcp_target = tcp_pose_before; tcp_target[2] += 0.3; // Send unfeasible targets to the robot - g_my_robot->readDataPackage(data_pkg); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); g_my_robot->getUrDriver()->writeJointCommand(tcp_target, comm::ControlMode::MODE_POSE, RobotReceiveTimeout::millisec(200)); // Ensure that the robot didn't move - g_my_robot->readDataPackage(data_pkg); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); urcl::vector6d_t tcp_pose; - ASSERT_TRUE(data_pkg->getData("actual_TCP_pose", tcp_pose)); + ASSERT_TRUE(data_pkg.getData("actual_TCP_pose", tcp_pose)); for (unsigned int i = 0; i < 6; ++i) { EXPECT_FLOAT_EQ(tcp_pose_before[i], tcp_pose[i]); } // Make sure the program is stopped - g_my_robot->startConsumingRTDEData(); g_my_robot->getUrDriver()->stopControl(); g_my_robot->waitForProgramNotRunning(1000); } @@ -299,7 +295,6 @@ TEST_F(UrDriverTest, send_robot_program_retry_on_failure) TEST_F(UrDriverTest, reset_rtde_client) { - g_my_robot->stopConsumingRTDEData(); double target_frequency = 50; g_my_robot->getUrDriver()->resetRTDEClient(OUTPUT_RECIPE, INPUT_RECIPE, target_frequency); ASSERT_EQ(g_my_robot->getUrDriver()->getControlFrequency(), target_frequency);