Remove direct primary and secondary stream from UrDriver (#283)

And replace them with the PrimaryClient.

---------

Co-authored-by: Mads Holm Peters <[email protected]>

Author: urfeex

README.md

@@ -87,8 +87,8 @@ See [Architecture documentation](doc/architecture.rst)
 
 Currently, this library doesn't support the primary interface very well, as the [Universal Robots
 ROS driver](https://github.com/UniversalRobots/Universal_Robots_ROS_Driver) was built mainly upon
-the RTDE interface. Therefore, there is also no `PrimaryClient` for directly accessing the primary
-interface. This may change in future, though.
+the RTDE interface. The `PrimaryClient` for directly accessing the primary
+interface doesn't support all features of the primary interface.
 
 The `comm::URStream` class can be used to open a connection to the primary / secondary interface
 and send data to it. The [producer/consumer](#producer--consumer-architecture) pipeline structure

doc/architecture/ur_driver.rst

@@ -14,7 +14,7 @@ When creating an instance of the ``UrDriver`` class, the created object will con
 - a :ref:`ScriptCommandInterface <script_command_interface>`
 - a :ref:`ScriptSender <script_sender>`
 - a :ref:`TrajectoryPointInterface <trajectory_point_interface>`.
-- a rudimentary client to the robot's secondary interface
+- a rudimentary client to the robot's primary interface
 - a couple of helper functions
 
 As this page is not meant to be a full-blown API documentation, not every public method will be
@@ -42,7 +42,7 @@ given to this function. Connection to the primary interface is dropped afterward
 ``sendScript(const std::string& program)``
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
-This function sends given URScript code directly to the secondary interface. The
+This function sends given URScript code directly to the primary interface. The
 ``sendRobotProgram()`` function is a special case that will send the script code given in the
 ``RTDEClient`` constructor.
 

doc/setup/robot_setup.rst

@@ -37,7 +37,7 @@ The robot needs to be prepared before it can be used with the client library.
       #. Go to the hamburger menu -> settings.
       #. Go to Security -> Services.
       #. Unlock the menu using the admin password.
-      #. Enable the Dashboard Server, Primary Client Interface, Secondary Client Interface and Real-Time Data Exchange (RTDE) interfaces.
+      #. Enable the Dashboard Server, Primary Client Interface and Real-Time Data Exchange (RTDE) interfaces.
       #. Lock the menu and press exit.
 
       .. image:: ../images/initial_setup_images/services_polyscope5.png
@@ -52,7 +52,7 @@ The robot needs to be prepared before it can be used with the client library.
       #. Go to the hamburger menu -> settings.
       #. Go to Security -> Services.
       #. Unlock the menu using the admin password.
-      #. Enable the Primary Client Interface, Secondary Client Interface and Real-Time Data Exchange (RTDE) interfaces.
+      #. Enable the Primary Client Interface and Real-Time Data Exchange (RTDE) interfaces.
       #. Lock the menu and press exit.
 
       .. image:: ../images/initial_setup_images/services_polyscopex.png

include/ur_client_library/primary/primary_client.h

@@ -68,14 +68,37 @@ 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);
+
+  bool checkCalibration(const std::string& checksum);
+
 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/primary/primary_consumer.h

@@ -97,6 +97,8 @@ class PrimaryConsumer : public AbstractPrimaryConsumer
    */
   virtual bool consume(KinematicsInfo& pkg) override
   {
+    URCL_LOG_DEBUG("%s", pkg.toString().c_str());
+    kinematics_info_ = std::make_shared<KinematicsInfo>(pkg);
     return true;
   }
 
@@ -162,8 +164,19 @@ class PrimaryConsumer : public AbstractPrimaryConsumer
     error_code_message_callback_ = callback_function;
   }
 
+  /*!
+   * \brief Get the kinematics info
+   *
+   * \returns Shared pointer to the kinematics info
+   */
+  std::shared_ptr<KinematicsInfo> getKinematicsInfo()
+  {
+    return kinematics_info_;
+  }
+
 private:
   std::function<void(ErrorCode&)> error_code_message_callback_;
+  std::shared_ptr<KinematicsInfo> kinematics_info_;
 };
 
 }  // namespace primary_interface

include/ur_client_library/primary/robot_state/kinematics_info.h

@@ -43,14 +43,26 @@ namespace primary_interface
 class KinematicsInfo : public RobotState
 {
 public:
-  KinematicsInfo() = delete;
+  KinematicsInfo() : RobotState(RobotStateType::KINEMATICS_INFO)
+  {
+  }
   /*!
    * \brief Creates a new KinematicsInfo object.
    *
    * \param type The type of RobotState message received
    */
-  KinematicsInfo(const RobotStateType type) : RobotState(type)
+  explicit KinematicsInfo(const RobotStateType type) : RobotState(type)
+  {
+  }
+
+  KinematicsInfo(const KinematicsInfo& other) : RobotState(RobotStateType::KINEMATICS_INFO)
   {
+    checksum_ = other.checksum_;
+    dh_theta_ = other.dh_theta_;
+    dh_a_ = other.dh_a_;
+    dh_d_ = other.dh_d_;
+    dh_alpha_ = other.dh_alpha_;
+    calibration_status_ = other.calibration_status_;
   }
   virtual ~KinematicsInfo() = default;
 

include/ur_client_library/ur/calibration_checker.h

@@ -39,7 +39,9 @@ namespace urcl
  * packages. These are then checked against the used kinematics to see if the correct calibration
  * is used.
  */
-class CalibrationChecker : public comm::IConsumer<primary_interface::PrimaryPackage>
+class [[deprecated("Using the standalone CalibrationChecker consumer to compare the kinematics hash is deprecated. "
+                   "Please use the PrimaryClient instead.")]] CalibrationChecker
+  : public comm::IConsumer<primary_interface::PrimaryPackage>
 {
 public:
   /*!

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);
 
@@ -912,8 +903,6 @@ class UrDriver
   std::unique_ptr<control::TrajectoryPointInterface> trajectory_interface_;
   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,68 @@ 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;
+}
+
+bool PrimaryClient::checkCalibration(const std::string& checksum)
+{
+  std::shared_ptr<primary_interface::KinematicsInfo> kin_info = consumer_->getKinematicsInfo();
+  while (kin_info == nullptr)
+  {
+    std::this_thread::sleep_for(std::chrono::seconds(1));
+    kin_info = consumer_->getKinematicsInfo();
+  }
+  URCL_LOG_DEBUG("Got calibration information from robot.");
+
+  return kin_info->toHash() == checksum;
+}
+
 }  // namespace primary_interface
 }  // namespace urcl