MotionTarget helpers

include/ur_client_library/helpers.h

@@ -186,5 +186,7 @@ RobotSeries robotSeriesFromTypeAndVersion(const RobotType type, const VersionInf
  */
 RobotType robotTypeFromString(const std::string& robot_type_str);
 
+std::string stringFromMotionTarget(const MotionTarget& target);
+
 }  // namespace urcl
 #endif  // ifndef UR_CLIENT_LIBRARY_HELPERS_H_INCLUDED

include/ur_client_library/types.h

@@ -59,6 +59,8 @@ class Q
   void setValues(const vector6d_t& values);
   void setValues(const std::vector<double>& values);
 
+  std::string toString() const;
+
 private:
   std::vector<double> values_;
 };
@@ -89,6 +91,8 @@ class Pose
 
   void setPose(const double x, const double y, const double z, const double rx, const double ry, const double rz);
 
+  std::string toString() const;
+
   double x;
   double y;
   double z;

src/helpers.cpp

@@ -225,4 +225,9 @@ RobotType robotTypeFromString(const std::string& robot_type_str)
   return it->second;
 }
 
+std::string stringFromMotionTarget(const MotionTarget& target)
+{
+  return std::visit([](const auto& target) { return target.toString(); }, target);
+}
+
 }  // namespace urcl

src/types.cpp

@@ -31,6 +31,7 @@
 #include <ur_client_library/types.h>
 
 #include <algorithm>
+#include <sstream>
 
 namespace urcl
 {
@@ -71,6 +72,22 @@ bool operator==(const Q& lhs, const Q& rhs)
          std::equal(lhs.getValues().begin(), lhs.getValues().end(), rhs.getValues().begin());
 }
 
+std::string Q::toString() const
+{
+  std::stringstream ss;
+  ss << "Q([";
+  for (size_t i = 0; i < values_.size(); ++i)
+  {
+    ss << values_[i];
+    if (i < values_.size() - 1)
+    {
+      ss << ", ";
+    }
+  }
+  ss << "])";
+  return ss.str();
+}
+
 Pose::Pose() : x(0.0), y(0.0), z(0.0), rx(0.0), ry(0.0), rz(0.0), q_near_(std::nullopt)
 {
 }
@@ -129,4 +146,12 @@ void Pose::setPose(const double x, const double y, const double z, const double
   this->rz = rz;
 }
 
+std::string Pose::toString() const
+{
+  std::stringstream ss;
+  ss << "Pose(x = " << x << ", y = " << y << ", z = " << z << ", rx = " << rx << ", ry = " << ry << ", rz = " << rz
+     << ")";
+  return ss.str();
+}
+
 }  // namespace urcl

tests/test_helpers.cpp

@@ -145,3 +145,12 @@ TEST(TestHelpers, robotSeriesString)
   EXPECT_EQ(robotSeriesString(RobotSeries::UR_SERIES), "UR_SERIES");
   EXPECT_EQ(robotSeriesString(RobotSeries::UNDEFINED), "UNDEFINED");
 }
+
+TEST(TestHelpers, stringFromMotionTarget)
+{
+  const MotionTarget joint_target = Q{ 0.0, 0.1, 0.2, 0.3, 0.4, 0.5 };
+  EXPECT_EQ(stringFromMotionTarget(joint_target), "Q([0, 0.1, 0.2, 0.3, 0.4, 0.5])");
+
+  const MotionTarget pose_target = Pose{ 1.0, 2.0, 3.0, 0.1, 0.2, 0.3 };
+  EXPECT_EQ(stringFromMotionTarget(pose_target), "Pose(x = 1, y = 2, z = 3, rx = 0.1, ry = 0.2, rz = 0.3)");
+}