Added configuration data to packages parsed from the primary interface (#327)

Co-authored-by: Felix Exner <feex@universal-robots.com>

Author: urmahp

CMakeLists.txt

@@ -29,6 +29,7 @@ add_library(urcl
     src/primary/robot_message/error_code_message.cpp
     src/primary/robot_state/kinematics_info.cpp
     src/primary/robot_state/robot_mode_data.cpp
+    src/primary/robot_state/configuration_data.cpp
     src/rtde/control_package_pause.cpp
     src/rtde/control_package_setup_inputs.cpp
     src/rtde/control_package_setup_outputs.cpp

include/ur_client_library/primary/abstract_primary_consumer.h

@@ -35,6 +35,7 @@
 #include "ur_client_library/primary/robot_message/error_code_message.h"
 #include "ur_client_library/primary/robot_state/kinematics_info.h"
 #include "ur_client_library/primary/robot_state/robot_mode_data.h"
+#include "ur_client_library/primary/robot_state/configuration_data.h"
 
 namespace urcl
 {
@@ -77,6 +78,7 @@ class AbstractPrimaryConsumer : public comm::IConsumer<PrimaryPackage>
   virtual bool consume(KinematicsInfo& pkg) = 0;
   virtual bool consume(ErrorCodeMessage& pkg) = 0;
   virtual bool consume(RobotModeData& pkg) = 0;
+  virtual bool consume(ConfigurationData& pkg) = 0;
 
 private:
   /* data */

include/ur_client_library/primary/primary_client.h

@@ -218,6 +218,25 @@ class PrimaryClient
     return robot_mode_data->is_protective_stopped_;
   }
 
+  /*!
+   * \brief Get the latest configuration data
+   *
+   * The configuration data will be updated in the background. This will always show the latest
+   * received configuration data independent of the time that has passed since receiving it. If no
+   * configuration data has been received yet, this will return a nullptr.
+   */
+  std::shared_ptr<ConfigurationData> getConfigurationData()
+  {
+    return consumer_->getConfigurationData();
+  }
+
+  /*!
+   * \brief Get the Robot type
+   *
+   * If no robot type data has been received yet, this will return UNDEFINED.
+   */
+  RobotType getRobotType();
+
 private:
   /*!
    * \brief Reconnects the primary stream used to send program to the robot.

include/ur_client_library/primary/primary_consumer.h

@@ -170,6 +170,13 @@ class PrimaryConsumer : public AbstractPrimaryConsumer
     return true;
   }
 
+  virtual bool consume(ConfigurationData& pkg) override
+  {
+    std::scoped_lock lock(configuration_data_mutex_);
+    configuration_data_ = std::make_shared<ConfigurationData>(pkg);
+    return true;
+  }
+
   /*!
    * \brief Set callback function which will be triggered whenever error code messages are received
    *
@@ -216,13 +223,28 @@ class PrimaryConsumer : public AbstractPrimaryConsumer
     return version_information_;
   }
 
+  /*!
+   * \brief Get the latest configuration data.
+   *
+   * The configuration data will be updated in the background. This will always show the latest
+   * received configuration data independent of the time that has passed since receiving it. If no
+   * configuration data has been received yet, this will return a nullptr.
+   */
+  std::shared_ptr<ConfigurationData> getConfigurationData()
+  {
+    std::scoped_lock lock(configuration_data_mutex_);
+    return configuration_data_;
+  }
+
 private:
   std::function<void(ErrorCode&)> error_code_message_callback_;
   std::shared_ptr<KinematicsInfo> kinematics_info_;
   std::mutex robot_mode_mutex_;
   std::shared_ptr<RobotModeData> robot_mode_;
   std::mutex version_information_mutex_;
   std::shared_ptr<VersionInformation> version_information_;
+  std::shared_ptr<ConfigurationData> configuration_data_;
+  std::mutex configuration_data_mutex_;
 };
 
 }  // namespace primary_interface

include/ur_client_library/primary/primary_parser.h

@@ -30,6 +30,7 @@
 #include "ur_client_library/primary/robot_message/version_message.h"
 #include "ur_client_library/primary/robot_message/error_code_message.h"
 #include "ur_client_library/primary/robot_state/robot_mode_data.h"
+#include "ur_client_library/primary/robot_state/configuration_data.h"
 
 namespace urcl
 {
@@ -159,6 +160,8 @@ class PrimaryParser : public comm::Parser<PrimaryPackage>
       // return new MBD;*/
       case RobotStateType::KINEMATICS_INFO:
         return new KinematicsInfo(type);
+      case RobotStateType::CONFIGURATION_DATA:
+        return new ConfigurationData(type);
       default:
         return new RobotState(type);
     }

include/ur_client_library/primary/robot_state/configuration_data.h

@@ -0,0 +1,123 @@
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+// Copyright 2025 Universal Robots A/S
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the {copyright_holder} nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+// -- END LICENSE BLOCK ------------------------------------------------
+
+#pragma once
+
+#include "ur_client_library/types.h"
+#include "ur_client_library/primary/robot_state.h"
+#include <iostream>
+
+namespace urcl
+{
+namespace primary_interface
+{
+/*!
+ * \brief The ConfigurationData class handles the configuration data sent via the primary UR interface.
+ */
+class ConfigurationData : public RobotState
+{
+public:
+  struct JointPositionLimits
+  {
+    double joint_min_limit;
+    double joint_max_limit;
+  };
+
+  struct JointMotionLimits
+  {
+    double joint_max_speed;
+    double joint_max_acceleration;
+  };
+
+  ConfigurationData() = delete;
+
+  /*!
+   * \brief Creates a new ConfigurationData object.
+   *
+   * \param type The type of RobotState message received
+   */
+  ConfigurationData(const RobotStateType type) : RobotState(type)
+  {
+  }
+
+  /*!
+   * \brief Creates a copy of a ConfigurationData object.
+   *
+   * \param pkg The ConfigurationData object to be copied
+   */
+  ConfigurationData(const ConfigurationData& pkg);
+
+  virtual ~ConfigurationData() = default;
+
+  /*!
+   * \brief Sets the attributes of the package by parsing a serialized representation of the
+   * package.
+   *
+   * \param bp A parser containing a serialized version of the package
+   *
+   * \returns True, if the package was parsed successfully, false otherwise
+   */
+  virtual bool parseWith(comm::BinParser& bp);
+
+  /*!
+   * \brief Consume this package with a specific consumer.
+   *
+   * \param consumer Placeholder for the consumer calling this
+   *
+   * \returns true on success
+   */
+  virtual bool consumeWith(AbstractPrimaryConsumer& consumer);
+
+  /*!
+   * \brief Produces a human readable representation of the package object.
+   *
+   * \returns A string representing the object
+   */
+  virtual std::string toString() const;
+
+  std::array<JointPositionLimits, 6> joint_position_limits_;
+  std::array<JointMotionLimits, 6> joint_motion_limits_;
+  double v_joint_default_;
+  double a_joint_default_;
+  double v_tool_default_;
+  double a_tool_default_;
+  double eq_radius_;
+  urcl::vector6d_t dh_a_;
+  urcl::vector6d_t dh_d_;
+  urcl::vector6d_t dh_alpha_;
+  urcl::vector6d_t dh_theta_;
+  int32_t masterboard_version_;
+  int32_t controller_box_type_;
+  int32_t robot_type_;
+  int32_t robot_sub_type_;
+};
+
+}  // namespace primary_interface
+}  // namespace urcl

include/ur_client_library/ur/datatypes.h

@@ -29,6 +29,7 @@
 #pragma once
 
 #include <ur_client_library/types.h>
+#include "ur_client_library/log.h"
 
 namespace urcl
 {
@@ -86,6 +87,18 @@ enum class AnalogOutputType : int8_t
   VOLTAGE = 1
 };
 
+enum class RobotType : int8_t
+{
+  UNDEFINED = -128,  // This is not defined by UR but only inside this driver
+  UR5 = 1,
+  UR10 = 2,
+  UR3 = 3,
+  UR16 = 4,
+  UR20 = 7,
+  UR30 = 8,
+  UR15 = 9
+};
+
 inline std::string robotModeString(const RobotMode& mode)
 {
   switch (mode)
@@ -186,4 +199,31 @@ inline std::string safetyStatusString(const SafetyStatus& status)
       throw std::invalid_argument(ss.str());
   }
 }
+
+inline std::string robotTypeString(const RobotType& type)
+{
+  switch (type)
+  {
+    case RobotType::UR3:
+      return "UR3";
+    case RobotType::UR5:
+      return "UR5";
+    case RobotType::UR10:
+      return "UR10";
+    case RobotType::UR15:
+      return "UR15";
+    case RobotType::UR16:
+      return "UR16";
+    case RobotType::UR20:
+      return "UR20";
+    case RobotType::UR30:
+      return "UR30";
+    default:
+      std::stringstream ss;
+      ss << "Unknown Robot Type: " << static_cast<int>(type);
+      URCL_LOG_WARN(ss.str().c_str());
+      return "UNDEFINED";
+  }
+}
+
 }  // namespace urcl

src/primary/primary_client.cpp

@@ -279,5 +279,15 @@ std::shared_ptr<VersionInformation> PrimaryClient::getRobotVersion(bool blocking
   return consumer_->getVersionInformation();
 }
 
+RobotType PrimaryClient::getRobotType()
+{
+  std::shared_ptr<ConfigurationData> configuration_data = consumer_->getConfigurationData();
+  if (configuration_data == nullptr)
+  {
+    return RobotType::UNDEFINED;
+  }
+  return static_cast<RobotType>(configuration_data->robot_type_);
+}
+
 }  // namespace primary_interface
 }  // namespace urcl