Feat/landmark support (#30)

* landmark array to pose vector

* landmark transform

* workflow msgs dependency

* split declarations

Author: jorgenfj

.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 }}

.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'

dependencies.repos

@@ -0,0 +1,4 @@
+repositories:
+  vortex-msgs:
+    type: git
+    url: https://github.com/vortexntnu/vortex-msgs.git

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(

vortex_utils_ros/cpp_test/test_ros_conversions.cpp

@@ -7,6 +7,7 @@
 #include <geometry_msgs/msg/pose_array.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <vortex_msgs/msg/landmark_array.hpp>
 
 #include <vortex/utils/math.hpp>
 #include <vortex/utils/types.hpp>
@@ -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);
+}

vortex_utils_ros/include/vortex/utils/ros/ros_conversions.hpp

@@ -13,6 +13,7 @@
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/pose_with_covariance.hpp>
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <vortex_msgs/msg/landmark_array.hpp>
 
 #include <vortex/utils/concepts.hpp>
 #include <vortex/utils/math.hpp>
@@ -159,6 +160,22 @@ inline std::vector<vortex::utils::types::Pose> 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<vortex::utils::types::Pose> Internal pose representation
+ */
+inline std::vector<vortex::utils::types::Pose> ros_to_pose_vec(
+    const vortex_msgs::msg::LandmarkArray& msg) {
+    std::vector<vortex::utils::types::Pose> 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_

vortex_utils_ros/package.xml

@@ -16,6 +16,7 @@
   <depend>python3-numpy</depend>
   <depend>python3-scipy</depend>
   <depend>geometry_msgs</depend>
+  <depend>vortex_msgs</depend>
 
 
   <test_depend>ament_cmake_pytest</test_depend>

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(

vortex_utils_ros_tf/cpp_test/test_ros_transforms.cpp

@@ -6,6 +6,7 @@
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
 #include <geometry_msgs/msg/transform_stamped.hpp>
+#include <vortex_msgs/msg/landmark_array.hpp>
 
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/static_transform_broadcaster.h>
@@ -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);

vortex_utils_ros_tf/include/vortex/utils/ros/ros_transforms.hpp

@@ -4,6 +4,7 @@
 #include <geometry_msgs/msg/pose_array.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <vortex_msgs/msg/landmark_array.hpp>
 
 #include <tf2/exceptions.h>
 #include <tf2/time.h>
@@ -103,6 +104,49 @@ 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;
+        geometry_msgs::msg::PoseWithCovarianceStamped 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_