From 20a0753a384e75b63aa3e5c68de9f572d59a3946 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Wed, 24 Dec 2025 12:59:15 +0100 Subject: [PATCH 1/4] landmark array to pose vector --- vortex_utils_ros/CMakeLists.txt | 2 ++ .../cpp_test/test_ros_conversions.cpp | 15 +++++++++++++++ .../vortex/utils/ros/ros_conversions.hpp | 17 +++++++++++++++++ vortex_utils_ros/package.xml | 1 + 4 files changed, 35 insertions(+) diff --git a/vortex_utils_ros/CMakeLists.txt b/vortex_utils_ros/CMakeLists.txt index 20c7f3c..1736966 100644 --- a/vortex_utils_ros/CMakeLists.txt +++ b/vortex_utils_ros/CMakeLists.txt @@ -10,6 +10,7 @@ find_package(rclcpp REQUIRED) find_package(ament_cmake_python REQUIRED) find_package(Eigen3 REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(vortex_msgs REQUIRED) add_library(vortex_utils_ros INTERFACE) @@ -23,6 +24,7 @@ ament_target_dependencies(vortex_utils_ros INTERFACE rclcpp Eigen3 geometry_msgs + vortex_msgs ) install( diff --git a/vortex_utils_ros/cpp_test/test_ros_conversions.cpp b/vortex_utils_ros/cpp_test/test_ros_conversions.cpp index 9eec425..8d569a6 100644 --- a/vortex_utils_ros/cpp_test/test_ros_conversions.cpp +++ b/vortex_utils_ros/cpp_test/test_ros_conversions.cpp @@ -7,6 +7,7 @@ #include #include #include +#include #include #include @@ -214,3 +215,17 @@ TEST(ros_to_pose_vec, pose_array) { EXPECT_NEAR(v[0].x, 1.0, 1e-6); EXPECT_NEAR(v[1].x, 2.0, 1e-6); } + +TEST(ros_to_pose_vec, landmark_array) { + vortex_msgs::msg::LandmarkArray arr; + arr.landmarks.resize(2); + + arr.landmarks[0].pose.pose.position.x = 1; + arr.landmarks[1].pose.pose.position.x = 2; + + auto v = vortex::utils::ros_conversions::ros_to_pose_vec(arr); + + 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/vortex_utils_ros/include/vortex/utils/ros/ros_conversions.hpp b/vortex_utils_ros/include/vortex/utils/ros/ros_conversions.hpp index eb3999c..1c57074 100644 --- a/vortex_utils_ros/include/vortex/utils/ros/ros_conversions.hpp +++ b/vortex_utils_ros/include/vortex/utils/ros/ros_conversions.hpp @@ -13,6 +13,7 @@ #include #include #include +#include #include #include @@ -159,6 +160,22 @@ inline std::vector ros_to_pose_vec( return poses; } +/** + * @brief Converts a ROS vortex_msgs::msg::LandmarkArray to an internal Pose + * vector type. + * @param pose vortex_msgs::msg::LandmarkArray + * @return std::vector Internal pose representation + */ +inline std::vector ros_to_pose_vec( + const vortex_msgs::msg::LandmarkArray& msg) { + std::vector poses; + poses.reserve(msg.landmarks.size()); + for (const auto& landmark : msg.landmarks) { + poses.push_back(ros_pose_to_pose(landmark.pose.pose)); + } + return poses; +} + } // namespace vortex::utils::ros_conversions #endif // VORTEX_UTILS__ROS_CONVERSIONS_HPP_ diff --git a/vortex_utils_ros/package.xml b/vortex_utils_ros/package.xml index 087735b..d307818 100644 --- a/vortex_utils_ros/package.xml +++ b/vortex_utils_ros/package.xml @@ -16,6 +16,7 @@ python3-numpy python3-scipy geometry_msgs + vortex_msgs ament_cmake_pytest From 102cafe44fb41ba994b678480d86f7b537778bd3 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Wed, 24 Dec 2025 14:09:41 +0100 Subject: [PATCH 2/4] landmark transform --- vortex_utils_ros_tf/CMakeLists.txt | 2 + .../cpp_test/test_ros_transforms.cpp | 34 +++++++++++++++ .../vortex/utils/ros/ros_transforms.hpp | 43 +++++++++++++++++++ vortex_utils_ros_tf/package.xml | 1 + 4 files changed, 80 insertions(+) diff --git a/vortex_utils_ros_tf/CMakeLists.txt b/vortex_utils_ros_tf/CMakeLists.txt index f5ceb49..f7ca731 100644 --- a/vortex_utils_ros_tf/CMakeLists.txt +++ b/vortex_utils_ros_tf/CMakeLists.txt @@ -11,6 +11,7 @@ find_package(geometry_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_geometry_msgs REQUIRED) +find_package(vortex_msgs REQUIRED) add_library(vortex_utils_ros_tf INTERFACE) @@ -25,6 +26,7 @@ ament_target_dependencies(vortex_utils_ros_tf INTERFACE tf2_ros tf2_geometry_msgs geometry_msgs + vortex_msgs ) install( diff --git a/vortex_utils_ros_tf/cpp_test/test_ros_transforms.cpp b/vortex_utils_ros_tf/cpp_test/test_ros_transforms.cpp index f87feac..6075522 100644 --- a/vortex_utils_ros_tf/cpp_test/test_ros_transforms.cpp +++ b/vortex_utils_ros_tf/cpp_test/test_ros_transforms.cpp @@ -6,6 +6,7 @@ #include #include #include +#include #include #include @@ -142,6 +143,39 @@ TEST_F(RosTransformsTest, pose_array_failure) { tf2::TransformException); } +TEST_F(RosTransformsTest, landmark_array_failure) { + vortex_msgs::msg::LandmarkArray in, out; + in.header.frame_id = "unknown_frame"; + in.header.stamp = node_->get_clock()->now(); + in.landmarks.resize(1); + + EXPECT_THROW( + { + vortex::utils::ros_transforms::transform_pose(*buffer_, in, + "target", out); + }, + tf2::TransformException); +} + +TEST_F(RosTransformsTest, landmark_array_success) { + vortex_msgs::msg::LandmarkArray in, out; + in.header.frame_id = "source"; + in.header.stamp = node_->get_clock()->now(); + + in.landmarks.resize(2); + in.landmarks[0].pose.pose.position.x = 0.0; + in.landmarks[1].pose.pose.position.x = 1.0; + + EXPECT_NO_THROW({ + vortex::utils::ros_transforms::transform_pose(*buffer_, in, "target", + out); + }); + + ASSERT_EQ(out.landmarks.size(), 2); + EXPECT_NEAR(out.landmarks[0].pose.pose.position.x, -1.0, 1e-6); + EXPECT_NEAR(out.landmarks[1].pose.pose.position.x, 0.0, 1e-6); +} + int main(int argc, char** argv) { rclcpp::init(argc, argv); ::testing::InitGoogleTest(&argc, argv); diff --git a/vortex_utils_ros_tf/include/vortex/utils/ros/ros_transforms.hpp b/vortex_utils_ros_tf/include/vortex/utils/ros/ros_transforms.hpp index 4264790..70aadb5 100644 --- a/vortex_utils_ros_tf/include/vortex/utils/ros/ros_transforms.hpp +++ b/vortex_utils_ros_tf/include/vortex/utils/ros/ros_transforms.hpp @@ -4,6 +4,7 @@ #include #include #include +#include #include #include @@ -103,6 +104,48 @@ inline void transform_pose(tf2_ros::Buffer& tf_buffer, } } +/** + * @brief Transform all poses in a LandmarkArray into a target frame using TF. + * + * Each pose in the input LandmarkArray is individually transformed using the + * array header (frame ID and timestamp). The output LandmarkArray 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 vortex_msgs::msg::LandmarkArray& in, + const std::string& target_frame, + vortex_msgs::msg::LandmarkArray& out, + tf2::Duration timeout = tf2::durationFromSec(0.0)) { + out.header.stamp = in.header.stamp; + out.header.frame_id = target_frame; + out.landmarks.clear(); + out.landmarks.reserve(in.landmarks.size()); + + for (const auto& lm : in.landmarks) { + vortex_msgs::msg::Landmark lm_out = lm; + + geometry_msgs::msg::PoseWithCovarianceStamped in_ps, out_ps; + in_ps.header = in.header; + in_ps.pose = lm.pose; + + tf_buffer.transform(in_ps, out_ps, target_frame, timeout); + + lm_out.pose = out_ps.pose; + out.landmarks.push_back(lm_out); + } +} + } // namespace vortex::utils::ros_transforms #endif // VORTEX_UTILS__ROS_TRANSFORMS_HPP_ diff --git a/vortex_utils_ros_tf/package.xml b/vortex_utils_ros_tf/package.xml index c783321..45c459f 100644 --- a/vortex_utils_ros_tf/package.xml +++ b/vortex_utils_ros_tf/package.xml @@ -15,6 +15,7 @@ tf2_ros tf2_geometry_msgs geometry_msgs + vortex_msgs ament_cmake_gtest From 9fc8d8ce223b7ae801cf64897fc483b804346ee0 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Fri, 26 Dec 2025 16:17:07 +0100 Subject: [PATCH 3/4] workflow msgs dependency --- .github/workflows/code-coverage.yml | 1 + .github/workflows/industrial-ci.yml | 1 + dependencies.repos | 4 ++++ 3 files changed, 6 insertions(+) create mode 100644 dependencies.repos diff --git a/.github/workflows/code-coverage.yml b/.github/workflows/code-coverage.yml index f685149..8856d7f 100644 --- a/.github/workflows/code-coverage.yml +++ b/.github/workflows/code-coverage.yml @@ -11,6 +11,7 @@ jobs: call_reusable_workflow: uses: vortexntnu/vortex-ci/.github/workflows/reusable-code-coverage.yml@main with: + vcs-repo-file-url: './dependencies.repos' before_install_target_dependencies: 'scripts/ci_install_dependencies.sh' secrets: CODECOV_TOKEN: ${{ secrets.CODECOV_TOKEN }} diff --git a/.github/workflows/industrial-ci.yml b/.github/workflows/industrial-ci.yml index c9adce3..68d427e 100644 --- a/.github/workflows/industrial-ci.yml +++ b/.github/workflows/industrial-ci.yml @@ -10,5 +10,6 @@ jobs: call_reusable_workflow: uses: vortexntnu/vortex-ci/.github/workflows/reusable-industrial-ci.yml@main with: + upstream_workspace: './dependencies.repos' ros_repo: '["main", "testing"]' before_install_target_dependencies: 'scripts/ci_install_dependencies.sh' diff --git a/dependencies.repos b/dependencies.repos new file mode 100644 index 0000000..4475316 --- /dev/null +++ b/dependencies.repos @@ -0,0 +1,4 @@ +repositories: + vortex-msgs: + type: git + url: https://github.com/vortexntnu/vortex-msgs.git From c45ce9b64495c49c671b7b6007b8e2c9096307c0 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Tue, 6 Jan 2026 13:21:31 +0100 Subject: [PATCH 4/4] split declarations --- .../include/vortex/utils/ros/ros_transforms.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/vortex_utils_ros_tf/include/vortex/utils/ros/ros_transforms.hpp b/vortex_utils_ros_tf/include/vortex/utils/ros/ros_transforms.hpp index 70aadb5..0fe4cbb 100644 --- a/vortex_utils_ros_tf/include/vortex/utils/ros/ros_transforms.hpp +++ b/vortex_utils_ros_tf/include/vortex/utils/ros/ros_transforms.hpp @@ -135,7 +135,8 @@ inline void transform_pose(tf2_ros::Buffer& tf_buffer, for (const auto& lm : in.landmarks) { vortex_msgs::msg::Landmark lm_out = lm; - geometry_msgs::msg::PoseWithCovarianceStamped in_ps, out_ps; + geometry_msgs::msg::PoseWithCovarianceStamped in_ps; + geometry_msgs::msg::PoseWithCovarianceStamped out_ps; in_ps.header = in.header; in_ps.pose = lm.pose;