Adds additional conversions for tf2, KDL, Eigen

Originally PR #292 at ros/geometry2#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

@@ -546,6 +546,16 @@ geometry_msgs::msg::Quaternion toMsg(const Eigen::Quaterniond& in) {
   return tf2::toMsg(in);
 }
 
+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/msg/point_stamped.hpp>
 #include <geometry_msgs/msg/twist_stamped.hpp>
 #include <geometry_msgs/msg/wrench_stamped.hpp>
+#include <geometry_msgs/msg/pose.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <builtin_interfaces/msg/time.hpp>
 #include <geometry_msgs/msg/transform_stamped.hpp>
@@ -247,6 +248,36 @@ inline
     t_out = tf2::Stamped<KDL::Frame>(transformToKDL(transform) * t_in,  tf2_ros::fromMsg(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::msg::Pose toMsg(const KDL::Frame& in)
+{
+  geometry_msgs::msg::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::msg::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.
@@ -258,15 +289,12 @@ geometry_msgs::msg::PoseStamped toMsg(const tf2::Stamped<KDL::Frame>& in)
   geometry_msgs::msg::PoseStamped msg;
   msg.header.stamp = tf2_ros::toMsg(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.
  */
@@ -275,13 +303,9 @@ void fromMsg(const geometry_msgs::msg::PoseStamped& msg, tf2::Stamped<KDL::Frame
 {
   out.stamp_ = tf2_ros::fromMsg(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