From 3a28c61efdf0a1908a121e5b7f562b40ca3bc2b5 Mon Sep 17 00:00:00 2001
From: Jorgen Fjermedal <jorgen.fjermedal@hotmail.com>
Date: Sun, 21 Dec 2025 19:27:14 +0100
Subject: [PATCH] fix vec bug, added support for PoseWithCovariance

---
 cpp_test/test_ros_conversions.cpp        | 69 ++++++++++++++++++++++++
 include/vortex/utils/ros_conversions.hpp | 13 ++++-
 2 files changed, 80 insertions(+), 2 deletions(-)

diff --git a/cpp_test/test_ros_conversions.cpp b/cpp_test/test_ros_conversions.cpp
index 02a58e2..e8aa097 100644
--- a/cpp_test/test_ros_conversions.cpp
+++ b/cpp_test/test_ros_conversions.cpp
@@ -148,6 +148,75 @@ TEST(ros_to_pose_vec, pose) {
     EXPECT_NEAR(v[0].qz, 0.0, 1e-6);
 }
 
+TEST(ros_to_pose_vec, pose_stamped) {
+    geometry_msgs::msg::PoseStamped p;
+    p.pose.position.x = 1;
+    p.pose.position.y = 2;
+    p.pose.position.z = 3;
+    p.pose.orientation.w = 1;
+    p.pose.orientation.x = 0;
+    p.pose.orientation.y = 0;
+    p.pose.orientation.z = 0;
+
+    auto v = vortex::utils::ros_conversions::ros_to_pose_vec(p);
+
+    ASSERT_EQ(v.size(), 1);
+    EXPECT_NEAR(v[0].x, 1.0, 1e-6);
+    EXPECT_NEAR(v[0].y, 2.0, 1e-6);
+    EXPECT_NEAR(v[0].z, 3.0, 1e-6);
+
+    EXPECT_NEAR(v[0].qw, 1.0, 1e-6);
+    EXPECT_NEAR(v[0].qz, 0.0, 1e-6);
+    EXPECT_NEAR(v[0].qz, 0.0, 1e-6);
+    EXPECT_NEAR(v[0].qz, 0.0, 1e-6);
+}
+
+TEST(ros_to_pose_vec, pose_with_covariance) {
+    geometry_msgs::msg::PoseWithCovariance p;
+    p.pose.position.x = 1;
+    p.pose.position.y = 2;
+    p.pose.position.z = 3;
+    p.pose.orientation.w = 1;
+    p.pose.orientation.x = 0;
+    p.pose.orientation.y = 0;
+    p.pose.orientation.z = 0;
+
+    auto v = vortex::utils::ros_conversions::ros_to_pose_vec(p);
+
+    ASSERT_EQ(v.size(), 1);
+    EXPECT_NEAR(v[0].x, 1.0, 1e-6);
+    EXPECT_NEAR(v[0].y, 2.0, 1e-6);
+    EXPECT_NEAR(v[0].z, 3.0, 1e-6);
+
+    EXPECT_NEAR(v[0].qw, 1.0, 1e-6);
+    EXPECT_NEAR(v[0].qz, 0.0, 1e-6);
+    EXPECT_NEAR(v[0].qz, 0.0, 1e-6);
+    EXPECT_NEAR(v[0].qz, 0.0, 1e-6);
+}
+
+TEST(ros_to_pose_vec, pose_with_covariance_stamped) {
+    geometry_msgs::msg::PoseWithCovarianceStamped p;
+    p.pose.pose.position.x = 1;
+    p.pose.pose.position.y = 2;
+    p.pose.pose.position.z = 3;
+    p.pose.pose.orientation.w = 1;
+    p.pose.pose.orientation.x = 0;
+    p.pose.pose.orientation.y = 0;
+    p.pose.pose.orientation.z = 0;
+
+    auto v = vortex::utils::ros_conversions::ros_to_pose_vec(p);
+
+    ASSERT_EQ(v.size(), 1);
+    EXPECT_NEAR(v[0].x, 1.0, 1e-6);
+    EXPECT_NEAR(v[0].y, 2.0, 1e-6);
+    EXPECT_NEAR(v[0].z, 3.0, 1e-6);
+
+    EXPECT_NEAR(v[0].qw, 1.0, 1e-6);
+    EXPECT_NEAR(v[0].qz, 0.0, 1e-6);
+    EXPECT_NEAR(v[0].qz, 0.0, 1e-6);
+    EXPECT_NEAR(v[0].qz, 0.0, 1e-6);
+}
+
 TEST(ros_to_pose_vec, pose_array) {
     geometry_msgs::msg::PoseArray arr;
     arr.poses.resize(2);
diff --git a/include/vortex/utils/ros_conversions.hpp b/include/vortex/utils/ros_conversions.hpp
index aa2067f..be58750 100644
--- a/include/vortex/utils/ros_conversions.hpp
+++ b/include/vortex/utils/ros_conversions.hpp
@@ -11,6 +11,7 @@
 #include <geometry_msgs/msg/pose.hpp>
 #include <geometry_msgs/msg/pose_array.hpp>
 #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 "concepts.hpp"
@@ -39,6 +40,8 @@ concept same_bare_as = std::same_as<std::remove_cvref_t<T>, U>;
  *
  *  - `geometry_msgs::msg::PoseStamped`
  *
+ *  - `geometry_msgs::msg::PoseWithCovariance`
+ *
  *  - `geometry_msgs::msg::PoseWithCovarianceStamped`
  *
  *  - `geometry_msgs::msg::PoseArray`
@@ -49,6 +52,7 @@ template <typename T>
 concept ROSPoseLike =
     same_bare_as<T, geometry_msgs::msg::Pose> ||
     same_bare_as<T, geometry_msgs::msg::PoseStamped> ||
+    same_bare_as<T, geometry_msgs::msg::PoseWithCovariance> ||
     same_bare_as<T, geometry_msgs::msg::PoseWithCovarianceStamped> ||
     same_bare_as<T, geometry_msgs::msg::PoseArray>;
 
@@ -140,6 +144,8 @@ inline vortex::utils::types::Pose ros_pose_to_pose(
  *
  *  - `geometry_msgs::msg::PoseStamped`
  *
+ *  - `geometry_msgs::msg::PoseWithCovariance`
+ *
  *  - `geometry_msgs::msg::PoseWithCovarianceStamped`
  *
  *  - `geometry_msgs::msg::PoseArray`
@@ -156,11 +162,14 @@ std::vector<vortex::utils::types::Pose> ros_to_pose_vec(const T& msg) {
     if constexpr (same_bare_as<T, geometry_msgs::msg::Pose>) {
         return {ros_pose_to_pose(msg)};
     } else if constexpr (same_bare_as<T, geometry_msgs::msg::PoseStamped>) {
-        return ros_pose_to_pose(msg.pose);
+        return {ros_pose_to_pose(msg.pose)};
+    } else if constexpr (same_bare_as<T,
+                                      geometry_msgs::msg::PoseWithCovariance>) {
+        return {ros_pose_to_pose(msg.pose)};
     } else if constexpr (same_bare_as<
                              T,
                              geometry_msgs::msg::PoseWithCovarianceStamped>) {
-        return ros_pose_to_pose(msg.pose.pose);
+        return {ros_pose_to_pose(msg.pose.pose)};
     } else if constexpr (same_bare_as<T, geometry_msgs::msg::PoseArray>) {
         std::vector<vortex::utils::types::Pose> poses;
         poses.reserve(msg.poses.size());