Add helper function to get robot series (UniversalRobots#488)

As discussed in
UniversalRobots/Universal_Robots_ROS2_Driver#842
it might be useful to compare a robot model with the connected robot for
verification purposes.

Since at least for the UR3, UR5 and UR10 there are two models
respectively (CB3 and e-series), so we would need to be able to
distinguish between them. To do that, this PR adds a `getRobotSeries()`
function to the primary client and a `robotSeriesFromTypeAndVersion()`
function to deduce the series from the robot model and software version
(as the series isn't directly available to my knowledge).

I decided to return `UNDEFINED` for combinations that shouldn't show up
in the real world (e.g. a CB3 software version on a UR16 or any
UR-Series robot).

Author: urfeex

include/ur_client_library/helpers.h

@@ -32,6 +32,9 @@
 #include <string>
 #include <chrono>
 #include <functional>
+
+#include "ur_client_library/ur/version_information.h"
+#include "ur_client_library/ur/datatypes.h"
 #ifdef _WIN32
 
 #  define NOMINMAX
@@ -128,5 +131,15 @@ void clampToUnitRange(std::array<T, N>& values)
   }
 }
 
+/*!
+ * \brief Get the robot series from the robot type and version information.
+ *
+ * \param type The robot type.
+ * \param version The version information of the robot.
+ *
+ * \returns The robot series corresponding to the given robot type and version information.
+ */
+RobotSeries robotSeriesFromTypeAndVersion(const RobotType type, const VersionInformation& version);
+
 }  // namespace urcl
 #endif  // ifndef UR_CLIENT_LIBRARY_HELPERS_H_INCLUDED

include/ur_client_library/primary/primary_client.h

@@ -265,6 +265,13 @@ class PrimaryClient
    */
   RobotType getRobotType();
 
+  /*!
+   * \brief Get the Robot series
+   *
+   * If no robot type data has been received yet, this will return UNDEFINED.
+   */
+  RobotSeries getRobotSeries();
+
 private:
   /*!
    * \brief Reconnects the primary stream used to send program to the robot.

include/ur_client_library/ur/datatypes.h

@@ -112,6 +112,14 @@ enum class RobotType : int8_t
   UR15 = 9
 };
 
+enum class RobotSeries
+{
+  UNDEFINED = -128,
+  CB3 = 1,
+  E_SERIES = 2,
+  UR_SERIES = 3
+};
+
 inline std::string robotModeString(const RobotMode& mode)
 {
   switch (mode)

src/helpers.cpp

@@ -160,4 +160,47 @@ std::vector<std::string> splitString(const std::string& input, const std::string
   return result;
 }
 
+RobotSeries robotSeriesFromTypeAndVersion(const RobotType type, const VersionInformation& version_info)
+{
+  switch (type)
+  {
+    case RobotType::UR3:
+    case RobotType::UR5:
+    case RobotType::UR10:
+      if (version_info.major >= 5)
+      {
+        return RobotSeries::E_SERIES;
+      }
+      else
+      {
+        return RobotSeries::CB3;
+      }
+    case RobotType::UR16:
+      if (version_info.major >= 5)
+      {
+        return RobotSeries::E_SERIES;
+      }
+      else
+      {
+        return RobotSeries::UNDEFINED;
+      }
+    case RobotType::UR15:
+    case RobotType::UR18:
+    case RobotType::UR20:
+    case RobotType::UR30:
+    case RobotType::UR8LONG:
+      if (version_info.major >= 5)
+      {
+        return RobotSeries::UR_SERIES;
+      }
+      else
+      {
+        return RobotSeries::UNDEFINED;
+      }
+    case RobotType::UNDEFINED:
+      return RobotSeries::UNDEFINED;
+  }
+  return RobotSeries::UNDEFINED;
+}
+
 }  // namespace urcl

src/primary/primary_client.cpp

@@ -297,5 +297,22 @@ RobotType PrimaryClient::getRobotType()
   return static_cast<RobotType>(configuration_data->robot_type_);
 }
 
+RobotSeries PrimaryClient::getRobotSeries()
+{
+  auto robot_type = getRobotType();
+  if (robot_type == RobotType::UNDEFINED)
+  {
+    return RobotSeries::UNDEFINED;
+  }
+
+  auto version_info = getRobotVersion();
+  if (version_info == nullptr)
+  {
+    return RobotSeries::UNDEFINED;
+  }
+
+  return robotSeriesFromTypeAndVersion(robot_type, *version_info);
+}
+
 }  // namespace primary_interface
 }  // namespace urcl

tests/test_helpers.cpp

@@ -32,6 +32,8 @@
 
 #include <ur_client_library/helpers.h>
 #include <ur_client_library/exceptions.h>
+#include <ur_client_library/ur/datatypes.h>
+#include <ur_client_library/ur/version_information.h>
 
 using namespace urcl;
 
@@ -86,3 +88,52 @@ TEST(TestHelpers, splitString)
   std::vector<std::string> expected = { "5", "12", "0", "1101319" };
   EXPECT_EQ(expected, splitString(version_string1, "."));
 }
+
+TEST(TestHelpers, robotSeriesFromTypeAndVersion)
+{
+  const VersionInformation cb3_version = VersionInformation::fromString("3.15.0.0");
+  const VersionInformation polyscope_5_version = VersionInformation::fromString("5.12.0.1101319");
+  const VersionInformation polyscope_x_version = VersionInformation::fromString("10.0.0.0");
+
+  // UR3/UR5/UR10: major >= 5 -> E_SERIES, otherwise CB3
+  EXPECT_EQ(robotSeriesFromTypeAndVersion(RobotType::UR3, cb3_version), RobotSeries::CB3);
+  EXPECT_EQ(robotSeriesFromTypeAndVersion(RobotType::UR5, cb3_version), RobotSeries::CB3);
+  EXPECT_EQ(robotSeriesFromTypeAndVersion(RobotType::UR10, cb3_version), RobotSeries::CB3);
+
+  EXPECT_EQ(robotSeriesFromTypeAndVersion(RobotType::UR3, polyscope_5_version), RobotSeries::E_SERIES);
+  EXPECT_EQ(robotSeriesFromTypeAndVersion(RobotType::UR5, polyscope_5_version), RobotSeries::E_SERIES);
+  EXPECT_EQ(robotSeriesFromTypeAndVersion(RobotType::UR10, polyscope_5_version), RobotSeries::E_SERIES);
+
+  EXPECT_EQ(robotSeriesFromTypeAndVersion(RobotType::UR3, polyscope_x_version), RobotSeries::E_SERIES);
+  EXPECT_EQ(robotSeriesFromTypeAndVersion(RobotType::UR5, polyscope_x_version), RobotSeries::E_SERIES);
+  EXPECT_EQ(robotSeriesFromTypeAndVersion(RobotType::UR10, polyscope_x_version), RobotSeries::E_SERIES);
+
+  // UR16: major >= 5 -> E_SERIES, otherwise UNDEFINED
+  EXPECT_EQ(robotSeriesFromTypeAndVersion(RobotType::UR16, polyscope_5_version), RobotSeries::E_SERIES);
+  EXPECT_EQ(robotSeriesFromTypeAndVersion(RobotType::UR16, polyscope_x_version), RobotSeries::E_SERIES);
+  EXPECT_EQ(robotSeriesFromTypeAndVersion(RobotType::UR16, cb3_version), RobotSeries::UNDEFINED);
+
+  // UR15/UR18/UR20/UR30/UR8LONG: major >= 5 -> UR_SERIES, otherwise UNDEFINED
+  EXPECT_EQ(robotSeriesFromTypeAndVersion(RobotType::UR15, polyscope_x_version), RobotSeries::UR_SERIES);
+  EXPECT_EQ(robotSeriesFromTypeAndVersion(RobotType::UR18, polyscope_x_version), RobotSeries::UR_SERIES);
+  EXPECT_EQ(robotSeriesFromTypeAndVersion(RobotType::UR20, polyscope_x_version), RobotSeries::UR_SERIES);
+  EXPECT_EQ(robotSeriesFromTypeAndVersion(RobotType::UR30, polyscope_x_version), RobotSeries::UR_SERIES);
+  EXPECT_EQ(robotSeriesFromTypeAndVersion(RobotType::UR8LONG, polyscope_x_version), RobotSeries::UR_SERIES);
+
+  EXPECT_EQ(robotSeriesFromTypeAndVersion(RobotType::UR15, polyscope_5_version), RobotSeries::UR_SERIES);
+  EXPECT_EQ(robotSeriesFromTypeAndVersion(RobotType::UR18, polyscope_5_version), RobotSeries::UR_SERIES);
+  EXPECT_EQ(robotSeriesFromTypeAndVersion(RobotType::UR20, polyscope_5_version), RobotSeries::UR_SERIES);
+  EXPECT_EQ(robotSeriesFromTypeAndVersion(RobotType::UR30, polyscope_5_version), RobotSeries::UR_SERIES);
+  EXPECT_EQ(robotSeriesFromTypeAndVersion(RobotType::UR8LONG, polyscope_5_version), RobotSeries::UR_SERIES);
+
+  EXPECT_EQ(robotSeriesFromTypeAndVersion(RobotType::UR15, cb3_version), RobotSeries::UNDEFINED);
+  EXPECT_EQ(robotSeriesFromTypeAndVersion(RobotType::UR18, cb3_version), RobotSeries::UNDEFINED);
+  EXPECT_EQ(robotSeriesFromTypeAndVersion(RobotType::UR20, cb3_version), RobotSeries::UNDEFINED);
+  EXPECT_EQ(robotSeriesFromTypeAndVersion(RobotType::UR30, cb3_version), RobotSeries::UNDEFINED);
+  EXPECT_EQ(robotSeriesFromTypeAndVersion(RobotType::UR8LONG, cb3_version), RobotSeries::UNDEFINED);
+
+  // UNDEFINED robot type yields UNDEFINED series
+  EXPECT_EQ(robotSeriesFromTypeAndVersion(RobotType::UNDEFINED, polyscope_5_version), RobotSeries::UNDEFINED);
+  EXPECT_EQ(robotSeriesFromTypeAndVersion(RobotType::UNDEFINED, cb3_version), RobotSeries::UNDEFINED);
+  EXPECT_EQ(robotSeriesFromTypeAndVersion(RobotType::UNDEFINED, polyscope_x_version), RobotSeries::UNDEFINED);
+}

tests/test_primary_client.cpp

@@ -197,6 +197,8 @@ TEST_F(PrimaryClientTest, test_uninitialized_primary_client)
   ASSERT_THROW(client_->isRobotProtectiveStopped(), UrException);
   // The client is not started yet, so the robot type should be UNDEFINED
   ASSERT_EQ(client_->getRobotType(), RobotType::UNDEFINED);
+  // Without a robot type (and version), the robot series cannot be determined.
+  ASSERT_EQ(client_->getRobotSeries(), RobotSeries::UNDEFINED);
 }
 
 TEST_F(PrimaryClientTest, test_stop_command)
@@ -266,6 +268,34 @@ TEST_F(PrimaryClientTest, test_configuration_data)
   EXPECT_NE(client_->getRobotType(), RobotType::UNDEFINED);
 }
 
+TEST_F(PrimaryClientTest, test_get_robot_series)
+{
+  EXPECT_NO_THROW(client_->start());
+
+  // Wait until we have received configuration data so that the robot type is known.
+  auto start_time = std::chrono::system_clock::now();
+  const auto timeout = std::chrono::seconds(2);
+  while (client_->getConfigurationData() == nullptr && std::chrono::system_clock::now() - start_time < timeout)
+  {
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  }
+  ASSERT_NE(client_->getConfigurationData(), nullptr);
+
+  const RobotType robot_type = client_->getRobotType();
+  ASSERT_NE(robot_type, RobotType::UNDEFINED);
+
+  auto version_info = client_->getRobotVersion();
+  ASSERT_NE(version_info, nullptr);
+
+  // The series returned by the client must match what robotSeriesFromTypeAndVersion computes
+  // from the type and version the client itself reports.
+  const RobotSeries expected_series = robotSeriesFromTypeAndVersion(robot_type, *version_info);
+  EXPECT_EQ(client_->getRobotSeries(), expected_series);
+
+  // A robot we successfully connected to should map to a known series.
+  EXPECT_NE(client_->getRobotSeries(), RobotSeries::UNDEFINED);
+}
+
 TEST_F(PrimaryClientTest, test_program_execution)
 {
   auto consumer = std::make_shared<RobotMessageConsumer>();