diff --git a/CMakeLists.txt b/CMakeLists.txt index 490e425..e4195a2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,6 +8,7 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp) find_package(ament_cmake_python REQUIRED) find_package(Eigen3 REQUIRED) +find_package(geometry_msgs REQUIRED) include_directories(include) @@ -18,7 +19,11 @@ target_include_directories(${PROJECT_NAME} PUBLIC $ $) -ament_target_dependencies(${PROJECT_NAME} PUBLIC rclcpp Eigen3) +ament_target_dependencies(${PROJECT_NAME} PUBLIC + rclcpp + Eigen3 + geometry_msgs +) ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) diff --git a/cpp_test/CMakeLists.txt b/cpp_test/CMakeLists.txt index b2fc415..01eaf9a 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_ros_conversions.cpp ) target_link_libraries( diff --git a/cpp_test/test_ros_conversions.cpp b/cpp_test/test_ros_conversions.cpp new file mode 100644 index 0000000..16a68be --- /dev/null +++ b/cpp_test/test_ros_conversions.cpp @@ -0,0 +1,199 @@ +#include +#include +#include + +#include "vortex/utils/ros_conversions.hpp" +#include "vortex/utils/types.hpp" + +// ================================================================ +// Compile-Time Concept Tests +// ================================================================ + +struct ValidEulerPose { + double x = 0, y = 0, z = 0; + double roll = 0, pitch = 0, yaw = 0; +}; + +struct ValidEulerPoseExtra { + double x = 0, y = 0, z = 0; + double roll = 0, pitch = 0, yaw = 0; + double something_extra = 42.0; +}; + +struct ValidQuatPose { + double x = 1, y = 2, z = 3; + double qw = 1, qx = 0, qy = 0, qz = 0; +}; + +struct MissingYaw { + double x = 0, y = 0, z = 0; + double roll = 0, pitch = 0; +}; + +struct WrongType { + double x = 0, y = 0, z = 0; + std::string roll; // not convertible to double + 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"); + +static_assert(AcceptsPose, "Quaternion pose should be accepted"); + +static_assert(AcceptsPose>, + "Eigen6d should be accepted"); + +static_assert(!AcceptsPose, "MissingYaw should NOT be accepted"); + +static_assert(!AcceptsPose, "WrongType should NOT be accepted"); + +// ================================================================ +// 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); + + 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.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; + + auto pose = vortex::utils::ros_conversions::pose_like_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 expected = vortex::utils::math::euler_to_quat(1, 1, 1); + + 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); +} + +// ================================================================ +// Runtime Tests: Quaternion Pose +// ================================================================ +TEST(pose_like_to_pose_msg, quat_pose_conversion) { + ValidQuatPose qp; + + auto pose = vortex::utils::ros_conversions::pose_like_to_pose_msg(qp); + + 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.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); +} + +// ================================================================ +// 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); + + Eigen::Quaterniond expected = + vortex::utils::math::euler_to_quat(0.1, 0.2, 0.3); + + 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); +} diff --git a/include/vortex/utils/ros_conversions.hpp b/include/vortex/utils/ros_conversions.hpp new file mode 100644 index 0000000..a671adc --- /dev/null +++ b/include/vortex/utils/ros_conversions.hpp @@ -0,0 +1,174 @@ +#ifndef VORTEX_UTILS__ROS_CONVERSIONS_HPP_ +#define VORTEX_UTILS__ROS_CONVERSIONS_HPP_ + +#include +#include +#include + +#include "math.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; + + 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; +} + +} // namespace vortex::utils::ros_conversions + +#endif // VORTEX_UTILS__ROS_CONVERSIONS_HPP_ diff --git a/package.xml b/package.xml index d0f1e3d..d565156 100644 --- a/package.xml +++ b/package.xml @@ -16,6 +16,7 @@ python3-scipy geometry_msgs + ament_cmake_pytest ament_cmake_gtest