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..cddc053 --- /dev/null +++ b/cpp_test/test_concepts.cpp @@ -0,0 +1,161 @@ +#include +#include + +#include + +#include "vortex/utils/accessors.hpp" +#include "vortex/utils/concepts.hpp" +#include "vortex/utils/views.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; +}; + +// ---------- Member Concepts ---------- + +static_assert(vortex::utils::concepts::HasPositionMembers); +static_assert(vortex::utils::concepts::HasEulerMembers); +static_assert( + !vortex::utils::concepts::HasQuaternionMembers); + +static_assert( + vortex::utils::concepts::HasQuaternionMembers); +static_assert( + !vortex::utils::concepts::HasEulerMembers); + +static_assert(!vortex::utils::concepts::HasPositionMembers); +static_assert( + !vortex::utils::concepts::HasEulerMembers); +static_assert(!vortex::utils::concepts::HasQuaternionMembers< + IncompleteQuaternionMembers>); + +// ---------- 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"); + +// ---------- View Tests ---------- + +template +concept AcceptsPosVector = + requires(const T& t) { vortex::utils::views::pos_vector(t); }; + +template +concept AcceptsOriVector = + requires(const T& t) { vortex::utils::views::ori_vector(t); }; + +template +concept AcceptsOriQuaternion = + requires(const T& t) { vortex::utils::views::ori_quaternion(t); }; + +// ---------- pos_vector ---------- + +static_assert(AcceptsPosVector); +static_assert(AcceptsPosVector); +static_assert(AcceptsPosVector); +static_assert(AcceptsPosVector); + +// ---------- ori_vector ---------- + +static_assert(AcceptsOriVector); +static_assert(!AcceptsOriVector); +static_assert(!AcceptsOriVector); +static_assert(!AcceptsOriVector); + +// ---------- ori_quaternion ---------- + +static_assert(AcceptsOriQuaternion); +static_assert(!AcceptsOriQuaternion); +static_assert(!AcceptsOriQuaternion); 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/include/vortex/utils/accessors.hpp b/include/vortex/utils/accessors.hpp new file mode 100644 index 0000000..79f4a43 --- /dev/null +++ b/include/vortex/utils/accessors.hpp @@ -0,0 +1,60 @@ +#ifndef VORTEX_UTILS__ACCESSORS_HPP_ +#define VORTEX_UTILS__ACCESSORS_HPP_ + +#include "member_concepts.hpp" + +namespace vortex::utils { + +template +constexpr double x_of(const T& t) { + return t.x; +} + +template +constexpr double y_of(const T& t) { + return t.y; +} + +template +constexpr double z_of(const T& t) { + return t.z; +} + +template +constexpr double roll_of(const T& t) { + return t.roll; +} + +template +constexpr double pitch_of(const T& t) { + return t.pitch; +} + +template +constexpr double yaw_of(const T& t) { + return t.yaw; +} + +template +constexpr double qw_of(const T& q) { + return q.qw; +} + +template +constexpr double qx_of(const T& q) { + return q.qx; +} + +template +constexpr double qy_of(const T& q) { + return q.qy; +} + +template +constexpr double qz_of(const T& q) { + return q.qz; +} + +} // namespace vortex::utils + +#endif // VORTEX_UTILS__ACCESSORS_HPP_ diff --git a/include/vortex/utils/concepts.hpp b/include/vortex/utils/concepts.hpp new file mode 100644 index 0000000..d9acabe --- /dev/null +++ b/include/vortex/utils/concepts.hpp @@ -0,0 +1,42 @@ +#ifndef VORTEX_UTILS__CONCEPTS_HPP_ +#define VORTEX_UTILS__CONCEPTS_HPP_ + +#include +#include "accessors.hpp" + +namespace vortex::utils::concepts { + +template +concept PositionLike = requires(const T& t) { + { x_of(t) } -> std::convertible_to; + { y_of(t) } -> std::convertible_to; + { z_of(t) } -> std::convertible_to; +}; + +template +concept QuaternionLike = requires(const T& q) { + { qw_of(q) } -> std::convertible_to; + { qx_of(q) } -> std::convertible_to; + { qy_of(q) } -> std::convertible_to; + { qz_of(q) } -> std::convertible_to; +}; + +template +concept EulerLike = requires(const T& t) { + { roll_of(t) } -> std::convertible_to; + { pitch_of(t) } -> std::convertible_to; + { yaw_of(t) } -> std::convertible_to; +}; + +template +concept QuatPoseLike = PositionLike && QuaternionLike && (!EulerLike); + +template +concept EulerPoseLike = PositionLike && EulerLike && (!QuaternionLike); + +template +concept PoseLike = QuatPoseLike || EulerPoseLike; + +} // namespace vortex::utils::concepts + +#endif // VORTEX_UTILS__CONCEPTS_HPP_ diff --git a/include/vortex/utils/member_concepts.hpp b/include/vortex/utils/member_concepts.hpp new file mode 100644 index 0000000..988aa97 --- /dev/null +++ b/include/vortex/utils/member_concepts.hpp @@ -0,0 +1,32 @@ +#ifndef VORTEX_UTILS__MEMBER_CONCEPTS_HPP_ +#define VORTEX_UTILS__MEMBER_CONCEPTS_HPP_ + +#include + +namespace vortex::utils::concepts { + +template +concept HasPositionMembers = requires(const T& t) { + { t.x } -> std::convertible_to; + { t.y } -> std::convertible_to; + { t.z } -> std::convertible_to; +}; + +template +concept HasQuaternionMembers = requires(const T& t) { + { t.qw } -> std::convertible_to; + { t.qx } -> std::convertible_to; + { t.qy } -> std::convertible_to; + { t.qz } -> std::convertible_to; +}; + +template +concept HasEulerMembers = requires(const T& t) { + { t.roll } -> std::convertible_to; + { t.pitch } -> std::convertible_to; + { t.yaw } -> std::convertible_to; +}; + +} // namespace vortex::utils::concepts + +#endif // VORTEX_UTILS__MEMBER_CONCEPTS_HPP_ diff --git a/include/vortex/utils/ros_conversions.hpp b/include/vortex/utils/ros_conversions.hpp index b148a4f..1a2bfec 100644 --- a/include/vortex/utils/ros_conversions.hpp +++ b/include/vortex/utils/ros_conversions.hpp @@ -2,265 +2,110 @@ #define VORTEX_UTILS__ROS_CONVERSIONS_HPP_ #include -#include +#include +#include + +#include +#include + #include #include #include #include -#include + +#include "accessors.hpp" +#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) - * - * - * @tparam T The candidate type to check. - */ -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; -}; - -/** - * @brief Concept for Eigen-based 6-vector Euler poses: - * [x, y, z, roll, pitch, yaw]. - * - * Accepts: - * - Eigen::Matrix - * - * - * @tparam T The candidate type. - */ -template -concept Eigen6dEuler = std::same_as>; - -/** - * @brief Master concept representing any supported pose-like structure. - * - * A type satisfies PoseLike if it matches *any* of the following: - * - EulerPoseLike (XYZ + RPY) - * - QuatPoseLike (XYZ + quaternion) - * - Eigen6dEuler (Matrix<6,1>) - * - * - * @tparam T The candidate pose type. - */ -template -concept PoseLike = EulerPoseLike || QuatPoseLike || Eigen6dEuler; - -/** - * @brief Convert a Euler pose-like structure into a ROS Pose message. - * - * The function reads position (x, y, z) and orientation (roll, pitch, yaw) - * from the input object and constructs a corresponding - * `geometry_msgs::msg::Pose`. - * - * 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. - */ -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; - - 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(); - - return pose; -} - -/** - * @brief Convert a quaternion pose-like structure into a ROS Pose message. - * - * 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`. - * - * @tparam T A type satisfying the `QuatPoseLike` concept. - * @param ref The input `QuatPoseLike` object. - * @return A `geometry_msgs::msg::Pose` containing the converted pose. - */ -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; +using vortex::utils::concepts::EulerPoseLike; +using vortex::utils::concepts::PoseLike; +using vortex::utils::concepts::QuatPoseLike; - pose.orientation.x = ref.qx; - pose.orientation.y = ref.qy; - pose.orientation.z = ref.qz; - pose.orientation.w = ref.qw; - - return pose; -} - -/** - * @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. - */ -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; -} - -/** - * @brief Helper concept to check if two types are the same - * after removing cv-ref qualifiers. - * - * @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: - * - * - `geometry_msgs::msg::Pose` - * - * - `geometry_msgs::msg::PoseStamped` - * - * - `geometry_msgs::msg::PoseWithCovarianceStamped` - * - * - `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; + 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` - * - * @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. - */ -template -inline Eigen::Matrix ros_to_eigen6d(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; +template +geometry_msgs::msg::Pose to_pose_msg(const T& pose_like) { + geometry_msgs::msg::Pose pose; - // 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); + pose.position.x = x_of(pose_like); + pose.position.y = y_of(pose_like); + pose.position.z = z_of(pose_like); + + if constexpr (QuatPoseLike) { + pose.orientation.w = qw_of(pose_like); + pose.orientation.x = qx_of(pose_like); + pose.orientation.y = qy_of(pose_like); + pose.orientation.z = qz_of(pose_like); + } else if constexpr (EulerPoseLike) { + const auto q = vortex::utils::math::euler_to_quat( + roll_of(pose_like), pitch_of(pose_like), yaw_of(pose_like)); + pose.orientation.w = q.w(); + pose.orientation.x = q.x(); + pose.orientation.y = q.y(); + pose.orientation.z = q.z(); + } - const Eigen::Vector3d euler = vortex::utils::math::quat_to_euler(q); - v.tail<3>() = euler; + return pose; +} - return v; - } +template + requires PoseLike> +std::vector to_pose_msgs(const R& poses) { + std::vector out; - else if constexpr (same_bare_as) { - return ros_to_eigen6d(msg.pose); + if constexpr (std::ranges::sized_range) { + out.reserve(std::ranges::size(poses)); } - else if constexpr (same_bare_as< - T, geometry_msgs::msg::PoseWithCovarianceStamped>) { - return ros_to_eigen6d(msg.pose.pose); + for (const auto& p : poses) { + out.push_back(to_pose_msg(p)); } + return out; +} - 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); - }); +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; +} - return X; +template +std::vector ros_to_pose_vec(const T& msg) { + if constexpr (same_bare_as) { + 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..75b8161 100644 --- a/include/vortex/utils/types.hpp +++ b/include/vortex/utils/types.hpp @@ -19,6 +19,8 @@ struct Eta; */ struct EtaQuat; +struct Pose; + struct Eta { double x{}; double y{}; @@ -282,6 +284,16 @@ inline Eta EtaQuat::as_eta_euler() const { return eta; } +struct Pose { + double x{}; + double y{}; + double z{}; + double qw{1.0}; + double qx{}; + double qy{}; + double qz{}; +}; + } // namespace vortex::utils::types #endif // VORTEX_UTILS_TYPES_HPP diff --git a/include/vortex/utils/views.hpp b/include/vortex/utils/views.hpp new file mode 100644 index 0000000..cb20cc0 --- /dev/null +++ b/include/vortex/utils/views.hpp @@ -0,0 +1,46 @@ +#ifndef VORTEX_UTILS__VIEWS_HPP_ +#define VORTEX_UTILS__VIEWS_HPP_ + +#include +#include +#include "accessors.hpp" +#include "concepts.hpp" + +namespace vortex::utils::views { + +using vortex::utils::concepts::EulerLike; +using vortex::utils::concepts::EulerPoseLike; +using vortex::utils::concepts::PositionLike; +using vortex::utils::concepts::QuaternionLike; +using vortex::utils::concepts::QuatPoseLike; + +template +inline Eigen::Vector3d pos_vector(const T& t) { + return Eigen::Vector3d{x_of(t), y_of(t), z_of(t)}; +} + +template +inline Eigen::Vector3d ori_vector(const T& t) { + return Eigen::Vector3d{roll_of(t), pitch_of(t), yaw_of(t)}; +} + +template +inline Eigen::Quaterniond ori_quaternion(const T& t) { + return Eigen::Quaterniond{qw_of(t), qx_of(t), qy_of(t), qz_of(t)}; +} + +template +inline Eigen::Vector to_vector(const T& t) { + return Eigen::Vector{x_of(t), y_of(t), z_of(t), + roll_of(t), pitch_of(t), yaw_of(t)}; +} + +template +inline Eigen::Vector to_vector(const T& t) { + return Eigen::Vector{x_of(t), y_of(t), z_of(t), qw_of(t), + qx_of(t), qy_of(t), qz_of(t)}; +} + +} // namespace vortex::utils::views + +#endif // VORTEX_UTILS__VIEWS_HPP_