diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 02d248a1..138d3c72 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -35,7 +35,7 @@ jobs: - name: build run: cmake --build build --config Debug - name: test - run: cd build && make test + run: cd build && ctest --output-on-failure - name: install gcovr run: sudo apt-get install -y gcovr - name: gcovr diff --git a/include/ur_client_library/rtde/rtde_client.h b/include/ur_client_library/rtde/rtde_client.h index 363d4058..de2b12dc 100644 --- a/include/ur_client_library/rtde/rtde_client.h +++ b/include/ur_client_library/rtde/rtde_client.h @@ -122,7 +122,7 @@ class RTDEClient /*! * \brief Pauses RTDE data package communication * - * \returns Wheter the RTDE data package communication was paussed succesfully + * \returns Whether the RTDE data package communication was paused successfully */ bool pause(); /*! @@ -135,15 +135,25 @@ class RTDEClient std::unique_ptr getDataPackage(std::chrono::milliseconds timeout); /*! - * \brief Getter for the frequency the robot will publish RTDE data packages with. + * \brief Getter for the maximum frequency the robot can publish RTDE data packages with. * - * \returns The used frequency + * \returns The maximum frequency */ double getMaxFrequency() const { return max_frequency_; } + /*! + * \brief Getter for the target frequency that the robot will publish RTDE data packages with. + * + * \returns The target frequency + */ + double getTargetFrequency() const + { + return target_frequency_; + } + /*! * \brief Getter for the UR control version received from the robot. * @@ -208,7 +218,7 @@ class RTDEClient void disconnect(); /*! - * \brief Checks wheter the robot is booted, this is done by looking at the timestamp from the robot controller, this + * \brief Checks whether the robot is booted, this is done by looking at the timestamp from the robot controller, this * will show the time in seconds since the controller was started. If the timestamp is below 40, we will read from * the stream for approximately 1 second to ensure that the RTDE interface is up and running. This will ensure that we * don't finalize setting up communication, before the controller is up and running. Else we could end up connecting diff --git a/include/ur_client_library/rtde/rtde_writer.h b/include/ur_client_library/rtde/rtde_writer.h index da2e5842..611f8154 100644 --- a/include/ur_client_library/rtde/rtde_writer.h +++ b/include/ur_client_library/rtde/rtde_writer.h @@ -61,7 +61,6 @@ class RTDEWriter ~RTDEWriter() { running_ = false; - std::this_thread::sleep_for(std::chrono::seconds(5)); if (writer_thread_.joinable()) { writer_thread_.join(); @@ -118,7 +117,7 @@ class RTDEWriter * \brief Creates a package to request setting a new value for one of the standard analog output pins. * * \param output_pin The pin to change - * \param value The new value + * \param value The new value, it should be between 0 and 1, where 0 is 4mA and 1 is 20mA. * * \returns Success of the package creation */ diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index 1d82deb5..6765ea75 100644 --- a/src/rtde/rtde_client.cpp +++ b/src/rtde/rtde_client.cpp @@ -254,6 +254,11 @@ void RTDEClient::setupOutputs(const uint16_t protocol_version) } else { + if (target_frequency_ != max_frequency_) + { + URCL_LOG_WARN("It is not possible to set a target frequency when using protocol version 1. A frequency " + "equivalent to the maximum frequency will be used instead."); + } size = ControlPackageSetupOutputsRequest::generateSerializedRequest(buffer, output_recipe_); } @@ -403,7 +408,9 @@ bool RTDEClient::isRobotBooted() while (timestamp < 40 && reading_count < target_frequency_ * 2) { - if (pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000))) + // 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))) { rtde_interface::DataPackage* tmp_input = dynamic_cast(package.get()); tmp_input->getData("timestamp", timestamp); @@ -520,35 +527,24 @@ bool RTDEClient::sendPause() URCL_LOG_ERROR("Sending RTDE pause command failed!"); return false; } - static unsigned num_retries = 0; - while (num_retries < MAX_REQUEST_RETRIES) + std::unique_ptr package; + std::chrono::time_point start = std::chrono::steady_clock::now(); + int seconds = 5; + while (std::chrono::steady_clock::now() - start < std::chrono::seconds(seconds)) { - std::unique_ptr package; - if (!pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000))) { URCL_LOG_ERROR("Could not get response to RTDE communication pause request from robot"); return false; } - if (rtde_interface::ControlPackagePause* tmp = dynamic_cast(package.get())) { client_state_ = ClientState::PAUSED; return tmp->accepted_; } - else - { - std::stringstream ss; - ss << "Did not receive answer to RTDE pause request. Message received instead: " << std::endl - << package->toString(); - URCL_LOG_WARN("%s", ss.str().c_str()); - num_retries++; - } } std::stringstream ss; - ss << "Could not pause RTDE communication after " << MAX_REQUEST_RETRIES - << " tries. Please check the output of the " - "negotiation attempts above to get a hint what could be wrong."; + ss << "Could not receive answer to pause RTDE communication after " << seconds << " seconds."; throw UrException(ss.str()); } diff --git a/src/rtde/rtde_writer.cpp b/src/rtde/rtde_writer.cpp index ed4e48bf..d0d14253 100644 --- a/src/rtde/rtde_writer.cpp +++ b/src/rtde/rtde_writer.cpp @@ -65,6 +65,15 @@ void RTDEWriter::run() bool RTDEWriter::sendSpeedSlider(double speed_slider_fraction) { + if (speed_slider_fraction > 1.0 || speed_slider_fraction < 0.0) + { + std::stringstream ss; + ss << "Speed slider fraction should be between 0 and 1. The speed slider fraction is " + << static_cast(speed_slider_fraction); + URCL_LOG_ERROR(ss.str().c_str()); + return false; + } + std::lock_guard guard(package_mutex_); uint32_t mask = 1; bool success = true; @@ -85,6 +94,14 @@ bool RTDEWriter::sendSpeedSlider(double speed_slider_fraction) bool RTDEWriter::sendStandardDigitalOutput(uint8_t output_pin, bool value) { + if (output_pin > 7) + { + std::stringstream ss; + ss << "Standard digital output pins goes from 0 to 7. The output pin to change is " << static_cast(output_pin); + URCL_LOG_ERROR(ss.str().c_str()); + return false; + } + std::lock_guard guard(package_mutex_); uint8_t mask = pinToMask(output_pin); bool success = true; @@ -114,6 +131,15 @@ bool RTDEWriter::sendStandardDigitalOutput(uint8_t output_pin, bool value) bool RTDEWriter::sendConfigurableDigitalOutput(uint8_t output_pin, bool value) { + if (output_pin > 7) + { + std::stringstream ss; + ss << "Configurable digital output pins goes from 0 to 7. The output pin to change is " + << static_cast(output_pin); + URCL_LOG_ERROR(ss.str().c_str()); + return false; + } + std::lock_guard guard(package_mutex_); uint8_t mask = pinToMask(output_pin); bool success = true; @@ -143,6 +169,14 @@ bool RTDEWriter::sendConfigurableDigitalOutput(uint8_t output_pin, bool value) bool RTDEWriter::sendToolDigitalOutput(uint8_t output_pin, bool value) { + if (output_pin > 1) + { + std::stringstream ss; + ss << "Tool digital output pins goes from 0 to 1. The output pin to change is " << static_cast(output_pin); + URCL_LOG_ERROR(ss.str().c_str()); + return false; + } + std::lock_guard guard(package_mutex_); uint8_t mask = pinToMask(output_pin); bool success = true; @@ -172,6 +206,21 @@ bool RTDEWriter::sendToolDigitalOutput(uint8_t output_pin, bool value) bool RTDEWriter::sendStandardAnalogOutput(uint8_t output_pin, double value) { + if (output_pin > 1) + { + std::stringstream ss; + ss << "Standard analog output goes from 0 to 1. The output pin to change is " << static_cast(output_pin); + URCL_LOG_ERROR(ss.str().c_str()); + return false; + } + if (value > 1.0 || value < 0.0) + { + std::stringstream ss; + ss << "Analog output value should be between 0 and 1. The value is " << static_cast(value); + URCL_LOG_ERROR(ss.str().c_str()); + return false; + } + std::lock_guard guard(package_mutex_); uint8_t mask = pinToMask(output_pin); // default to current for now, as no functionality to choose included in set io service @@ -206,6 +255,14 @@ uint8_t RTDEWriter::pinToMask(uint8_t pin) bool RTDEWriter::sendInputBitRegister(uint32_t register_id, bool value) { + if (register_id < 64 || register_id > 127) + { + std::stringstream ss; + ss << "Input bit register goes from 64 to 127. The register id to change is " << static_cast(register_id); + URCL_LOG_ERROR(ss.str().c_str()); + return false; + } + std::lock_guard guard(package_mutex_); std::stringstream ss; ss << "input_bit_register_" << register_id; @@ -224,6 +281,14 @@ bool RTDEWriter::sendInputBitRegister(uint32_t register_id, bool value) bool RTDEWriter::sendInputIntRegister(uint32_t register_id, int32_t value) { + if (register_id < 24 || register_id > 47) + { + std::stringstream ss; + ss << "Input int register goes from 24 to 47. The register id to change is " << static_cast(register_id); + URCL_LOG_ERROR(ss.str().c_str()); + return false; + } + std::lock_guard guard(package_mutex_); std::stringstream ss; ss << "input_int_register_" << register_id; @@ -242,6 +307,14 @@ bool RTDEWriter::sendInputIntRegister(uint32_t register_id, int32_t value) bool RTDEWriter::sendInputDoubleRegister(uint32_t register_id, double value) { + if (register_id < 24 || register_id > 47) + { + std::stringstream ss; + ss << "Input double register goes from 24 to 47. The register id to change is " << static_cast(register_id); + URCL_LOG_ERROR(ss.str().c_str()); + return false; + } + std::lock_guard guard(package_mutex_); std::stringstream ss; ss << "input_double_register_" << register_id; diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index b93e49ba..ed87d488 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -48,11 +48,11 @@ target_link_libraries(primary_parser_tests PRIVATE ur_client_library::urcl ${GTE gtest_add_tests(TARGET primary_parser_tests ) -add_executable(rtde_data_package test_rtde_data_package.cpp) -target_compile_options(rtde_data_package PRIVATE ${CXX17_FLAG}) -target_include_directories(rtde_data_package PRIVATE ${GTEST_INCLUDE_DIRS}) -target_link_libraries(rtde_data_package PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES}) -gtest_add_tests(TARGET rtde_data_package +add_executable(rtde_data_package_tests test_rtde_data_package.cpp) +target_compile_options(rtde_data_package_tests PRIVATE ${CXX17_FLAG}) +target_include_directories(rtde_data_package_tests PRIVATE ${GTEST_INCLUDE_DIRS}) +target_link_libraries(rtde_data_package_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES}) +gtest_add_tests(TARGET rtde_data_package_tests ) add_executable(rtde_parser_tests test_rtde_parser.cpp) @@ -89,3 +89,53 @@ target_include_directories(trajectory_point_interface_tests PRIVATE ${GTEST_INCL target_link_libraries(trajectory_point_interface_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES}) gtest_add_tests(TARGET trajectory_point_interface_tests ) + +add_executable(rtde_control_package_pause_tests test_rtde_control_package_pause.cpp) +target_compile_options(rtde_control_package_pause_tests PRIVATE ${CXX17_FLAG}) +target_include_directories(rtde_control_package_pause_tests PRIVATE ${GTEST_INCLUDE_DIRS}) +target_link_libraries(rtde_control_package_pause_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES}) +gtest_add_tests(TARGET rtde_control_package_pause_tests +) + +add_executable(rtde_control_package_start_tests test_rtde_control_package_start.cpp) +target_compile_options(rtde_control_package_start_tests PRIVATE ${CXX17_FLAG}) +target_include_directories(rtde_control_package_start_tests PRIVATE ${GTEST_INCLUDE_DIRS}) +target_link_libraries(rtde_control_package_start_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES}) +gtest_add_tests(TARGET rtde_control_package_start_tests +) + +add_executable(rtde_control_package_setup_outputs_tests test_rtde_control_package_setup_outputs.cpp) +target_compile_options(rtde_control_package_setup_outputs_tests PRIVATE ${CXX17_FLAG}) +target_include_directories(rtde_control_package_setup_outputs_tests PRIVATE ${GTEST_INCLUDE_DIRS}) +target_link_libraries(rtde_control_package_setup_outputs_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES}) +gtest_add_tests(TARGET rtde_control_package_setup_outputs_tests +) + +add_executable(rtde_control_package_setup_inputs_tests test_rtde_control_package_setup_inputs.cpp) +target_compile_options(rtde_control_package_setup_inputs_tests PRIVATE ${CXX17_FLAG}) +target_include_directories(rtde_control_package_setup_inputs_tests PRIVATE ${GTEST_INCLUDE_DIRS}) +target_link_libraries(rtde_control_package_setup_inputs_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES}) +gtest_add_tests(TARGET rtde_control_package_setup_inputs_tests +) + +add_executable(rtde_get_urcontrol_version_tests test_rtde_get_urcontrol_version.cpp) +target_compile_options(rtde_get_urcontrol_version_tests PRIVATE ${CXX17_FLAG}) +target_include_directories(rtde_get_urcontrol_version_tests PRIVATE ${GTEST_INCLUDE_DIRS}) +target_link_libraries(rtde_get_urcontrol_version_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES}) +gtest_add_tests(TARGET rtde_get_urcontrol_version_tests +) + +add_executable(rtde_request_protocol_version_tests test_rtde_request_protocol_version.cpp) +target_compile_options(rtde_request_protocol_version_tests PRIVATE ${CXX17_FLAG}) +target_include_directories(rtde_request_protocol_version_tests PRIVATE ${GTEST_INCLUDE_DIRS}) +target_link_libraries(rtde_request_protocol_version_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES}) +gtest_add_tests(TARGET rtde_request_protocol_version_tests +) + +add_executable(rtde_writer_tests test_rtde_writer.cpp) +target_compile_options(rtde_writer_tests PRIVATE ${CXX17_FLAG}) +target_include_directories(rtde_writer_tests PRIVATE ${GTEST_INCLUDE_DIRS}) +target_link_libraries(rtde_writer_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES}) +gtest_add_tests(TARGET rtde_writer_tests +) + diff --git a/tests/test_rtde_client.cpp b/tests/test_rtde_client.cpp index cfc42fb4..7d6523fd 100644 --- a/tests/test_rtde_client.cpp +++ b/tests/test_rtde_client.cpp @@ -27,6 +27,7 @@ //---------------------------------------------------------------------- #include +#include #include @@ -34,55 +35,265 @@ using namespace urcl; std::string ROBOT_IP = "192.168.56.101"; -TEST(UrRobotDriver, rtde_handshake) +class RTDEClientTest : public ::testing::Test { - comm::INotifier notifier; - std::string output_recipe = "resources/rtde_output_recipe.txt"; - std::string input_recipe = "resources/rtde_input_recipe.txt"; - rtde_interface::RTDEClient client(ROBOT_IP, notifier, output_recipe, input_recipe); +protected: + void SetUp() + { + client_.reset(new rtde_interface::RTDEClient(ROBOT_IP, notifier_, output_recipe_, input_recipe_)); + } + + 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::string output_recipe_ = "resources/rtde_output_recipe.txt"; + std::string input_recipe_ = "resources/rtde_input_recipe.txt"; + comm::INotifier notifier_; + std::unique_ptr client_; +}; - EXPECT_TRUE(client.init()); +TEST_F(RTDEClientTest, rtde_handshake) +{ + EXPECT_TRUE(client_->init()); } -/* -* Currently these tests wont work, since we no longer throw an exception at a wrong IP address -* TODO fix these tests -TEST(UrRobotDriver, rtde_handshake_wrong_ip) +TEST_F(RTDEClientTest, no_recipe) { - comm::INotifier notifier; - std::string output_recipe = "resources/rtde_output_recipe.txt"; - std::string input_recipe = "resources/rtde_input_recipe.txt"; - rtde_interface::RTDEClient client("192.168.56.123", notifier, output_recipe, input_recipe); + std::string output_recipe = ""; + std::string input_recipe = ""; + EXPECT_THROW(client_.reset(new rtde_interface::RTDEClient(ROBOT_IP, notifier_, output_recipe, input_recipe)), + UrException); - EXPECT_THROW(client.init(), UrException); + // Only input recipe is unconfigured + EXPECT_THROW(client_.reset(new rtde_interface::RTDEClient(ROBOT_IP, notifier_, output_recipe_, input_recipe)), + UrException); } -TEST(UrRobotDriver, rtde_handshake_illegal_ip) +TEST_F(RTDEClientTest, empty_recipe) { - comm::INotifier notifier; - std::string output_recipe = "resources/rtde_output_recipe.txt"; - std::string input_recipe = "resources/rtde_input_recipe.txt"; - rtde_interface::RTDEClient client("abcd", notifier, output_recipe, input_recipe); + std::string output_recipe = "resources/empty.txt"; + std::string input_recipe = "resources/empty.txt"; + client_.reset(new rtde_interface::RTDEClient(ROBOT_IP, notifier_, output_recipe, input_recipe)); + + EXPECT_THROW(client_->init(), UrException); - EXPECT_THROW(client.init(), UrException); -}*/ + // Only input recipe is empty + client_.reset(new rtde_interface::RTDEClient(ROBOT_IP, notifier_, output_recipe_, input_recipe)); -TEST(UrRobotDriver, no_recipe) + EXPECT_THROW(client_->init(), UrException); +} + +TEST_F(RTDEClientTest, invalid_target_frequency) { - comm::INotifier notifier; - std::string output_recipe = ""; - std::string input_recipe = ""; - EXPECT_THROW(rtde_interface::RTDEClient client(ROBOT_IP, notifier, output_recipe, input_recipe), UrException); + // Setting target frequency below 0 or above 500, should throw an exception + client_.reset(new rtde_interface::RTDEClient(ROBOT_IP, notifier_, output_recipe_, input_recipe_, -1.0)); + + EXPECT_THROW(client_->init(), UrException); + + client_.reset(new rtde_interface::RTDEClient(ROBOT_IP, notifier_, output_recipe_, input_recipe_, 1000)); + + EXPECT_THROW(client_->init(), UrException); } -TEST(UrRobotDriver, empty_recipe) +TEST_F(RTDEClientTest, unconfigured_target_frequency) { - comm::INotifier notifier; - std::string output_recipe = "resources/empty.txt"; - std::string input_recipe = "resources/empty.txt"; - rtde_interface::RTDEClient client(ROBOT_IP, notifier, output_recipe, input_recipe); + // When the target frequency is unconfigured, it should be zero before the client has been initialized + double expected_target_frequency = 0.0; + EXPECT_EQ(client_->getTargetFrequency(), expected_target_frequency); + + client_->init(); + + // When the target frequency is unconfigured, it should be equal to the maximum frequency after initialization + EXPECT_EQ(client_->getTargetFrequency(), client_->getMaxFrequency()); +} + +TEST_F(RTDEClientTest, set_target_frequency) +{ + client_.reset(new rtde_interface::RTDEClient(ROBOT_IP, notifier_, output_recipe_, input_recipe_, 1)); + client_->init(); + + // Maximum frequency should still be equal to the robot's maximum frequency + if (client_->getVersion().major >= 5) + { + double expected_max_frequency = 500; + EXPECT_EQ(client_->getMaxFrequency(), expected_max_frequency); + } + else + { + double expected_max_frequency = 125; + EXPECT_EQ(client_->getMaxFrequency(), expected_max_frequency); + } + + double expected_target_frequency = 1; + EXPECT_EQ(client_->getTargetFrequency(), expected_target_frequency); + + EXPECT_TRUE(client_->start()); + + // 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(); + } + + 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(); + } + + double second_time_stamp = 0.0; + data_pkg->getData("timestamp", second_time_stamp); + + // There should be 1 second between each timestamp + EXPECT_EQ(second_time_stamp - first_time_stamp, 1); + + client_->pause(); +} + +TEST_F(RTDEClientTest, start_uninitialized_client) +{ + // It shouldn't be possible to start an uninitialized client + EXPECT_FALSE(client_->start()); +} + +TEST_F(RTDEClientTest, start_client) +{ + client_->init(); + EXPECT_TRUE(client_->start()); + + client_->pause(); + + // We should be able to start the client again after it has been paused + EXPECT_TRUE(client_->start()); + + client_->pause(); +} + +TEST_F(RTDEClientTest, pause_client_before_it_was_started) +{ + // We shouldn't be able to pause the client before it has been initialized + EXPECT_FALSE(client_->pause()); + + // We shouldn't be able to pause the client after it has been initialized + client_->init(); + EXPECT_FALSE(client_->pause()); +} + +TEST_F(RTDEClientTest, pause_client) +{ + client_->init(); + client_->start(); + + EXPECT_TRUE(client_->pause()); +} + +TEST_F(RTDEClientTest, output_recipe) +{ + std::vector expected_output_recipe = { "timestamp", + "actual_q", + "actual_qd", + "speed_scaling", + "target_speed_fraction", + "runtime_state", + "actual_TCP_force", + "actual_TCP_pose", + "actual_digital_input_bits", + "actual_digital_output_bits", + "standard_analog_input0", + "standard_analog_input1", + "standard_analog_output0", + "standard_analog_output1", + "analog_io_types", + "tool_mode", + "tool_analog_input_types", + "tool_analog_input0", + "tool_analog_input1", + "tool_output_voltage", + "tool_output_current", + "tool_temperature", + "robot_mode", + "safety_mode", + "robot_status_bits", + "safety_status_bits", + "actual_current", + "tcp_offset" }; + + std::vector actual_output_recipe = client_->getOutputRecipe(); + for (unsigned int i = 0; i < expected_output_recipe.size(); ++i) + { + EXPECT_EQ(expected_output_recipe[i], actual_output_recipe[i]); + } +} + +TEST_F(RTDEClientTest, get_data_package) +{ + 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 }; + 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(); + } + + urcl::vector6d_t actual_q; + EXPECT_TRUE(data_pkg->getData("actual_q", actual_q)); + + client_->pause(); +} + +TEST_F(RTDEClientTest, write_rtde_data) +{ + client_->init(); + client_->start(); + + bool send_digital_output = true; + EXPECT_TRUE(client_->getWriter().sendStandardDigitalOutput(0, send_digital_output)); + + // 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(); + } + + std::bitset<18> 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 + int max_tries = 100; + 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); + if (counter == max_tries) + { + break; + } + counter++; + } + + EXPECT_EQ(send_digital_output, actual_dig_out_bits[0]); - EXPECT_THROW(client.init(), UrException); + client_->pause(); } int main(int argc, char* argv[]) diff --git a/tests/test_rtde_control_package_pause.cpp b/tests/test_rtde_control_package_pause.cpp new file mode 100644 index 00000000..a54df238 --- /dev/null +++ b/tests/test_rtde_control_package_pause.cpp @@ -0,0 +1,71 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#include +#include + +using namespace urcl; + +TEST(rtde_control_package_pause, generate_serialized_pause_request) +{ + uint8_t buffer[4096]; + size_t expected_size = 3; + size_t actual_size = rtde_interface::ControlPackagePauseRequest::generateSerializedRequest(buffer); + + EXPECT_EQ(expected_size, actual_size); + + uint8_t expected[] = { 0x00, 0x03, 0x50 }; + for (unsigned int i = 0; i < actual_size; ++i) + { + EXPECT_EQ(expected[i], buffer[i]); + } +} + +TEST(rtde_control_package_pause, parse_accepted_pause_request) +{ + rtde_interface::ControlPackagePause pause_package; + + uint8_t pause_answer[] = { 0x01 }; + comm::BinParser bp(pause_answer, sizeof(pause_answer)); + + EXPECT_TRUE(pause_package.parseWith(bp)); + + uint8_t expected_answer = 1; + uint8_t actual_answer = pause_package.accepted_; + + EXPECT_EQ(expected_answer, actual_answer); +} + +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/tests/test_rtde_control_package_setup_inputs.cpp b/tests/test_rtde_control_package_setup_inputs.cpp new file mode 100644 index 00000000..fa980ed8 --- /dev/null +++ b/tests/test_rtde_control_package_setup_inputs.cpp @@ -0,0 +1,107 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#include +#include + +using namespace urcl; + +TEST(rtde_control_package_setup_inputs, generate_serialized_setup_inputs_request) +{ + uint8_t buffer[4096]; + size_t expected_size = 42; + std::vector input_recipe; + input_recipe.push_back("speed_slider_mask"); + input_recipe.push_back("speed_slider_fraction"); + size_t actual_size = + rtde_interface::ControlPackageSetupInputsRequest::generateSerializedRequest(buffer, input_recipe); + + EXPECT_EQ(expected_size, actual_size); + + uint8_t expected[] = { 0x00, 0x2a, 0x49, 0x73, 0x70, 0x65, 0x65, 0x64, 0x5f, 0x73, 0x6c, 0x69, 0x64, 0x65, + 0x72, 0x5f, 0x6d, 0x61, 0x73, 0x6b, 0x2c, 0x73, 0x70, 0x65, 0x65, 0x64, 0x5f, 0x73, + 0x6c, 0x69, 0x64, 0x65, 0x72, 0x5f, 0x66, 0x72, 0x61, 0x63, 0x74, 0x69, 0x6f, 0x6e }; + for (unsigned int i = 0; i < actual_size; ++i) + { + EXPECT_EQ(expected[i], buffer[i]); + } +} + +TEST(rtde_control_package_setup_inputs, empty_input_recipe) +{ + uint8_t buffer[4096]; + size_t expected_size = 0; + std::vector input_recipe; + size_t actual_size = + rtde_interface::ControlPackageSetupInputsRequest::generateSerializedRequest(buffer, input_recipe); + + EXPECT_EQ(expected_size, actual_size); +} + +TEST(rtde_control_package_setup_inputs, parse_accepted_setup_inputs) +{ + uint8_t setup_inputs_answer[] = { + 0x01, 0x55, 0x49, 0x4e, 0x54, 0x33, 0x32, 0x2c, 0x44, 0x4f, 0x55, 0x42, 0x4c, 0x45 + }; + comm::BinParser bp(setup_inputs_answer, sizeof(setup_inputs_answer)); + rtde_interface::ControlPackageSetupInputs setup_inputs; + + EXPECT_TRUE(setup_inputs.parseWith(bp)); + + uint8_t expected_input_recipe_id = 1; + std::string expected_variable_types = "UINT32,DOUBLE"; + + EXPECT_EQ(expected_input_recipe_id, setup_inputs.input_recipe_id_); + EXPECT_EQ(expected_variable_types, setup_inputs.variable_types_); +} + +TEST(rtde_control_package_setup_inputs, parse_not_accepted_setup_inputs) +{ + uint8_t setup_inputs_answer[] = { + 0x00, 0x49, 0x4e, 0x5f, 0x55, 0x53, 0x45, 0x2c, 0x49, 0x4e, 0x5f, 0x55, 0x53, 0x45 + }; + comm::BinParser bp(setup_inputs_answer, sizeof(setup_inputs_answer)); + rtde_interface::ControlPackageSetupInputs setup_inputs; + + EXPECT_TRUE(setup_inputs.parseWith(bp)); + + uint8_t expected_input_recipe_id = 0; + std::string expected_variable_types = "IN_USE,IN_USE"; + + EXPECT_EQ(expected_input_recipe_id, setup_inputs.input_recipe_id_); + EXPECT_EQ(expected_variable_types, setup_inputs.variable_types_); +} + +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/tests/test_rtde_control_package_setup_outputs.cpp b/tests/test_rtde_control_package_setup_outputs.cpp new file mode 100644 index 00000000..80975a15 --- /dev/null +++ b/tests/test_rtde_control_package_setup_outputs.cpp @@ -0,0 +1,161 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#include +#include + +using namespace urcl; + +TEST(rtde_control_package_setup_outputs, generate_serialized_setup_output_request_protocolv2) +{ + uint8_t buffer[4096]; + std::vector output_recipe; + output_recipe.push_back("actual_q"); + output_recipe.push_back("actual_qd"); + double output_frequency = 500; + size_t expected_size = 29; + size_t actual_size = rtde_interface::ControlPackageSetupOutputsRequest::generateSerializedRequest( + buffer, output_frequency, output_recipe); + + EXPECT_EQ(expected_size, actual_size); + + uint8_t expected[] = { 0x00, 0x1d, 0x4f, 0x40, 0x7f, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x61, 0x63, 0x74, 0x75, + 0x61, 0x6c, 0x5f, 0x71, 0x2c, 0x61, 0x63, 0x74, 0x75, 0x61, 0x6c, 0x5f, 0x71, 0x64 }; + + for (unsigned int i = 0; i < actual_size; ++i) + { + EXPECT_EQ(expected[i], buffer[i]); + } +} + +TEST(rtde_control_package_setup_outputs, generate_serialized_setup_output_request_protocolv1) +{ + uint8_t buffer[4096]; + std::vector output_recipe; + output_recipe.push_back("actual_q"); + output_recipe.push_back("actual_qd"); + size_t expected_size = 21; + size_t actual_size = + rtde_interface::ControlPackageSetupOutputsRequest::generateSerializedRequest(buffer, output_recipe); + + EXPECT_EQ(expected_size, actual_size); + + uint8_t expected[] = { 0x00, 0x15, 0x4f, 0x61, 0x63, 0x74, 0x75, 0x61, 0x6c, 0x5f, 0x71, + 0x2c, 0x61, 0x63, 0x74, 0x75, 0x61, 0x6c, 0x5f, 0x71, 0x64 }; + + for (unsigned int i = 0; i < actual_size; ++i) + { + EXPECT_EQ(expected[i], buffer[i]); + } +} + +TEST(rtde_control_package_setup_outputs, empty_output_recipe) +{ + uint8_t buffer[4096]; + std::vector output_recipe; + size_t expected_size = 0; + size_t actual_size = + rtde_interface::ControlPackageSetupOutputsRequest::generateSerializedRequest(buffer, output_recipe); + + EXPECT_EQ(expected_size, actual_size); + + double output_frequency = 100; + actual_size = rtde_interface::ControlPackageSetupOutputsRequest::generateSerializedRequest(buffer, output_frequency, + output_recipe); + + EXPECT_EQ(expected_size, actual_size); +} + +TEST(rtde_control_package_setup_outputs, parse_accepted_setup_output_protocolv2) +{ + uint8_t setup_outputs_answer[] = { 0x01, 0x44, 0x4f, 0x55, 0x42, 0x4c, 0x45, 0x2c, + 0x56, 0x45, 0x43, 0x54, 0x4f, 0x52, 0x36, 0x44 }; + comm::BinParser bp(setup_outputs_answer, sizeof(setup_outputs_answer)); + rtde_interface::ControlPackageSetupOutputs setup_outputs(2); + + EXPECT_TRUE(setup_outputs.parseWith(bp)); + + uint8_t expected_output_recipe_id = 1; + std::string expected_variable_types = "DOUBLE,VECTOR6D"; + + EXPECT_EQ(expected_output_recipe_id, setup_outputs.output_recipe_id_); + EXPECT_EQ(expected_variable_types, setup_outputs.variable_types_); +} + +TEST(rtde_control_package_setup_outputs, parse_not_accepted_setup_output_protocolv2) +{ + uint8_t setup_outputs_answer[] = { 0x00, 0x4e, 0x4f, 0x54, 0x5f, 0x46, 0x4f, 0x55, 0x4e, 0x44, + 0x2c, 0x56, 0x45, 0x43, 0x54, 0x4f, 0x52, 0x36, 0x44 }; + comm::BinParser bp(setup_outputs_answer, sizeof(setup_outputs_answer)); + rtde_interface::ControlPackageSetupOutputs setup_outputs(2); + + EXPECT_TRUE(setup_outputs.parseWith(bp)); + + uint8_t expected_output_recipe_id = 0; + std::string expected_variable_types = "NOT_FOUND,VECTOR6D"; + + EXPECT_EQ(expected_output_recipe_id, setup_outputs.output_recipe_id_); + EXPECT_EQ(expected_variable_types, setup_outputs.variable_types_); +} + +TEST(rtde_control_package_setup_outputs, parse_accepted_setup_output_protocolv1) +{ + uint8_t setup_outputs_answer[] = { 0x44, 0x4f, 0x55, 0x42, 0x4c, 0x45, 0x2c, 0x56, + 0x45, 0x43, 0x54, 0x4f, 0x52, 0x36, 0x44 }; + comm::BinParser bp(setup_outputs_answer, sizeof(setup_outputs_answer)); + rtde_interface::ControlPackageSetupOutputs setup_outputs(1); + + EXPECT_TRUE(setup_outputs.parseWith(bp)); + + std::string expected_variable_types = "DOUBLE,VECTOR6D"; + + EXPECT_EQ(expected_variable_types, setup_outputs.variable_types_); +} + +TEST(rtde_control_package_setup_outputs, parse_not_accepted_setup_output_protocolv1) +{ + uint8_t setup_outputs_answer[] = { 0x4e, 0x4f, 0x54, 0x5f, 0x46, 0x4f, 0x55, 0x4e, 0x44, + 0x2c, 0x56, 0x45, 0x43, 0x54, 0x4f, 0x52, 0x36, 0x44 }; + comm::BinParser bp(setup_outputs_answer, sizeof(setup_outputs_answer)); + rtde_interface::ControlPackageSetupOutputs setup_outputs(1); + + EXPECT_TRUE(setup_outputs.parseWith(bp)); + + std::string expected_variable_types = "NOT_FOUND,VECTOR6D"; + + EXPECT_EQ(expected_variable_types, setup_outputs.variable_types_); +} + +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/tests/test_rtde_control_package_start.cpp b/tests/test_rtde_control_package_start.cpp new file mode 100644 index 00000000..863d4529 --- /dev/null +++ b/tests/test_rtde_control_package_start.cpp @@ -0,0 +1,86 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#include +#include + +using namespace urcl; + +TEST(rtde_control_package_start, generate_serialized_start_request) +{ + uint8_t buffer[4096]; + size_t expected_size = 3; + size_t actual_size = rtde_interface::ControlPackageStartRequest::generateSerializedRequest(buffer); + + EXPECT_EQ(expected_size, actual_size); + + uint8_t expected[] = { 0x00, 0x03, 0x53 }; + for (unsigned int i = 0; i < actual_size; ++i) + { + EXPECT_EQ(expected[i], buffer[i]); + } +} + +TEST(rtde_control_package_start, parse_accepted_start_request) +{ + rtde_interface::ControlPackageStart start_package; + + uint8_t start_answer[] = { 0x01 }; + comm::BinParser bp(start_answer, sizeof(start_answer)); + + EXPECT_TRUE(start_package.parseWith(bp)); + + uint8_t expected_answer = 1; + uint8_t actual_answer = start_package.accepted_; + + EXPECT_EQ(expected_answer, actual_answer); +} + +TEST(rtde_control_package_start, parse_not_accepted_start_request) +{ + rtde_interface::ControlPackageStart start_package; + + uint8_t start_answer[] = { 0x00 }; + comm::BinParser bp(start_answer, sizeof(start_answer)); + + EXPECT_TRUE(start_package.parseWith(bp)); + + uint8_t expected_answer = 0; + uint8_t actual_answer = start_package.accepted_; + + EXPECT_EQ(expected_answer, actual_answer); +} + +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/tests/test_rtde_data_package.cpp b/tests/test_rtde_data_package.cpp index 4f805440..e1c34298 100644 --- a/tests/test_rtde_data_package.cpp +++ b/tests/test_rtde_data_package.cpp @@ -48,12 +48,116 @@ TEST(rtde_data_package, serialize_pkg) EXPECT_EQ(size, 8); uint8_t expected[] = { 0x0, 0x08, 0x55, 0x01, 0x00, 0x00, 0x00, 0x01 }; - std::cout << "Serialized buffer: " << std::endl; + for (size_t i = 0; i < size; ++i) { EXPECT_EQ(buffer[i], expected[i]); } - std::cout << std::endl; +} + +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, + 0x88, 0x5a, 0x30, 0xbf, 0xe9, 0xdb, 0x22, 0xa2, 0x21, 0x68, 0xc0, 0x3f, 0xf9, 0x85, 0x87, + 0xa0, 0x00, 0x00, 0x00, 0xbf, 0x9f, 0xbe, 0x74, 0x44, 0x2d, 0x18, 0x00 }; + + comm::BinParser bp(data_package, sizeof(data_package)); + + EXPECT_TRUE(package.parseWith(bp)); + + vector6d_t expected_q = { -1.6007, -1.7271, -2.203, -0.808, 1.5951, -0.031 }; + vector6d_t actual_q; + package.getData("actual_q", actual_q); + + double abs = 1e-4; + EXPECT_NEAR(expected_q[0], actual_q[0], abs); + EXPECT_NEAR(expected_q[1], actual_q[1], abs); + EXPECT_NEAR(expected_q[2], actual_q[2], abs); + EXPECT_NEAR(expected_q[3], actual_q[3], abs); + EXPECT_NEAR(expected_q[4], actual_q[4], abs); + EXPECT_NEAR(expected_q[5], actual_q[5], abs); + + double expected_timestamp = 16854.1919; + double actual_timestamp; + package.getData("timestamp", actual_timestamp); + + EXPECT_NEAR(expected_timestamp, actual_timestamp, abs); +} + +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, + 0x68, 0x88, 0x5a, 0x30, 0xbf, 0xe9, 0xdb, 0x22, 0xa2, 0x21, 0x68, 0xc0, 0x3f, 0xf9, + 0x85, 0x87, 0xa0, 0x00, 0x00, 0x00, 0xbf, 0x9f, 0xbe, 0x74, 0x44, 0x2d, 0x18, 0x00 }; + comm::BinParser bp(data_package, sizeof(data_package)); + + EXPECT_TRUE(package.parseWith(bp)); + + vector6d_t expected_q = { -1.6007, -1.7271, -2.203, -0.808, 1.5951, -0.031 }; + vector6d_t actual_q; + package.getData("actual_q", actual_q); + + double abs = 1e-4; + EXPECT_NEAR(expected_q[0], actual_q[0], abs); + EXPECT_NEAR(expected_q[1], actual_q[1], abs); + EXPECT_NEAR(expected_q[2], actual_q[2], abs); + EXPECT_NEAR(expected_q[3], actual_q[3], abs); + EXPECT_NEAR(expected_q[4], actual_q[4], abs); + EXPECT_NEAR(expected_q[5], actual_q[5], abs); + + double expected_timestamp = 16854.1919; + double actual_timestamp; + package.getData("timestamp", actual_timestamp); + + EXPECT_NEAR(expected_timestamp, actual_timestamp, abs); +} + +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)); +} + +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)); +} + +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)); + + EXPECT_TRUE(package.parseWith(bp)); + + std::bitset<4> expected_robot_status_bits = 0000; + std::bitset<4> actual_robot_status_bits; + package.getData("robot_status_bits", actual_robot_status_bits); + + EXPECT_EQ(expected_robot_status_bits, actual_robot_status_bits); } int main(int argc, char* argv[]) diff --git a/tests/test_rtde_get_urcontrol_version.cpp b/tests/test_rtde_get_urcontrol_version.cpp new file mode 100644 index 00000000..d8cf2ac3 --- /dev/null +++ b/tests/test_rtde_get_urcontrol_version.cpp @@ -0,0 +1,77 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#include +#include + +using namespace urcl; + +TEST(rtde_get_urcontrol_version, generate_serialized_get_urcontrol_version_request) +{ + uint8_t buffer[4096]; + size_t expected_size = 3; + size_t actual_size = rtde_interface::GetUrcontrolVersionRequest::generateSerializedRequest(buffer); + + EXPECT_EQ(expected_size, actual_size); + + uint8_t expected[] = { 0x00, 0x03, 0x76 }; + for (unsigned int i = 0; i < actual_size; ++i) + { + EXPECT_EQ(expected[i], buffer[i]); + } +} + +TEST(rtde_get_urcontrol_version, parse_get_urcontrol_version) +{ + uint8_t urcontrol_version_answer[] = { 0x00, 0x00, 0x00, 0x05, 0x00, 0x00, 0x00, 0x0c, + 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00 }; + comm::BinParser bp(urcontrol_version_answer, sizeof(urcontrol_version_answer)); + rtde_interface::GetUrcontrolVersion ur_control_version; + + EXPECT_TRUE(ur_control_version.parseWith(bp)); + + VersionInformation expected_version_information; + expected_version_information.major = 5; + expected_version_information.minor = 12; + expected_version_information.bugfix = 2; + expected_version_information.build = 0; + + EXPECT_EQ(expected_version_information.major, ur_control_version.version_information_.major); + EXPECT_EQ(expected_version_information.minor, ur_control_version.version_information_.minor); + EXPECT_EQ(expected_version_information.bugfix, ur_control_version.version_information_.bugfix); + EXPECT_EQ(expected_version_information.build, ur_control_version.version_information_.build); +} + +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/tests/test_rtde_parser.cpp b/tests/test_rtde_parser.cpp index 8770eafc..cc869189 100644 --- a/tests/test_rtde_parser.cpp +++ b/tests/test_rtde_parser.cpp @@ -30,7 +30,6 @@ #include #include -#include using namespace urcl; @@ -51,6 +50,11 @@ TEST(rtde_parser, request_protocol_version) { EXPECT_EQ(data->accepted_, true); } + else + { + std::cout << "Failed to get request protocol version data" << std::endl; + GTEST_FAIL(); + } } TEST(rtde_parser, get_urcontrol_version) @@ -73,6 +77,141 @@ TEST(rtde_parser, get_urcontrol_version) EXPECT_EQ(data->version_information_.bugfix, 0); EXPECT_EQ(data->version_information_.build, 0); } + else + { + std::cout << "Failed to get urcontrol version data" << std::endl; + GTEST_FAIL(); + } +} + +TEST(rtde_parser, control_package_pause) +{ + // Accepted control package pause + unsigned char raw_data[] = { 0x00, 0x04, 0x50, 0x01 }; + comm::BinParser bp(raw_data, sizeof(raw_data)); + + std::vector> products; + rtde_interface::RTDEParser parser({ "" }); + parser.parse(bp, products); + + EXPECT_EQ(products.size(), 1); + + if (rtde_interface::ControlPackagePause* data = dynamic_cast(products[0].get())) + { + EXPECT_EQ(data->accepted_, true); + } + else + { + std::cout << "Failed to get control package pause data" << std::endl; + GTEST_FAIL(); + } +} + +TEST(rtde_parser, control_package_start) +{ + // Accepted control package start + unsigned char raw_data[] = { 0x00, 0x04, 0x53, 0x01 }; + comm::BinParser bp(raw_data, sizeof(raw_data)); + + std::vector> products; + rtde_interface::RTDEParser parser({ "" }); + parser.parse(bp, products); + + EXPECT_EQ(products.size(), 1); + + if (rtde_interface::ControlPackageStart* data = dynamic_cast(products[0].get())) + { + EXPECT_EQ(data->accepted_, true); + } + else + { + std::cout << "Failed to get control package start data" << std::endl; + GTEST_FAIL(); + } +} + +TEST(rtde_parser, control_package_setup_inputs) +{ + // Accepted control package setup inputs, variable types are uint32 and double + unsigned char raw_data[] = { 0x00, 0x11, 0x49, 0x01, 0x55, 0x49, 0x4e, 0x54, 0x33, + 0x32, 0x2c, 0x44, 0x4f, 0x55, 0x42, 0x4c, 0x45 }; + comm::BinParser bp(raw_data, sizeof(raw_data)); + + std::vector> products; + rtde_interface::RTDEParser parser({ "" }); + parser.parse(bp, products); + + EXPECT_EQ(products.size(), 1); + + if (rtde_interface::ControlPackageSetupInputs* data = + dynamic_cast(products[0].get())) + { + EXPECT_EQ(data->input_recipe_id_, 1); + EXPECT_EQ(data->variable_types_, "UINT32,DOUBLE"); + } + else + { + std::cout << "Failed to get control package setup inputs data" << std::endl; + GTEST_FAIL(); + } +} + +TEST(rtde_parser, control_package_setup_outputs) +{ + // Accepted control package setup outputs, variable types are double and vector6d + unsigned char raw_data[] = { 0x00, 0x11, 0x4f, 0x01, 0x44, 0x4f, 0x55, 0x42, 0x4c, 0x45, + 0x2c, 0x56, 0x45, 0x43, 0x54, 0x4f, 0x52, 0x36, 0x44 }; + comm::BinParser bp(raw_data, sizeof(raw_data)); + + std::vector> products; + rtde_interface::RTDEParser parser({ "" }); + parser.setProtocolVersion(2); + parser.parse(bp, products); + + EXPECT_EQ(products.size(), 1); + + if (rtde_interface::ControlPackageSetupOutputs* data = + dynamic_cast(products[0].get())) + { + EXPECT_EQ(data->output_recipe_id_, 1); + EXPECT_EQ(data->variable_types_, "DOUBLE,VECTOR6D"); + } + else + { + std::cout << "Failed to get control package setup outputs data" << std::endl; + GTEST_FAIL(); + } +} + +TEST(rtde_parser, data_package) +{ + // received data package, + unsigned char raw_data[] = { 0x00, 0x14, 0x55, 0x01, 0x40, 0xd0, 0x07, 0x0d, 0x2f, 0x1a, + 0x9f, 0xbe, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + comm::BinParser bp(raw_data, sizeof(raw_data)); + + std::vector> products; + std::vector recipe = { "timestamp", "target_speed_fraction" }; + rtde_interface::RTDEParser parser(recipe); + parser.setProtocolVersion(2); + parser.parse(bp, products); + + EXPECT_EQ(products.size(), 1); + + if (rtde_interface::DataPackage* data = dynamic_cast(products[0].get())) + { + double timestamp, target_speed_fraction; + data->getData("timestamp", timestamp); + data->getData("target_speed_fraction", target_speed_fraction); + + EXPECT_FLOAT_EQ(timestamp, 16412.2); + EXPECT_EQ(target_speed_fraction, 1); + } + else + { + std::cout << "Failed to get data package data" << std::endl; + GTEST_FAIL(); + } } TEST(rtde_parser, test_to_string) @@ -94,7 +233,7 @@ TEST(rtde_parser, test_to_string) EXPECT_EQ(products[0]->toString(), expected.str()); } -TEST(rtde_parser, test_illegal_length) +TEST(rtde_parser, test_buffer_too_short) { // Non-existent type with false size information unsigned char raw_data[] = { 0x00, 0x06, 0x02, 0x00, 0x00 }; @@ -105,6 +244,17 @@ TEST(rtde_parser, test_illegal_length) EXPECT_FALSE(parser.parse(bp, products)); } +TEST(rtde_parser, test_buffer_too_long) +{ + // Non-existent type with false size information + unsigned char raw_data[] = { 0x00, 0x04, 0x56, 0x01, 0x02, 0x01, 0x02 }; + comm::BinParser bp(raw_data, sizeof(raw_data)); + + std::vector> products; + rtde_interface::RTDEParser parser({ "" }); + EXPECT_FALSE(parser.parse(bp, products)); +} + int main(int argc, char* argv[]) { ::testing::InitGoogleTest(&argc, argv); diff --git a/tests/test_rtde_request_protocol_version.cpp b/tests/test_rtde_request_protocol_version.cpp new file mode 100644 index 00000000..716d3427 --- /dev/null +++ b/tests/test_rtde_request_protocol_version.cpp @@ -0,0 +1,103 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#include +#include + +using namespace urcl; + +TEST(rtde_request_protocol_version, generate_serialized_protocol_v2_request) +{ + uint8_t buffer[4096]; + uint16_t protocol_version = 2; + size_t expected_size = 5; + size_t actual_size = + rtde_interface::RequestProtocolVersionRequest::generateSerializedRequest(buffer, protocol_version); + + EXPECT_EQ(expected_size, actual_size); + + uint8_t expected[] = { 0x00, 0x05, 0x56, 0x00, 0x02 }; + + for (unsigned int i = 0; i < actual_size; ++i) + { + EXPECT_EQ(expected[i], buffer[i]); + } +} + +TEST(rtde_request_protocol_version, generate_serialized_protocol_v1_request) +{ + uint8_t buffer[4096]; + uint16_t protocol_version = 1; + size_t expected_size = 5; + size_t actual_size = + rtde_interface::RequestProtocolVersionRequest::generateSerializedRequest(buffer, protocol_version); + + EXPECT_EQ(expected_size, actual_size); + + uint8_t expected[] = { 0x00, 0x05, 0x56, 0x00, 0x01 }; + + for (unsigned int i = 0; i < actual_size; ++i) + { + EXPECT_EQ(expected[i], buffer[i]); + } +} + +TEST(rtde_request_protocol_version, parse_accepted_request) +{ + uint8_t request_answer[] = { 0x01 }; + comm::BinParser bp(request_answer, sizeof(request_answer)); + rtde_interface::RequestProtocolVersion protocol_version; + + EXPECT_TRUE(protocol_version.parseWith(bp)); + + uint8_t expected_anwser = 1; + + EXPECT_EQ(expected_anwser, protocol_version.accepted_); +} + +TEST(rtde_request_protocol_version, parse_not_accepted_request) +{ + uint8_t request_answer[] = { 0x00 }; + comm::BinParser bp(request_answer, sizeof(request_answer)); + rtde_interface::RequestProtocolVersion protocol_version; + + EXPECT_TRUE(protocol_version.parseWith(bp)); + + uint8_t expected_anwser = 0; + + EXPECT_EQ(expected_anwser, protocol_version.accepted_); +} + +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/tests/test_rtde_writer.cpp b/tests/test_rtde_writer.cpp new file mode 100644 index 00000000..bec76bd9 --- /dev/null +++ b/tests/test_rtde_writer.cpp @@ -0,0 +1,372 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#include +#include + +#include +#include +#include + +using namespace urcl; + +class RTDEWriterTest : public ::testing::Test +{ +protected: + using input_types = std::variant; + + void SetUp() + { + // The port shouldn't collide with any of the ports the robot is using + server_.reset(new comm::TCPServer(60004)); + server_->setMessageCallback(std::bind(&RTDEWriterTest::messageCallback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); + server_->start(); + + stream_.reset(new comm::URStream("127.0.0.1", 60004)); + stream_->connect(); + + writer_.reset(new rtde_interface::RTDEWriter(stream_.get(), input_recipe_)); + writer_->init(1); + } + + void TearDown() + { + // Clean up + writer_.reset(); + stream_.reset(); + server_.reset(); + } + + void messageCallback(const int filedescriptor, char* buffer, int nbytesrecv) + { + std::lock_guard lk(message_mutex_); + uint8_t* buf = reinterpret_cast(buffer); + comm::BinParser bp(buf, nbytesrecv); + // These might be needed in the test + uint16_t size; + uint8_t type, recipe_id; + bp.parse(size); + bp.parse(type); + bp.parse(recipe_id); + parseMessage(bp); + message_cv_.notify_one(); + message_callback_ = true; + } + + bool waitForMessageCallback(int milliseconds = 100) + { + std::unique_lock lk(message_mutex_); + if (message_cv_.wait_for(lk, std::chrono::milliseconds(milliseconds)) == std::cv_status::no_timeout || + message_callback_ == true) + { + message_callback_ = false; + return true; + } + else + { + return false; + } + } + + // Helper function to see if data field exists in the parsed message + bool dataFieldExist(std::string name) + { + if (parsed_data_.find(name) != parsed_data_.end()) + { + return true; + } + std::cout << "Failed to find data field " << name << " this should not happen! Have a look at the test case" + << std::endl; + return false; + } + + std::vector input_recipe_ = { + "speed_slider_mask", + "speed_slider_fraction", + "standard_digital_output_mask", + "standard_digital_output", + "configurable_digital_output_mask", + "configurable_digital_output", + "tool_digital_output_mask", + "tool_digital_output", + "standard_analog_output_mask", + "standard_analog_output_type", + "standard_analog_output_0", + "standard_analog_output_1", + "input_bit_register_65", + "input_int_register_25", + "input_double_register_25", + }; + std::unique_ptr writer_; + std::unique_ptr server_; + std::unique_ptr> stream_; + std::unordered_map parsed_data_; + +private: + void parseMessage(comm::BinParser bp) + { + for (auto& item : input_recipe_) + { + if (input_map_types_.find(item) != input_map_types_.end()) + { + input_types entry = input_map_types_[item]; + std::visit([&bp](auto&& arg) { bp.parse(arg); }, entry); + parsed_data_[item] = entry; + } + } + } + + std::condition_variable message_cv_; + std::mutex message_mutex_; + bool message_callback_ = false; + + std::unordered_map input_map_types_ = { + { "speed_slider_mask", uint32_t() }, + { "speed_slider_fraction", double() }, + { "standard_digital_output_mask", uint8_t() }, + { "standard_digital_output", uint8_t() }, + { "configurable_digital_output_mask", uint8_t() }, + { "configurable_digital_output", uint8_t() }, + { "tool_digital_output_mask", uint8_t() }, + { "tool_digital_output", uint8_t() }, + { "standard_analog_output_mask", uint8_t() }, + { "standard_analog_output_type", uint8_t() }, + { "standard_analog_output_0", double() }, + { "standard_analog_output_1", double() }, + { "input_bit_register_65", bool() }, + { "input_int_register_25", int32_t() }, + { "input_double_register_25", double() }, + }; +}; + +// Use other port and create test fixture +TEST_F(RTDEWriterTest, send_speed_slider) +{ + uint32_t expected_speed_slider_mask = 1; + double send_speed_slider_fraction = 0.5; + + EXPECT_TRUE(writer_->sendSpeedSlider(send_speed_slider_fraction)); + + waitForMessageCallback(1000); + + ASSERT_TRUE(dataFieldExist("speed_slider_fraction")); + ASSERT_TRUE(dataFieldExist("speed_slider_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"]); + + EXPECT_EQ(send_speed_slider_fraction, received_speed_slider_fraction); + EXPECT_EQ(expected_speed_slider_mask, received_speed_slider_mask); + + // Setting speed slider fraction below 0 or above 1, should return false + EXPECT_FALSE(writer_->sendSpeedSlider(-1)); + EXPECT_FALSE(writer_->sendSpeedSlider(2)); +} + +TEST_F(RTDEWriterTest, send_standard_digital_output) +{ + uint8_t expected_standard_digital_output_mask = 4; + uint8_t pin = 2; + bool send_pin_value = true; + EXPECT_TRUE(writer_->sendStandardDigitalOutput(pin, send_pin_value)); + + waitForMessageCallback(1000); + + ASSERT_TRUE(dataFieldExist("standard_digital_output")); + ASSERT_TRUE(dataFieldExist("standard_digital_output_mask")); + + bool received_pin_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_pin_value, received_pin_value); + EXPECT_EQ(expected_standard_digital_output_mask, received_standard_digital_output_mask); + + // Changing pins above 7, should return false. + pin = 8; + EXPECT_FALSE(writer_->sendStandardDigitalOutput(pin, send_pin_value)); +} + +TEST_F(RTDEWriterTest, send_configurable_digital_output) +{ + uint8_t expected_configurable_digital_output_mask = 8; + uint8_t pin = 3; + bool send_pin_value = true; + EXPECT_TRUE(writer_->sendConfigurableDigitalOutput(pin, send_pin_value)); + + waitForMessageCallback(1000); + + ASSERT_TRUE(dataFieldExist("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"]); + + EXPECT_EQ(send_pin_value, received_pin_value); + EXPECT_EQ(expected_configurable_digital_output_mask, received_standard_digital_output_mask); + + // Changing pins above 7, should return false. + pin = 8; + EXPECT_FALSE(writer_->sendStandardDigitalOutput(pin, send_pin_value)); +} + +TEST_F(RTDEWriterTest, send_tool_digital_output) +{ + uint8_t expected_tool_digital_output_mask = 1; + uint8_t pin = 0; + bool send_pin_value = true; + EXPECT_TRUE(writer_->sendToolDigitalOutput(pin, send_pin_value)); + + waitForMessageCallback(1000); + + ASSERT_TRUE(dataFieldExist("tool_digital_output")); + ASSERT_TRUE(dataFieldExist("tool_digital_output_mask")); + + bool received_pin_value = std::get(parsed_data_["tool_digital_output"]) != 0; + uint8_t received_tool_digital_output_mask = std::get(parsed_data_["tool_digital_output_mask"]); + + EXPECT_EQ(send_pin_value, received_pin_value); + EXPECT_EQ(expected_tool_digital_output_mask, received_tool_digital_output_mask); + + // Changing pins above 1, should return false. + pin = 2; + EXPECT_FALSE(writer_->sendToolDigitalOutput(pin, send_pin_value)); +} + +TEST_F(RTDEWriterTest, send_standard_analog_output) +{ + uint8_t expected_standard_analog_output_mask = 1; + uint8_t pin = 0; + double send_analog_output = 1; + EXPECT_TRUE(writer_->sendStandardAnalogOutput(pin, send_analog_output)); + + waitForMessageCallback(1000); + + ASSERT_TRUE(dataFieldExist("standard_analog_output_0")); + ASSERT_TRUE(dataFieldExist("standard_analog_output_1")); + ASSERT_TRUE(dataFieldExist("standard_analog_output_mask")); + + double received_analog_output = std::get(parsed_data_["standard_analog_output_0"]); + uint8_t received_standard_analog_output_mask = std::get(parsed_data_["standard_analog_output_mask"]); + + EXPECT_EQ(send_analog_output, received_analog_output); + EXPECT_EQ(expected_standard_analog_output_mask, received_standard_analog_output_mask); + + pin = 1; + expected_standard_analog_output_mask = 2; + EXPECT_TRUE(writer_->sendStandardAnalogOutput(pin, send_analog_output)); + + waitForMessageCallback(1000); + + received_analog_output = std::get(parsed_data_["standard_analog_output_1"]); + received_standard_analog_output_mask = std::get(parsed_data_["standard_analog_output_mask"]); + + EXPECT_EQ(send_analog_output, received_analog_output); + EXPECT_EQ(expected_standard_analog_output_mask, received_standard_analog_output_mask); + + // Changing pins above 1, should return false. + pin = 2; + EXPECT_FALSE(writer_->sendStandardAnalogOutput(pin, send_analog_output)); + + // Setting analog output below 0 or above 1, should return false + pin = 1; + EXPECT_FALSE(writer_->sendStandardAnalogOutput(pin, 1.1)); + EXPECT_FALSE(writer_->sendStandardAnalogOutput(pin, -0.1)); +} + +TEST_F(RTDEWriterTest, send_input_bit_register) +{ + uint32_t register_id = 65; + bool send_register_value = true; + EXPECT_TRUE(writer_->sendInputBitRegister(register_id, send_register_value)); + + waitForMessageCallback(1000); + + ASSERT_TRUE(dataFieldExist("input_bit_register_65")); + + bool received_register_value = std::get(parsed_data_["input_bit_register_65"]); + + EXPECT_EQ(send_register_value, received_register_value); + + // Changing registers below 64 and above 127, should return false. + register_id = 63; + EXPECT_FALSE(writer_->sendInputBitRegister(register_id, send_register_value)); + register_id = 128; + EXPECT_FALSE(writer_->sendInputBitRegister(register_id, send_register_value)); +} + +TEST_F(RTDEWriterTest, send_input_int_register) +{ + uint32_t register_id = 25; + int32_t send_register_value = 21; + EXPECT_TRUE(writer_->sendInputIntRegister(register_id, send_register_value)); + + waitForMessageCallback(1000); + + ASSERT_TRUE(dataFieldExist("input_int_register_25")); + + int32_t received_register_value = std::get(parsed_data_["input_int_register_25"]); + + EXPECT_EQ(send_register_value, received_register_value); + + // Changing registers below 23 and above 48, should return false. + register_id = 23; + EXPECT_FALSE(writer_->sendInputIntRegister(register_id, send_register_value)); + register_id = 48; + EXPECT_FALSE(writer_->sendInputIntRegister(register_id, send_register_value)); +} + +TEST_F(RTDEWriterTest, send_input_double_register) +{ + uint32_t register_id = 25; + double send_register_value = 2.1; + EXPECT_TRUE(writer_->sendInputDoubleRegister(register_id, send_register_value)); + + waitForMessageCallback(1000); + + ASSERT_TRUE(dataFieldExist("input_double_register_25")); + + double received_register_value = std::get(parsed_data_["input_double_register_25"]); + + EXPECT_EQ(send_register_value, received_register_value); + + // Changing registers below 23 and above 48, should return false. + register_id = 23; + EXPECT_FALSE(writer_->sendInputDoubleRegister(register_id, send_register_value)); + register_id = 48; + EXPECT_FALSE(writer_->sendInputDoubleRegister(register_id, send_register_value)); +} + +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +}