From 57afc76c735e6302dd2898a25c04e030181e5666 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Sun, 1 Mar 2026 14:28:49 +0100 Subject: [PATCH] eigen to pose msg --- .../vortex/utils/ros/ros_conversions.hpp | 24 +++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/vortex_utils_ros/include/vortex/utils/ros/ros_conversions.hpp b/vortex_utils_ros/include/vortex/utils/ros/ros_conversions.hpp index eb4782d..fa2b16f 100644 --- a/vortex_utils_ros/include/vortex/utils/ros/ros_conversions.hpp +++ b/vortex_utils_ros/include/vortex/utils/ros/ros_conversions.hpp @@ -57,6 +57,30 @@ geometry_msgs::msg::Pose to_pose_msg(const T& pose_like) { return pose; } +/** + * @brief Converts position and orientation in Eigen types to a ROS + * geometry_msgs::msg::Pose. + * + * @param pos Position as Eigen::Vector3d + * @param ori Orientation as Eigen::Quaterniond + * @return geometry_msgs::msg::Pose ROS pose message + */ +inline geometry_msgs::msg::Pose eigen_to_pose_msg( + const Eigen::Vector3d& pos, + const Eigen::Quaterniond& ori) { + geometry_msgs::msg::Pose pose; + pose.position.x = pos.x(); + pose.position.y = pos.y(); + pose.position.z = pos.z(); + + pose.orientation.w = ori.w(); + pose.orientation.x = ori.x(); + pose.orientation.y = ori.y(); + pose.orientation.z = ori.z(); + + return pose; +} + /** * @brief Converts a range of PoseLike objects to a vector of ROS pose messages. *