Use primary client in UrDriver for checking the calibration checksum

As the primary client already receives the calibration data, we can use
it directly to check the checksum.

Author: urfeex

include/ur_client_library/primary/primary_client.h

@@ -87,6 +87,8 @@ class PrimaryClient
    */
   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.

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 standaline 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

@@ -903,7 +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_;
 
   size_t socket_connection_attempts_ = 0;
   std::chrono::milliseconds socket_reconnection_timeout_ = std::chrono::milliseconds(10000);

src/primary/primary_client.cpp

@@ -146,5 +146,19 @@ bool PrimaryClient::reconnectStream()
   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;
+  while (kin_info == nullptr)
+  {
+    kin_info = consumer_->getKinematicsInfo();
+    std::this_thread::sleep_for(std::chrono::seconds(1));
+  }
+  URCL_LOG_DEBUG("Got calibration information from robot.");
+
+  return kin_info->toHash() == checksum;
+}
+
 }  // namespace primary_interface
-}  // namespace urcl
+}  // namespace urcl

src/ur/ur_driver.cpp

@@ -70,9 +70,6 @@ void UrDriver::init(const UrDriverConfiguration& config)
   rtde_client_.reset(
       new rtde_interface::RTDEClient(robot_ip_, notifier_, config.output_recipe_file, config.input_recipe_file));
 
-  primary_stream_.reset(
-      new comm::URStream<primary_interface::PrimaryPackage>(robot_ip_, urcl::primary_interface::UR_PRIMARY_PORT));
-
   primary_client_.reset(new urcl::primary_interface::PrimaryClient(robot_ip_, notifier_));
 
   get_packet_timeout_ = non_blocking_read_ ? 0 : 100;
@@ -559,27 +556,7 @@ std::string UrDriver::readScriptFile(const std::string& filename)
 
 bool UrDriver::checkCalibration(const std::string& checksum)
 {
-  if (primary_stream_ == nullptr)
-  {
-    throw std::runtime_error("checkCalibration() called without a primary interface connection being established.");
-  }
-  primary_interface::PrimaryParser parser;
-  comm::URProducer<primary_interface::PrimaryPackage> prod(*primary_stream_, parser);
-  prod.setupProducer();
-
-  CalibrationChecker consumer(checksum);
-
-  comm::INotifier notifier;
-
-  comm::Pipeline<primary_interface::PrimaryPackage> pipeline(prod, &consumer, "Pipeline", notifier);
-  pipeline.run();
-
-  while (!consumer.isChecked())
-  {
-    std::this_thread::sleep_for(std::chrono::seconds(1));
-  }
-  URCL_LOG_DEBUG("Got calibration information from robot.");
-  return consumer.checkSuccessful();
+  return primary_client_->checkCalibration(checksum);
 }
 
 rtde_interface::RTDEWriter& UrDriver::getRTDEWriter()
@@ -670,4 +647,4 @@ std::deque<urcl::primary_interface::ErrorCode> UrDriver::getErrorCodes()
 {
   return primary_client_->getErrorCodes();
 }
-}  // namespace urcl
+}  // namespace urcl

tests/test_primary_client.cpp

@@ -57,7 +57,10 @@ TEST_F(PrimaryClientTest, start_communication_succeeds)
 
 TEST_F(PrimaryClientTest, add_and_remove_consumer)
 {
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
   auto calibration_consumer = std::make_shared<urcl::CalibrationChecker>("test");
+#pragma GCC diagnostic pop
 
   client_->addPrimaryConsumer(calibration_consumer);