Replace the secondary stream with using the primary client

Author: urfeex

include/ur_client_library/primary/primary_client.h

@@ -68,14 +68,35 @@ class PrimaryClient
   void removePrimaryConsumer(std::shared_ptr<comm::IConsumer<PrimaryPackage>> primary_consumer);
   void start(const size_t max_connection_attempts = 0,
              const std::chrono::milliseconds reconnection_timeout = urcl::comm::TCPSocket::DEFAULT_RECONNECTION_TIME);
+  void stop();
 
   /*!
    * \brief Retrieves previously raised error codes from PrimaryClient. After calling this, recorded errors will be
    * deleted.
    */
   std::deque<ErrorCode> getErrorCodes();
 
+  /*!
+   * \brief Sends a custom script program to the robot.
+   *
+   * The given code must be valid according the UR Scripting Manual.
+   *
+   * \param program URScript code that shall be executed by the robot.
+   *
+   * \returns true on successful upload, false otherwise.
+   */
+  bool sendScript(const std::string& program);
+
 private:
+  /*!
+   * \brief Reconnects the primary stream used to send program to the robot.
+   *
+   * Only for use in headless mode, as it replaces the use of the URCaps program.
+   *
+   * \returns true of on successful reconnection, false otherwise
+   */
+  bool reconnectStream();
+
   // The function is called whenever an error code message is received
   void errorMessageCallback(ErrorCode& code);
 

include/ur_client_library/ur/ur_driver.h

@@ -868,18 +868,18 @@ class UrDriver
    */
   void startPrimaryClientCommunication();
 
+  void stopPrimaryClientCommunication()
+  {
+    primary_client_->stop();
+  }
+
   void registerTrajectoryInterfaceDisconnectedCallback(std::function<void(const int)> fun)
   {
     trajectory_interface_->registerDisconnectionCallback(fun);
   }
 
   static std::string readScriptFile(const std::string& filename);
 
-  void closeSecondaryStream()
-  {
-    secondary_stream_->close();
-  }
-
   bool isReverseInterfaceConnected() const
   {
     return reverse_interface_->isConnected();
@@ -893,15 +893,6 @@ class UrDriver
 private:
   void init(const UrDriverConfiguration& config);
 
-  /*!
-   * \brief Reconnects the secondary stream used to send program to the robot.
-   *
-   * Only for use in headless mode, as it replaces the use of the URCaps program.
-   *
-   * \returns true of on successful reconnection, false otherwise
-   */
-  bool reconnectSecondaryStream();
-
   void initRTDE();
   void setupReverseInterface(const uint32_t reverse_port);
 
@@ -913,7 +904,6 @@ class UrDriver
   std::unique_ptr<control::ScriptCommandInterface> script_command_interface_;
   std::unique_ptr<control::ScriptSender> script_sender_;
   std::unique_ptr<comm::URStream<primary_interface::PrimaryPackage>> primary_stream_;
-  std::unique_ptr<comm::URStream<primary_interface::PrimaryPackage>> secondary_stream_;
 
   size_t socket_connection_attempts_ = 0;
   std::chrono::milliseconds socket_reconnection_timeout_ = std::chrono::milliseconds(10000);

src/primary/primary_client.cpp

@@ -67,6 +67,12 @@ void PrimaryClient::start(const size_t max_num_tries, const std::chrono::millise
   pipeline_->run();
 }
 
+void PrimaryClient::stop()
+{
+  pipeline_->stop();
+  stream_.close();
+}
+
 void PrimaryClient::addPrimaryConsumer(std::shared_ptr<comm::IConsumer<PrimaryPackage>> primary_consumer)
 {
   multi_consumer_->addConsumer(primary_consumer);
@@ -91,5 +97,54 @@ std::deque<ErrorCode> PrimaryClient::getErrorCodes()
   error_code_queue_.clear();
   return error_codes;
 }
+
+bool PrimaryClient::sendScript(const std::string& program)
+{
+  // urscripts (snippets) must end with a newline, or otherwise the controller's runtime will
+  // not execute them. To avoid problems, we always just append a newline here, even if
+  // there may already be one.
+  auto program_with_newline = program + '\n';
+
+  size_t len = program_with_newline.size();
+  const uint8_t* data = reinterpret_cast<const uint8_t*>(program_with_newline.c_str());
+  size_t written;
+
+  const auto send_script_contents = [this, program_with_newline, data, len,
+                                     &written](const std::string&& description) -> bool {
+    if (stream_.write(data, len, written))
+    {
+      URCL_LOG_DEBUG("Sent program to robot:\n%s", program_with_newline.c_str());
+      return true;
+    }
+    const std::string error_message = "Could not send program to robot: " + description;
+    URCL_LOG_ERROR(error_message.c_str());
+    return false;
+  };
+
+  if (send_script_contents("initial attempt"))
+  {
+    return true;
+  }
+
+  if (reconnectStream())
+  {
+    return send_script_contents("after reconnecting primary stream");
+  }
+
+  return false;
+}
+
+bool PrimaryClient::reconnectStream()
+{
+  URCL_LOG_DEBUG("Closing primary stream...");
+  stream_.close();
+  if (stream_.connect())
+  {
+    URCL_LOG_DEBUG("Primary stream connected");
+    return true;
+  }
+  URCL_LOG_ERROR("Failed to reconnect primary stream!");
+  return false;
+}
 }  // namespace primary_interface
-}  // namespace urcl
+}  // namespace urcl

src/ur/ur_driver.cpp

@@ -72,9 +72,6 @@ void UrDriver::init(const UrDriverConfiguration& config)
 
   primary_stream_.reset(
       new comm::URStream<primary_interface::PrimaryPackage>(robot_ip_, urcl::primary_interface::UR_PRIMARY_PORT));
-  secondary_stream_.reset(
-      new comm::URStream<primary_interface::PrimaryPackage>(robot_ip_, urcl::primary_interface::UR_SECONDARY_PORT));
-  secondary_stream_->connect(socket_connection_attempts_, socket_reconnection_timeout_);
 
   primary_client_.reset(new urcl::primary_interface::PrimaryClient(robot_ip_, notifier_));
 
@@ -151,6 +148,7 @@ void UrDriver::init(const UrDriverConfiguration& config)
   trajectory_interface_.reset(new control::TrajectoryPointInterface(config.trajectory_port));
   script_command_interface_.reset(new control::ScriptCommandInterface(config.script_command_port));
 
+  startPrimaryClientCommunication();
   if (in_headless_mode_)
   {
     full_robot_program_ = "stop program\n";
@@ -591,44 +589,11 @@ rtde_interface::RTDEWriter& UrDriver::getRTDEWriter()
 
 bool UrDriver::sendScript(const std::string& program)
 {
-  if (secondary_stream_ == nullptr)
+  if (primary_client_ == nullptr)
   {
-    throw std::runtime_error("Sending script to robot requested while there is no secondary interface established. "
-                             "This should not happen.");
-  }
-
-  // urscripts (snippets) must end with a newline, or otherwise the controller's runtime will
-  // not execute them. To avoid problems, we always just append a newline here, even if
-  // there may already be one.
-  auto program_with_newline = program + '\n';
-
-  size_t len = program_with_newline.size();
-  const uint8_t* data = reinterpret_cast<const uint8_t*>(program_with_newline.c_str());
-  size_t written;
-
-  const auto send_script_contents = [this, program_with_newline, data, len,
-                                     &written](const std::string&& description) -> bool {
-    if (secondary_stream_->write(data, len, written))
-    {
-      URCL_LOG_DEBUG("Sent program to robot:\n%s", program_with_newline.c_str());
-      return true;
-    }
-    const std::string error_message = "Could not send program to robot: " + description;
-    URCL_LOG_ERROR(error_message.c_str());
     return false;
-  };
-
-  if (send_script_contents("initial attempt"))
-  {
-    return true;
   }
-
-  if (reconnectSecondaryStream())
-  {
-    return send_script_contents("after reconnecting secondary stream");
-  }
-
-  return false;
+  return primary_client_->sendScript(program);
 }
 
 bool UrDriver::sendRobotProgram()
@@ -644,19 +609,6 @@ bool UrDriver::sendRobotProgram()
   }
 }
 
-bool UrDriver::reconnectSecondaryStream()
-{
-  URCL_LOG_DEBUG("Closing secondary stream...");
-  secondary_stream_->close();
-  if (secondary_stream_->connect())
-  {
-    URCL_LOG_DEBUG("Secondary stream connected");
-    return true;
-  }
-  URCL_LOG_ERROR("Failed to reconnect secondary stream!");
-  return false;
-}
-
 std::vector<std::string> UrDriver::getRTDEOutputRecipe()
 {
   return rtde_client_->getOutputRecipe();
@@ -718,4 +670,4 @@ std::deque<urcl::primary_interface::ErrorCode> UrDriver::getErrorCodes()
 {
   return primary_client_->getErrorCodes();
 }
-}  // namespace urcl
+}  // namespace urcl

tests/test_ur_driver.cpp

@@ -235,14 +235,14 @@ TEST_F(UrDriverTest, target_outside_limits_pose)
 
 TEST_F(UrDriverTest, send_robot_program_retry_on_failure)
 {
-  // Check that sendRobotProgram is robust to the secondary stream being disconnected. This is what happens when
+  // Check that sendRobotProgram is robust to the primary stream being disconnected. This is what happens when
   // switching from Remote to Local and back to Remote mode for example.
 
   // To be able to re-send the robot program we'll have to make sure it isn't running
   g_my_robot->ur_driver_->stopControl();
   g_my_robot->waitForProgramNotRunning();
 
-  g_my_robot->ur_driver_->closeSecondaryStream();
+  g_my_robot->ur_driver_->stopPrimaryClientCommunication();
 
   EXPECT_TRUE(g_my_robot->resendRobotProgram());
 
@@ -259,7 +259,6 @@ TEST_F(UrDriverTest, reset_rtde_client)
 
 TEST_F(UrDriverTest, read_error_code)
 {
-  g_my_robot->ur_driver_->startPrimaryClientCommunication();
   // Wait until we actually received a package
   std::this_thread::sleep_for(std::chrono::milliseconds(1000));
   std::stringstream cmd;