ros pose-tf transform functions

CMakeLists.txt

@@ -5,9 +5,12 @@ set(CMAKE_CXX_STANDARD 20)
 add_compile_options(-Wall -Wextra -Wpedantic)
 
 find_package(ament_cmake REQUIRED)
-find_package(rclcpp)
+find_package(rclcpp REQUIRED)
 find_package(ament_cmake_python REQUIRED)
 find_package(Eigen3 REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 
 include_directories(include)
@@ -22,6 +25,9 @@ target_include_directories(${PROJECT_NAME} PUBLIC
 ament_target_dependencies(${PROJECT_NAME} PUBLIC
   rclcpp
   Eigen3
+  tf2
+  tf2_ros
+  tf2_geometry_msgs
   geometry_msgs
 )
 

cpp_test/CMakeLists.txt

@@ -23,3 +23,26 @@ target_link_libraries(
 ament_target_dependencies(${TEST_BINARY_NAME} PUBLIC Eigen3)
 
 gtest_discover_tests(${TEST_BINARY_NAME})
+
+set(TEST_BINARY_ROS_NAME ${PROJECT_NAME}_ros_test)
+
+add_executable(
+  ${TEST_BINARY_ROS_NAME}
+  test_ros_transforms.cpp
+)
+
+target_link_libraries(
+  ${TEST_BINARY_ROS_NAME}
+    ${PROJECT_NAME}
+    GTest::GTest
+)
+
+ament_target_dependencies(
+  ${TEST_BINARY_ROS_NAME}
+  rclcpp
+  tf2
+  tf2_ros
+  tf2_geometry_msgs
+)
+
+gtest_discover_tests(${TEST_BINARY_ROS_NAME})

cpp_test/test_ros_transforms.cpp

@@ -0,0 +1,151 @@
+#include <gtest/gtest.h>
+
+#include <rclcpp/rclcpp.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 <geometry_msgs/msg/transform_stamped.hpp>
+
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/static_transform_broadcaster.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
+#include "vortex/utils/ros_transforms.hpp"
+
+class RosTransformsTest : public ::testing::Test {
+   protected:
+    void SetUp() override {
+        node_ = std::make_shared<rclcpp::Node>("ros_transforms_test_node");
+
+        buffer_ = std::make_unique<tf2_ros::Buffer>(node_->get_clock());
+        buffer_->setUsingDedicatedThread(true);
+
+        listener_ =
+            std::make_unique<tf2_ros::TransformListener>(*buffer_, node_);
+        broadcaster_ =
+            std::make_unique<tf2_ros::StaticTransformBroadcaster>(node_);
+
+        publish_static_transform();
+
+        rclcpp::spin_some(node_);
+        rclcpp::spin_some(node_);
+    }
+
+    void publish_static_transform() {
+        geometry_msgs::msg::TransformStamped tf;
+        tf.header.stamp = node_->get_clock()->now();
+        tf.header.frame_id = "source";
+        tf.child_frame_id = "target";
+
+        tf.transform.translation.x = 1.0;
+        tf.transform.translation.y = 2.0;
+        tf.transform.translation.z = 3.0;
+
+        tf.transform.rotation.w = 1.0;
+
+        broadcaster_->sendTransform(tf);
+    }
+
+    std::shared_ptr<rclcpp::Node> node_;
+    std::unique_ptr<tf2_ros::Buffer> buffer_;
+    std::unique_ptr<tf2_ros::TransformListener> listener_;
+    std::unique_ptr<tf2_ros::StaticTransformBroadcaster> broadcaster_;
+};
+
+TEST_F(RosTransformsTest, pose_stamped_success) {
+    geometry_msgs::msg::PoseStamped in, out;
+    in.header.frame_id = "source";
+    in.header.stamp = node_->get_clock()->now();
+
+    in.pose.position.x = 1.0;
+    in.pose.position.y = 1.0;
+    in.pose.position.z = 1.0;
+    in.pose.orientation.w = 1.0;
+
+    EXPECT_NO_THROW({
+        vortex::utils::ros_transforms::transform_pose(*buffer_, in, "target",
+                                                      out);
+    });
+
+    EXPECT_EQ(out.header.frame_id, "target");
+
+    // Correct TF semantics: source → target subtracts translation
+    EXPECT_NEAR(out.pose.position.x, 0.0, 1e-6);
+    EXPECT_NEAR(out.pose.position.y, -1.0, 1e-6);
+    EXPECT_NEAR(out.pose.position.z, -2.0, 1e-6);
+}
+
+TEST_F(RosTransformsTest, pose_stamped_failure) {
+    geometry_msgs::msg::PoseStamped in, out;
+    in.header.frame_id = "unknown_frame";
+    in.header.stamp = node_->get_clock()->now();
+
+    EXPECT_THROW(
+        {
+            vortex::utils::ros_transforms::transform_pose(*buffer_, in,
+                                                          "target", out);
+        },
+        tf2::TransformException);
+}
+
+TEST_F(RosTransformsTest, pose_with_covariance_success) {
+    geometry_msgs::msg::PoseWithCovarianceStamped in, out;
+    in.header.frame_id = "source";
+    in.header.stamp = node_->get_clock()->now();
+
+    in.pose.pose.orientation.w = 1.0;
+
+    EXPECT_NO_THROW({
+        vortex::utils::ros_transforms::transform_pose(*buffer_, in, "target",
+                                                      out);
+    });
+
+    EXPECT_EQ(out.header.frame_id, "target");
+
+    EXPECT_NEAR(out.pose.pose.position.x, -1.0, 1e-6);
+    EXPECT_NEAR(out.pose.pose.position.y, -2.0, 1e-6);
+    EXPECT_NEAR(out.pose.pose.position.z, -3.0, 1e-6);
+}
+
+TEST_F(RosTransformsTest, pose_array_success) {
+    geometry_msgs::msg::PoseArray in, out;
+    in.header.frame_id = "source";
+    in.header.stamp = node_->get_clock()->now();
+
+    in.poses.resize(2);
+    in.poses[0].position.x = 0.0;
+    in.poses[1].position.x = 1.0;
+
+    EXPECT_NO_THROW({
+        vortex::utils::ros_transforms::transform_pose(*buffer_, in, "target",
+                                                      out);
+    });
+
+    ASSERT_EQ(out.poses.size(), 2);
+    EXPECT_NEAR(out.poses[0].position.x, -1.0, 1e-6);
+    EXPECT_NEAR(out.poses[1].position.x, 0.0, 1e-6);
+}
+
+TEST_F(RosTransformsTest, pose_array_failure) {
+    geometry_msgs::msg::PoseArray in, out;
+    in.header.frame_id = "unknown_frame";
+    in.header.stamp = node_->get_clock()->now();
+    in.poses.resize(1);
+
+    EXPECT_THROW(
+        {
+            vortex::utils::ros_transforms::transform_pose(*buffer_, in,
+                                                          "target", out);
+        },
+        tf2::TransformException);
+}
+
+int main(int argc, char** argv) {
+    rclcpp::init(argc, argv);
+    ::testing::InitGoogleTest(&argc, argv);
+    int ret = RUN_ALL_TESTS();
+    rclcpp::shutdown();
+    return ret;
+}

include/vortex/utils/ros_transforms.hpp

@@ -0,0 +1,108 @@
+#ifndef VORTEX_UTILS__ROS_TRANSFORMS_HPP_
+#define VORTEX_UTILS__ROS_TRANSFORMS_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 <tf2/exceptions.h>
+#include <tf2/time.h>
+#include <tf2_ros/buffer.h>
+
+namespace vortex::utils::ros_transforms {
+
+/**
+ * @brief Transform a stamped pose into a target frame using TF.
+ *
+ * Applies a TF transform to a geometry_msgs::msg::PoseStamped message,
+ * producing a pose expressed in the specified target frame.
+ *
+ * The input pose timestamp is used for TF lookup. If no valid transform
+ * is available within the specified timeout, the transformation fails.
+ *
+ * @param tf_buffer TF buffer used for transform lookup
+ * @param in Input PoseStamped message
+ * @param target_frame Target frame ID
+ * @param out Output PoseStamped message in the target frame
+ * @param timeout Maximum duration to wait for a valid transform, defaults to 0
+ * @note This function throws tf2::TransformException on failure.
+ *       Callers are expected to handle errors via try/catch.
+ */
+inline void transform_pose(tf2_ros::Buffer& tf_buffer,
+                           const geometry_msgs::msg::PoseStamped& in,
+                           const std::string& target_frame,
+                           geometry_msgs::msg::PoseStamped& out,
+                           tf2::Duration timeout = tf2::durationFromSec(0.00)) {
+    tf_buffer.transform(in, out, target_frame, timeout);
+}
+
+/**
+ * @brief Transform a stamped pose with covariance into a target frame using TF.
+ *
+ * Applies a TF transform to a geometry_msgs::msg::PoseWithCovarianceStamped
+ * message. Both the pose and its 6×6 covariance matrix are transformed
+ * according to the rigid-body frame transform.
+ *
+ * The covariance is rotated into the target frame but is not otherwise
+ * modified (no uncertainty inflation or filtering is applied).
+ *
+ * @param tf_buffer TF buffer used for transform lookup
+ * @param in Input PoseWithCovarianceStamped message
+ * @param target_frame Target frame ID
+ * @param out Output PoseWithCovarianceStamped message in the target frame
+ * @param timeout Maximum duration to wait for a valid transform
+ * @note This function throws tf2::TransformException on failure.
+ *       Callers are expected to handle errors via try/catch.
+ */
+inline void transform_pose(
+    tf2_ros::Buffer& tf_buffer,
+    const geometry_msgs::msg::PoseWithCovarianceStamped& in,
+    const std::string& target_frame,
+    geometry_msgs::msg::PoseWithCovarianceStamped& out,
+    tf2::Duration timeout = tf2::durationFromSec(0.00)) {
+    tf_buffer.transform(in, out, target_frame, timeout);
+}
+
+/**
+ * @brief Transform all poses in a PoseArray into a target frame using TF.
+ *
+ * Each pose in the input PoseArray is individually transformed using the
+ * array header (frame ID and timestamp). The output PoseArray contains
+ * only successfully transformed poses and is expressed in the target frame.
+ *
+ * TF does not natively support PoseArray, so each pose is internally
+ * wrapped as a PoseStamped for transformation.
+ *
+ * @param tf_buffer TF buffer used for transform lookup
+ * @param in Input PoseArray message
+ * @param target_frame Target frame ID
+ * @param out Output PoseArray message in the target frame
+ * @param timeout Maximum duration to wait for each pose transform
+ * @note This function throws tf2::TransformException on failure.
+ *       Callers are expected to handle errors via try/catch.
+ */
+inline void transform_pose(tf2_ros::Buffer& tf_buffer,
+                           const geometry_msgs::msg::PoseArray& in,
+                           const std::string& target_frame,
+                           geometry_msgs::msg::PoseArray& out,
+                           tf2::Duration timeout = tf2::durationFromSec(0.0)) {
+    out.poses.clear();
+    out.poses.reserve(in.poses.size());
+
+    out.header.stamp = in.header.stamp;
+    out.header.frame_id = target_frame;
+
+    geometry_msgs::msg::PoseStamped in_ps, out_ps;
+    in_ps.header = in.header;
+
+    for (const auto& pose : in.poses) {
+        in_ps.pose = pose;
+
+        tf_buffer.transform(in_ps, out_ps, target_frame, timeout);
+        out.poses.push_back(out_ps.pose);
+    }
+}
+
+}  // namespace vortex::utils::ros_transforms
+
+#endif  // VORTEX_UTILS__ROS_TRANSFORMS_HPP_

package.xml

@@ -14,6 +14,9 @@
   <depend>eigen</depend>
   <depend>python3-numpy</depend>
   <depend>python3-scipy</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_geometry_msgs</depend>
   <depend>geometry_msgs</depend>