Skip to content

Feat/pose type #20

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
jorgenfj merged 8 commits into from
Dec 10, 2025
Merged

Feat/pose type #20

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
2b10921
pose type
jorgenfj Oct 22, 2025
c98554c
pose ros conversions. Cmake cpp 20
jorgenfj Oct 22, 2025
6aacb5c
ros pose to eigen 6d
jorgenfj Oct 22, 2025
9995c2c
eigen6d to ros-pose
jorgenfj Oct 26, 2025
2659fbe
Merge branch 'main' into feat/pose-type
jorgenfj Dec 10, 2025
0e00e69
added remove_cvref_t fix. Added tests
jorgenfj Dec 10, 2025
c02d584
remove merge artifact
jorgenfj Dec 10, 2025
b4c6911
added same_bare_as concept helper
jorgenfj Dec 10, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
183 changes: 183 additions & 0 deletions cpp_test/test_ros_conversions.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -197,3 +197,186 @@ TEST(pose_like_to_pose_msg, eigen6d_conversion) {
EXPECT_NEAR(pose.orientation.y, expected.y(), 1e-6);
EXPECT_NEAR(pose.orientation.z, expected.z(), 1e-6);
}

// ================================================================
// Compile-Time Tests for ros_to_eigen6d Type Acceptance
// ================================================================

template <typename T>
concept AcceptsRosToEigen =
requires(const T& t) { vortex::utils::ros_conversions::ros_to_eigen6d(t); };

static_assert(AcceptsRosToEigen<geometry_msgs::msg::Pose>,
"Pose must be accepted");

static_assert(AcceptsRosToEigen<geometry_msgs::msg::PoseStamped>,
"PoseStamped must be accepted");

static_assert(AcceptsRosToEigen<geometry_msgs::msg::PoseWithCovarianceStamped>,
"PoseWithCovarianceStamped must be accepted");

static_assert(AcceptsRosToEigen<geometry_msgs::msg::PoseArray>,
"PoseArray must be accepted");

struct RandomType {
double x = 0;
};

static_assert(!AcceptsRosToEigen<int>, "int must NOT be accepted");

static_assert(!AcceptsRosToEigen<double>, "double must NOT be accepted");

static_assert(!AcceptsRosToEigen<Eigen::Vector3d>,
"Eigen::Vector3d must NOT be accepted");

static_assert(!AcceptsRosToEigen<RandomType>,
"RandomType must NOT be accepted");

static_assert(!AcceptsRosToEigen<std::shared_ptr<geometry_msgs::msg::Pose>>,
"shared_ptr<Pose> must NOT be accepted");

static_assert(!AcceptsRosToEigen<geometry_msgs::msg::Pose*>,
"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);
}

TEST(ros_to_eigen6d, pose_array_single) {
geometry_msgs::msg::PoseArray arr;

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);
}

TEST(ros_to_eigen6d, pose_array_multiple) {
geometry_msgs::msg::PoseArray arr;

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);

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);

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);
}
}
97 changes: 96 additions & 1 deletion include/vortex/utils/ros_conversions.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,10 @@
#include <concepts>
#include <eigen3/Eigen/Dense>
#include <geometry_msgs/msg/pose.hpp>

#include <geometry_msgs/msg/pose_array.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <ranges>
#include "math.hpp"

namespace vortex::utils::ros_conversions {
Expand Down Expand Up @@ -169,6 +172,98 @@ geometry_msgs::msg::Pose pose_like_to_pose_msg(const T& v) {
return pose;
}

/**
* @brief Helper concept to check if two types are the same
* after removing cv-ref qualifiers.
*
* @tparam T First type.
* @tparam U Second type.
*/
template <typename T, typename U>
concept same_bare_as = std::same_as<std::remove_cvref_t<T>, 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 <typename T>
concept ROSPoseLike =
same_bare_as<T, geometry_msgs::msg::Pose> ||
same_bare_as<T, geometry_msgs::msg::PoseArray> ||
same_bare_as<T, geometry_msgs::msg::PoseStamped> ||
same_bare_as<T, geometry_msgs::msg::PoseWithCovarianceStamped>;
Comment on lines +201 to +206
Copy link
Contributor

@Andeshog Andeshog Dec 10, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why not just have one function that converts from Pose to Eigen::Matrix<double, 6, 1>? All of the types include Pose, so it will be pretty straightforward to use on them anyway 🤷‍♂️

Copy link
Contributor Author

@jorgenfj jorgenfj Dec 10, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Because I have a test node I'm working on where the pose subscriber callback function is templated so the function call becomes uniform for all pose types:

template <ValidPoseMsg MsgT>
void IPDAPoseFilteringNode::pose_callback(
    const typename MsgT::ConstSharedPtr& msg) {
    Measurements measurements = vortex::utils::ros_conversions::ros_to_eigen6d(*msg);

Where measurements defined:

using Measurements = Eigen::Matrix<double, 6, Eigen::Dynamic>;


/**
* @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<double, 6, Dynamic>` containing the converted
* poses where each column is [x, y, z, roll, pitch, yaw]^T.
*/
template <ROSPoseLike T>
inline Eigen::Matrix<double, 6, Eigen::Dynamic> ros_to_eigen6d(const T& msg) {
if constexpr (same_bare_as<T, geometry_msgs::msg::Pose>) {
Eigen::Matrix<double, 6, 1> v;
v(0) = msg.position.x;
v(1) = msg.position.y;
v(2) = msg.position.z;

// ROS/tf2 uses (x, y, z, w)
// while Eigen stores quaternions as (w, x, y, z)
Eigen::Quaterniond q(msg.orientation.w, msg.orientation.x,
msg.orientation.y, msg.orientation.z);

const Eigen::Vector3d euler = vortex::utils::math::quat_to_euler(q);
v.tail<3>() = euler;

return v;
}

else if constexpr (same_bare_as<T, geometry_msgs::msg::PoseStamped>) {
return ros_to_eigen6d(msg.pose);
}

else if constexpr (same_bare_as<
T, geometry_msgs::msg::PoseWithCovarianceStamped>) {
return ros_to_eigen6d(msg.pose.pose);
}

else if constexpr (same_bare_as<T, geometry_msgs::msg::PoseArray>) {
const size_t n = msg.poses.size();
Eigen::Matrix<double, 6, Eigen::Dynamic> X(6, n);

std::ranges::for_each(std::views::iota(size_t{0}, n), [&](size_t i) {
const auto& pose = msg.poses[i];
X.col(i) = ros_to_eigen6d(pose);
});

return X;
}
}

} // namespace vortex::utils::ros_conversions

#endif // VORTEX_UTILS__ROS_CONVERSIONS_HPP_