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/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..9335be3 --- /dev/null +++ b/cpp_test/test_ros_transforms.cpp @@ -0,0 +1,151 @@ +#include + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "vortex/utils/ros_transforms.hpp" + +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(); + + 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 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; + + 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; +} diff --git a/include/vortex/utils/ros_transforms.hpp b/include/vortex/utils/ros_transforms.hpp new file mode 100644 index 0000000..4264790 --- /dev/null +++ b/include/vortex/utils/ros_transforms.hpp @@ -0,0 +1,108 @@ +#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 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_ 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