New approach for the tf2::toMsg() hassle

test_tf2/test/test_convert.cpp

@@ -110,9 +110,81 @@ TEST(tf2Convert, ConvertTf2Quaternion)
   EXPECT_EQ(tq.z(), eq.z());
 }
 
+TEST(tf2Convert, PointVectorDefaultMessagetype)
+{
+  // Verify the return type of `toMsg()`
+  // as it can return a Vector3 or a Point for certain datatypes
+  {
+    // Bullet
+    const tf2::Stamped<btVector3> b1{btVector3{1.0, 3.0, 4.0}, ros::Time(), "my_frame" };
+    const geometry_msgs::PointStamped msg = tf2::toMsg(b1);
+
+    EXPECT_EQ(msg.point.x, 1.0);
+    EXPECT_EQ(msg.point.y, 3.0);
+    EXPECT_EQ(msg.point.z, 4.0);
+    EXPECT_EQ(msg.header.frame_id, b1.frame_id_);
+    EXPECT_EQ(msg.header.stamp, b1.stamp_);
+  }
+  {
+    // Eigen
+    const Eigen::Vector3d e1{2.0, 4.0, 5.0};
+    const geometry_msgs::Point msg = tf2::toMsg(e1);
+
+    EXPECT_EQ(msg.x, 2.0);
+    EXPECT_EQ(msg.y, 4.0);
+    EXPECT_EQ(msg.z, 5.0);
+  }
+  {
+    // tf2
+    const tf2::Vector3 t1{2.0, 4.0, 5.0};
+    const geometry_msgs::Vector3 msg = tf2::toMsg(t1);
+
+    EXPECT_EQ(msg.x, 2.0);
+    EXPECT_EQ(msg.y, 4.0);
+    EXPECT_EQ(msg.z, 5.0);
+  }
+  {
+    // KDL
+    const tf2::Stamped<KDL::Vector> k1{KDL::Vector{1.0, 3.0, 4.0}, ros::Time(), "my_frame"};
+    const geometry_msgs::PointStamped msg = tf2::toMsg(k1);
+
+    EXPECT_EQ(msg.point.x, 1.0);
+    EXPECT_EQ(msg.point.y, 3.0);
+    EXPECT_EQ(msg.point.z, 4.0);
+    EXPECT_EQ(msg.header.frame_id, k1.frame_id_);
+    EXPECT_EQ(msg.header.stamp, k1.stamp_);
+  }
+}
+
+TEST(tf2Convert, PointVectorOtherMessagetype)
+{
+  {
+    const tf2::Vector3 t1{2.0, 4.0, 5.0};
+    geometry_msgs::Point msg;
+    const geometry_msgs::Point& msg2 = tf2::toMsg(t1, msg);
+
+    // returned reference is second argument
+    EXPECT_EQ(&msg2, &msg);
+    EXPECT_EQ(msg.x, 2.0);
+    EXPECT_EQ(msg.y, 4.0);
+    EXPECT_EQ(msg.z, 5.0);
+  }
+  {
+    // Eigen
+    const Eigen::Vector3d e1{2.0, 4.0, 5.0};
+    geometry_msgs::Vector3 msg;
+    const geometry_msgs::Vector3& msg2 = tf2::toMsg(e1, msg);
+
+    // returned reference is second argument
+    EXPECT_EQ(&msg2, &msg);
+    EXPECT_EQ(msg.x, 2.0);
+    EXPECT_EQ(msg.y, 4.0);
+    EXPECT_EQ(msg.z, 5.0);
+  }
+}
+
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);
   return RUN_ALL_TESTS();
 }
-

tf2/include/tf2/convert.h

@@ -32,99 +32,190 @@
 #ifndef TF2_CONVERT_H
 #define TF2_CONVERT_H
 
-
 #include <tf2/transform_datatypes.h>
 #include <tf2/exceptions.h>
 #include <geometry_msgs/TransformStamped.h>
-#include <tf2/impl/convert.h>
 
-namespace tf2 {
+namespace tf2
+{
+namespace impl
+{
+/**
+ * \brief Mapping between Datatypes (like \c Vector3d ) and their default ROS Message types.
+ *
+ * This struct should be specialized for each non-Message datatypes,
+ * and it should contain an alias of the Message class with the name \c type .
+ * This alias will be used to deduce the return value of tf2::toMsg().
+ *
+ * \tparam Datatype Non-Message datatype like \c Vector3d
+ */
+template <class Datatype, class = void>
+struct defaultMessage
+{
+  // using type = ...;
+};
+
+/**
+ * \brief Conversion details between a Message and a non-Message datatype.
+ * \tparam Datatype Non-Message datatype like \c Vector3d
+ * \tparam Message  The ROS Message class
+ *
+ * The specializations of this struct should contain two static methods,
+ * which convert a ROS Message into the requested datatype and vice versa.
+ * They should have the following signature:
+ * \code
+ * template<>
+ * struct defautMessage<Datatype, Message>
+ * {
+ *   static void toMsg(const Datatype&, Message&);
+ *   static void fromMsg(const Message&, Datatype&);
+ * }:
+ * \endcode
+ * Note that the conversion between tf2::Stamped\<Datatype\> and
+ * geometry_msgs::...Stamped is done automatically.
+ */
+template <class Datatype, class Message, class = void>
+struct ImplDetails
+{
+  // void toMsg(const Datatype&, Message&);
+  // void fromMsg(const Message&, Datatype&);
+};
+
+// Forward declaration for the extraction of timestamps and frame IDs
+template <typename T, int = 0>
+struct DefaultStampedImpl;
+// Forward declaration for the tf2::convert() implementation
+template <bool, bool>
+class Converter;
+
+}  // namespace impl
 
 /**\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.   
+ * 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.  
- * 
+ * \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);
+void doTransform(const T& data_in, T& data_out, const geometry_msgs::TransformStamped& transform);
 
-/**\brief Get the timestamp from data 
+/**\brief Get the timestamp from data
  * \param t The data input.
+ * \tparam T The type of the data input.
  * \return The timestamp associated with the data. The lifetime of the returned
  * reference is bound to the lifetime of the argument.
+ *
+ * Library developers need to check whether the default implementation
+ * is sufficient, extend the default implementation
+ * or provide a specialization of this function.
  */
 template <class T>
-  const ros::Time& getTimestamp(const T& t);
+inline
+const ros::Time& getTimestamp(const T& t)
+{
+  return impl::DefaultStampedImpl<T>::getTimestamp(t);
+}
 
-/**\brief Get the frame_id from data 
+/**\brief Get the frame_id from data
  * \param t The data input.
+ * \tparam T The type of 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.
+ *
+ * Library developers need to check whether the default implementation
+ * is sufficient, extend the default implementation
+ * or provide a specialization of this function.
  */
 template <class T>
-  const std::string& getFrameId(const T& t);
-
-
+inline
+const std::string& getFrameId(const T& t)
+{
+  return impl::DefaultStampedImpl<T>::getFrameId(t);
+}
 
-/* An implementation for Stamped<P> datatypes */
-template <class P>
-  const ros::Time& getTimestamp(const tf2::Stamped<P>& t)
-  {
-    return t.stamp_;
-  }
+/**
+ * \brief Function that converts from one type to a ROS message type.
+ *
+ * The implementation of this function should be done in the tf2_* packages
+ * for each datatypes. Preferably in a specialization of the impl::ImplDetails struct.
+ * \param a an object of whatever type
+ * \tparam A Non-message Datatype
+ * \tparam B ROS message Datatype. The default value will be taken from impl::defaultMessage\<A\>::type.
+ * \return the conversion as a ROS message
+ */
+template <typename A, typename B = typename impl::defaultMessage<A>::type>
+inline B toMsg(const A& a)
+{
+  B b;
+  impl::ImplDetails<A, B>::toMsg(a, b);
+  return b;
+}
 
-/* 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.
+/**
+ * \brief Function that converts from one type to a ROS message type.
+ *
+ * The implementation of this function should be done in the tf2_* packages
+ * for each datatypes. Preferably in a specialization of the impl::ImplDetails struct.
  * \param a an object of whatever type
- * \return the conversion as a ROS message
+ * \param b ROS message
+ * \tparam A Non-message Datatype
+ * \tparam B Type of the ROS Message
+ * \return Reference to the parameter b
  */
-template<typename A, typename B>
-  B toMsg(const A& a);
+template <typename A, typename B>
+inline B& toMsg(const A& a, B& b)
+{
+  impl::ImplDetails<A, B>::toMsg(a, b);
+  return b;
+}
 
-/** 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.
+/**
+ * \brief Function that converts from a ROS message type to another type.
+ *
+ * The implementation of this function should be done in the tf2_* packages
+ * for each datatypes. Preferably in a specialization of the impl::ImplDetails struct.
  * \param a a ROS message to convert from
  * \param b the object to convert to
+ * \tparam A ROS message type
+ * \tparam B Arbitrary type
  */
-template<typename A, typename B>
-  void fromMsg(const A&, B& b);
+template <typename A, typename B>
+inline void fromMsg(const A& a, B& b)
+{
+  impl::ImplDetails<B, A>::fromMsg(a, b);
+}
 
-/** Function that converts any type to any type (messages or not).
+/**
+ * \brief 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
  * classes are ROS messages), just write a specialization of the function.
  * \param a an object to convert from
  * \param b the object to convert to
+ * \tparam A Type of the object to convert from
+ * \tparam B Type of the object to convert to
  */
 template <class A, class B>
-  void convert(const A& a, B& b)
-  {
-    //printf("In double type convert\n");
-    impl::Converter<ros::message_traits::IsMessage<A>::value, ros::message_traits::IsMessage<B>::value>::convert(a, b);
-  }
+inline void convert(const A& a, B& b)
+{
+  impl::Converter<ros::message_traits::IsMessage<A>::value, ros::message_traits::IsMessage<B>::value>::convert(a, b);
+}
 
 template <class A>
-  void convert(const A& a1, A& a2)
-  {
-    //printf("In single type convert\n");
-    if(&a1 != &a2)
-      a2 = a1;
-  }
+inline void convert(const A& a1, A& a2)
+{
+  if (&a1 != &a2)
+    a2 = a1;
+}
 
+}  // namespace tf2
 
-}
+#include <tf2/impl/convert.h>
+#include <tf2/impl/stamped_traits.h>
 
-#endif //TF2_CONVERT_H
+#endif  // TF2_CONVERT_H