Merge pull request #52 from ayden175/odom

Add odometry conversion
dfki-ric · Oct 24, 2024 · 1e2bc36 · 1e2bc36
2 parents 602afe2 + 97f7a24
commit 1e2bc36
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 1 deletion.
diff --git 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
diff --git 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