landmark array to pose vector

Author: jorgenfj

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>