Merge branch 'master' into run_flaky

Author: urfeex

include/ur_client_library/primary/primary_client.h

@@ -258,6 +258,20 @@ class PrimaryClient
     return consumer_->getConfigurationData();
   }
 
+  /*!
+   * \brief Get the latest kinematics info.
+   *
+   * The kinematics info contains the controller's calibrated DH parameters: the nominal model
+   * combined with the per-arm calibration deltas, plus the calibration checksum/status. It
+   * will be updated in the background. This will always show the latest received kinematics
+   * info independent of the time that has passed since receiving it. If no kinematics info
+   * has been received yet, this will return a nullptr.
+   */
+  std::shared_ptr<KinematicsInfo> getKinematicsInfo()
+  {
+    return consumer_->getKinematicsInfo();
+  }
+
   /*!
    * \brief Get the Robot type
    *

include/ur_client_library/primary/primary_consumer.h

@@ -35,7 +35,6 @@
 
 #include <functional>
 #include <mutex>
-#include <condition_variable>
 
 namespace urcl
 {
@@ -89,7 +88,7 @@ class PrimaryConsumer : public AbstractPrimaryConsumer
   virtual bool consume(VersionMessage& pkg) override
   {
     std::scoped_lock lock(version_information_mutex_);
-    version_information_ = std::make_shared<VersionInformation>();
+    version_information_ = VersionInformation();
     version_information_->major = pkg.major_version_;
     version_information_->minor = pkg.minor_version_;
     version_information_->bugfix = pkg.svn_version_;
@@ -107,7 +106,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);
+    std::scoped_lock lock(kinematics_info_mutex_);
+    kinematics_info_ = std::make_unique<KinematicsInfo>(pkg);
     return true;
   }
 
@@ -166,22 +166,22 @@ class PrimaryConsumer : public AbstractPrimaryConsumer
   {
     URCL_LOG_DEBUG("Robot mode is now %s", robotModeString(static_cast<RobotMode>(pkg.robot_mode_)).c_str());
     std::scoped_lock lock(robot_mode_mutex_);
-    robot_mode_ = std::make_shared<RobotModeData>(pkg);
+    robot_mode_ = std::make_unique<RobotModeData>(pkg);
     return true;
   }
 
   virtual bool consume(SafetyModeMessage& pkg) override
   {
     URCL_LOG_DEBUG("Robot safety mode is now %s", safetyModeString(pkg.safety_mode_type_).c_str());
     std::scoped_lock lock(safety_mode_mutex_);
-    safety_mode_ = std::make_shared<SafetyModeMessage>(pkg);
+    safety_mode_ = std::make_unique<SafetyModeMessage>(pkg);
     return true;
   }
 
   virtual bool consume(ConfigurationData& pkg) override
   {
     std::scoped_lock lock(configuration_data_mutex_);
-    configuration_data_ = std::make_shared<ConfigurationData>(pkg);
+    configuration_data_ = std::make_unique<ConfigurationData>(pkg);
     return true;
   }
 
@@ -203,7 +203,12 @@ class PrimaryConsumer : public AbstractPrimaryConsumer
    */
   std::shared_ptr<KinematicsInfo> getKinematicsInfo()
   {
-    return kinematics_info_;
+    std::scoped_lock lock(kinematics_info_mutex_);
+    if (kinematics_info_ == nullptr)
+    {
+      return nullptr;
+    }
+    return std::make_shared<KinematicsInfo>(*kinematics_info_);
   }
 
   /*!
@@ -215,7 +220,11 @@ class PrimaryConsumer : public AbstractPrimaryConsumer
   std::shared_ptr<RobotModeData> getRobotModeData()
   {
     std::scoped_lock lock(robot_mode_mutex_);
-    return robot_mode_;
+    if (robot_mode_ == nullptr)
+    {
+      return nullptr;
+    }
+    return std::make_shared<RobotModeData>(*robot_mode_);
   }
 
   /*!
@@ -227,7 +236,11 @@ class PrimaryConsumer : public AbstractPrimaryConsumer
   std::shared_ptr<SafetyModeMessage> getSafetyModeMessage()
   {
     std::scoped_lock lock(safety_mode_mutex_);
-    return safety_mode_;
+    if (safety_mode_ == nullptr)
+    {
+      return nullptr;
+    }
+    return std::make_shared<SafetyModeMessage>(*safety_mode_);
   }
 
   /*!
@@ -240,7 +253,11 @@ class PrimaryConsumer : public AbstractPrimaryConsumer
   std::shared_ptr<VersionInformation> getVersionInformation()
   {
     std::scoped_lock lock(version_information_mutex_);
-    return version_information_;
+    if (!version_information_.has_value())
+    {
+      return nullptr;
+    }
+    return std::make_shared<VersionInformation>(*version_information_);
   }
 
   /*!
@@ -253,20 +270,25 @@ class PrimaryConsumer : public AbstractPrimaryConsumer
   std::shared_ptr<ConfigurationData> getConfigurationData()
   {
     std::scoped_lock lock(configuration_data_mutex_);
-    return configuration_data_;
+    if (configuration_data_ == nullptr)
+    {
+      return nullptr;
+    }
+    return std::make_shared<ConfigurationData>(*configuration_data_);
   }
 
 private:
   std::function<void(ErrorCode&)> error_code_message_callback_;
-  std::shared_ptr<KinematicsInfo> kinematics_info_;
+  std::mutex kinematics_info_mutex_;
+  std::unique_ptr<KinematicsInfo> kinematics_info_;
   std::mutex robot_mode_mutex_;
-  std::shared_ptr<RobotModeData> robot_mode_;
+  std::unique_ptr<RobotModeData> robot_mode_;
   std::mutex version_information_mutex_;
-  std::shared_ptr<VersionInformation> version_information_;
-  std::shared_ptr<ConfigurationData> configuration_data_;
+  std::optional<VersionInformation> version_information_;
   std::mutex configuration_data_mutex_;
+  std::unique_ptr<ConfigurationData> configuration_data_;
   std::mutex safety_mode_mutex_;
-  std::shared_ptr<SafetyModeMessage> safety_mode_;
+  std::unique_ptr<SafetyModeMessage> safety_mode_;
 };
 
 }  // namespace primary_interface

tests/test_primary_client.cpp

@@ -268,6 +268,24 @@ TEST_F(PrimaryClientTest, test_configuration_data)
   EXPECT_NE(client_->getRobotType(), RobotType::UNDEFINED);
 }
 
+TEST_F(PrimaryClientTest, test_kinematics_info)
+{
+  EXPECT_NO_THROW(client_->start());
+
+  // Once we connect to the primary client we should receive kinematics info
+  auto start_time = std::chrono::system_clock::now();
+  const auto timeout = std::chrono::seconds(1);
+  auto kinematics_info = client_->getKinematicsInfo();
+  while (kinematics_info == nullptr && std::chrono::system_clock::now() - start_time < timeout)
+  {
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    kinematics_info = client_->getKinematicsInfo();
+  }
+
+  // We should have received a kinematics info package
+  EXPECT_NE(kinematics_info, nullptr);
+}
+
 TEST_F(PrimaryClientTest, test_get_robot_series)
 {
   EXPECT_NO_THROW(client_->start());