From c4dedbd2b22be751dc67cd808007349f43120eb7 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Mon, 22 Dec 2025 17:22:19 +0100 Subject: [PATCH 1/3] ros pose-tf transform functions --- CMakeLists.txt | 8 +- include/vortex/utils/ros_transforms.hpp | 137 ++++++++++++++++++++++++ package.xml | 3 + 3 files changed, 147 insertions(+), 1 deletion(-) create mode 100644 include/vortex/utils/ros_transforms.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index e4195a2..4a75054 100644 --- a/CMakeLists.txt +++ b/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 ) diff --git a/include/vortex/utils/ros_transforms.hpp b/include/vortex/utils/ros_transforms.hpp new file mode 100644 index 0000000..1cf200c --- /dev/null +++ b/include/vortex/utils/ros_transforms.hpp @@ -0,0 +1,137 @@ +#ifndef VORTEX_UTILS__ROS_TRANSFORMS_HPP_ +#define VORTEX_UTILS__ROS_TRANSFORMS_HPP_ + +#include +#include +#include + +#include +#include +#include + +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 logger ROS logger used for warning output + * @param timeout Maximum duration to wait for a valid transform + * @return true if the transform succeeded, false otherwise + */ +inline bool transform_pose(tf2_ros::Buffer& tf_buffer, + const geometry_msgs::msg::PoseStamped& in, + const std::string& target_frame, + geometry_msgs::msg::PoseStamped& out, + rclcpp::Logger logger, + tf2::Duration timeout = tf2::durationFromSec(0.00)) { + try { + tf_buffer.transform(in, out, target_frame, timeout); + return true; + } catch (const tf2::TransformException& ex) { + RCLCPP_WARN(logger, "TF transform failed from '%s' to '%s': %s", + in.header.frame_id.c_str(), target_frame.c_str(), + ex.what()); + return false; + } +} + +/** + * @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 logger ROS logger used for warning output + * @param timeout Maximum duration to wait for a valid transform + * @return true if the transform succeeded, false otherwise + */ +inline bool transform_pose( + tf2_ros::Buffer& tf_buffer, + const geometry_msgs::msg::PoseWithCovarianceStamped& in, + const std::string& target_frame, + geometry_msgs::msg::PoseWithCovarianceStamped& out, + rclcpp::Logger logger, + tf2::Duration timeout = tf2::durationFromSec(0.00)) { + try { + tf_buffer.transform(in, out, target_frame, timeout); + return true; + } catch (const tf2::TransformException& ex) { + RCLCPP_WARN(logger, "TF transform failed from '%s' to '%s': %s", + in.header.frame_id.c_str(), target_frame.c_str(), + ex.what()); + return false; + } +} + +/** + * @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 logger ROS logger used for warning output + * @param timeout Maximum duration to wait for each pose transform + * @return true if all poses were successfully transformed, false otherwise + */ +inline bool transform_pose(tf2_ros::Buffer& tf_buffer, + const geometry_msgs::msg::PoseArray& in, + const std::string& target_frame, + geometry_msgs::msg::PoseArray& out, + rclcpp::Logger logger, + 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; + + try { + tf_buffer.transform(in_ps, out_ps, target_frame, timeout); + out.poses.push_back(out_ps.pose); + } catch (const tf2::TransformException& ex) { + RCLCPP_WARN(logger, + "TF transform failed for PoseArray element " + "from '%s' to '%s': %s", + in.header.frame_id.c_str(), target_frame.c_str(), + ex.what()); + return false; + } + } + return true; +} + +} // namespace vortex::utils::ros_transforms + +#endif // VORTEX_UTILS__ROS_TRANSFORMS_HPP_ diff --git a/package.xml b/package.xml index d565156..61ae562 100644 --- a/package.xml +++ b/package.xml @@ -14,6 +14,9 @@ eigen python3-numpy python3-scipy + tf2 + tf2_ros + tf2_geometry_msgs geometry_msgs From a28c1e65f5cce8359cde0566f537e9316ef682b4 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Mon, 22 Dec 2025 18:11:29 +0100 Subject: [PATCH 2/3] ros specific test binary + transform tests --- cpp_test/CMakeLists.txt | 23 +++++ cpp_test/test_ros_transforms.cpp | 158 +++++++++++++++++++++++++++++++ 2 files changed, 181 insertions(+) create mode 100644 cpp_test/test_ros_transforms.cpp diff --git a/cpp_test/CMakeLists.txt b/cpp_test/CMakeLists.txt index af64830..33f573c 100644 --- a/cpp_test/CMakeLists.txt +++ b/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}) diff --git a/cpp_test/test_ros_transforms.cpp b/cpp_test/test_ros_transforms.cpp new file mode 100644 index 0000000..9b7e9a3 --- /dev/null +++ b/cpp_test/test_ros_transforms.cpp @@ -0,0 +1,158 @@ +#include + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "vortex/utils/ros_transforms.hpp" + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return ret; +} + +class RosTransformsTest : public ::testing::Test { + protected: + void SetUp() override { + node_ = std::make_shared("ros_transforms_test_node"); + + buffer_ = std::make_unique(node_->get_clock()); + buffer_->setUsingDedicatedThread(true); + listener_ = + std::make_unique(*buffer_, node_); + broadcaster_ = + std::make_unique(node_); + + publish_static_transform(); + + // Give TF some time to populate + 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; + tf.transform.rotation.x = 0.0; + tf.transform.rotation.y = 0.0; + tf.transform.rotation.z = 0.0; + + broadcaster_->sendTransform(tf); + } + + rclcpp::Logger logger_ = rclcpp::get_logger("ros_transforms_test"); + + std::shared_ptr node_; + std::unique_ptr buffer_; + std::unique_ptr listener_; + std::unique_ptr 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; + + bool ok = vortex::utils::ros_transforms::transform_pose( + *buffer_, in, "target", out, logger_); + + ASSERT_TRUE(ok); + EXPECT_EQ(out.header.frame_id, "target"); + + 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(); + + bool ok = vortex::utils::ros_transforms::transform_pose( + *buffer_, in, "target", out, logger_); + + EXPECT_FALSE(ok); +} + +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.position.x = 0.0; + in.pose.pose.position.y = 0.0; + in.pose.pose.position.z = 0.0; + in.pose.pose.orientation.w = 1.0; + + // Simple diagonal covariance + in.pose.covariance[0] = 0.1; + in.pose.covariance[7] = 0.1; + in.pose.covariance[14] = 0.1; + + bool ok = vortex::utils::ros_transforms::transform_pose( + *buffer_, in, "target", out, logger_); + + ASSERT_TRUE(ok); + 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; + + bool ok = vortex::utils::ros_transforms::transform_pose( + *buffer_, in, "target", out, logger_); + + ASSERT_TRUE(ok); + 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); + + bool ok = vortex::utils::ros_transforms::transform_pose( + *buffer_, in, "target", out, logger_); + + EXPECT_FALSE(ok); +} From 8d66ed44b3749ec1d29870b048b7af4acc4fe6e7 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Mon, 22 Dec 2025 20:59:46 +0100 Subject: [PATCH 3/3] removed log and exec handling in transform functions --- cpp_test/test_ros_transforms.cpp | 73 +++++++++++-------------- include/vortex/utils/ros_transforms.hpp | 57 +++++-------------- 2 files changed, 47 insertions(+), 83 deletions(-) diff --git a/cpp_test/test_ros_transforms.cpp b/cpp_test/test_ros_transforms.cpp index 9b7e9a3..9335be3 100644 --- a/cpp_test/test_ros_transforms.cpp +++ b/cpp_test/test_ros_transforms.cpp @@ -14,14 +14,6 @@ #include "vortex/utils/ros_transforms.hpp" -int main(int argc, char** argv) { - rclcpp::init(argc, argv); - ::testing::InitGoogleTest(&argc, argv); - int ret = RUN_ALL_TESTS(); - rclcpp::shutdown(); - return ret; -} - class RosTransformsTest : public ::testing::Test { protected: void SetUp() override { @@ -29,6 +21,7 @@ class RosTransformsTest : public ::testing::Test { buffer_ = std::make_unique(node_->get_clock()); buffer_->setUsingDedicatedThread(true); + listener_ = std::make_unique(*buffer_, node_); broadcaster_ = @@ -36,7 +29,6 @@ class RosTransformsTest : public ::testing::Test { publish_static_transform(); - // Give TF some time to populate rclcpp::spin_some(node_); rclcpp::spin_some(node_); } @@ -52,15 +44,10 @@ class RosTransformsTest : public ::testing::Test { tf.transform.translation.z = 3.0; tf.transform.rotation.w = 1.0; - tf.transform.rotation.x = 0.0; - tf.transform.rotation.y = 0.0; - tf.transform.rotation.z = 0.0; broadcaster_->sendTransform(tf); } - rclcpp::Logger logger_ = rclcpp::get_logger("ros_transforms_test"); - std::shared_ptr node_; std::unique_ptr buffer_; std::unique_ptr listener_; @@ -77,12 +64,14 @@ TEST_F(RosTransformsTest, pose_stamped_success) { in.pose.position.z = 1.0; in.pose.orientation.w = 1.0; - bool ok = vortex::utils::ros_transforms::transform_pose( - *buffer_, in, "target", out, logger_); + EXPECT_NO_THROW({ + vortex::utils::ros_transforms::transform_pose(*buffer_, in, "target", + out); + }); - ASSERT_TRUE(ok); 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); @@ -93,10 +82,12 @@ TEST_F(RosTransformsTest, pose_stamped_failure) { in.header.frame_id = "unknown_frame"; in.header.stamp = node_->get_clock()->now(); - bool ok = vortex::utils::ros_transforms::transform_pose( - *buffer_, in, "target", out, logger_); - - EXPECT_FALSE(ok); + EXPECT_THROW( + { + vortex::utils::ros_transforms::transform_pose(*buffer_, in, + "target", out); + }, + tf2::TransformException); } TEST_F(RosTransformsTest, pose_with_covariance_success) { @@ -104,20 +95,13 @@ TEST_F(RosTransformsTest, pose_with_covariance_success) { in.header.frame_id = "source"; in.header.stamp = node_->get_clock()->now(); - in.pose.pose.position.x = 0.0; - in.pose.pose.position.y = 0.0; - in.pose.pose.position.z = 0.0; in.pose.pose.orientation.w = 1.0; - // Simple diagonal covariance - in.pose.covariance[0] = 0.1; - in.pose.covariance[7] = 0.1; - in.pose.covariance[14] = 0.1; + EXPECT_NO_THROW({ + vortex::utils::ros_transforms::transform_pose(*buffer_, in, "target", + out); + }); - bool ok = vortex::utils::ros_transforms::transform_pose( - *buffer_, in, "target", out, logger_); - - ASSERT_TRUE(ok); EXPECT_EQ(out.header.frame_id, "target"); EXPECT_NEAR(out.pose.pose.position.x, -1.0, 1e-6); @@ -134,12 +118,12 @@ TEST_F(RosTransformsTest, pose_array_success) { in.poses[0].position.x = 0.0; in.poses[1].position.x = 1.0; - bool ok = vortex::utils::ros_transforms::transform_pose( - *buffer_, in, "target", out, logger_); + EXPECT_NO_THROW({ + vortex::utils::ros_transforms::transform_pose(*buffer_, in, "target", + out); + }); - ASSERT_TRUE(ok); 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); } @@ -148,11 +132,20 @@ 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); - bool ok = vortex::utils::ros_transforms::transform_pose( - *buffer_, in, "target", out, logger_); + EXPECT_THROW( + { + vortex::utils::ros_transforms::transform_pose(*buffer_, in, + "target", out); + }, + tf2::TransformException); +} - EXPECT_FALSE(ok); +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return ret; } diff --git a/include/vortex/utils/ros_transforms.hpp b/include/vortex/utils/ros_transforms.hpp index 1cf200c..4264790 100644 --- a/include/vortex/utils/ros_transforms.hpp +++ b/include/vortex/utils/ros_transforms.hpp @@ -24,25 +24,16 @@ namespace vortex::utils::ros_transforms { * @param in Input PoseStamped message * @param target_frame Target frame ID * @param out Output PoseStamped message in the target frame - * @param logger ROS logger used for warning output - * @param timeout Maximum duration to wait for a valid transform - * @return true if the transform succeeded, false otherwise + * @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 bool transform_pose(tf2_ros::Buffer& tf_buffer, +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, - rclcpp::Logger logger, tf2::Duration timeout = tf2::durationFromSec(0.00)) { - try { - tf_buffer.transform(in, out, target_frame, timeout); - return true; - } catch (const tf2::TransformException& ex) { - RCLCPP_WARN(logger, "TF transform failed from '%s' to '%s': %s", - in.header.frame_id.c_str(), target_frame.c_str(), - ex.what()); - return false; - } + tf_buffer.transform(in, out, target_frame, timeout); } /** @@ -59,26 +50,17 @@ inline bool transform_pose(tf2_ros::Buffer& tf_buffer, * @param in Input PoseWithCovarianceStamped message * @param target_frame Target frame ID * @param out Output PoseWithCovarianceStamped message in the target frame - * @param logger ROS logger used for warning output * @param timeout Maximum duration to wait for a valid transform - * @return true if the transform succeeded, false otherwise + * @note This function throws tf2::TransformException on failure. + * Callers are expected to handle errors via try/catch. */ -inline bool transform_pose( +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, - rclcpp::Logger logger, tf2::Duration timeout = tf2::durationFromSec(0.00)) { - try { - tf_buffer.transform(in, out, target_frame, timeout); - return true; - } catch (const tf2::TransformException& ex) { - RCLCPP_WARN(logger, "TF transform failed from '%s' to '%s': %s", - in.header.frame_id.c_str(), target_frame.c_str(), - ex.what()); - return false; - } + tf_buffer.transform(in, out, target_frame, timeout); } /** @@ -95,15 +77,14 @@ inline bool transform_pose( * @param in Input PoseArray message * @param target_frame Target frame ID * @param out Output PoseArray message in the target frame - * @param logger ROS logger used for warning output * @param timeout Maximum duration to wait for each pose transform - * @return true if all poses were successfully transformed, false otherwise + * @note This function throws tf2::TransformException on failure. + * Callers are expected to handle errors via try/catch. */ -inline bool transform_pose(tf2_ros::Buffer& tf_buffer, +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, - rclcpp::Logger logger, tf2::Duration timeout = tf2::durationFromSec(0.0)) { out.poses.clear(); out.poses.reserve(in.poses.size()); @@ -117,19 +98,9 @@ inline bool transform_pose(tf2_ros::Buffer& tf_buffer, for (const auto& pose : in.poses) { in_ps.pose = pose; - try { - tf_buffer.transform(in_ps, out_ps, target_frame, timeout); - out.poses.push_back(out_ps.pose); - } catch (const tf2::TransformException& ex) { - RCLCPP_WARN(logger, - "TF transform failed for PoseArray element " - "from '%s' to '%s': %s", - in.header.frame_id.c_str(), target_frame.c_str(), - ex.what()); - return false; - } + tf_buffer.transform(in_ps, out_ps, target_frame, timeout); + out.poses.push_back(out_ps.pose); } - return true; } } // namespace vortex::utils::ros_transforms