Revert "rework Eigen functions namespace hack"

tf2/include/tf2/convert.h

@@ -32,11 +32,76 @@
 #ifndef TF2_CONVERT_H
 #define TF2_CONVERT_H
 
-#include <tf2/transform_functions.h>
+
+#include <tf2/transform_datatypes.h>
+#include <tf2/exceptions.h>
+#include <geometry_msgs/TransformStamped.h>
 #include <tf2/impl/convert.h>
 
 namespace tf2 {
 
+/**\brief The templated function expected to be able to do a transform
+ *
+ * This is the method which tf2 will use to try to apply a transform for any given datatype.   
+ * \param data_in The data to be transformed.
+ * \param data_out A reference to the output data.  Note this can point to data in and the method should be mutation safe.
+ * \param transform The transform to apply to data_in to fill data_out.  
+ * 
+ * This method needs to be implemented by client library developers
+ */
+template <class T>
+  void doTransform(const T& data_in, T& data_out, const geometry_msgs::TransformStamped& transform);
+
+/**\brief Get the timestamp from data 
+ * \param t The data input.
+ * \return The timestamp associated with the data. The lifetime of the returned
+ * reference is bound to the lifetime of the argument.
+ */
+template <class T>
+  const ros::Time& getTimestamp(const T& t);
+
+/**\brief Get the frame_id from data 
+ * \param t The data input.
+ * \return The frame_id associated with the data. The lifetime of the returned
+ * reference is bound to the lifetime of the argument.
+ */
+template <class T>
+  const std::string& getFrameId(const T& t);
+
+
+
+/* An implementation for Stamped<P> datatypes */
+template <class P>
+  const ros::Time& getTimestamp(const tf2::Stamped<P>& t)
+  {
+    return t.stamp_;
+  }
+
+/* An implementation for Stamped<P> datatypes */
+template <class P>
+  const std::string& getFrameId(const tf2::Stamped<P>& t)
+  {
+    return t.frame_id_;
+  }
+
+/** Function that converts from one type to a ROS message type. It has to be
+ * implemented by each data type in tf2_* (except ROS messages) as it is
+ * used in the "convert" function.
+ * \param a an object of whatever type
+ * \return the conversion as a ROS message
+ */
+template<typename A, typename B>
+  B toMsg(const A& a);
+
+/** Function that converts from a ROS message type to another type. It has to be
+ * implemented by each data type in tf2_* (except ROS messages) as it is used
+ * in the "convert" function.
+ * \param a a ROS message to convert from
+ * \param b the object to convert to
+ */
+template<typename A, typename B>
+  void fromMsg(const A&, B& b);
+
 /** Function that converts any type to any type (messages or not).
  * Matching toMsg and from Msg conversion functions need to exist.
  * If they don't exist or do not apply (for example, if your two
@@ -59,6 +124,7 @@ template <class A>
       a2 = a1;
   }
 
+
 }
 
 #endif //TF2_CONVERT_H

tf2_eigen/include/tf2_eigen/tf2_eigen.h

@@ -29,7 +29,7 @@
 #ifndef TF2_EIGEN_H
 #define TF2_EIGEN_H
 
-#include <tf2/transform_functions.h>
+#include <tf2/convert.h>
 #include <Eigen/Geometry>
 #include <geometry_msgs/QuaternionStamped.h>
 #include <geometry_msgs/PointStamped.h>
@@ -520,7 +520,66 @@ void fromMsg(const geometry_msgs::PoseStamped& msg, tf2::Stamped<Eigen::Isometry
 
 } // namespace
 
-// tf2/convert.h needs to be included after all toMsg/fromMsg are defined
-#include <tf2/convert.h>
+
+namespace Eigen {
+// This is needed to make the usage of the following conversion functions usable in tf2::convert().
+// According to clangs error note 'fromMsg'/'toMsg' should be declared prior to the call site or
+// in an associated namespace of one of its arguments. The stamped versions of this conversion
+// functions work because they have tf2::Stamped as an argument which is the same namespace as
+// which 'fromMsg'/'toMsg' is defined in. The non-stamped versions have no argument which is
+// defined in tf2, so it take the following definitions in Eigen namespace to make them usable in
+// tf2::convert().
+
+inline
+geometry_msgs::Pose toMsg(const Eigen::Affine3d& in) {
+  return tf2::toMsg(in);
+}
+
+inline
+geometry_msgs::Pose toMsg(const Eigen::Isometry3d& in) {
+  return tf2::toMsg(in);
+}
+
+inline
+void fromMsg(const geometry_msgs::Point& msg, Eigen::Vector3d& out) {
+  tf2::fromMsg(msg, out);
+}
+
+inline
+geometry_msgs::Point toMsg(const Eigen::Vector3d& in) {
+  return tf2::toMsg(in);
+}
+
+inline
+void fromMsg(const geometry_msgs::Pose& msg, Eigen::Affine3d& out) {
+  tf2::fromMsg(msg, out);
+}
+
+inline
+void fromMsg(const geometry_msgs::Pose& msg, Eigen::Isometry3d& out) {
+  tf2::fromMsg(msg, out);
+}
+
+inline
+geometry_msgs::Quaternion toMsg(const Eigen::Quaterniond& in) {
+  return tf2::toMsg(in);
+}
+
+inline
+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