From 26f1694b75af899a4a098035eb0456cac3cdb781 Mon Sep 17 00:00:00 2001 From: Ian McMahon Date: Thu, 26 Apr 2018 18:56:54 -0400 Subject: [PATCH] Adds additional conversions for tf2, KDL, Eigen (#292) - adds non-stamped Eigen to Transform function - converts Eigen Matrix Vectors to and from geometry_msgs::Twist - adds to/from message for geometry_msgs::Pose and KDL::Frame --- tf2_eigen/include/tf2_eigen/tf2_eigen.h | 58 +++++++++++++++++++++++-- tf2_kdl/include/tf2_kdl/tf2_kdl.h | 44 ++++++++++++++----- 2 files changed, 89 insertions(+), 13 deletions(-) diff --git a/tf2_eigen/include/tf2_eigen/tf2_eigen.h b/tf2_eigen/include/tf2_eigen/tf2_eigen.h index de0e825f2..1f5d1b84d 100644 --- a/tf2_eigen/include/tf2_eigen/tf2_eigen.h +++ b/tf2_eigen/include/tf2_eigen/tf2_eigen.h @@ -34,20 +34,30 @@ #include #include #include +#include namespace tf2 { +/** \brief Convert a timestamped transform to the equivalent Eigen data type. + * \param t The transform to convert, as a geometry_msgs Transform message. + * \return The transform message converted to an Eigen Affine3d transform. + */ + inline + Eigen::Affine3d transformToEigen(const geometry_msgs::Transform& t) { + return Eigen::Affine3d(Eigen::Translation3d(t.translation.x, t.translation.y, t.translation.z) + * Eigen::Quaterniond(t.rotation.w, + t.rotation.x, t.rotation.y, t.rotation.z)); +} + /** \brief Convert a timestamped transform to the equivalent Eigen data type. * \param t The transform to convert, as a geometry_msgs TransformedStamped message. * \return The transform message converted to an Eigen Affine3d transform. */ inline Eigen::Affine3d transformToEigen(const geometry_msgs::TransformStamped& t) { - return Eigen::Affine3d(Eigen::Translation3d(t.transform.translation.x, t.transform.translation.y, t.transform.translation.z) - * Eigen::Quaterniond(t.transform.rotation.w, - t.transform.rotation.x, t.transform.rotation.y, t.transform.rotation.z)); + return transformToEigen(t.transform); } /** \brief Convert an Eigen Affine3d transform to the equivalent geometry_msgs message type. @@ -294,6 +304,38 @@ void fromMsg(const geometry_msgs::Pose& msg, Eigen::Affine3d& out) { msg.orientation.z)); } +/** \brief Convert a Eigen 6x1 Matrix type to a Twist message. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param in The 6x1 Eigen Matrix to convert. + * \return The Eigen Matrix converted to a Twist message. + */ +inline +geometry_msgs::Twist toMsg(const Eigen::Matrix& in) { + geometry_msgs::Twist msg; + msg.linear.x = in[0]; + msg.linear.y = in[1]; + msg.linear.z = in[2]; + msg.angular.x = in[3]; + msg.angular.y = in[4]; + msg.angular.z = in[5]; + return msg; +} + +/** \brief Convert a Pose message transform type to a Eigen Affine3d. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param msg The Twist message to convert. + * \param out The twist converted to a Eigen Matrix. + */ +inline +void fromMsg(const geometry_msgs::Twist &msg, Eigen::Matrix& out) { + out[0] = msg.linear.x; + out[1] = msg.linear.y; + out[2] = msg.linear.z; + out[3] = msg.angular.x; + out[4] = msg.angular.y; + out[5] = msg.angular.z; +} + /** \brief Apply a geometry_msgs TransformStamped to a Eigen-specific affine transform data type. * This function is a specialization of the doTransform template defined in tf2/convert.h, * although it can not be used in tf2_ros::BufferInterface::transform because this @@ -381,6 +423,16 @@ void fromMsg(const geometry_msgs::Quaternion& msg, Eigen::Quaterniond& out) { tf2::fromMsg(msg, out); } +inline +geometry_msgs::Twist toMsg(const Eigen::Matrix& in) { + return tf2::toMsg(in); +} + +inline +void fromMsg(const geometry_msgs::Twist &msg, Eigen::Matrix& out) { + tf2::fromMsg(msg, out); +} + } // namespace #endif // TF2_EIGEN_H diff --git a/tf2_kdl/include/tf2_kdl/tf2_kdl.h b/tf2_kdl/include/tf2_kdl/tf2_kdl.h index 3d258f630..e1f28bfb1 100644 --- a/tf2_kdl/include/tf2_kdl/tf2_kdl.h +++ b/tf2_kdl/include/tf2_kdl/tf2_kdl.h @@ -38,6 +38,7 @@ #include #include #include +#include namespace tf2 @@ -245,6 +246,36 @@ inline t_out = tf2::Stamped(transformToKDL(transform) * t_in, transform.header.stamp, transform.header.frame_id); } +/** \brief Convert a stamped KDL Frame type to a Pose message. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param in The timestamped KDL Frame to convert. + * \return The frame converted to a Pose message. + */ +inline +geometry_msgs::Pose toMsg(const KDL::Frame& in) +{ + geometry_msgs::Pose msg; + msg.position.x = in.p[0]; + msg.position.y = in.p[1]; + msg.position.z = in.p[2]; + in.M.GetQuaternion(msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w); + return msg; +} + +/** \brief Convert a Pose message type to a KDL Frame. + * This function is a specialization of the fromMsg template defined in tf2/convert.h. + * \param msg The Pose message to convert. + * \param out The pose converted to a KDL Frame. + */ +inline +void fromMsg(const geometry_msgs::Pose& msg, KDL::Frame& out) +{ + out.p[0] = msg.position.x; + out.p[1] = msg.position.y; + out.p[2] = msg.position.z; + out.M = KDL::Rotation::Quaternion(msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w); +} + /** \brief Convert a stamped KDL Frame type to a Pose message. * This function is a specialization of the toMsg template defined in tf2/convert.h. * \param in The timestamped KDL Frame to convert. @@ -256,15 +287,12 @@ geometry_msgs::PoseStamped toMsg(const tf2::Stamped& in) geometry_msgs::PoseStamped msg; msg.header.stamp = in.stamp_; msg.header.frame_id = in.frame_id_; - msg.pose.position.x = in.p[0]; - msg.pose.position.y = in.p[1]; - msg.pose.position.z = in.p[2]; - in.M.GetQuaternion(msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w); + msg.pose = toMsg(static_cast(in)); return msg; } /** \brief Convert a Pose message transform type to a stamped KDL Frame. - * This function is a specialization of the toMsg template defined in tf2/convert.h. + * This function is a specialization of the fromMsg template defined in tf2/convert.h. * \param msg The PoseStamped message to convert. * \param out The pose converted to a timestamped KDL Frame. */ @@ -273,13 +301,9 @@ void fromMsg(const geometry_msgs::PoseStamped& msg, tf2::Stamped& ou { out.stamp_ = msg.header.stamp; out.frame_id_ = msg.header.frame_id; - out.p[0] = msg.pose.position.x; - out.p[1] = msg.pose.position.y; - out.p[2] = msg.pose.position.z; - out.M = KDL::Rotation::Quaternion(msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w); + fromMsg(msg.pose, static_cast(out)); } - } // namespace #endif // TF2_KDL_H