From 34a76f2267cea817b2cd6c13722ca25b0f4b7fe7 Mon Sep 17 00:00:00 2001 From: urmahp Date: Wed, 23 Aug 2023 12:16:24 +0200 Subject: [PATCH] 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);