diff --git a/include/ur_client_library/helpers.h b/include/ur_client_library/helpers.h index f0af4e248..10590cd12 100644 --- a/include/ur_client_library/helpers.h +++ b/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 diff --git a/include/ur_client_library/types.h b/include/ur_client_library/types.h index f50ab3e4e..90684a83a 100644 --- a/include/ur_client_library/types.h +++ b/include/ur_client_library/types.h @@ -59,6 +59,8 @@ class Q void setValues(const vector6d_t& values); void setValues(const std::vector& values); + std::string toString() const; + private: std::vector 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; diff --git a/src/helpers.cpp b/src/helpers.cpp index 4535c6d3b..b8f33588c 100644 --- a/src/helpers.cpp +++ b/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 diff --git a/src/types.cpp b/src/types.cpp index 5237735a5..55278bbe1 100644 --- a/src/types.cpp +++ b/src/types.cpp @@ -31,6 +31,7 @@ #include #include +#include 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 diff --git a/tests/test_helpers.cpp b/tests/test_helpers.cpp index 3499b53c2..527d68388 100644 --- a/tests/test_helpers.cpp +++ b/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)"); +}