further conversions for moveit

ros · Apr 9, 2018 · fe70597 · fe70597
1 parent 45ae53b
commit fe70597
Show file tree

Hide file tree

Showing 2 changed files with 54 additions and 20 deletions.
diff --git a/tf2_eigen/include/tf2_eigen/tf2_eigen.h b/tf2_eigen/include/tf2_eigen/tf2_eigen.h
@@ -304,6 +304,23 @@ 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.
@@ -406,6 +423,11 @@ 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);

diff --git a/tf2_kdl/include/tf2_kdl/tf2_kdl.h b/tf2_kdl/include/tf2_kdl/tf2_kdl.h
@@ -246,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.
@@ -257,10 +287,7 @@ 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;
 }
 
@@ -274,24 +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;
-  fromMsg(msg.pose, static_cast<KDL::Frame&>(out))
+  fromMsg(msg.pose, static_cast<KDL::Frame&>(out));
 }
 
-/** \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);
-}
-
-
 } // namespace
 
 #endif // TF2_KDL_H