From 97f7a24888a3e4130ca0d5ab638b817ac37d46e1 Mon Sep 17 00:00:00 2001
From: ayden175 <47821208+ayden175@users.noreply.github.com>
Date: Tue, 22 Oct 2024 16:53:31 +0200
Subject: [PATCH] Add rrc odom to ros2 odom conversion

---
 src/Types/Conversions/ros2/geometry_msgs/Pose.hpp | 9 ++++++++-
 src/Types/Conversions/ros2/nav_msgs/Odometry.hpp  | 6 ++++++
 2 files changed, 14 insertions(+), 1 deletion(-)

diff --git a/src/Types/Conversions/ros2/geometry_msgs/Pose.hpp b/src/Types/Conversions/ros2/geometry_msgs/Pose.hpp
index 1483d7a..95d22c0 100644
--- a/src/Types/Conversions/ros2/geometry_msgs/Pose.hpp
+++ b/src/Types/Conversions/ros2/geometry_msgs/Pose.hpp
@@ -39,5 +39,12 @@ namespace RosConversion {
         }
     }
 
+    inline static void convert(const robot_remote_control::PoseWithCovariance &from, geometry_msgs::msg::PoseWithCovariance *to ) {
+        convert(from.pose(), &to->pose);
+        for (int i = 0; i < from.covariance_size(); i++) {
+            to->covariance[i] = from.covariance(i);
+        }
+    }
+
 }  // namespace RosConversion
-}  // namespace robot_remote_control
+}  // namespace robot_remote_control
\ No newline at end of file
diff --git a/src/Types/Conversions/ros2/nav_msgs/Odometry.hpp b/src/Types/Conversions/ros2/nav_msgs/Odometry.hpp
index a7fa30f..cf1795a 100644
--- a/src/Types/Conversions/ros2/nav_msgs/Odometry.hpp
+++ b/src/Types/Conversions/ros2/nav_msgs/Odometry.hpp
@@ -21,5 +21,11 @@ namespace RosConversion {
         to->set_child_frame_id(from.child_frame_id);
     }
 
+    static void convert(const robot_remote_control::Odometry &from, nav_msgs::msg::Odometry *to) {
+        convert(from.header(), &to->header);
+        to->child_frame_id = from.child_frame_id();
+        convert(from.pose(), &to->pose);
+    }
+
 }  // namespace RosConversion
 }  // namespace robot_remote_control
\ No newline at end of file