diff --git a/README.md b/README.md index e72daf0..2417dfd 100644 --- a/README.md +++ b/README.md @@ -27,4 +27,4 @@ for common QoS profile definitions, and ```C++ #include ``` -for common structs like 6DOF `Eta` and `Nu`. +for common structs like 6DOF `PoseEuler`, `Pose` and `Twist`. diff --git a/cpp_test/CMakeLists.txt b/cpp_test/CMakeLists.txt index 01eaf9a..af64830 100644 --- a/cpp_test/CMakeLists.txt +++ b/cpp_test/CMakeLists.txt @@ -8,6 +8,7 @@ add_executable( ${TEST_BINARY_NAME} test_math.cpp test_types.cpp + test_concepts.cpp test_ros_conversions.cpp ) diff --git a/cpp_test/test_concepts.cpp b/cpp_test/test_concepts.cpp new file mode 100644 index 0000000..d81f443 --- /dev/null +++ b/cpp_test/test_concepts.cpp @@ -0,0 +1,107 @@ +#include +#include + +#include + +#include "vortex/utils/concepts.hpp" + +struct HasPositionAndEuler { + double x, y, z; + double roll, pitch, yaw; +}; + +struct HasPositionAndEulerExtra { + double x, y, z; + double roll, pitch, yaw; + int extra; +}; + +struct HasPositionAndQuaternion { + double x, y, z; + double qw, qx, qy, qz; +}; + +struct PositionOnly { + double x, y, z; +}; + +struct EulerOnly { + double roll, pitch, yaw; +}; + +struct IncompleteEulerMembers { + double x, y, z; + double roll, pitch; // missing yaw +}; + +struct IncompleteQuaternionMembers { + double x, y, z; + double qx, qy, qz; // missing qw +}; + +struct WrongEulerMemberType { + double x, y, z; + std::string roll; // wrong type + double pitch, yaw; +}; + +struct HasEulerAndQuaternionMembers { + double x, y, z; + double roll, pitch, yaw; + double qw, qx, qy, qz; +}; + +// ---------- PositionLike ---------- + +static_assert(vortex::utils::concepts::PositionLike); +static_assert(vortex::utils::concepts::PositionLike); +static_assert(vortex::utils::concepts::PositionLike); +static_assert(!vortex::utils::concepts::PositionLike); + +// ---------- EulerLike ---------- + +static_assert(vortex::utils::concepts::EulerLike); +static_assert(vortex::utils::concepts::EulerLike); +static_assert(!vortex::utils::concepts::EulerLike); +static_assert(!vortex::utils::concepts::EulerLike); +static_assert(!vortex::utils::concepts::EulerLike); + +// ---------- QuaternionLike ---------- + +static_assert( + vortex::utils::concepts::QuaternionLike); +static_assert(!vortex::utils::concepts::QuaternionLike); +static_assert( + !vortex::utils::concepts::QuaternionLike); + +// ---------- Pose Concepts ---------- + +static_assert(vortex::utils::concepts::EulerPoseLike); +static_assert(vortex::utils::concepts::EulerPoseLike); +static_assert( + !vortex::utils::concepts::EulerPoseLike); + +static_assert(vortex::utils::concepts::QuatPoseLike); +static_assert(!vortex::utils::concepts::QuatPoseLike); + +static_assert(!vortex::utils::concepts::PoseLike, + "Types exposing both Euler and Quaternion representations must " + "be rejected"); + +static_assert(vortex::utils::concepts::PoseLike); +static_assert(vortex::utils::concepts::PoseLike); +static_assert(!vortex::utils::concepts::PoseLike); +static_assert(!vortex::utils::concepts::PoseLike); + +// ---------- Eigen Tests ---------- + +static_assert( + !vortex::utils::concepts::EulerPoseLike>, + "Eigen::Vector6d should NOT satisfy EulerPoseLike without adapters"); + +static_assert( + !vortex::utils::concepts::QuatPoseLike>, + "Eigen::Vector7d should NOT satisfy QuatPoseLike"); + +static_assert(!vortex::utils::concepts::PositionLike, + "Eigen::Vector3d must NOT satisfy PositionLike"); diff --git a/cpp_test/test_ros_conversions.cpp b/cpp_test/test_ros_conversions.cpp index 2c3a228..02a58e2 100644 --- a/cpp_test/test_ros_conversions.cpp +++ b/cpp_test/test_ros_conversions.cpp @@ -1,26 +1,29 @@ #include -#include #include +#include + +#include +#include +#include +#include + +#include "vortex/utils/math.hpp" #include "vortex/utils/ros_conversions.hpp" #include "vortex/utils/types.hpp" -// ================================================================ -// Compile-Time Concept Tests -// ================================================================ - -struct ValidEulerPose { +struct HasEulerPose { double x = 0, y = 0, z = 0; double roll = 0, pitch = 0, yaw = 0; }; -struct ValidEulerPoseExtra { +struct HasEulerPoseExtra { double x = 0, y = 0, z = 0; double roll = 0, pitch = 0, yaw = 0; - double something_extra = 42.0; + double extra = 1.0; }; -struct ValidQuatPose { +struct HasQuatPose { double x = 1, y = 2, z = 3; double qw = 1, qx = 0, qy = 0, qz = 0; }; @@ -30,140 +33,63 @@ struct MissingYaw { double roll = 0, pitch = 0; }; -struct WrongType { +struct WrongEulerType { double x = 0, y = 0, z = 0; - std::string roll; // not convertible to double + std::string roll; double pitch = 0, yaw = 0; }; -struct MissingQuaternionField { - double x = 0, y = 0, z = 0; - double qx = 0, qy = 0, qz = 0; // missing qw -}; - -// ================================================================ -// Concept: EulerPoseLike -// ================================================================ -static_assert(vortex::utils::ros_conversions::EulerPoseLike, - "ValidEulerPose should satisfy EulerPoseLike"); - -static_assert( - vortex::utils::ros_conversions::EulerPoseLike, - "ValidEulerPoseExtra should satisfy EulerPoseLike"); - -static_assert(!vortex::utils::ros_conversions::EulerPoseLike, - "MissingYaw should NOT satisfy EulerPoseLike"); - -static_assert(!vortex::utils::ros_conversions::EulerPoseLike, - "WrongType should NOT satisfy EulerPoseLike"); - -static_assert( - !vortex::utils::ros_conversions::EulerPoseLike, - "ValidQuatPose uses quaternion fields and must NOT satisfy EulerPoseLike"); - -// ================================================================ -// Concept: QuatPoseLike -// ================================================================ -static_assert(vortex::utils::ros_conversions::QuatPoseLike, - "ValidQuatPose should satisfy QuatPoseLike"); - -static_assert( - !vortex::utils::ros_conversions::QuatPoseLike, - "MissingQuaternionField should NOT satisfy QuatPoseLike"); - -static_assert(!vortex::utils::ros_conversions::QuatPoseLike, - "Euler pose must NOT satisfy QuatPoseLike"); - -// ================================================================ -// Concept: Eigen6dEuler -// ================================================================ -static_assert( - vortex::utils::ros_conversions::Eigen6dEuler>, - "Eigen::Vector6d should satisfy Eigen6dEuler"); - -static_assert(!vortex::utils::ros_conversions::Eigen6dEuler, - "Eigen::Vector3d must NOT satisfy Eigen6dEuler"); - -// ================================================================ -// Concept: PoseLike (master) -// ================================================================ -static_assert(vortex::utils::ros_conversions::PoseLike, - "Euler pose should satisfy PoseLike"); - -static_assert(vortex::utils::ros_conversions::PoseLike, - "Quat pose should satisfy PoseLike"); - -static_assert( - vortex::utils::ros_conversions::PoseLike>, - "Eigen::Vector6d pose should satisfy PoseLike"); - -static_assert(!vortex::utils::ros_conversions::PoseLike, - "WrongType must NOT satisfy PoseLike"); - -// ================================================================ -// Function Acceptance Using Concepts (Overload Resolution) -// ================================================================ template -concept AcceptsPose = - requires(T t) { vortex::utils::ros_conversions::pose_like_to_pose_msg(t); }; - -static_assert(AcceptsPose, "Euler pose should be accepted"); +concept AcceptsToPoseMsg = + requires(const T& t) { vortex::utils::ros_conversions::to_pose_msg(t); }; -static_assert(AcceptsPose, "Quaternion pose should be accepted"); +static_assert(AcceptsToPoseMsg); +static_assert(AcceptsToPoseMsg); +static_assert(AcceptsToPoseMsg); -static_assert(AcceptsPose>, - "Eigen6d should be accepted"); +static_assert(!AcceptsToPoseMsg); +static_assert(!AcceptsToPoseMsg); -static_assert(!AcceptsPose, "MissingYaw should NOT be accepted"); +static_assert(!AcceptsToPoseMsg); +static_assert(!AcceptsToPoseMsg>); +static_assert(!AcceptsToPoseMsg>); -static_assert(!AcceptsPose, "WrongType should NOT be accepted"); +TEST(to_pose_msg, euler_zero) { + HasEulerPose p; -// ================================================================ -// Runtime Tests: Euler -// ================================================================ -TEST(pose_like_to_pose_msg, euler_zero_eta) { - vortex::utils::types::Eta eta; - - auto pose = vortex::utils::ros_conversions::pose_like_to_pose_msg(eta); + auto pose = vortex::utils::ros_conversions::to_pose_msg(p); EXPECT_NEAR(pose.position.x, 0.0, 1e-6); EXPECT_NEAR(pose.position.y, 0.0, 1e-6); EXPECT_NEAR(pose.position.z, 0.0, 1e-6); + EXPECT_NEAR(pose.orientation.w, 1.0, 1e-6); EXPECT_NEAR(pose.orientation.x, 0.0, 1e-6); EXPECT_NEAR(pose.orientation.y, 0.0, 1e-6); EXPECT_NEAR(pose.orientation.z, 0.0, 1e-6); - EXPECT_NEAR(pose.orientation.w, 1.0, 1e-6); } -TEST(pose_like_to_pose_msg, euler_nonzero_angles) { - struct EP { - double x = 1, y = 2, z = 3; - double roll = 1, pitch = 1, yaw = 1; - }; - EP p; +TEST(to_pose_msg, euler_nonzero) { + HasEulerPose p{1, 2, 3, 0.1, 0.2, 0.3}; - auto pose = vortex::utils::ros_conversions::pose_like_to_pose_msg(p); + auto pose = vortex::utils::ros_conversions::to_pose_msg(p); - EXPECT_NEAR(pose.position.x, 1, 1e-6); - EXPECT_NEAR(pose.position.y, 2, 1e-6); - EXPECT_NEAR(pose.position.z, 3, 1e-6); + Eigen::Quaterniond q = vortex::utils::math::euler_to_quat(0.1, 0.2, 0.3); - Eigen::Quaterniond expected = vortex::utils::math::euler_to_quat(1, 1, 1); + EXPECT_NEAR(pose.position.x, 1.0, 1e-6); + EXPECT_NEAR(pose.position.y, 2.0, 1e-6); + EXPECT_NEAR(pose.position.z, 3.0, 1e-6); - EXPECT_NEAR(pose.orientation.x, expected.x(), 1e-6); - EXPECT_NEAR(pose.orientation.y, expected.y(), 1e-6); - EXPECT_NEAR(pose.orientation.z, expected.z(), 1e-6); - EXPECT_NEAR(pose.orientation.w, expected.w(), 1e-6); + EXPECT_NEAR(pose.orientation.w, q.w(), 1e-6); + EXPECT_NEAR(pose.orientation.x, q.x(), 1e-6); + EXPECT_NEAR(pose.orientation.y, q.y(), 1e-6); + EXPECT_NEAR(pose.orientation.z, q.z(), 1e-6); } -// ================================================================ -// Runtime Tests: Quaternion Pose -// ================================================================ -TEST(pose_like_to_pose_msg, quat_pose_conversion) { - ValidQuatPose qp; +TEST(to_pose_msg, quaternion_pose) { + HasQuatPose p; - auto pose = vortex::utils::ros_conversions::pose_like_to_pose_msg(qp); + auto pose = vortex::utils::ros_conversions::to_pose_msg(p); EXPECT_NEAR(pose.position.x, 1.0, 1e-6); EXPECT_NEAR(pose.position.y, 2.0, 1e-6); @@ -175,208 +101,63 @@ TEST(pose_like_to_pose_msg, quat_pose_conversion) { EXPECT_NEAR(pose.orientation.z, 0.0, 1e-6); } -// ================================================================ -// Runtime Tests: Eigen::Vector6d -// ================================================================ -TEST(pose_like_to_pose_msg, eigen6d_conversion) { - Eigen::Matrix v; - v << 1, 2, 3, // xyz - 0.1, 0.2, 0.3; // roll pitch yaw - - auto pose = vortex::utils::ros_conversions::pose_like_to_pose_msg(v); - - EXPECT_NEAR(pose.position.x, 1.0, 1e-6); - EXPECT_NEAR(pose.position.y, 2.0, 1e-6); - EXPECT_NEAR(pose.position.z, 3.0, 1e-6); +TEST(to_pose_msgs, vector_of_euler) { + std::vector poses(3); - Eigen::Quaterniond expected = - vortex::utils::math::euler_to_quat(0.1, 0.2, 0.3); + auto out = vortex::utils::ros_conversions::to_pose_msgs(poses); - EXPECT_NEAR(pose.orientation.w, expected.w(), 1e-6); - EXPECT_NEAR(pose.orientation.x, expected.x(), 1e-6); - EXPECT_NEAR(pose.orientation.y, expected.y(), 1e-6); - EXPECT_NEAR(pose.orientation.z, expected.z(), 1e-6); + ASSERT_EQ(out.size(), 3); } -// ================================================================ -// Compile-Time Tests for ros_to_eigen6d Type Acceptance -// ================================================================ - template -concept AcceptsRosToEigen = - requires(const T& t) { vortex::utils::ros_conversions::ros_to_eigen6d(t); }; - -static_assert(AcceptsRosToEigen, - "Pose must be accepted"); - -static_assert(AcceptsRosToEigen, - "PoseStamped must be accepted"); - -static_assert(AcceptsRosToEigen, - "PoseWithCovarianceStamped must be accepted"); - -static_assert(AcceptsRosToEigen, - "PoseArray must be accepted"); - -struct RandomType { - double x = 0; +concept AcceptsRosToPoseVec = requires(const T& t) { + vortex::utils::ros_conversions::ros_to_pose_vec(t); }; -static_assert(!AcceptsRosToEigen, "int must NOT be accepted"); - -static_assert(!AcceptsRosToEigen, "double must NOT be accepted"); - -static_assert(!AcceptsRosToEigen, - "Eigen::Vector3d must NOT be accepted"); - -static_assert(!AcceptsRosToEigen, - "RandomType must NOT be accepted"); - -static_assert(!AcceptsRosToEigen>, - "shared_ptr must NOT be accepted"); - -static_assert(!AcceptsRosToEigen, - "raw pointer Pose* must NOT be accepted"); - -// ================================================================ -// Runtime Tests: ros_to_eigen6d -// ================================================================ -TEST(ros_to_eigen6d, pose_basic) { - geometry_msgs::msg::Pose p; - p.position.x = 1.0; - p.position.y = 2.0; - p.position.z = 3.0; - - Eigen::Quaterniond q = vortex::utils::math::euler_to_quat(0.1, 0.2, 0.3); - p.orientation.x = q.x(); - p.orientation.y = q.y(); - p.orientation.z = q.z(); - p.orientation.w = q.w(); - - auto v = vortex::utils::ros_conversions::ros_to_eigen6d(p); - - EXPECT_NEAR(v(0), 1.0, 1e-6); - EXPECT_NEAR(v(1), 2.0, 1e-6); - EXPECT_NEAR(v(2), 3.0, 1e-6); - - Eigen::Vector3d euler = vortex::utils::math::quat_to_euler(q); - - EXPECT_NEAR(v(3), euler(0), 1e-6); - EXPECT_NEAR(v(4), euler(1), 1e-6); - EXPECT_NEAR(v(5), euler(2), 1e-6); -} - -TEST(ros_to_eigen6d, pose_stamped) { - geometry_msgs::msg::PoseStamped ps; - ps.pose.position.x = -1.0; - ps.pose.position.y = 5.0; - ps.pose.position.z = 10.0; - - Eigen::Quaterniond q = vortex::utils::math::euler_to_quat(0.3, 0.2, 0.1); - ps.pose.orientation.x = q.x(); - ps.pose.orientation.y = q.y(); - ps.pose.orientation.z = q.z(); - ps.pose.orientation.w = q.w(); - - auto v = vortex::utils::ros_conversions::ros_to_eigen6d(ps); - - EXPECT_NEAR(v(0), -1.0, 1e-6); - EXPECT_NEAR(v(1), 5.0, 1e-6); - EXPECT_NEAR(v(2), 10.0, 1e-6); - - Eigen::Vector3d euler = vortex::utils::math::quat_to_euler(q); - EXPECT_NEAR(v(3), euler(0), 1e-6); - EXPECT_NEAR(v(4), euler(1), 1e-6); - EXPECT_NEAR(v(5), euler(2), 1e-6); -} - -TEST(ros_to_eigen6d, pose_with_covariance_stamped) { - geometry_msgs::msg::PoseWithCovarianceStamped pc; - pc.pose.pose.position.x = 7.0; - pc.pose.pose.position.y = 8.0; - pc.pose.pose.position.z = 9.0; - - Eigen::Quaterniond q = vortex::utils::math::euler_to_quat(-0.1, 0.5, -0.3); - pc.pose.pose.orientation.x = q.x(); - pc.pose.pose.orientation.y = q.y(); - pc.pose.pose.orientation.z = q.z(); - pc.pose.pose.orientation.w = q.w(); - - auto v = vortex::utils::ros_conversions::ros_to_eigen6d(pc); - - EXPECT_NEAR(v(0), 7.0, 1e-6); - EXPECT_NEAR(v(1), 8.0, 1e-6); - EXPECT_NEAR(v(2), 9.0, 1e-6); - - Eigen::Vector3d euler = vortex::utils::math::quat_to_euler(q); - EXPECT_NEAR(v(3), euler(0), 1e-6); - EXPECT_NEAR(v(4), euler(1), 1e-6); - EXPECT_NEAR(v(5), euler(2), 1e-6); -} +static_assert(AcceptsRosToPoseVec); +static_assert(AcceptsRosToPoseVec); +static_assert( + AcceptsRosToPoseVec); +static_assert(AcceptsRosToPoseVec); -TEST(ros_to_eigen6d, pose_array_single) { - geometry_msgs::msg::PoseArray arr; +static_assert(!AcceptsRosToPoseVec); +static_assert(!AcceptsRosToPoseVec); +static_assert(!AcceptsRosToPoseVec); +static_assert(!AcceptsRosToPoseVec); +TEST(ros_to_pose_vec, pose) { geometry_msgs::msg::Pose p; - p.position.x = 4.0; - p.position.y = 3.0; - p.position.z = 2.0; - - Eigen::Quaterniond q = vortex::utils::math::euler_to_quat(0.4, -0.2, 1.0); - p.orientation.x = q.x(); - p.orientation.y = q.y(); - p.orientation.z = q.z(); - p.orientation.w = q.w(); - - arr.poses.push_back(p); - - auto X = vortex::utils::ros_conversions::ros_to_eigen6d(arr); - - ASSERT_EQ(X.cols(), 1); - EXPECT_NEAR(X(0, 0), 4.0, 1e-6); - EXPECT_NEAR(X(1, 0), 3.0, 1e-6); - EXPECT_NEAR(X(2, 0), 2.0, 1e-6); - - Eigen::Vector3d euler = vortex::utils::math::quat_to_euler(q); - EXPECT_NEAR(X(3, 0), euler(0), 1e-6); - EXPECT_NEAR(X(4, 0), euler(1), 1e-6); - EXPECT_NEAR(X(5, 0), euler(2), 1e-6); + p.position.x = 1; + p.position.y = 2; + p.position.z = 3; + p.orientation.w = 1; + p.orientation.x = 0; + p.orientation.y = 0; + p.orientation.z = 0; + + auto v = vortex::utils::ros_conversions::ros_to_pose_vec(p); + + ASSERT_EQ(v.size(), 1); + EXPECT_NEAR(v[0].x, 1.0, 1e-6); + EXPECT_NEAR(v[0].y, 2.0, 1e-6); + EXPECT_NEAR(v[0].z, 3.0, 1e-6); + + EXPECT_NEAR(v[0].qw, 1.0, 1e-6); + EXPECT_NEAR(v[0].qz, 0.0, 1e-6); + EXPECT_NEAR(v[0].qz, 0.0, 1e-6); + EXPECT_NEAR(v[0].qz, 0.0, 1e-6); } -TEST(ros_to_eigen6d, pose_array_multiple) { +TEST(ros_to_pose_vec, pose_array) { geometry_msgs::msg::PoseArray arr; + arr.poses.resize(2); - for (int i = 0; i < 3; i++) { - geometry_msgs::msg::Pose p; - p.position.x = i; - p.position.y = i + 1; - p.position.z = i + 2; - - Eigen::Quaterniond q = - vortex::utils::math::euler_to_quat(0.1 * i, 0.2 * i, 0.3 * i); - p.orientation.x = q.x(); - p.orientation.y = q.y(); - p.orientation.z = q.z(); - p.orientation.w = q.w(); - - arr.poses.push_back(p); - } - - auto X = vortex::utils::ros_conversions::ros_to_eigen6d(arr); - - ASSERT_EQ(X.cols(), 3); - - for (int i = 0; i < 3; i++) { - EXPECT_NEAR(X(0, i), i, 1e-6); - EXPECT_NEAR(X(1, i), i + 1, 1e-6); - EXPECT_NEAR(X(2, i), i + 2, 1e-6); + arr.poses[0].position.x = 1; + arr.poses[1].position.x = 2; - Eigen::Quaterniond q = - vortex::utils::math::euler_to_quat(0.1 * i, 0.2 * i, 0.3 * i); - Eigen::Vector3d euler = vortex::utils::math::quat_to_euler(q); + auto v = vortex::utils::ros_conversions::ros_to_pose_vec(arr); - EXPECT_NEAR(X(3, i), euler(0), 1e-6); - EXPECT_NEAR(X(4, i), euler(1), 1e-6); - EXPECT_NEAR(X(5, i), euler(2), 1e-6); - } + ASSERT_EQ(v.size(), 2); + EXPECT_NEAR(v[0].x, 1.0, 1e-6); + EXPECT_NEAR(v[1].x, 2.0, 1e-6); } diff --git a/cpp_test/test_types.cpp b/cpp_test/test_types.cpp index a8537ed..a5b63cf 100644 --- a/cpp_test/test_types.cpp +++ b/cpp_test/test_types.cpp @@ -10,7 +10,7 @@ class TypesTests : public ::testing::Test { }; TEST_F(TypesTests, test_eta) { - vortex::utils::types::Eta eta; + vortex::utils::types::PoseEuler eta; // Test correct zero initialization EXPECT_EQ(eta.x, 0.0); EXPECT_EQ(eta.y, 0.0); @@ -42,8 +42,8 @@ TEST_F(TypesTests, test_eta) { EXPECT_TRUE(result_v.isApprox(expected_v, 1e-12)); // Test operator- - vortex::utils::types::Eta other{1.0, 2.0, 3.0, 0.1, 0.2, 0.3}; - vortex::utils::types::Eta diff{eta - other}; + vortex::utils::types::PoseEuler other{1.0, 2.0, 3.0, 0.1, 0.2, 0.3}; + vortex::utils::types::PoseEuler diff{eta - other}; EXPECT_NEAR(diff.x, 4.0, 1e-12); EXPECT_NEAR(diff.y, -6.0, 1e-12); EXPECT_NEAR(diff.z, -0.9, 1e-12); @@ -53,7 +53,7 @@ TEST_F(TypesTests, test_eta) { } TEST_F(TypesTests, test_eta_quat) { - vortex::utils::types::EtaQuat eta_quat; + vortex::utils::types::Pose eta_quat; // Test correct zero initialization EXPECT_EQ(eta_quat.x, 0.0); EXPECT_EQ(eta_quat.y, 0.0); @@ -76,8 +76,8 @@ TEST_F(TypesTests, test_eta_quat) { EXPECT_TRUE(result_v.isApprox(expected_v, 1e-12)); // Test operator- - vortex::utils::types::EtaQuat other{1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.4}; - vortex::utils::types::EtaQuat diff{eta_quat - other}; + vortex::utils::types::Pose other{1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.4}; + vortex::utils::types::Pose diff{eta_quat - other}; auto pos = diff.pos_vector(); EXPECT_TRUE(pos.isApprox(Eigen::Vector3d(4.0, -6.0, -0.9), 1e-12)); auto q = diff.ori_quaternion(); @@ -88,7 +88,7 @@ TEST_F(TypesTests, test_eta_quat) { } TEST_F(TypesTests, test_nu) { - vortex::utils::types::Nu nu; + vortex::utils::types::Twist nu; // Test correct zero initialization EXPECT_EQ(nu.u, 0.0); EXPECT_EQ(nu.v, 0.0); @@ -109,8 +109,8 @@ TEST_F(TypesTests, test_nu) { EXPECT_TRUE(result_v.isApprox(expected_v, 1e-12)); // Test operator- - vortex::utils::types::Nu other{1.0, 2.0, 3.0, 0.1, 0.2, 0.3}; - vortex::utils::types::Nu diff{nu - other}; + vortex::utils::types::Twist other{1.0, 2.0, 3.0, 0.1, 0.2, 0.3}; + vortex::utils::types::Twist diff{nu - other}; EXPECT_NEAR(diff.u, 4.0, 1e-12); EXPECT_NEAR(diff.v, -6.0, 1e-12); EXPECT_NEAR(diff.w, -0.9, 1e-12); diff --git a/include/vortex/utils/concepts.hpp b/include/vortex/utils/concepts.hpp new file mode 100644 index 0000000..0abd462 --- /dev/null +++ b/include/vortex/utils/concepts.hpp @@ -0,0 +1,126 @@ +#ifndef VORTEX_UTILS__CONCEPTS_HPP_ +#define VORTEX_UTILS__CONCEPTS_HPP_ + +#include + +namespace vortex::utils::concepts { + +/** + * @brief Concept for types that expose 3D positional components. + * + * A type satisfies PositionLike if it contains public members: + * + * - `x` + * + * - `y` + * + * - `z` + * + * Each component must be convertible to double. + * + * @tparam T Type to be checked + */ +template +concept PositionLike = requires(const T& t) { + { t.x } -> std::convertible_to; + { t.y } -> std::convertible_to; + { t.z } -> std::convertible_to; +}; + +/** + * @brief Concept for types that expose orientation as a quaternion. + * + * A type satisfies QuaternionLike if it contains public members: + * + * - `qw` + * + * - `qx` + * + * - `qy` + * + * - `qz` + * + * Each component must be convertible to double. + * + * @tparam T Type to be checked + */ +template +concept QuaternionLike = requires(const T& t) { + { t.qw } -> std::convertible_to; + { t.qx } -> std::convertible_to; + { t.qy } -> std::convertible_to; + { t.qz } -> std::convertible_to; +}; + +/** + * @brief Concept for types that expose orientation as Euler angles. + * + * A type satisfies EulerLike if it contains public members: + * + * - `roll` + * + * - `pitch` + * + * - `yaw` + * + * Each component must be convertible to double. + * + * @tparam T Type to be checked + */ +template +concept EulerLike = requires(const T& t) { + { t.roll } -> std::convertible_to; + { t.pitch } -> std::convertible_to; + { t.yaw } -> std::convertible_to; +}; + +/** + * @brief Concept for pose-like types with quaternion orientation. + * + * A type satisfies QuatPoseLike if it: + * - Has positional components (PositionLike) + * - Has quaternion orientation (QuaternionLike) + * - Does NOT expose Euler orientation (not EulerLike) + * + * This prevents ambiguity for types that might provide both + * Euler and quaternion accessors. + * + * @tparam T Type to be checked + */ +template +concept QuatPoseLike = PositionLike && QuaternionLike && (!EulerLike); + +/** + * @brief Concept for pose-like types with Euler angle orientation. + * + * A type satisfies EulerPoseLike if it: + * - Has positional components (PositionLike) + * - Has Euler orientation (EulerLike) + * - Does NOT expose quaternion orientation (not QuaternionLike) + * + * This prevents ambiguity for types that might provide both + * Euler and quaternion accessors. + * + * @tparam T Type to be checked + */ +template +concept EulerPoseLike = PositionLike && EulerLike && (!QuaternionLike); + +/** + * @brief Concept for pose-like types with either Euler or quaternion + * orientation. + * + * PoseLike is satisfied if the type represents a 3D pose with: + * - Position + * - Exactly one orientation representation: + * - quaternion (QuatPoseLike), or + * - Euler angles (EulerPoseLike) + * + * @tparam T Type to be checked + */ +template +concept PoseLike = QuatPoseLike || EulerPoseLike; + +} // namespace vortex::utils::concepts + +#endif // VORTEX_UTILS__CONCEPTS_HPP_ diff --git a/include/vortex/utils/ros_conversions.hpp b/include/vortex/utils/ros_conversions.hpp index b148a4f..aa2067f 100644 --- a/include/vortex/utils/ros_conversions.hpp +++ b/include/vortex/utils/ros_conversions.hpp @@ -2,191 +2,139 @@ #define VORTEX_UTILS__ROS_CONVERSIONS_HPP_ #include -#include +#include +#include + +#include +#include + #include #include #include #include -#include + +#include "concepts.hpp" #include "math.hpp" +#include "types.hpp" namespace vortex::utils::ros_conversions { /** - * @brief Concept describing an Euler pose type expressed in XYZ + RPY form. - * - * A type satisfies this concept if it exposes the following fields, - * all convertible to double: - * - x, y, z (position components) - * - roll, pitch, yaw (orientation expressed as Euler/RPY angles) - * - * - * @tparam T The candidate type to check. - */ -template -concept EulerPoseLike = requires(const T& t) { - { t.x } -> std::convertible_to; - { t.y } -> std::convertible_to; - { t.z } -> std::convertible_to; - - { t.roll } -> std::convertible_to; - { t.pitch } -> std::convertible_to; - { t.yaw } -> std::convertible_to; -}; - -/** - * @brief Concept describing a pose type expressed in XYZ + quaternion form. - * - * A type satisfies this concept if it exposes the following fields: - * - x, y, z (position components) - * - qw, qx, qy, qz (quaternion orientation) - * + * @brief Helper concept to check if two types are the same + * after removing cv-ref qualifiers. * - * @tparam T The candidate type to check. + * @tparam T First type. + * @tparam U Second type. */ -template -concept QuatPoseLike = requires(const T& t) { - { t.x } -> std::convertible_to; - { t.y } -> std::convertible_to; - { t.z } -> std::convertible_to; - - { t.qw } -> std::convertible_to; - { t.qx } -> std::convertible_to; - { t.qy } -> std::convertible_to; - { t.qz } -> std::convertible_to; -}; +template +concept same_bare_as = std::same_as, U>; /** - * @brief Concept for Eigen-based 6-vector Euler poses: - * [x, y, z, roll, pitch, yaw]. + * @brief Concept describing ROS pose message types supported by + * ros_to_eigen6d(). * - * Accepts: - * - Eigen::Matrix + * Supported types are: * + * - `geometry_msgs::msg::Pose` * - * @tparam T The candidate type. - */ -template -concept Eigen6dEuler = std::same_as>; - -/** - * @brief Master concept representing any supported pose-like structure. + * - `geometry_msgs::msg::PoseStamped` * - * A type satisfies PoseLike if it matches *any* of the following: - * - EulerPoseLike (XYZ + RPY) - * - QuatPoseLike (XYZ + quaternion) - * - Eigen6dEuler (Matrix<6,1>) + * - `geometry_msgs::msg::PoseWithCovarianceStamped` * + * - `geometry_msgs::msg::PoseArray` * - * @tparam T The candidate pose type. + * @tparam T The candidate type to test. */ template -concept PoseLike = EulerPoseLike || QuatPoseLike || Eigen6dEuler; +concept ROSPoseLike = + same_bare_as || + same_bare_as || + same_bare_as || + same_bare_as; /** - * @brief Convert a Euler pose-like structure into a ROS Pose message. + * @brief Converts a PoseLike object to a ROS geometry_msgs::msg::Pose. * - * The function reads position (x, y, z) and orientation (roll, pitch, yaw) - * from the input object and constructs a corresponding - * `geometry_msgs::msg::Pose`. + * The input type must satisfy PoseLike, i.e. provide position components + * and exactly one orientation representation (Euler or quaternion). * - * Orientation is internally converted from Euler angles (roll, pitch, yaw) - * into a quaternion via `vortex::utils::math::euler_to_quat()`. - * - * @tparam T A type satisfying the `EulerPoseLike` concept. - * @param ref The input `EulerPoseLike` object. - * @return A `geometry_msgs::msg::Pose` containing the converted pose. + * @tparam T Type satisfying PoseLike + * @param pose_like Input pose object + * @return geometry_msgs::msg::Pose ROS pose message */ -template -geometry_msgs::msg::Pose pose_like_to_pose_msg(const T& ref) { +template +geometry_msgs::msg::Pose to_pose_msg(const T& pose_like) { geometry_msgs::msg::Pose pose; - pose.position.x = ref.x; - pose.position.y = ref.y; - pose.position.z = ref.z; - - Eigen::Quaterniond quat = - vortex::utils::math::euler_to_quat(ref.roll, ref.pitch, ref.yaw); - - pose.orientation.x = quat.x(); - pose.orientation.y = quat.y(); - pose.orientation.z = quat.z(); - pose.orientation.w = quat.w(); + pose.position.x = pose_like.x; + pose.position.y = pose_like.y; + pose.position.z = pose_like.z; + + if constexpr (vortex::utils::concepts::QuatPoseLike) { + pose.orientation.w = pose_like.qw; + pose.orientation.x = pose_like.qx; + pose.orientation.y = pose_like.qy; + pose.orientation.z = pose_like.qz; + } else if constexpr (vortex::utils::concepts::EulerPoseLike) { + const auto q = vortex::utils::math::euler_to_quat( + pose_like.roll, pose_like.pitch, pose_like.yaw); + pose.orientation.w = q.w(); + pose.orientation.x = q.x(); + pose.orientation.y = q.y(); + pose.orientation.z = q.z(); + } return pose; } /** - * @brief Convert a quaternion pose-like structure into a ROS Pose message. + * @brief Converts a range of PoseLike objects to a vector of ROS pose messages. * - * The function reads position (x, y, z) and orientation (qw, qx, qy, qz) - * from the input object and constructs a corresponding - * `geometry_msgs::msg::Pose`. + * The input range may be any std::ranges::input_range whose value type + * satisfies PoseLike (e.g. std::vector, std::array, std::span, or views). * - * @tparam T A type satisfying the `QuatPoseLike` concept. - * @param ref The input `QuatPoseLike` object. - * @return A `geometry_msgs::msg::Pose` containing the converted pose. + * @tparam R Range type whose value_type satisfies PoseLike + * @param poses Range of pose-like objects + * @return std::vector Converted ROS poses */ -template -geometry_msgs::msg::Pose pose_like_to_pose_msg(const T& ref) { - geometry_msgs::msg::Pose pose; - - pose.position.x = ref.x; - pose.position.y = ref.y; - pose.position.z = ref.z; +template + requires vortex::utils::concepts::PoseLike> +std::vector to_pose_msgs(const R& poses) { + std::vector out; - pose.orientation.x = ref.qx; - pose.orientation.y = ref.qy; - pose.orientation.z = ref.qz; - pose.orientation.w = ref.qw; + if constexpr (std::ranges::sized_range) { + out.reserve(std::ranges::size(poses)); + } - return pose; + for (const auto& p : poses) { + out.push_back(to_pose_msg(p)); + } + return out; } /** - * @brief Convert an Eigen 6d-vector [x, y, z, roll, pitch, yaw] - * into a ROS Pose message. - * - * Orientation is internally converted from v(3), v(4), v(5) - * into a quaternion via `vortex::utils::math::euler_to_quat()`. - * - * @tparam T A type satisfying the `Eigen6dEuler` concept. - * @param v The 6d-vector containing position and Euler angles. - * @return A `geometry_msgs::msg::Pose` containing the converted pose. + * @brief Converts a ROS geometry_msgs::msg::Pose to an internal Pose type. + * @param pose ROS pose message + * @return vortex::utils::types::Pose Internal pose representation */ -template -geometry_msgs::msg::Pose pose_like_to_pose_msg(const T& v) { - geometry_msgs::msg::Pose pose; - - pose.position.x = v(0); - pose.position.y = v(1); - pose.position.z = v(2); - - Eigen::Quaterniond q = vortex::utils::math::euler_to_quat(v(3), v(4), v(5)); - - pose.orientation.x = q.x(); - pose.orientation.y = q.y(); - pose.orientation.z = q.z(); - pose.orientation.w = q.w(); - - return pose; +inline vortex::utils::types::Pose ros_pose_to_pose( + const geometry_msgs::msg::Pose& pose) { + vortex::utils::types::Pose p; + p.x = pose.position.x; + p.y = pose.position.y; + p.z = pose.position.z; + + p.qw = pose.orientation.w; + p.qx = pose.orientation.x; + p.qy = pose.orientation.y; + p.qz = pose.orientation.z; + return p; } /** - * @brief Helper concept to check if two types are the same - * after removing cv-ref qualifiers. + * @brief Extracts one or more internal Pose objects from a ROS pose message. * - * @tparam T First type. - * @tparam U Second type. - */ -template -concept same_bare_as = std::same_as, U>; - -/** - * @brief Concept describing ROS pose message types supported by - * ros_to_eigen6d(). - * - * Supported types are: + * Supported input types are: * * - `geometry_msgs::msg::Pose` * @@ -196,71 +144,31 @@ concept same_bare_as = std::same_as, U>; * * - `geometry_msgs::msg::PoseArray` * - * @tparam T The candidate type to test. - */ -template -concept ROSPoseLike = - same_bare_as || - same_bare_as || - same_bare_as || - same_bare_as; - -/** - * @brief Convert various ROS pose messages to Eigen 6d types. - * - * The function supports the following input types: - * - * - `geometry_msgs::msg::Pose` - * - * - `geometry_msgs::msg::PoseStamped` - * - * - `geometry_msgs::msg::PoseWithCovarianceStamped` - * - * - `geometry_msgs::msg::PoseArray` + * Messages containing a single pose produce a vector with one element. + * PoseArray messages produce a vector with one element per pose. * - * @tparam T A type satisfying the `ROSPoseLike` concept. - * @param msg The input ROS message. - * @return An `Eigen::Matrix` containing the converted - * poses where each column is [x, y, z, roll, pitch, yaw]^T. + * @tparam T ROS message type satisfying ROSPoseLike + * @param msg ROS pose message + * @return std::vector Extracted internal poses */ template -inline Eigen::Matrix ros_to_eigen6d(const T& msg) { +std::vector ros_to_pose_vec(const T& msg) { if constexpr (same_bare_as) { - Eigen::Matrix v; - v(0) = msg.position.x; - v(1) = msg.position.y; - v(2) = msg.position.z; - - // ROS/tf2 uses (x, y, z, w) - // while Eigen stores quaternions as (w, x, y, z) - Eigen::Quaterniond q(msg.orientation.w, msg.orientation.x, - msg.orientation.y, msg.orientation.z); - - const Eigen::Vector3d euler = vortex::utils::math::quat_to_euler(q); - v.tail<3>() = euler; - - return v; - } - - else if constexpr (same_bare_as) { - return ros_to_eigen6d(msg.pose); - } - - else if constexpr (same_bare_as< - T, geometry_msgs::msg::PoseWithCovarianceStamped>) { - return ros_to_eigen6d(msg.pose.pose); - } - - else if constexpr (same_bare_as) { - const size_t n = msg.poses.size(); - Eigen::Matrix X(6, n); - - std::ranges::for_each(std::views::iota(size_t{0}, n), [&](size_t i) { - const auto& pose = msg.poses[i]; - X.col(i) = ros_to_eigen6d(pose); - }); - - return X; + return {ros_pose_to_pose(msg)}; + } else if constexpr (same_bare_as) { + return ros_pose_to_pose(msg.pose); + } else if constexpr (same_bare_as< + T, + geometry_msgs::msg::PoseWithCovarianceStamped>) { + return ros_pose_to_pose(msg.pose.pose); + } else if constexpr (same_bare_as) { + std::vector poses; + poses.reserve(msg.poses.size()); + + for (const auto& pose : msg.poses) { + poses.push_back(ros_pose_to_pose(pose)); + } + return poses; } } diff --git a/include/vortex/utils/types.hpp b/include/vortex/utils/types.hpp index 49e0af2..1efec1d 100644 --- a/include/vortex/utils/types.hpp +++ b/include/vortex/utils/types.hpp @@ -8,18 +8,24 @@ namespace vortex::utils::types { /** - * @brief Struct to represent the state vector eta according to eq. 2.3 in - * Fossen, 2021, containing the position and orientation. + * @brief Struct to represent the pose vector according to eq. 2.3 in + * Fossen, 2021, containing the position and orientation as euler angles. */ -struct Eta; +struct PoseEuler; /** - * @brief Struct to represent the state vector eta according to eq. 2.3 in + * @brief Struct to represent the pose vector according to eq. 2.3 in * Fossen, 2021, containing the position and orientation as quaternion. */ -struct EtaQuat; +struct Pose; -struct Eta { +/** + * @brief Struct to represent the velocity vector according to eq. 2.5 in + * Fossen, 2021, containing the linear and angular velocities. + */ +struct Twist; + +struct PoseEuler { double x{}; double y{}; double z{}; @@ -27,8 +33,8 @@ struct Eta { double pitch{}; double yaw{}; - Eta operator-(const Eta& other) const { - Eta eta; + PoseEuler operator-(const PoseEuler& other) const { + PoseEuler eta; eta.x = x - other.x; eta.y = y - other.y; eta.z = z - other.z; @@ -125,13 +131,13 @@ struct Eta { return j_matrix; } /** - * @brief Convert to EtaQuat - * @return EtaQuat + * @brief Convert to Pose + * @return Pose */ - EtaQuat as_eta_quat() const; + Pose as_pose() const; }; -struct EtaQuat { +struct Pose { double x{}; double y{}; double z{}; @@ -167,8 +173,8 @@ struct EtaQuat { return Eigen::Vector{x, y, z, qw, qx, qy, qz}; } - EtaQuat operator-(const EtaQuat& other) const { - EtaQuat eta; + Pose operator-(const Pose& other) const { + Pose eta; eta.x = x - other.x; eta.y = y - other.y; eta.z = z - other.z; @@ -217,17 +223,17 @@ struct EtaQuat { return j_matrix; } /** - * @brief Convert to Eta with Euler angles - * @return Eta + * @brief Convert to PoseEuler with Euler angles + * @return PoseEuler */ - Eta as_eta_euler() const; + PoseEuler as_pose_euler() const; }; /** * @brief Struct to represent the state vector nu according to eq. 2.5 in * Fossen, 2021, containing the linear and angular velocities. */ -struct Nu { +struct Twist { double u{}; double v{}; double w{}; @@ -235,8 +241,8 @@ struct Nu { double q{}; double r{}; - Nu operator-(const Nu& other) const { - Nu nu; + Twist operator-(const Twist& other) const { + Twist nu; nu.u = u - other.u; nu.v = v - other.v; nu.w = w - other.w; @@ -255,30 +261,30 @@ struct Nu { } }; -inline EtaQuat Eta::as_eta_quat() const { +inline Pose PoseEuler::as_pose() const { Eigen::Quaterniond quat = vortex::utils::math::euler_to_quat(roll, pitch, yaw); - EtaQuat eta_quat{.x = x, - .y = y, - .z = z, - .qw = quat.w(), - .qx = quat.x(), - .qy = quat.y(), - .qz = quat.z()}; + Pose eta_quat{.x = x, + .y = y, + .z = z, + .qw = quat.w(), + .qx = quat.x(), + .qy = quat.y(), + .qz = quat.z()}; return eta_quat; } -inline Eta EtaQuat::as_eta_euler() const { +inline PoseEuler Pose::as_pose_euler() const { Eigen::Vector3d euler_angles = vortex::utils::math::quat_to_euler(ori_quaternion()); - Eta eta{.x = x, - .y = y, - .z = z, - .roll = euler_angles(0), - .pitch = euler_angles(1), - .yaw = euler_angles(2)}; + PoseEuler eta{.x = x, + .y = y, + .z = z, + .roll = euler_angles(0), + .pitch = euler_angles(1), + .yaw = euler_angles(2)}; return eta; }