From 70c529277993b7f9abae4e2f33fb765e90b47717 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Tue, 16 Dec 2025 19:16:01 +0100 Subject: [PATCH 01/10] smol refactor --- include/vortex/utils/accessors.hpp | 76 ++++++ include/vortex/utils/concepts.hpp | 41 +++ include/vortex/utils/ops.hpp | 218 +++++++++++++++ include/vortex/utils/ros_conversions.hpp | 331 ++++++++--------------- include/vortex/utils/types.hpp | 258 +----------------- include/vortex/utils/views.hpp | 59 ++++ 6 files changed, 522 insertions(+), 461 deletions(-) create mode 100644 include/vortex/utils/accessors.hpp create mode 100644 include/vortex/utils/concepts.hpp create mode 100644 include/vortex/utils/ops.hpp create mode 100644 include/vortex/utils/views.hpp diff --git a/include/vortex/utils/accessors.hpp b/include/vortex/utils/accessors.hpp new file mode 100644 index 0000000..d8ee40e --- /dev/null +++ b/include/vortex/utils/accessors.hpp @@ -0,0 +1,76 @@ +#ifndef VORTEX_UTILS__ACCESSORS_HPP_ +#define VORTEX_UTILS__ACCESSORS_HPP_ + +#include "types.hpp" + +namespace vortex::utils::types { + +inline double x_of(const Eta& e) { + return e.x; +} +inline double y_of(const Eta& e) { + return e.y; +} +inline double z_of(const Eta& e) { + return e.z; +} + +inline double roll_of(const Eta& e) { + return e.roll; +} +inline double pitch_of(const Eta& e) { + return e.pitch; +} +inline double yaw_of(const Eta& e) { + return e.yaw; +} + +inline double x_of(const EtaQuat& e) { + return e.x; +} +inline double y_of(const EtaQuat& e) { + return e.y; +} +inline double z_of(const EtaQuat& e) { + return e.z; +} + +inline double qw_of(const EtaQuat& e) { + return e.qw; +} +inline double qx_of(const EtaQuat& e) { + return e.qx; +} +inline double qy_of(const EtaQuat& e) { + return e.qy; +} +inline double qz_of(const EtaQuat& e) { + return e.qz; +} + +inline double x_of(const PoseQuatEigen& p) { + return p.position.x(); +} +inline double y_of(const PoseQuatEigen& p) { + return p.position.y(); +} +inline double z_of(const PoseQuatEigen& p) { + return p.position.z(); +} + +inline double qw_of(const PoseQuatEigen& p) { + return p.orientation.w(); +} +inline double qx_of(const PoseQuatEigen& p) { + return p.orientation.x(); +} +inline double qy_of(const PoseQuatEigen& p) { + return p.orientation.y(); +} +inline double qz_of(const PoseQuatEigen& p) { + return p.orientation.z(); +} + +} // namespace vortex::utils::types + +#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..6440b47 --- /dev/null +++ b/include/vortex/utils/concepts.hpp @@ -0,0 +1,41 @@ +#ifndef VORTEX_UTILS__CONCEPTS_HPP_ +#define VORTEX_UTILS__CONCEPTS_HPP_ + +#include + +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; + +template +concept EulerPoseLike = PositionLike && EulerLike; + +template +concept PoseLike = QuatPoseLike || EulerPoseLike; + +} // namespace vortex::utils::concepts + +#endif // VORTEX_UTILS__CONCEPTS_HPP_ diff --git a/include/vortex/utils/ops.hpp b/include/vortex/utils/ops.hpp new file mode 100644 index 0000000..a2bea6b --- /dev/null +++ b/include/vortex/utils/ops.hpp @@ -0,0 +1,218 @@ +#ifndef VORTEX_UTILS__OPS_HPP_ +#define VORTEX_UTILS__OPS_HPP_ + +#include "math.hpp" +#include "types.hpp" +#include "views.hpp" + +namespace vortex::utils::types { + +inline EtaQuat eta_error(const EtaQuat& a, const EtaQuat& b) { + EtaQuat out{}; + out.x = a.x - b.x; + out.y = a.y - b.y; + out.z = a.z - b.z; + + const Eigen::Quaterniond qa = + vortex::utils::views::ori_quaternion(a).normalized(); + const Eigen::Quaterniond qb = + vortex::utils::views::ori_quaternion(b).normalized(); + Eigen::Quaterniond q_error = qb.conjugate() * qa; + q_error.normalize(); + + out.qw = q_error.w(); + out.qx = q_error.x(); + out.qy = q_error.y(); + out.qz = q_error.z(); + return out; +} + +inline Eta eta_error(const Eta& a, const Eta& b) { + Eta out{}; + out.x = a.x - b.x; + out.y = a.y - b.y; + out.z = a.z - b.z; + + out.roll = a.roll - b.roll; + out.pitch = a.pitch - b.pitch; + out.yaw = a.yaw - b.yaw; + + return out; +} + +/** + * @brief Apply smallest signed angle to roll, pitch, and yaw. + */ +inline void apply_ssa(Eta& e) { + using vortex::utils::math::ssa; + e.roll = ssa(e.roll); + e.pitch = ssa(e.pitch); + e.yaw = ssa(e.yaw); +} + +/** + * @brief Make the rotation matrix corresponding to eq. 2.31 in Fossen, 2021 + * @return Eigen::Matrix3d rotation matrix + */ +Eigen::Matrix3d as_rotation_matrix(const Eta& eta) { + return vortex::utils::math::get_rotation_matrix(eta.roll, eta.pitch, + eta.yaw); +} + +/** + * @brief Make the transformation matrix according to eq. 2.41 in Fossen, + * 2021 + * @return Eigen::Matrix3d transformation matrix + */ +Eigen::Matrix3d as_transformation_matrix(const Eta& eta) { + double cphi = cos(eta.roll); + double sphi = sin(eta.roll); + double ctheta = cos(eta.pitch); + double stheta = sin(eta.pitch); + + if (ctheta == 0) { + throw std::runtime_error("Division by zero in transformation matrix."); + } + + double t11 = 1; + double t12 = sphi * stheta / ctheta; + double t13 = cphi * stheta / ctheta; + double t21 = 0; + double t22 = cphi; + double t23 = -sphi; + double t31 = 0; + double t32 = sphi / ctheta; + double t33 = cphi / ctheta; + + Eigen::Matrix3d transformation_matrix; + + transformation_matrix << t11, t12, t13, t21, t22, t23, t31, t32, t33; + + return transformation_matrix; +} + +/** + * @brief Make the J matrix according to eq. 2.53 in Fossen, 2021 + * @return Eigen::Matrix6d J matrix + */ +Eigen::Matrix as_j_matrix(const Eta& eta) { + Eigen::Matrix3d rotation_matrix = as_rotation_matrix(eta); + Eigen::Matrix3d transformation_matrix = as_transformation_matrix(eta); + + Eigen::Matrix j_matrix = Eigen::Matrix::Zero(); + j_matrix.topLeftCorner<3, 3>() = rotation_matrix; + j_matrix.bottomRightCorner<3, 3>() = transformation_matrix; + + return j_matrix; +} + +/** + * @brief Convert to Eigen::Vector6d + * @return Eigen::Vector6d{x, y, z, roll, pitch, yaw} + */ +Eigen::Vector to_vector(const Eta& eta) { + return Eigen::Vector{eta.x, eta.y, eta.z, + eta.roll, eta.pitch, eta.yaw}; +} + +/** + * @brief Convert to Eta with Euler angles + * @return Eta + */ +inline EtaQuat as_eta_quat(const Eta& eta) { + Eigen::Quaterniond quat = + vortex::utils::math::euler_to_quat(eta.roll, eta.pitch, eta.yaw); + + EtaQuat eta_quat{.x = eta.x, + .y = eta.y, + .z = eta.z, + .qw = quat.w(), + .qx = quat.x(), + .qy = quat.y(), + .qz = quat.z()}; + return eta_quat; +} + +/** + * @brief Make the rotation matrix corresponding to eq. 2.31 in Fossen, 2021 + * @return Eigen::Matrix3d rotation matrix + */ +Eigen::Matrix3d as_rotation_matrix(const EtaQuat& eta) { + return vortex::utils::views::ori_quaternion(eta) + .normalized() + .toRotationMatrix(); +} + +/** + * @brief Make the transformation matrix according to eq. 2.78 in Fossen, + * 2021 + * @return Eigen::Matrix transformation matrix + */ +Eigen::Matrix as_transformation_matrix(const EtaQuat& eta) { + return vortex::utils::math::get_transformation_matrix_attitude_quat( + vortex::utils::views::ori_quaternion(eta).normalized()); +} + +/** + * @brief Make the J matrix according to eq. 2.83 in Fossen, 2021 + * @return Eigen::Matrix J matrix + */ +Eigen::Matrix as_j_matrix(const EtaQuat& eta) { + Eigen::Matrix3d R = as_rotation_matrix(eta); + Eigen::Matrix T = as_transformation_matrix(eta); + + Eigen::Matrix j_matrix = Eigen::Matrix::Zero(); + j_matrix.topLeftCorner<3, 3>() = R; + j_matrix.bottomRightCorner<4, 3>() = T; + + return j_matrix; +} + +/** + * @brief Convert to Eigen::Vector7d + * @return Eigen::Vector7d{x, y, z, qw, qx, qy, qz} + */ +Eigen::Vector to_vector(const EtaQuat& eta) { + return Eigen::Vector{eta.x, eta.y, eta.z, eta.qw, + eta.qx, eta.qy, eta.qz}; +} + +/** + * @brief Convert to Eta with Euler angles + * @return Eta + */ +inline Eta as_eta_euler(const EtaQuat& eta_quat) { + Eigen::Vector3d euler_angles = vortex::utils::math::quat_to_euler( + vortex::utils::views::ori_quaternion(eta_quat).normalized()); + + Eta eta{.x = eta.x, + .y = eta.y, + .z = eta.z, + .roll = euler_angles(0), + .pitch = euler_angles(1), + .yaw = euler_angles(2)}; + return eta; +} + +inline Nu nu_error(const Nu& nu1, const Nu& nu2) { + Nu nu; + nu.u = nu1.u - nu2.u; + nu.v = nu1.v - nu2.v; + nu.w = nu1.w - nu2.w; + nu.p = nu1.p - nu2.p; + nu.q = nu1.q - nu2.q; + nu.r = nu1.r - nu2.r; + return nu; +} + +/** + * @brief Convert to Eigen::Vector6d + * @return Eigen::Vector6d{u, v, w, p, q, r} + */ +Eigen::Vector to_vector(const Nu& nu) { + return Eigen::Vector{nu.u, nu.v, nu.w, nu.p, nu.q, nu.r}; +} + +} // namespace vortex::utils::types + +#endif // VORTEX_UTILS__OPS_HPP_ diff --git a/include/vortex/utils/ros_conversions.hpp b/include/vortex/utils/ros_conversions.hpp index b148a4f..ec8316e 100644 --- a/include/vortex/utils/ros_conversions.hpp +++ b/include/vortex/utils/ros_conversions.hpp @@ -2,268 +2,163 @@ #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; +using vortex::utils::concepts::EulerPoseLike; +using vortex::utils::concepts::PoseLike; +using vortex::utils::concepts::QuatPoseLike; +using vortex::utils::types::x_of; +using vortex::utils::types::y_of; +using vortex::utils::types::z_of; - Eigen::Quaterniond quat = - vortex::utils::math::euler_to_quat(ref.roll, ref.pitch, ref.yaw); +using vortex::utils::types::qw_of; +using vortex::utils::types::qx_of; +using vortex::utils::types::qy_of; +using vortex::utils::types::qz_of; - pose.orientation.x = quat.x(); - pose.orientation.y = quat.y(); - pose.orientation.z = quat.z(); - pose.orientation.w = quat.w(); +using vortex::utils::types::pitch_of; +using vortex::utils::types::roll_of; +using vortex::utils::types::yaw_of; - 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) { +template +geometry_msgs::msg::Pose pose_like_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; - - pose.orientation.x = ref.qx; - pose.orientation.y = ref.qy; - pose.orientation.z = ref.qz; - pose.orientation.w = ref.qw; + 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(); + } 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)); +template + requires PoseLike> +std::vector pose_like_to_pose_msgs(const R& poses) { + std::vector out; - pose.orientation.x = q.x(); - pose.orientation.y = q.y(); - pose.orientation.z = q.z(); - pose.orientation.w = q.w(); + if constexpr (std::ranges::sized_range) { + out.reserve(std::ranges::size(poses)); + } - return pose; + for (const auto& p : poses) { + out.push_back(pose_like_to_pose_msg(p)); + } + return out; } -/** - * @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; - -/** - * @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; + same_bare_as || + same_bare_as; - // 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); +inline Eigen::Matrix ros_pose_to_eigen6d( + const geometry_msgs::msg::Pose& msg) { + Eigen::Matrix v; - const Eigen::Vector3d euler = vortex::utils::math::quat_to_euler(q); - v.tail<3>() = euler; - - return v; - } + v(0) = msg.position.x; + v(1) = msg.position.y; + v(2) = msg.position.z; - else if constexpr (same_bare_as) { - return ros_to_eigen6d(msg.pose); - } + Eigen::Quaterniond q(msg.orientation.w, msg.orientation.x, + msg.orientation.y, msg.orientation.z); - else if constexpr (same_bare_as< - T, geometry_msgs::msg::PoseWithCovarianceStamped>) { - return ros_to_eigen6d(msg.pose.pose); - } + v.tail<3>() = vortex::utils::math::quat_to_euler(q); + return v; +} - else if constexpr (same_bare_as) { +template +Eigen::Matrix ros_to_eigen6d(const T& msg) { + if constexpr (same_bare_as) { + return ros_pose_to_eigen6d(msg); + } else if constexpr (same_bare_as) { + return ros_pose_to_eigen6d(msg.pose); + } else if constexpr (same_bare_as< + T, + geometry_msgs::msg::PoseWithCovarianceStamped>) { + return ros_pose_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); + X.col(i) = ros_pose_to_eigen6d(pose); }); return X; } } +using PoseQuatEigen = vortex::utils::types::PoseQuatEigen; + +inline PoseQuatEigen ros_pose_to_pose_quat( + const geometry_msgs::msg::Pose& pose) { + PoseQuatEigen p; + p.position = {pose.position.x, pose.position.y, pose.position.z}; + + p.orientation = Eigen::Quaterniond(pose.orientation.w, pose.orientation.x, + pose.orientation.y, pose.orientation.z) + .normalized(); + + return p; +} + +template +std::vector ros_to_pose_quat(const T& msg) { + if constexpr (same_bare_as) { + return {ros_pose_to_pose_quat(msg)}; + } else if constexpr (same_bare_as) { + return ros_to_pose_quat(msg.pose); + } else if constexpr (same_bare_as< + T, + geometry_msgs::msg::PoseWithCovarianceStamped>) { + return ros_to_pose_quat(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_quat(pose)); + } + return poses; + } +} + } // namespace vortex::utils::ros_conversions #endif // VORTEX_UTILS__ROS_CONVERSIONS_HPP_ diff --git a/include/vortex/utils/types.hpp b/include/vortex/utils/types.hpp index 49e0af2..83dec17 100644 --- a/include/vortex/utils/types.hpp +++ b/include/vortex/utils/types.hpp @@ -1,9 +1,8 @@ -#ifndef VORTEX_UTILS_TYPES_HPP -#define VORTEX_UTILS_TYPES_HPP +#ifndef VORTEX_UTILS__POSE_TYPES_HPP_ +#define VORTEX_UTILS__POSE_TYPES_HPP_ #include -#include -#include "math.hpp" +#include namespace vortex::utils::types { @@ -11,14 +10,6 @@ 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. */ -struct Eta; - -/** - * @brief Struct to represent the state vector eta according to eq. 2.3 in - * Fossen, 2021, containing the position and orientation as quaternion. - */ -struct EtaQuat; - struct Eta { double x{}; double y{}; @@ -26,111 +17,12 @@ struct Eta { double roll{}; double pitch{}; double yaw{}; - - Eta operator-(const Eta& other) const { - Eta eta; - eta.x = x - other.x; - eta.y = y - other.y; - eta.z = z - other.z; - eta.roll = roll - other.roll; - eta.pitch = pitch - other.pitch; - eta.yaw = yaw - other.yaw; - return eta; - } - - /** - * @brief Get the position vector (x, y, z). - * @return Eigen::Vector3d{x, y, z} - */ - Eigen::Vector3d pos_vector() const { return Eigen::Vector3d{x, y, z}; } - - /** - * @brief Get the orientation vector (roll, pitch, yaw). - * @return Eigen::Vector3d{roll, pitch, yaw} - */ - Eigen::Vector3d ori_vector() const { - return Eigen::Vector3d{roll, pitch, yaw}; - } - - /** - * @brief Convert to Eigen::Vector6d - * @return Eigen::Vector6d{x, y, z, roll, pitch, yaw} - */ - Eigen::Vector to_vector() const { - return Eigen::Vector{x, y, z, roll, pitch, yaw}; - } - - /** - * @brief Apply smallest signed angle to roll, pitch, and yaw. - */ - void apply_ssa() { - roll = vortex::utils::math::ssa(roll); - pitch = vortex::utils::math::ssa(pitch); - yaw = vortex::utils::math::ssa(yaw); - } - - /** - * @brief Make the rotation matrix corresponding to eq. 2.31 in Fossen, 2021 - * @return Eigen::Matrix3d rotation matrix - */ - Eigen::Matrix3d as_rotation_matrix() const { - return vortex::utils::math::get_rotation_matrix(roll, pitch, yaw); - } - /** - * @brief Make the transformation matrix according to eq. 2.41 in Fossen, - * 2021 - * @return Eigen::Matrix3d transformation matrix - */ - Eigen::Matrix3d as_transformation_matrix() const { - double cphi = cos(roll); - double sphi = sin(roll); - double ctheta = cos(pitch); - double stheta = sin(pitch); - - if (ctheta == 0) { - throw std::runtime_error( - "Division by zero in transformation matrix."); - } - - double t11 = 1; - double t12 = sphi * stheta / ctheta; - double t13 = cphi * stheta / ctheta; - double t21 = 0; - double t22 = cphi; - double t23 = -sphi; - double t31 = 0; - double t32 = sphi / ctheta; - double t33 = cphi / ctheta; - - Eigen::Matrix3d transformation_matrix; - - transformation_matrix << t11, t12, t13, t21, t22, t23, t31, t32, t33; - - return transformation_matrix; - } - - /** - * @brief Make the J matrix according to eq. 2.53 in Fossen, 2021 - * @return Eigen::Matrix6d J matrix - */ - Eigen::Matrix as_j_matrix() const { - Eigen::Matrix3d rotation_matrix = as_rotation_matrix(); - Eigen::Matrix3d transformation_matrix = as_transformation_matrix(); - - Eigen::Matrix j_matrix = - Eigen::Matrix::Zero(); - j_matrix.topLeftCorner<3, 3>() = rotation_matrix; - j_matrix.bottomRightCorner<3, 3>() = transformation_matrix; - - return j_matrix; - } - /** - * @brief Convert to EtaQuat - * @return EtaQuat - */ - EtaQuat as_eta_quat() const; }; +/** + * @brief Struct to represent the state vector eta according to eq. 2.3 in + * Fossen, 2021, containing the position and orientation as quaternion. + */ struct EtaQuat { double x{}; double y{}; @@ -139,88 +31,14 @@ struct EtaQuat { double qx{}; double qy{}; double qz{}; +}; - /** - * @brief Get the position vector (x, y, z). - * @return Eigen::Vector3d{x, y, z} - */ - Eigen::Vector3d pos_vector() const { return Eigen::Vector3d{x, y, z}; } - - /** - * @brief Get the orientation as Eigen::Quaterniond. - * @return Eigen::Quaterniond - */ - Eigen::Quaterniond ori_quaternion() const { - Eigen::Quaterniond quat; - quat.w() = qw; - quat.x() = qx; - quat.y() = qy; - quat.z() = qz; - return quat.normalized(); - } - - /** - * @brief Convert to Eigen::Vector7d - * @return Eigen::Vector7d{x, y, z, qw, qx, qy, qz} - */ - Eigen::Vector to_vector() const { - return Eigen::Vector{x, y, z, qw, qx, qy, qz}; - } - - EtaQuat operator-(const EtaQuat& other) const { - EtaQuat eta; - eta.x = x - other.x; - eta.y = y - other.y; - eta.z = z - other.z; - Eigen::Quaterniond q1 = ori_quaternion(); - Eigen::Quaterniond q2 = other.ori_quaternion(); - Eigen::Quaterniond q_error = q2.conjugate() * q1; - q_error.normalize(); - eta.qw = q_error.w(); - eta.qx = q_error.x(); - eta.qy = q_error.y(); - eta.qz = q_error.z(); - return eta; - } - - /** - * @brief Make the rotation matrix corresponding to eq. 2.31 in Fossen, 2021 - * @return Eigen::Matrix3d rotation matrix - */ - Eigen::Matrix3d as_rotation_matrix() const { - return ori_quaternion().toRotationMatrix(); - } - - /** - * @brief Make the transformation matrix according to eq. 2.78 in Fossen, - * 2021 - * @return Eigen::Matrix transformation matrix - */ - Eigen::Matrix as_transformation_matrix() const { - return vortex::utils::math::get_transformation_matrix_attitude_quat( - ori_quaternion()); - } - - /** - * @brief Make the J matrix according to eq. 2.83 in Fossen, 2021 - * @return Eigen::Matrix J matrix - */ - Eigen::Matrix as_j_matrix() const { - Eigen::Matrix3d R = as_rotation_matrix(); - Eigen::Matrix T = as_transformation_matrix(); - - Eigen::Matrix j_matrix = - Eigen::Matrix::Zero(); - j_matrix.topLeftCorner<3, 3>() = R; - j_matrix.bottomRightCorner<4, 3>() = T; - - return j_matrix; - } - /** - * @brief Convert to Eta with Euler angles - * @return Eta - */ - Eta as_eta_euler() const; +/** + * @brief Eigen-based pose, for interfaces / measurements + */ +struct PoseQuatEigen { + Eigen::Vector3d position; + Eigen::Quaterniond orientation; }; /** @@ -234,54 +52,8 @@ struct Nu { double p{}; double q{}; double r{}; - - Nu operator-(const Nu& other) const { - Nu nu; - nu.u = u - other.u; - nu.v = v - other.v; - nu.w = w - other.w; - nu.p = p - other.p; - nu.q = q - other.q; - nu.r = r - other.r; - return nu; - } - - /** - * @brief Convert to Eigen::Vector6d - * @return Eigen::Vector6d{u, v, w, p, q, r} - */ - Eigen::Vector to_vector() const { - return Eigen::Vector{u, v, w, p, q, r}; - } }; -inline EtaQuat Eta::as_eta_quat() 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()}; - return eta_quat; -} - -inline Eta EtaQuat::as_eta_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)}; - return eta; -} - } // namespace vortex::utils::types -#endif // VORTEX_UTILS_TYPES_HPP +#endif // VORTEX_UTILS__POSE_TYPES_HPP_ diff --git a/include/vortex/utils/views.hpp b/include/vortex/utils/views.hpp new file mode 100644 index 0000000..f6ac9d6 --- /dev/null +++ b/include/vortex/utils/views.hpp @@ -0,0 +1,59 @@ +#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; + +using vortex::utils::types::x_of; +using vortex::utils::types::y_of; +using vortex::utils::types::z_of; + +using vortex::utils::types::qw_of; +using vortex::utils::types::qx_of; +using vortex::utils::types::qy_of; +using vortex::utils::types::qz_of; + +using vortex::utils::types::pitch_of; +using vortex::utils::types::roll_of; +using vortex::utils::types::yaw_of; + +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_ From 2898841a0f499b8125ef0ac0441084492e76a2e7 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Wed, 17 Dec 2025 14:48:29 +0100 Subject: [PATCH 02/10] reverted eta changes --- include/vortex/utils/accessors.hpp | 28 +-- include/vortex/utils/ops.hpp | 218 ------------------ include/vortex/utils/ros_conversions.hpp | 24 +- include/vortex/utils/types.hpp | 270 +++++++++++++++++++++-- 4 files changed, 281 insertions(+), 259 deletions(-) delete mode 100644 include/vortex/utils/ops.hpp diff --git a/include/vortex/utils/accessors.hpp b/include/vortex/utils/accessors.hpp index d8ee40e..d0d6da6 100644 --- a/include/vortex/utils/accessors.hpp +++ b/include/vortex/utils/accessors.hpp @@ -48,27 +48,27 @@ inline double qz_of(const EtaQuat& e) { return e.qz; } -inline double x_of(const PoseQuatEigen& p) { - return p.position.x(); +inline double x_of(const Pose& p) { + return p.x; } -inline double y_of(const PoseQuatEigen& p) { - return p.position.y(); +inline double y_of(const Pose& p) { + return p.y; } -inline double z_of(const PoseQuatEigen& p) { - return p.position.z(); +inline double z_of(const Pose& p) { + return p.z; } -inline double qw_of(const PoseQuatEigen& p) { - return p.orientation.w(); +inline double qw_of(const Pose& p) { + return p.qw; } -inline double qx_of(const PoseQuatEigen& p) { - return p.orientation.x(); +inline double qx_of(const Pose& p) { + return p.qx; } -inline double qy_of(const PoseQuatEigen& p) { - return p.orientation.y(); +inline double qy_of(const Pose& p) { + return p.qy; } -inline double qz_of(const PoseQuatEigen& p) { - return p.orientation.z(); +inline double qz_of(const Pose& p) { + return p.qz; } } // namespace vortex::utils::types diff --git a/include/vortex/utils/ops.hpp b/include/vortex/utils/ops.hpp deleted file mode 100644 index a2bea6b..0000000 --- a/include/vortex/utils/ops.hpp +++ /dev/null @@ -1,218 +0,0 @@ -#ifndef VORTEX_UTILS__OPS_HPP_ -#define VORTEX_UTILS__OPS_HPP_ - -#include "math.hpp" -#include "types.hpp" -#include "views.hpp" - -namespace vortex::utils::types { - -inline EtaQuat eta_error(const EtaQuat& a, const EtaQuat& b) { - EtaQuat out{}; - out.x = a.x - b.x; - out.y = a.y - b.y; - out.z = a.z - b.z; - - const Eigen::Quaterniond qa = - vortex::utils::views::ori_quaternion(a).normalized(); - const Eigen::Quaterniond qb = - vortex::utils::views::ori_quaternion(b).normalized(); - Eigen::Quaterniond q_error = qb.conjugate() * qa; - q_error.normalize(); - - out.qw = q_error.w(); - out.qx = q_error.x(); - out.qy = q_error.y(); - out.qz = q_error.z(); - return out; -} - -inline Eta eta_error(const Eta& a, const Eta& b) { - Eta out{}; - out.x = a.x - b.x; - out.y = a.y - b.y; - out.z = a.z - b.z; - - out.roll = a.roll - b.roll; - out.pitch = a.pitch - b.pitch; - out.yaw = a.yaw - b.yaw; - - return out; -} - -/** - * @brief Apply smallest signed angle to roll, pitch, and yaw. - */ -inline void apply_ssa(Eta& e) { - using vortex::utils::math::ssa; - e.roll = ssa(e.roll); - e.pitch = ssa(e.pitch); - e.yaw = ssa(e.yaw); -} - -/** - * @brief Make the rotation matrix corresponding to eq. 2.31 in Fossen, 2021 - * @return Eigen::Matrix3d rotation matrix - */ -Eigen::Matrix3d as_rotation_matrix(const Eta& eta) { - return vortex::utils::math::get_rotation_matrix(eta.roll, eta.pitch, - eta.yaw); -} - -/** - * @brief Make the transformation matrix according to eq. 2.41 in Fossen, - * 2021 - * @return Eigen::Matrix3d transformation matrix - */ -Eigen::Matrix3d as_transformation_matrix(const Eta& eta) { - double cphi = cos(eta.roll); - double sphi = sin(eta.roll); - double ctheta = cos(eta.pitch); - double stheta = sin(eta.pitch); - - if (ctheta == 0) { - throw std::runtime_error("Division by zero in transformation matrix."); - } - - double t11 = 1; - double t12 = sphi * stheta / ctheta; - double t13 = cphi * stheta / ctheta; - double t21 = 0; - double t22 = cphi; - double t23 = -sphi; - double t31 = 0; - double t32 = sphi / ctheta; - double t33 = cphi / ctheta; - - Eigen::Matrix3d transformation_matrix; - - transformation_matrix << t11, t12, t13, t21, t22, t23, t31, t32, t33; - - return transformation_matrix; -} - -/** - * @brief Make the J matrix according to eq. 2.53 in Fossen, 2021 - * @return Eigen::Matrix6d J matrix - */ -Eigen::Matrix as_j_matrix(const Eta& eta) { - Eigen::Matrix3d rotation_matrix = as_rotation_matrix(eta); - Eigen::Matrix3d transformation_matrix = as_transformation_matrix(eta); - - Eigen::Matrix j_matrix = Eigen::Matrix::Zero(); - j_matrix.topLeftCorner<3, 3>() = rotation_matrix; - j_matrix.bottomRightCorner<3, 3>() = transformation_matrix; - - return j_matrix; -} - -/** - * @brief Convert to Eigen::Vector6d - * @return Eigen::Vector6d{x, y, z, roll, pitch, yaw} - */ -Eigen::Vector to_vector(const Eta& eta) { - return Eigen::Vector{eta.x, eta.y, eta.z, - eta.roll, eta.pitch, eta.yaw}; -} - -/** - * @brief Convert to Eta with Euler angles - * @return Eta - */ -inline EtaQuat as_eta_quat(const Eta& eta) { - Eigen::Quaterniond quat = - vortex::utils::math::euler_to_quat(eta.roll, eta.pitch, eta.yaw); - - EtaQuat eta_quat{.x = eta.x, - .y = eta.y, - .z = eta.z, - .qw = quat.w(), - .qx = quat.x(), - .qy = quat.y(), - .qz = quat.z()}; - return eta_quat; -} - -/** - * @brief Make the rotation matrix corresponding to eq. 2.31 in Fossen, 2021 - * @return Eigen::Matrix3d rotation matrix - */ -Eigen::Matrix3d as_rotation_matrix(const EtaQuat& eta) { - return vortex::utils::views::ori_quaternion(eta) - .normalized() - .toRotationMatrix(); -} - -/** - * @brief Make the transformation matrix according to eq. 2.78 in Fossen, - * 2021 - * @return Eigen::Matrix transformation matrix - */ -Eigen::Matrix as_transformation_matrix(const EtaQuat& eta) { - return vortex::utils::math::get_transformation_matrix_attitude_quat( - vortex::utils::views::ori_quaternion(eta).normalized()); -} - -/** - * @brief Make the J matrix according to eq. 2.83 in Fossen, 2021 - * @return Eigen::Matrix J matrix - */ -Eigen::Matrix as_j_matrix(const EtaQuat& eta) { - Eigen::Matrix3d R = as_rotation_matrix(eta); - Eigen::Matrix T = as_transformation_matrix(eta); - - Eigen::Matrix j_matrix = Eigen::Matrix::Zero(); - j_matrix.topLeftCorner<3, 3>() = R; - j_matrix.bottomRightCorner<4, 3>() = T; - - return j_matrix; -} - -/** - * @brief Convert to Eigen::Vector7d - * @return Eigen::Vector7d{x, y, z, qw, qx, qy, qz} - */ -Eigen::Vector to_vector(const EtaQuat& eta) { - return Eigen::Vector{eta.x, eta.y, eta.z, eta.qw, - eta.qx, eta.qy, eta.qz}; -} - -/** - * @brief Convert to Eta with Euler angles - * @return Eta - */ -inline Eta as_eta_euler(const EtaQuat& eta_quat) { - Eigen::Vector3d euler_angles = vortex::utils::math::quat_to_euler( - vortex::utils::views::ori_quaternion(eta_quat).normalized()); - - Eta eta{.x = eta.x, - .y = eta.y, - .z = eta.z, - .roll = euler_angles(0), - .pitch = euler_angles(1), - .yaw = euler_angles(2)}; - return eta; -} - -inline Nu nu_error(const Nu& nu1, const Nu& nu2) { - Nu nu; - nu.u = nu1.u - nu2.u; - nu.v = nu1.v - nu2.v; - nu.w = nu1.w - nu2.w; - nu.p = nu1.p - nu2.p; - nu.q = nu1.q - nu2.q; - nu.r = nu1.r - nu2.r; - return nu; -} - -/** - * @brief Convert to Eigen::Vector6d - * @return Eigen::Vector6d{u, v, w, p, q, r} - */ -Eigen::Vector to_vector(const Nu& nu) { - return Eigen::Vector{nu.u, nu.v, nu.w, nu.p, nu.q, nu.r}; -} - -} // namespace vortex::utils::types - -#endif // VORTEX_UTILS__OPS_HPP_ diff --git a/include/vortex/utils/ros_conversions.hpp b/include/vortex/utils/ros_conversions.hpp index ec8316e..0787d9e 100644 --- a/include/vortex/utils/ros_conversions.hpp +++ b/include/vortex/utils/ros_conversions.hpp @@ -124,22 +124,22 @@ Eigen::Matrix ros_to_eigen6d(const T& msg) { } } -using PoseQuatEigen = vortex::utils::types::PoseQuatEigen; - -inline PoseQuatEigen ros_pose_to_pose_quat( +inline vortex::utils::types::Pose ros_pose_to_pose_quat( const geometry_msgs::msg::Pose& pose) { - PoseQuatEigen p; - p.position = {pose.position.x, pose.position.y, pose.position.z}; - - p.orientation = Eigen::Quaterniond(pose.orientation.w, pose.orientation.x, - pose.orientation.y, pose.orientation.z) - .normalized(); - + 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; } template -std::vector ros_to_pose_quat(const T& msg) { +std::vector ros_to_pose_quat(const T& msg) { if constexpr (same_bare_as) { return {ros_pose_to_pose_quat(msg)}; } else if constexpr (same_bare_as) { @@ -149,7 +149,7 @@ std::vector ros_to_pose_quat(const T& msg) { geometry_msgs::msg::PoseWithCovarianceStamped>) { return ros_to_pose_quat(msg.pose.pose); } else if constexpr (same_bare_as) { - std::vector poses; + std::vector poses; poses.reserve(msg.poses.size()); for (const auto& pose : msg.poses) { diff --git a/include/vortex/utils/types.hpp b/include/vortex/utils/types.hpp index 83dec17..75b8161 100644 --- a/include/vortex/utils/types.hpp +++ b/include/vortex/utils/types.hpp @@ -1,8 +1,9 @@ -#ifndef VORTEX_UTILS__POSE_TYPES_HPP_ -#define VORTEX_UTILS__POSE_TYPES_HPP_ +#ifndef VORTEX_UTILS_TYPES_HPP +#define VORTEX_UTILS_TYPES_HPP #include -#include +#include +#include "math.hpp" namespace vortex::utils::types { @@ -10,6 +11,16 @@ 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. */ +struct Eta; + +/** + * @brief Struct to represent the state vector eta according to eq. 2.3 in + * Fossen, 2021, containing the position and orientation as quaternion. + */ +struct EtaQuat; + +struct Pose; + struct Eta { double x{}; double y{}; @@ -17,12 +28,111 @@ struct Eta { double roll{}; double pitch{}; double yaw{}; + + Eta operator-(const Eta& other) const { + Eta eta; + eta.x = x - other.x; + eta.y = y - other.y; + eta.z = z - other.z; + eta.roll = roll - other.roll; + eta.pitch = pitch - other.pitch; + eta.yaw = yaw - other.yaw; + return eta; + } + + /** + * @brief Get the position vector (x, y, z). + * @return Eigen::Vector3d{x, y, z} + */ + Eigen::Vector3d pos_vector() const { return Eigen::Vector3d{x, y, z}; } + + /** + * @brief Get the orientation vector (roll, pitch, yaw). + * @return Eigen::Vector3d{roll, pitch, yaw} + */ + Eigen::Vector3d ori_vector() const { + return Eigen::Vector3d{roll, pitch, yaw}; + } + + /** + * @brief Convert to Eigen::Vector6d + * @return Eigen::Vector6d{x, y, z, roll, pitch, yaw} + */ + Eigen::Vector to_vector() const { + return Eigen::Vector{x, y, z, roll, pitch, yaw}; + } + + /** + * @brief Apply smallest signed angle to roll, pitch, and yaw. + */ + void apply_ssa() { + roll = vortex::utils::math::ssa(roll); + pitch = vortex::utils::math::ssa(pitch); + yaw = vortex::utils::math::ssa(yaw); + } + + /** + * @brief Make the rotation matrix corresponding to eq. 2.31 in Fossen, 2021 + * @return Eigen::Matrix3d rotation matrix + */ + Eigen::Matrix3d as_rotation_matrix() const { + return vortex::utils::math::get_rotation_matrix(roll, pitch, yaw); + } + /** + * @brief Make the transformation matrix according to eq. 2.41 in Fossen, + * 2021 + * @return Eigen::Matrix3d transformation matrix + */ + Eigen::Matrix3d as_transformation_matrix() const { + double cphi = cos(roll); + double sphi = sin(roll); + double ctheta = cos(pitch); + double stheta = sin(pitch); + + if (ctheta == 0) { + throw std::runtime_error( + "Division by zero in transformation matrix."); + } + + double t11 = 1; + double t12 = sphi * stheta / ctheta; + double t13 = cphi * stheta / ctheta; + double t21 = 0; + double t22 = cphi; + double t23 = -sphi; + double t31 = 0; + double t32 = sphi / ctheta; + double t33 = cphi / ctheta; + + Eigen::Matrix3d transformation_matrix; + + transformation_matrix << t11, t12, t13, t21, t22, t23, t31, t32, t33; + + return transformation_matrix; + } + + /** + * @brief Make the J matrix according to eq. 2.53 in Fossen, 2021 + * @return Eigen::Matrix6d J matrix + */ + Eigen::Matrix as_j_matrix() const { + Eigen::Matrix3d rotation_matrix = as_rotation_matrix(); + Eigen::Matrix3d transformation_matrix = as_transformation_matrix(); + + Eigen::Matrix j_matrix = + Eigen::Matrix::Zero(); + j_matrix.topLeftCorner<3, 3>() = rotation_matrix; + j_matrix.bottomRightCorner<3, 3>() = transformation_matrix; + + return j_matrix; + } + /** + * @brief Convert to EtaQuat + * @return EtaQuat + */ + EtaQuat as_eta_quat() const; }; -/** - * @brief Struct to represent the state vector eta according to eq. 2.3 in - * Fossen, 2021, containing the position and orientation as quaternion. - */ struct EtaQuat { double x{}; double y{}; @@ -31,14 +141,88 @@ struct EtaQuat { double qx{}; double qy{}; double qz{}; -}; -/** - * @brief Eigen-based pose, for interfaces / measurements - */ -struct PoseQuatEigen { - Eigen::Vector3d position; - Eigen::Quaterniond orientation; + /** + * @brief Get the position vector (x, y, z). + * @return Eigen::Vector3d{x, y, z} + */ + Eigen::Vector3d pos_vector() const { return Eigen::Vector3d{x, y, z}; } + + /** + * @brief Get the orientation as Eigen::Quaterniond. + * @return Eigen::Quaterniond + */ + Eigen::Quaterniond ori_quaternion() const { + Eigen::Quaterniond quat; + quat.w() = qw; + quat.x() = qx; + quat.y() = qy; + quat.z() = qz; + return quat.normalized(); + } + + /** + * @brief Convert to Eigen::Vector7d + * @return Eigen::Vector7d{x, y, z, qw, qx, qy, qz} + */ + Eigen::Vector to_vector() const { + return Eigen::Vector{x, y, z, qw, qx, qy, qz}; + } + + EtaQuat operator-(const EtaQuat& other) const { + EtaQuat eta; + eta.x = x - other.x; + eta.y = y - other.y; + eta.z = z - other.z; + Eigen::Quaterniond q1 = ori_quaternion(); + Eigen::Quaterniond q2 = other.ori_quaternion(); + Eigen::Quaterniond q_error = q2.conjugate() * q1; + q_error.normalize(); + eta.qw = q_error.w(); + eta.qx = q_error.x(); + eta.qy = q_error.y(); + eta.qz = q_error.z(); + return eta; + } + + /** + * @brief Make the rotation matrix corresponding to eq. 2.31 in Fossen, 2021 + * @return Eigen::Matrix3d rotation matrix + */ + Eigen::Matrix3d as_rotation_matrix() const { + return ori_quaternion().toRotationMatrix(); + } + + /** + * @brief Make the transformation matrix according to eq. 2.78 in Fossen, + * 2021 + * @return Eigen::Matrix transformation matrix + */ + Eigen::Matrix as_transformation_matrix() const { + return vortex::utils::math::get_transformation_matrix_attitude_quat( + ori_quaternion()); + } + + /** + * @brief Make the J matrix according to eq. 2.83 in Fossen, 2021 + * @return Eigen::Matrix J matrix + */ + Eigen::Matrix as_j_matrix() const { + Eigen::Matrix3d R = as_rotation_matrix(); + Eigen::Matrix T = as_transformation_matrix(); + + Eigen::Matrix j_matrix = + Eigen::Matrix::Zero(); + j_matrix.topLeftCorner<3, 3>() = R; + j_matrix.bottomRightCorner<4, 3>() = T; + + return j_matrix; + } + /** + * @brief Convert to Eta with Euler angles + * @return Eta + */ + Eta as_eta_euler() const; }; /** @@ -52,8 +236,64 @@ struct Nu { double p{}; double q{}; double r{}; + + Nu operator-(const Nu& other) const { + Nu nu; + nu.u = u - other.u; + nu.v = v - other.v; + nu.w = w - other.w; + nu.p = p - other.p; + nu.q = q - other.q; + nu.r = r - other.r; + return nu; + } + + /** + * @brief Convert to Eigen::Vector6d + * @return Eigen::Vector6d{u, v, w, p, q, r} + */ + Eigen::Vector to_vector() const { + return Eigen::Vector{u, v, w, p, q, r}; + } +}; + +inline EtaQuat Eta::as_eta_quat() 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()}; + return eta_quat; +} + +inline Eta EtaQuat::as_eta_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)}; + 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__POSE_TYPES_HPP_ +#endif // VORTEX_UTILS_TYPES_HPP From c37e8af12e6a8eb95b4d0310a5942ff10018fb82 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Wed, 17 Dec 2025 20:30:51 +0100 Subject: [PATCH 03/10] member concepts --- include/vortex/utils/member_concepts.hpp | 32 ++++++++++++++++++++++++ 1 file changed, 32 insertions(+) create mode 100644 include/vortex/utils/member_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_ From 948d0b5936aff73ddcb487ad8deacba7abf913f9 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Wed, 17 Dec 2025 20:31:27 +0100 Subject: [PATCH 04/10] member concept accessors --- include/vortex/utils/accessors.hpp | 90 ++++++++++++------------------ 1 file changed, 37 insertions(+), 53 deletions(-) diff --git a/include/vortex/utils/accessors.hpp b/include/vortex/utils/accessors.hpp index d0d6da6..79f4a43 100644 --- a/include/vortex/utils/accessors.hpp +++ b/include/vortex/utils/accessors.hpp @@ -1,76 +1,60 @@ #ifndef VORTEX_UTILS__ACCESSORS_HPP_ #define VORTEX_UTILS__ACCESSORS_HPP_ -#include "types.hpp" +#include "member_concepts.hpp" -namespace vortex::utils::types { +namespace vortex::utils { -inline double x_of(const Eta& e) { - return e.x; -} -inline double y_of(const Eta& e) { - return e.y; -} -inline double z_of(const Eta& e) { - return e.z; +template +constexpr double x_of(const T& t) { + return t.x; } -inline double roll_of(const Eta& e) { - return e.roll; -} -inline double pitch_of(const Eta& e) { - return e.pitch; -} -inline double yaw_of(const Eta& e) { - return e.yaw; +template +constexpr double y_of(const T& t) { + return t.y; } -inline double x_of(const EtaQuat& e) { - return e.x; -} -inline double y_of(const EtaQuat& e) { - return e.y; -} -inline double z_of(const EtaQuat& e) { - return e.z; +template +constexpr double z_of(const T& t) { + return t.z; } -inline double qw_of(const EtaQuat& e) { - return e.qw; -} -inline double qx_of(const EtaQuat& e) { - return e.qx; -} -inline double qy_of(const EtaQuat& e) { - return e.qy; -} -inline double qz_of(const EtaQuat& e) { - return e.qz; +template +constexpr double roll_of(const T& t) { + return t.roll; } -inline double x_of(const Pose& p) { - return p.x; +template +constexpr double pitch_of(const T& t) { + return t.pitch; } -inline double y_of(const Pose& p) { - return p.y; -} -inline double z_of(const Pose& p) { - return p.z; + +template +constexpr double yaw_of(const T& t) { + return t.yaw; } -inline double qw_of(const Pose& p) { - return p.qw; +template +constexpr double qw_of(const T& q) { + return q.qw; } -inline double qx_of(const Pose& p) { - return p.qx; + +template +constexpr double qx_of(const T& q) { + return q.qx; } -inline double qy_of(const Pose& p) { - return p.qy; + +template +constexpr double qy_of(const T& q) { + return q.qy; } -inline double qz_of(const Pose& p) { - return p.qz; + +template +constexpr double qz_of(const T& q) { + return q.qz; } -} // namespace vortex::utils::types +} // namespace vortex::utils #endif // VORTEX_UTILS__ACCESSORS_HPP_ From 0ff4017c49d4b03de295b21d859d002ad416b0b6 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Wed, 17 Dec 2025 20:33:05 +0100 Subject: [PATCH 05/10] removed eigen6d conversion for Pose struct --- cpp_test/CMakeLists.txt | 1 + cpp_test/test_concepts.cpp | 161 ++++++++++ cpp_test/test_ros_conversions.cpp | 389 +++++------------------ include/vortex/utils/concepts.hpp | 5 +- include/vortex/utils/ros_conversions.hpp | 84 +---- include/vortex/utils/views.hpp | 13 - 6 files changed, 267 insertions(+), 386 deletions(-) create mode 100644 cpp_test/test_concepts.cpp 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/concepts.hpp b/include/vortex/utils/concepts.hpp index 6440b47..d9acabe 100644 --- a/include/vortex/utils/concepts.hpp +++ b/include/vortex/utils/concepts.hpp @@ -2,6 +2,7 @@ #define VORTEX_UTILS__CONCEPTS_HPP_ #include +#include "accessors.hpp" namespace vortex::utils::concepts { @@ -28,10 +29,10 @@ concept EulerLike = requires(const T& t) { }; template -concept QuatPoseLike = PositionLike && QuaternionLike; +concept QuatPoseLike = PositionLike && QuaternionLike && (!EulerLike); template -concept EulerPoseLike = PositionLike && EulerLike; +concept EulerPoseLike = PositionLike && EulerLike && (!QuaternionLike); template concept PoseLike = QuatPoseLike || EulerPoseLike; diff --git a/include/vortex/utils/ros_conversions.hpp b/include/vortex/utils/ros_conversions.hpp index 0787d9e..1a2bfec 100644 --- a/include/vortex/utils/ros_conversions.hpp +++ b/include/vortex/utils/ros_conversions.hpp @@ -23,21 +23,19 @@ namespace vortex::utils::ros_conversions { using vortex::utils::concepts::EulerPoseLike; using vortex::utils::concepts::PoseLike; using vortex::utils::concepts::QuatPoseLike; -using vortex::utils::types::x_of; -using vortex::utils::types::y_of; -using vortex::utils::types::z_of; -using vortex::utils::types::qw_of; -using vortex::utils::types::qx_of; -using vortex::utils::types::qy_of; -using vortex::utils::types::qz_of; +template +concept same_bare_as = std::same_as, U>; -using vortex::utils::types::pitch_of; -using vortex::utils::types::roll_of; -using vortex::utils::types::yaw_of; +template +concept ROSPoseLike = + same_bare_as || + same_bare_as || + same_bare_as || + same_bare_as; template -geometry_msgs::msg::Pose pose_like_to_pose_msg(const T& pose_like) { +geometry_msgs::msg::Pose to_pose_msg(const T& pose_like) { geometry_msgs::msg::Pose pose; pose.position.x = x_of(pose_like); @@ -63,7 +61,7 @@ geometry_msgs::msg::Pose pose_like_to_pose_msg(const T& pose_like) { template requires PoseLike> -std::vector pose_like_to_pose_msgs(const R& poses) { +std::vector to_pose_msgs(const R& poses) { std::vector out; if constexpr (std::ranges::sized_range) { @@ -71,60 +69,12 @@ std::vector pose_like_to_pose_msgs(const R& poses) { } for (const auto& p : poses) { - out.push_back(pose_like_to_pose_msg(p)); + out.push_back(to_pose_msg(p)); } return out; } -template -concept same_bare_as = std::same_as, U>; - -template -concept ROSPoseLike = - same_bare_as || - same_bare_as || - same_bare_as || - same_bare_as; - -inline Eigen::Matrix ros_pose_to_eigen6d( - const geometry_msgs::msg::Pose& msg) { - Eigen::Matrix v; - - v(0) = msg.position.x; - v(1) = msg.position.y; - v(2) = msg.position.z; - - Eigen::Quaterniond q(msg.orientation.w, msg.orientation.x, - msg.orientation.y, msg.orientation.z); - - v.tail<3>() = vortex::utils::math::quat_to_euler(q); - return v; -} - -template -Eigen::Matrix ros_to_eigen6d(const T& msg) { - if constexpr (same_bare_as) { - return ros_pose_to_eigen6d(msg); - } else if constexpr (same_bare_as) { - return ros_pose_to_eigen6d(msg.pose); - } else if constexpr (same_bare_as< - T, - geometry_msgs::msg::PoseWithCovarianceStamped>) { - return ros_pose_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_pose_to_eigen6d(pose); - }); - - return X; - } -} - -inline vortex::utils::types::Pose ros_pose_to_pose_quat( +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; @@ -139,21 +89,21 @@ inline vortex::utils::types::Pose ros_pose_to_pose_quat( } template -std::vector ros_to_pose_quat(const T& msg) { +std::vector ros_to_pose_vec(const T& msg) { if constexpr (same_bare_as) { - return {ros_pose_to_pose_quat(msg)}; + return {ros_pose_to_pose(msg)}; } else if constexpr (same_bare_as) { - return ros_to_pose_quat(msg.pose); + return ros_pose_to_pose(msg.pose); } else if constexpr (same_bare_as< T, geometry_msgs::msg::PoseWithCovarianceStamped>) { - return ros_to_pose_quat(msg.pose.pose); + 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_quat(pose)); + poses.push_back(ros_pose_to_pose(pose)); } return poses; } diff --git a/include/vortex/utils/views.hpp b/include/vortex/utils/views.hpp index f6ac9d6..cb20cc0 100644 --- a/include/vortex/utils/views.hpp +++ b/include/vortex/utils/views.hpp @@ -14,19 +14,6 @@ using vortex::utils::concepts::PositionLike; using vortex::utils::concepts::QuaternionLike; using vortex::utils::concepts::QuatPoseLike; -using vortex::utils::types::x_of; -using vortex::utils::types::y_of; -using vortex::utils::types::z_of; - -using vortex::utils::types::qw_of; -using vortex::utils::types::qx_of; -using vortex::utils::types::qy_of; -using vortex::utils::types::qz_of; - -using vortex::utils::types::pitch_of; -using vortex::utils::types::roll_of; -using vortex::utils::types::yaw_of; - template inline Eigen::Vector3d pos_vector(const T& t) { return Eigen::Vector3d{x_of(t), y_of(t), z_of(t)}; From 2c6d30501f15ae3ec03a5da5bc3b6569f60ef42b Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Thu, 18 Dec 2025 18:21:51 +0100 Subject: [PATCH 06/10] moved to concept folder --- cpp_test/test_concepts.cpp | 6 +++--- include/vortex/utils/{ => concepts}/accessors.hpp | 0 include/vortex/utils/{ => concepts}/concepts.hpp | 0 include/vortex/utils/{ => concepts}/member_concepts.hpp | 0 include/vortex/utils/{ => concepts}/views.hpp | 0 include/vortex/utils/ros_conversions.hpp | 4 ++-- 6 files changed, 5 insertions(+), 5 deletions(-) rename include/vortex/utils/{ => concepts}/accessors.hpp (100%) rename include/vortex/utils/{ => concepts}/concepts.hpp (100%) rename include/vortex/utils/{ => concepts}/member_concepts.hpp (100%) rename include/vortex/utils/{ => concepts}/views.hpp (100%) diff --git a/cpp_test/test_concepts.cpp b/cpp_test/test_concepts.cpp index cddc053..5c2a2e9 100644 --- a/cpp_test/test_concepts.cpp +++ b/cpp_test/test_concepts.cpp @@ -3,9 +3,9 @@ #include -#include "vortex/utils/accessors.hpp" -#include "vortex/utils/concepts.hpp" -#include "vortex/utils/views.hpp" +#include "vortex/utils/concepts/accessors.hpp" +#include "vortex/utils/concepts/concepts.hpp" +#include "vortex/utils/concepts/views.hpp" struct HasPositionAndEuler { double x, y, z; diff --git a/include/vortex/utils/accessors.hpp b/include/vortex/utils/concepts/accessors.hpp similarity index 100% rename from include/vortex/utils/accessors.hpp rename to include/vortex/utils/concepts/accessors.hpp diff --git a/include/vortex/utils/concepts.hpp b/include/vortex/utils/concepts/concepts.hpp similarity index 100% rename from include/vortex/utils/concepts.hpp rename to include/vortex/utils/concepts/concepts.hpp diff --git a/include/vortex/utils/member_concepts.hpp b/include/vortex/utils/concepts/member_concepts.hpp similarity index 100% rename from include/vortex/utils/member_concepts.hpp rename to include/vortex/utils/concepts/member_concepts.hpp diff --git a/include/vortex/utils/views.hpp b/include/vortex/utils/concepts/views.hpp similarity index 100% rename from include/vortex/utils/views.hpp rename to include/vortex/utils/concepts/views.hpp diff --git a/include/vortex/utils/ros_conversions.hpp b/include/vortex/utils/ros_conversions.hpp index 1a2bfec..466b93f 100644 --- a/include/vortex/utils/ros_conversions.hpp +++ b/include/vortex/utils/ros_conversions.hpp @@ -13,8 +13,8 @@ #include #include -#include "accessors.hpp" -#include "concepts.hpp" +#include "concepts/accessors.hpp" +#include "concepts/concepts.hpp" #include "math.hpp" #include "types.hpp" From 0e164edb26f009e7213cb4037c6ad5c063d00c71 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Thu, 18 Dec 2025 19:25:04 +0100 Subject: [PATCH 07/10] doxygen comments --- include/vortex/utils/concepts/accessors.hpp | 3 + include/vortex/utils/concepts/concepts.hpp | 79 +++++++++++++++++++++ include/vortex/utils/concepts/views.hpp | 44 ++++++++++++ include/vortex/utils/ros_conversions.hpp | 68 ++++++++++++++++++ 4 files changed, 194 insertions(+) diff --git a/include/vortex/utils/concepts/accessors.hpp b/include/vortex/utils/concepts/accessors.hpp index 79f4a43..ba3946e 100644 --- a/include/vortex/utils/concepts/accessors.hpp +++ b/include/vortex/utils/concepts/accessors.hpp @@ -5,6 +5,9 @@ namespace vortex::utils { +// Accessors functions for types that satisfy concepts in "member_concepts.hpp" +// so that these types satisfy the requirements of concepts in "concepts.hpp" + template constexpr double x_of(const T& t) { return t.x; diff --git a/include/vortex/utils/concepts/concepts.hpp b/include/vortex/utils/concepts/concepts.hpp index d9acabe..4547dc8 100644 --- a/include/vortex/utils/concepts/concepts.hpp +++ b/include/vortex/utils/concepts/concepts.hpp @@ -2,10 +2,24 @@ #define VORTEX_UTILS__CONCEPTS_HPP_ #include + #include "accessors.hpp" namespace vortex::utils::concepts { +/** + * @brief Concept for types that expose 3D positional components. + * + * A type satisfies PositionLike if it provides read access to + * Cartesian coordinates via the accessors: + * - x_of(t) + * - y_of(t) + * - z_of(t) + * + * Each component must be convertible to double. + * + * @tparam T Type to be checked + */ template concept PositionLike = requires(const T& t) { { x_of(t) } -> std::convertible_to; @@ -13,6 +27,20 @@ concept PositionLike = requires(const T& t) { { z_of(t) } -> std::convertible_to; }; +/** + * @brief Concept for types that expose orientation as a quaternion. + * + * A type satisfies QuaternionLike if it provides read access to + * quaternion components via the accessors: + * - qw_of(q) + * - qx_of(q) + * - qy_of(q) + * - qz_of(q) + * + * Each component must be convertible to double. + * + * @tparam T Type to be checked + */ template concept QuaternionLike = requires(const T& q) { { qw_of(q) } -> std::convertible_to; @@ -21,6 +49,19 @@ concept QuaternionLike = requires(const T& q) { { qz_of(q) } -> std::convertible_to; }; +/** + * @brief Concept for types that expose orientation as Euler angles. + * + * A type satisfies EulerLike if it provides read access to + * orientation expressed as roll, pitch, and yaw via the accessors: + * - roll_of(t) + * - pitch_of(t) + * - yaw_of(t) + * + * Each component must be convertible to double. + * + * @tparam T Type to be checked + */ template concept EulerLike = requires(const T& t) { { roll_of(t) } -> std::convertible_to; @@ -28,12 +69,50 @@ concept EulerLike = requires(const T& t) { { yaw_of(t) } -> 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; diff --git a/include/vortex/utils/concepts/views.hpp b/include/vortex/utils/concepts/views.hpp index cb20cc0..93deee6 100644 --- a/include/vortex/utils/concepts/views.hpp +++ b/include/vortex/utils/concepts/views.hpp @@ -3,6 +3,7 @@ #include #include + #include "accessors.hpp" #include "concepts.hpp" @@ -14,27 +15,70 @@ using vortex::utils::concepts::PositionLike; using vortex::utils::concepts::QuaternionLike; using vortex::utils::concepts::QuatPoseLike; +/** + * @brief Extracts the position components of an object as an Eigen vector. + * + * + * @tparam T Type satisfying PositionLike + * @param t Object providing positional components + * @return Eigen::Vector3d containing (x, y, z) + */ template inline Eigen::Vector3d pos_vector(const T& t) { return Eigen::Vector3d{x_of(t), y_of(t), z_of(t)}; } +/** + * @brief Extracts Euler orientation components as an Eigen vector. + * + * + * @tparam T Type satisfying EulerLike + * @param t Object providing Euler orientation components + * @return Eigen::Vector3d containing (roll, pitch, yaw) + */ template inline Eigen::Vector3d ori_vector(const T& t) { return Eigen::Vector3d{roll_of(t), pitch_of(t), yaw_of(t)}; } +/** + * @brief Extracts a quaternion orientation as an Eigen quaternion. + * + * @tparam T Type satisfying QuaternionLike + * @param t Object providing quaternion components + * @return Eigen::Quaterniond representing the orientation + */ template inline Eigen::Quaterniond ori_quaternion(const T& t) { return Eigen::Quaterniond{qw_of(t), qx_of(t), qy_of(t), qz_of(t)}; } +/** + * @brief Converts a pose with Euler orientation to a 6-D vector. + * + * The resulting vector layout is: + * `[x, y, z, roll, pitch, yaw]`. + * + * @tparam T Type satisfying EulerPoseLike + * @param t Object providing position and Euler orientation + * @return Eigen::Vector representing the pose + */ 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)}; } +/** + * @brief Converts a pose with quaternion orientation to a 7-D vector. + * + * The resulting vector layout is: + * `[x, y, z, qw, qx, qy, qz]`. + * + * @tparam T Type satisfying QuatPoseLike + * @param t Object providing position and quaternion orientation + * @return Eigen::Vector representing the pose + */ template inline Eigen::Vector to_vector(const T& t) { return Eigen::Vector{x_of(t), y_of(t), z_of(t), qw_of(t), diff --git a/include/vortex/utils/ros_conversions.hpp b/include/vortex/utils/ros_conversions.hpp index 466b93f..52a333d 100644 --- a/include/vortex/utils/ros_conversions.hpp +++ b/include/vortex/utils/ros_conversions.hpp @@ -24,9 +24,32 @@ using vortex::utils::concepts::EulerPoseLike; using vortex::utils::concepts::PoseLike; using vortex::utils::concepts::QuatPoseLike; +/** + * @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 || @@ -34,6 +57,16 @@ concept ROSPoseLike = same_bare_as || same_bare_as; +/** + * @brief Converts a PoseLike object to a ROS geometry_msgs::msg::Pose. + * + * The input type must satisfy PoseLike, i.e. provide position components + * and exactly one orientation representation (Euler or quaternion). + * + * @tparam T Type satisfying PoseLike + * @param pose_like Input pose object + * @return geometry_msgs::msg::Pose ROS pose message + */ template geometry_msgs::msg::Pose to_pose_msg(const T& pose_like) { geometry_msgs::msg::Pose pose; @@ -59,6 +92,16 @@ geometry_msgs::msg::Pose to_pose_msg(const T& pose_like) { return pose; } +/** + * @brief Converts a range of PoseLike objects to a vector of ROS pose messages. + * + * 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 R Range type whose value_type satisfies PoseLike + * @param poses Range of pose-like objects + * @return std::vector Converted ROS poses + */ template requires PoseLike> std::vector to_pose_msgs(const R& poses) { @@ -74,6 +117,11 @@ std::vector to_pose_msgs(const R& poses) { return out; } +/** + * @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 + */ inline vortex::utils::types::Pose ros_pose_to_pose( const geometry_msgs::msg::Pose& pose) { vortex::utils::types::Pose p; @@ -88,6 +136,26 @@ inline vortex::utils::types::Pose ros_pose_to_pose( return p; } +/** + * @brief Extracts one or more internal Pose objects from a ROS pose message. + * + * Supported input types are: + * + * - `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 ROS message type satisfying ROSPoseLike + * @param msg ROS pose message + * @return std::vector Extracted internal poses + */ template std::vector ros_to_pose_vec(const T& msg) { if constexpr (same_bare_as) { From 6c96342346ad6e2c8c2383e1034795f0c79b15ce Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Thu, 18 Dec 2025 19:25:22 +0100 Subject: [PATCH 08/10] renamed types to Pose, PoseEuler & Twist --- README.md | 2 +- cpp_test/test_types.cpp | 18 ++++---- include/vortex/utils/types.hpp | 80 ++++++++++++++++------------------ 3 files changed, 47 insertions(+), 53 deletions(-) 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/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/types.hpp b/include/vortex/utils/types.hpp index 75b8161..95be9c5 100644 --- a/include/vortex/utils/types.hpp +++ b/include/vortex/utils/types.hpp @@ -11,17 +11,21 @@ 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. */ -struct Eta; +struct PoseEuler; /** * @brief Struct to represent the state vector eta 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 state vector nu according to eq. 2.5 in + * Fossen, 2021, containing the linear and angular velocities. + */ +struct Twist; + +struct PoseEuler { double x{}; double y{}; double z{}; @@ -29,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; @@ -127,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{}; @@ -169,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; @@ -219,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{}; @@ -237,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; @@ -257,43 +261,33 @@ 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; } -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 From 7e5fc6d28d28284aba0b46e64b636cd5fff93d26 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Fri, 19 Dec 2025 12:23:02 +0100 Subject: [PATCH 09/10] removed accessors, views & member concepts --- cpp_test/test_concepts.cpp | 56 +----------- .../vortex/utils/{concepts => }/concepts.hpp | 63 +++++++------ include/vortex/utils/concepts/accessors.hpp | 63 ------------- .../vortex/utils/concepts/member_concepts.hpp | 32 ------- include/vortex/utils/concepts/views.hpp | 90 ------------------- include/vortex/utils/ros_conversions.hpp | 31 +++---- 6 files changed, 48 insertions(+), 287 deletions(-) rename include/vortex/utils/{concepts => }/concepts.hpp (68%) delete mode 100644 include/vortex/utils/concepts/accessors.hpp delete mode 100644 include/vortex/utils/concepts/member_concepts.hpp delete mode 100644 include/vortex/utils/concepts/views.hpp diff --git a/cpp_test/test_concepts.cpp b/cpp_test/test_concepts.cpp index 5c2a2e9..d81f443 100644 --- a/cpp_test/test_concepts.cpp +++ b/cpp_test/test_concepts.cpp @@ -3,9 +3,7 @@ #include -#include "vortex/utils/concepts/accessors.hpp" -#include "vortex/utils/concepts/concepts.hpp" -#include "vortex/utils/concepts/views.hpp" +#include "vortex/utils/concepts.hpp" struct HasPositionAndEuler { double x, y, z; @@ -53,24 +51,6 @@ struct HasEulerAndQuaternionMembers { 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); @@ -125,37 +105,3 @@ static_assert( 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/include/vortex/utils/concepts/concepts.hpp b/include/vortex/utils/concepts.hpp similarity index 68% rename from include/vortex/utils/concepts/concepts.hpp rename to include/vortex/utils/concepts.hpp index 4547dc8..0abd462 100644 --- a/include/vortex/utils/concepts/concepts.hpp +++ b/include/vortex/utils/concepts.hpp @@ -3,18 +3,18 @@ #include -#include "accessors.hpp" - namespace vortex::utils::concepts { /** * @brief Concept for types that expose 3D positional components. * - * A type satisfies PositionLike if it provides read access to - * Cartesian coordinates via the accessors: - * - x_of(t) - * - y_of(t) - * - z_of(t) + * A type satisfies PositionLike if it contains public members: + * + * - `x` + * + * - `y` + * + * - `z` * * Each component must be convertible to double. * @@ -22,41 +22,46 @@ 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; + { 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 provides read access to - * quaternion components via the accessors: - * - qw_of(q) - * - qx_of(q) - * - qy_of(q) - * - qz_of(q) + * 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& 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; +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 provides read access to - * orientation expressed as roll, pitch, and yaw via the accessors: - * - roll_of(t) - * - pitch_of(t) - * - yaw_of(t) + * A type satisfies EulerLike if it contains public members: + * + * - `roll` + * + * - `pitch` + * + * - `yaw` * * Each component must be convertible to double. * @@ -64,9 +69,9 @@ concept QuaternionLike = requires(const T& q) { */ 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; + { t.roll } -> std::convertible_to; + { t.pitch } -> std::convertible_to; + { t.yaw } -> std::convertible_to; }; /** diff --git a/include/vortex/utils/concepts/accessors.hpp b/include/vortex/utils/concepts/accessors.hpp deleted file mode 100644 index ba3946e..0000000 --- a/include/vortex/utils/concepts/accessors.hpp +++ /dev/null @@ -1,63 +0,0 @@ -#ifndef VORTEX_UTILS__ACCESSORS_HPP_ -#define VORTEX_UTILS__ACCESSORS_HPP_ - -#include "member_concepts.hpp" - -namespace vortex::utils { - -// Accessors functions for types that satisfy concepts in "member_concepts.hpp" -// so that these types satisfy the requirements of concepts in "concepts.hpp" - -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/member_concepts.hpp b/include/vortex/utils/concepts/member_concepts.hpp deleted file mode 100644 index 988aa97..0000000 --- a/include/vortex/utils/concepts/member_concepts.hpp +++ /dev/null @@ -1,32 +0,0 @@ -#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/concepts/views.hpp b/include/vortex/utils/concepts/views.hpp deleted file mode 100644 index 93deee6..0000000 --- a/include/vortex/utils/concepts/views.hpp +++ /dev/null @@ -1,90 +0,0 @@ -#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; - -/** - * @brief Extracts the position components of an object as an Eigen vector. - * - * - * @tparam T Type satisfying PositionLike - * @param t Object providing positional components - * @return Eigen::Vector3d containing (x, y, z) - */ -template -inline Eigen::Vector3d pos_vector(const T& t) { - return Eigen::Vector3d{x_of(t), y_of(t), z_of(t)}; -} - -/** - * @brief Extracts Euler orientation components as an Eigen vector. - * - * - * @tparam T Type satisfying EulerLike - * @param t Object providing Euler orientation components - * @return Eigen::Vector3d containing (roll, pitch, yaw) - */ -template -inline Eigen::Vector3d ori_vector(const T& t) { - return Eigen::Vector3d{roll_of(t), pitch_of(t), yaw_of(t)}; -} - -/** - * @brief Extracts a quaternion orientation as an Eigen quaternion. - * - * @tparam T Type satisfying QuaternionLike - * @param t Object providing quaternion components - * @return Eigen::Quaterniond representing the orientation - */ -template -inline Eigen::Quaterniond ori_quaternion(const T& t) { - return Eigen::Quaterniond{qw_of(t), qx_of(t), qy_of(t), qz_of(t)}; -} - -/** - * @brief Converts a pose with Euler orientation to a 6-D vector. - * - * The resulting vector layout is: - * `[x, y, z, roll, pitch, yaw]`. - * - * @tparam T Type satisfying EulerPoseLike - * @param t Object providing position and Euler orientation - * @return Eigen::Vector representing the pose - */ -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)}; -} - -/** - * @brief Converts a pose with quaternion orientation to a 7-D vector. - * - * The resulting vector layout is: - * `[x, y, z, qw, qx, qy, qz]`. - * - * @tparam T Type satisfying QuatPoseLike - * @param t Object providing position and quaternion orientation - * @return Eigen::Vector representing the pose - */ -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_ diff --git a/include/vortex/utils/ros_conversions.hpp b/include/vortex/utils/ros_conversions.hpp index 52a333d..aa2067f 100644 --- a/include/vortex/utils/ros_conversions.hpp +++ b/include/vortex/utils/ros_conversions.hpp @@ -13,17 +13,12 @@ #include #include -#include "concepts/accessors.hpp" -#include "concepts/concepts.hpp" +#include "concepts.hpp" #include "math.hpp" #include "types.hpp" namespace vortex::utils::ros_conversions { -using vortex::utils::concepts::EulerPoseLike; -using vortex::utils::concepts::PoseLike; -using vortex::utils::concepts::QuatPoseLike; - /** * @brief Helper concept to check if two types are the same * after removing cv-ref qualifiers. @@ -67,22 +62,22 @@ concept ROSPoseLike = * @param pose_like Input pose object * @return geometry_msgs::msg::Pose ROS pose message */ -template +template geometry_msgs::msg::Pose to_pose_msg(const T& pose_like) { geometry_msgs::msg::Pose pose; - pose.position.x = x_of(pose_like); - pose.position.y = y_of(pose_like); - pose.position.z = z_of(pose_like); + pose.position.x = pose_like.x; + pose.position.y = pose_like.y; + pose.position.z = pose_like.z; - 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) { + 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( - roll_of(pose_like), pitch_of(pose_like), yaw_of(pose_like)); + pose_like.roll, pose_like.pitch, pose_like.yaw); pose.orientation.w = q.w(); pose.orientation.x = q.x(); pose.orientation.y = q.y(); @@ -103,7 +98,7 @@ geometry_msgs::msg::Pose to_pose_msg(const T& pose_like) { * @return std::vector Converted ROS poses */ template - requires PoseLike> + requires vortex::utils::concepts::PoseLike> std::vector to_pose_msgs(const R& poses) { std::vector out; From 29fd12f4728cd420244ae168c47493352858d466 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Fri, 19 Dec 2025 13:29:57 +0100 Subject: [PATCH 10/10] update struct doxygen comments --- include/vortex/utils/types.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/include/vortex/utils/types.hpp b/include/vortex/utils/types.hpp index 95be9c5..1efec1d 100644 --- a/include/vortex/utils/types.hpp +++ b/include/vortex/utils/types.hpp @@ -8,19 +8,19 @@ 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 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 Pose; /** - * @brief Struct to represent the state vector nu according to eq. 2.5 in + * @brief Struct to represent the velocity vector according to eq. 2.5 in * Fossen, 2021, containing the linear and angular velocities. */ struct Twist;