From 3d08107aab03d83072755572dc0a80c8e0c34802 Mon Sep 17 00:00:00 2001
From: Jochen Sprickerhof <git@jochen.sprickerhof.de>
Date: Fri, 25 Jan 2019 22:46:56 +0100
Subject: [PATCH] Move tf2::impl::toQuaternion to tf2_geometry_msgs

Closes: #275

This breaks the cycle between tf2 and tf2_geometry_msgs.
I didn't add deprecated attributes for functions having
tf2_geometry_msgs in the method signature, as they would need to have
the right header imported anyhow.
---
 tf2/include/tf2/impl/utils.h                  | 56 +++----------------
 .../tf2_geometry_msgs/tf2_geometry_msgs.h     | 55 ++++++++++++++++++
 2 files changed, 64 insertions(+), 47 deletions(-)

diff --git a/tf2/include/tf2/impl/utils.h b/tf2/include/tf2/impl/utils.h
index 5b545e54b..c29dcb8f2 100644
--- a/tf2/include/tf2/impl/utils.h
+++ b/tf2/include/tf2/impl/utils.h
@@ -15,64 +15,26 @@
 #ifndef TF2_IMPL_UTILS_H
 #define TF2_IMPL_UTILS_H
 
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #include <tf2/transform_datatypes.h>
 #include <tf2/LinearMath/Quaternion.h>
 
 namespace tf2 {
 namespace impl {
 
-/** Function needed for the generalization of toQuaternion
- * \param q a tf2::Quaternion
- * \return a copy of the same quaternion
- */
-inline
-tf2::Quaternion toQuaternion(const tf2::Quaternion& q) {
-    return q;
-  }
-
-/** Function needed for the generalization of toQuaternion
- * \param q a geometry_msgs::Quaternion
- * \return a copy of the same quaternion as a tf2::Quaternion
- */
+// functions moved to tf2_geometry_msgs/tf2_geometry_msgs.h to break a cycle
+#ifndef TF2_GEOMETRY_MSGS_H
 inline
-tf2::Quaternion toQuaternion(const geometry_msgs::Quaternion& q) {
-    tf2::Quaternion res;
-    fromMsg(q, res);
-    return res;
-  }
+tf2::Quaternion toQuaternion(const tf2::Quaternion& q)
+__attribute__((deprecated("please include tf2_geometry_msgs/tf2_geometry_msgs.h before tf2/impl/utils.h")));
 
-/** Function needed for the generalization of toQuaternion
- * \param q a geometry_msgs::QuaternionStamped
- * \return a copy of the same quaternion as a tf2::Quaternion
- */
-inline
-tf2::Quaternion toQuaternion(const geometry_msgs::QuaternionStamped& q) {
-    tf2::Quaternion res;
-    fromMsg(q.quaternion, res);
-    return res;
-  }
-
-/** Function needed for the generalization of toQuaternion
- * \param t some tf2::Stamped object
- * \return a copy of the same quaternion as a tf2::Quaternion
- */
 template<typename T>
-  tf2::Quaternion toQuaternion(const tf2::Stamped<T>& t) {
-    geometry_msgs::QuaternionStamped q = toMsg(t);
-    return toQuaternion(q);
-  }
+  tf2::Quaternion toQuaternion(const tf2::Stamped<T>& t)
+__attribute__((deprecated("please include tf2_geometry_msgs/tf2_geometry_msgs.h before tf2/impl/utils.h")));
 
-/** Generic version of toQuaternion. It tries to convert the argument
- * to a geometry_msgs::Quaternion
- * \param t some object
- * \return a copy of the same quaternion as a tf2::Quaternion
- */
 template<typename T>
-  tf2::Quaternion toQuaternion(const T& t) {
-    geometry_msgs::Quaternion q = toMsg(t);
-    return toQuaternion(q);
-  }
+  tf2::Quaternion toQuaternion(const T& t)
+__attribute__((deprecated("please include tf2_geometry_msgs/tf2_geometry_msgs.h before tf2/impl/utils.h")));
+#endif
 
 /** The code below is blantantly copied from urdfdom_headers
  * only the normalization has been added.
diff --git a/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.h b/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.h
index 3ef5dc2d4..6b2fd9566 100644
--- a/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.h
+++ b/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.h
@@ -312,6 +312,61 @@ void fromMsg(const geometry_msgs::Quaternion& in, tf2::Quaternion& out)
   out = tf2::Quaternion(in.x, in.y, in.z, in.w);
 }
 
+namespace impl {
+
+/** Function needed for the generalization of toQuaternion
+ * \param q a tf2::Quaternion
+ * \return a copy of the same quaternion
+ */
+inline
+tf2::Quaternion toQuaternion(const tf2::Quaternion& q) {
+    return q;
+  }
+
+/** Function needed for the generalization of toQuaternion
+ * \param q a geometry_msgs::Quaternion
+ * \return a copy of the same quaternion as a tf2::Quaternion
+ */
+inline
+tf2::Quaternion toQuaternion(const geometry_msgs::Quaternion& q) {
+    tf2::Quaternion res;
+    fromMsg(q, res);
+    return res;
+  }
+
+/** Function needed for the generalization of toQuaternion
+ * \param q a geometry_msgs::QuaternionStamped
+ * \return a copy of the same quaternion as a tf2::Quaternion
+ */
+inline
+tf2::Quaternion toQuaternion(const geometry_msgs::QuaternionStamped& q) {
+    tf2::Quaternion res;
+    fromMsg(q.quaternion, res);
+    return res;
+  }
+
+/** Function needed for the generalization of toQuaternion
+ * \param t some tf2::Stamped object
+ * \return a copy of the same quaternion as a tf2::Quaternion
+ */
+template<typename T>
+  tf2::Quaternion toQuaternion(const tf2::Stamped<T>& t) {
+    geometry_msgs::QuaternionStamped q = toMsg(t);
+    return toQuaternion(q);
+  }
+
+/** Generic version of toQuaternion. It tries to convert the argument
+ * to a geometry_msgs::Quaternion
+ * \param t some object
+ * \return a copy of the same quaternion as a tf2::Quaternion
+ */
+template<typename T>
+  tf2::Quaternion toQuaternion(const T& t) {
+    geometry_msgs::Quaternion q = toMsg(t);
+    return toQuaternion(q);
+  }
+} // namespace
+
 
 /***********************/
 /** QuaternionStamped **/