From 34a76f2267cea817b2cd6c13722ca25b0f4b7fe7 Mon Sep 17 00:00:00 2001 From: urmahp Date: Wed, 23 Aug 2023 12:16:24 +0200 Subject: [PATCH 1/4] Timestamp is now added to the output recipe correctly Added test to verify the addition of timestamp Only disconnect if client is initialized --- include/ur_client_library/rtde/rtde_client.h | 2 +- src/rtde/rtde_client.cpp | 40 +++++++++++++------ tests/resources/empty.txt | 1 - .../rtde_output_recipe_without_timestamp.txt | 27 +++++++++++++ tests/test_rtde_client.cpp | 21 +++++++--- 5 files changed, 70 insertions(+), 21 deletions(-) create mode 100644 tests/resources/rtde_output_recipe_without_timestamp.txt diff --git a/include/ur_client_library/rtde/rtde_client.h b/include/ur_client_library/rtde/rtde_client.h index de2b12dc..1d1e4860 100644 --- a/include/ur_client_library/rtde/rtde_client.h +++ b/include/ur_client_library/rtde/rtde_client.h @@ -208,7 +208,7 @@ class RTDEClient constexpr static const double CB3_MAX_FREQUENCY = 125.0; constexpr static const double URE_MAX_FREQUENCY = 500.0; - std::vector readRecipe(const std::string& recipe_file); + std::vector readRecipe(const std::string& recipe_file, bool output_recipe = false); void setupCommunication(); bool negotiateProtocolVersion(const uint16_t protocol_version); diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index e58ecbca..f3e190b2 100644 --- a/src/rtde/rtde_client.cpp +++ b/src/rtde/rtde_client.cpp @@ -37,8 +37,8 @@ namespace rtde_interface RTDEClient::RTDEClient(std::string robot_ip, comm::INotifier& notifier, const std::string& output_recipe_file, const std::string& input_recipe_file, double target_frequency) : stream_(robot_ip, UR_RTDE_PORT) - , output_recipe_(readRecipe(output_recipe_file)) - , input_recipe_(readRecipe(input_recipe_file)) + , output_recipe_(readRecipe(output_recipe_file, true)) + , input_recipe_(readRecipe(input_recipe_file, false)) , parser_(output_recipe_) , prod_(stream_, parser_) , pipeline_(prod_, PIPELINE_NAME, notifier, true) @@ -241,13 +241,6 @@ void RTDEClient::setupOutputs(const uint16_t protocol_version) size_t written; uint8_t buffer[4096]; URCL_LOG_INFO("Setting up RTDE communication with frequency %f", target_frequency_); - // Add timestamp to rtde output recipe, used to check if robot is booted - const std::string timestamp = "timestamp"; - auto it = std::find(output_recipe_.begin(), output_recipe_.end(), timestamp); - if (it == output_recipe_.end()) - { - output_recipe_.push_back(timestamp); - } if (protocol_version == 2) { size = ControlPackageSetupOutputsRequest::generateSerializedRequest(buffer, target_frequency_, output_recipe_); @@ -384,9 +377,12 @@ void RTDEClient::setupInputs() void RTDEClient::disconnect() { // If communication is started it should be paused before disconnecting - sendPause(); - pipeline_.stop(); - stream_.disconnect(); + if (client_state_ > ClientState::UNINITIALIZED) + { + sendPause(); + pipeline_.stop(); + stream_.disconnect(); + } client_state_ = ClientState::UNINITIALIZED; } @@ -548,7 +544,7 @@ bool RTDEClient::sendPause() throw UrException(ss.str()); } -std::vector RTDEClient::readRecipe(const std::string& recipe_file) +std::vector RTDEClient::readRecipe(const std::string& recipe_file, bool output_recipe) { std::vector recipe; std::ifstream file(recipe_file); @@ -559,11 +555,29 @@ std::vector RTDEClient::readRecipe(const std::string& recipe_file) URCL_LOG_ERROR("%s", msg.str().c_str()); throw UrException(msg.str()); } + + if (file.peek() == std::ifstream::traits_type::eof()) + { + std::stringstream msg; + msg << "The recipe '" << recipe_file << "' file is empty exiting "; + URCL_LOG_ERROR("%s", msg.str().c_str()); + throw UrException(msg.str()); + } + std::string line; while (std::getline(file, line)) { recipe.push_back(line); } + + // Add timestamp to rtde output recipe, used to check if robot is booted + const std::string timestamp = "timestamp"; + auto it = std::find(recipe.begin(), recipe.end(), timestamp); + if (it == recipe.end() && output_recipe == true) + { + recipe.push_back(timestamp); + } + return recipe; } diff --git a/tests/resources/empty.txt b/tests/resources/empty.txt index 8d1c8b69..e69de29b 100644 --- a/tests/resources/empty.txt +++ b/tests/resources/empty.txt @@ -1 +0,0 @@ - diff --git a/tests/resources/rtde_output_recipe_without_timestamp.txt b/tests/resources/rtde_output_recipe_without_timestamp.txt new file mode 100644 index 00000000..f2ee5f47 --- /dev/null +++ b/tests/resources/rtde_output_recipe_without_timestamp.txt @@ -0,0 +1,27 @@ +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 diff --git a/tests/test_rtde_client.cpp b/tests/test_rtde_client.cpp index 7d6523fd..dfeb79cb 100644 --- a/tests/test_rtde_client.cpp +++ b/tests/test_rtde_client.cpp @@ -77,14 +77,12 @@ TEST_F(RTDEClientTest, empty_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_.reset(new rtde_interface::RTDEClient(ROBOT_IP, notifier_, output_recipe, input_recipe)), + UrException); // Only input recipe is empty - client_.reset(new rtde_interface::RTDEClient(ROBOT_IP, notifier_, output_recipe_, input_recipe)); - - EXPECT_THROW(client_->init(), UrException); + EXPECT_THROW(client_.reset(new rtde_interface::RTDEClient(ROBOT_IP, notifier_, output_recipe_, input_recipe)), + UrException); } TEST_F(RTDEClientTest, invalid_target_frequency) @@ -296,6 +294,17 @@ TEST_F(RTDEClientTest, write_rtde_data) client_->pause(); } +TEST_F(RTDEClientTest, output_recipe_without_timestamp) +{ + std::string output_recipe = "resources/rtde_output_recipe_without_timestamp.txt"; + client_.reset(new rtde_interface::RTDEClient(ROBOT_IP, notifier_, output_recipe, input_recipe_)); + + std::vector actual_output_recipe = client_->getOutputRecipe(); + const std::string timestamp = "timestamp"; + auto it = std::find(actual_output_recipe.begin(), actual_output_recipe.end(), timestamp); + EXPECT_FALSE(it == actual_output_recipe.end()); +} + int main(int argc, char* argv[]) { ::testing::InitGoogleTest(&argc, argv); From 0b5aee380cf80be342ed7b52fb76233b84dca716 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Rune=20S=C3=B8e-Knudsen?= <41109954+urrsk@users.noreply.github.com> Date: Thu, 24 Aug 2023 10:30:13 +0200 Subject: [PATCH 2/4] Improve rtde output recipe test --- tests/test_rtde_client.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/tests/test_rtde_client.cpp b/tests/test_rtde_client.cpp index dfeb79cb..609cb054 100644 --- a/tests/test_rtde_client.cpp +++ b/tests/test_rtde_client.cpp @@ -228,6 +228,10 @@ TEST_F(RTDEClientTest, output_recipe) "tcp_offset" }; std::vector actual_output_recipe = client_->getOutputRecipe(); + // Verify that the size is the same + ASSERT_EQ(expected_output_recipe.size(), actual_output_recipe.size()); + + // Verify that the order and contect is equal for (unsigned int i = 0; i < expected_output_recipe.size(); ++i) { EXPECT_EQ(expected_output_recipe[i], actual_output_recipe[i]); From a2fd62ec7696d5c49bde0fe3f3ac982c380dca1e Mon Sep 17 00:00:00 2001 From: urmahp Date: Thu, 24 Aug 2023 13:43:55 +0200 Subject: [PATCH 3/4] Moved timestamp check to function and added enum argument --- include/ur_client_library/rtde/rtde_client.h | 12 ++++++++++- src/rtde/rtde_client.cpp | 22 ++++++++++++++------ tests/test_rtde_client.cpp | 2 +- 3 files changed, 28 insertions(+), 8 deletions(-) diff --git a/include/ur_client_library/rtde/rtde_client.h b/include/ur_client_library/rtde/rtde_client.h index 1d1e4860..9741d98d 100644 --- a/include/ur_client_library/rtde/rtde_client.h +++ b/include/ur_client_library/rtde/rtde_client.h @@ -190,6 +190,11 @@ class RTDEClient } private: + enum class RecipeType + { + INPUT_RECIPE = 0, + OUTPUT_RECIPE = 1 + }; comm::URStream stream_; std::vector output_recipe_; std::vector input_recipe_; @@ -208,7 +213,12 @@ class RTDEClient constexpr static const double CB3_MAX_FREQUENCY = 125.0; constexpr static const double URE_MAX_FREQUENCY = 500.0; - std::vector readRecipe(const std::string& recipe_file, bool output_recipe = false); + // Reads output or input recipe from a file + std::vector readRecipe(const std::string& recipe_file, const RecipeType& recipe_type) const; + + // Helper function to ensure that timestamp is present in the output recipe. The timestamp is needed to ensure that + // the robot is booted. + std::vector ensureTimestampIsPresent(const std::vector& output_recipe) const; void setupCommunication(); bool negotiateProtocolVersion(const uint16_t protocol_version); diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index f3e190b2..7a2a8b5c 100644 --- a/src/rtde/rtde_client.cpp +++ b/src/rtde/rtde_client.cpp @@ -37,8 +37,8 @@ namespace rtde_interface RTDEClient::RTDEClient(std::string robot_ip, comm::INotifier& notifier, const std::string& output_recipe_file, const std::string& input_recipe_file, double target_frequency) : stream_(robot_ip, UR_RTDE_PORT) - , output_recipe_(readRecipe(output_recipe_file, true)) - , input_recipe_(readRecipe(input_recipe_file, false)) + , output_recipe_(readRecipe(output_recipe_file, RecipeType::OUTPUT_RECIPE)) + , input_recipe_(readRecipe(input_recipe_file, RecipeType::INPUT_RECIPE)) , parser_(output_recipe_) , prod_(stream_, parser_) , pipeline_(prod_, PIPELINE_NAME, notifier, true) @@ -544,7 +544,7 @@ bool RTDEClient::sendPause() throw UrException(ss.str()); } -std::vector RTDEClient::readRecipe(const std::string& recipe_file, bool output_recipe) +std::vector RTDEClient::readRecipe(const std::string& recipe_file, const RecipeType& recipe_type) const { std::vector recipe; std::ifstream file(recipe_file); @@ -570,14 +570,24 @@ std::vector RTDEClient::readRecipe(const std::string& recipe_file, recipe.push_back(line); } - // Add timestamp to rtde output recipe, used to check if robot is booted + if (recipe_type == RecipeType::OUTPUT_RECIPE) + { + return ensureTimestampIsPresent(recipe); + } + return recipe; +} + +std::vector RTDEClient::ensureTimestampIsPresent(const std::vector& output_recipe) const +{ + // Add timestamp to rtde output recipe, if not already existing. + // The timestamp is used to check if robot is booted or not. + std::vector recipe = output_recipe; const std::string timestamp = "timestamp"; auto it = std::find(recipe.begin(), recipe.end(), timestamp); - if (it == recipe.end() && output_recipe == true) + if (it == recipe.end()) { recipe.push_back(timestamp); } - return recipe; } diff --git a/tests/test_rtde_client.cpp b/tests/test_rtde_client.cpp index 609cb054..f707e69a 100644 --- a/tests/test_rtde_client.cpp +++ b/tests/test_rtde_client.cpp @@ -230,7 +230,7 @@ TEST_F(RTDEClientTest, output_recipe) std::vector actual_output_recipe = client_->getOutputRecipe(); // Verify that the size is the same ASSERT_EQ(expected_output_recipe.size(), actual_output_recipe.size()); - + // Verify that the order and contect is equal for (unsigned int i = 0; i < expected_output_recipe.size(); ++i) { From 0178487e38a0ead33d4cf09208583beea1fe1ec1 Mon Sep 17 00:00:00 2001 From: urmahp Date: Mon, 28 Aug 2023 09:27:02 +0200 Subject: [PATCH 4/4] Removed enum dependency --- include/ur_client_library/rtde/rtde_client.h | 7 +------ src/rtde/rtde_client.cpp | 10 +++------- 2 files changed, 4 insertions(+), 13 deletions(-) diff --git a/include/ur_client_library/rtde/rtde_client.h b/include/ur_client_library/rtde/rtde_client.h index 9741d98d..2728d741 100644 --- a/include/ur_client_library/rtde/rtde_client.h +++ b/include/ur_client_library/rtde/rtde_client.h @@ -190,11 +190,6 @@ class RTDEClient } private: - enum class RecipeType - { - INPUT_RECIPE = 0, - OUTPUT_RECIPE = 1 - }; comm::URStream stream_; std::vector output_recipe_; std::vector input_recipe_; @@ -214,7 +209,7 @@ class RTDEClient constexpr static const double URE_MAX_FREQUENCY = 500.0; // Reads output or input recipe from a file - std::vector readRecipe(const std::string& recipe_file, const RecipeType& recipe_type) const; + std::vector readRecipe(const std::string& recipe_file) const; // Helper function to ensure that timestamp is present in the output recipe. The timestamp is needed to ensure that // the robot is booted. diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index 7a2a8b5c..062e3eb9 100644 --- a/src/rtde/rtde_client.cpp +++ b/src/rtde/rtde_client.cpp @@ -37,8 +37,8 @@ namespace rtde_interface RTDEClient::RTDEClient(std::string robot_ip, comm::INotifier& notifier, const std::string& output_recipe_file, const std::string& input_recipe_file, double target_frequency) : stream_(robot_ip, UR_RTDE_PORT) - , output_recipe_(readRecipe(output_recipe_file, RecipeType::OUTPUT_RECIPE)) - , input_recipe_(readRecipe(input_recipe_file, RecipeType::INPUT_RECIPE)) + , output_recipe_(ensureTimestampIsPresent(readRecipe(output_recipe_file))) + , input_recipe_(readRecipe(input_recipe_file)) , parser_(output_recipe_) , prod_(stream_, parser_) , pipeline_(prod_, PIPELINE_NAME, notifier, true) @@ -544,7 +544,7 @@ bool RTDEClient::sendPause() throw UrException(ss.str()); } -std::vector RTDEClient::readRecipe(const std::string& recipe_file, const RecipeType& recipe_type) const +std::vector RTDEClient::readRecipe(const std::string& recipe_file) const { std::vector recipe; std::ifstream file(recipe_file); @@ -570,10 +570,6 @@ std::vector RTDEClient::readRecipe(const std::string& recipe_file, recipe.push_back(line); } - if (recipe_type == RecipeType::OUTPUT_RECIPE) - { - return ensureTimestampIsPresent(recipe); - } return recipe; }