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

@@ -34,20 +34,30 @@
 #include <geometry_msgs/QuaternionStamped.h>
 #include <geometry_msgs/PointStamped.h>
 #include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/Twist.h>
 
 
 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<double,6,1>& 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<double,6,1>& 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<double,6,1>& in) {
+  return tf2::toMsg(in);
+}
+
+inline
+void fromMsg(const geometry_msgs::Twist &msg, Eigen::Matrix<double,6,1>& out) {
+  tf2::fromMsg(msg, out);
+}
+
 } // namespace
 
 #endif // TF2_EIGEN_H

tf2_kdl/include/tf2_kdl/tf2_kdl.h

@@ -38,6 +38,7 @@
 #include <geometry_msgs/TwistStamped.h>
 #include <geometry_msgs/WrenchStamped.h>
 #include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/Pose.h>
 
 
 namespace tf2
@@ -245,6 +246,36 @@ inline
     t_out = tf2::Stamped<KDL::Frame>(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<KDL::Frame>& 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<const KDL::Frame&>(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<KDL::Frame>& 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<KDL::Frame&>(out));
 }
 
-
 } // namespace
 
 #endif // TF2_KDL_H