Timestamp is now added to the output recipe correctly

include/ur_client_library/rtde/rtde_client.h

@@ -208,7 +208,12 @@ class RTDEClient
   constexpr static const double CB3_MAX_FREQUENCY = 125.0;
   constexpr static const double URE_MAX_FREQUENCY = 500.0;
 
-  std::vector<std::string> readRecipe(const std::string& recipe_file);
+  // Reads output or input recipe from a file
+  std::vector<std::string> 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.
+  std::vector<std::string> ensureTimestampIsPresent(const std::vector<std::string>& output_recipe) const;
 
   void setupCommunication();
   bool negotiateProtocolVersion(const uint16_t protocol_version);

src/rtde/rtde_client.cpp

@@ -37,7 +37,7 @@ 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))
+  , output_recipe_(ensureTimestampIsPresent(readRecipe(output_recipe_file)))
   , input_recipe_(readRecipe(input_recipe_file))
   , parser_(output_recipe_)
   , prod_(stream_, parser_)
@@ -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<std::string> RTDEClient::readRecipe(const std::string& recipe_file)
+std::vector<std::string> RTDEClient::readRecipe(const std::string& recipe_file) const
 {
   std::vector<std::string> recipe;
   std::ifstream file(recipe_file);
@@ -559,11 +555,35 @@ std::vector<std::string> 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);
   }
+
+  return recipe;
+}
+
+std::vector<std::string> RTDEClient::ensureTimestampIsPresent(const std::vector<std::string>& 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<std::string> recipe = output_recipe;
+  const std::string timestamp = "timestamp";
+  auto it = std::find(recipe.begin(), recipe.end(), timestamp);
+  if (it == recipe.end())
+  {
+    recipe.push_back(timestamp);
+  }
   return recipe;
 }
 

tests/resources/empty.txt

@@ -1 +0,0 @@
-

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

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)
@@ -230,6 +228,10 @@ TEST_F(RTDEClientTest, output_recipe)
                                                       "tcp_offset" };
 
   std::vector<std::string> 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]);
@@ -296,6 +298,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<std::string> 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);