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