add templated function definition.

Author: seanyen

tf2/include/tf2/impl/convert.h

@@ -39,7 +39,10 @@ namespace tf2 {
  * \return the conversion as a ROS message
  */
 template<typename A, typename B>
-  B toMsg(const A& a);
+  B toMsg(const A& a)
+  {
+    return tf2::toMsg(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
@@ -48,7 +51,10 @@ template<typename A, typename B>
  * \param b the object to convert to
  */
 template<typename A, typename B>
-  void fromMsg(const A&, B& b);
+  void fromMsg(const A& a, B& b)
+  {
+    tf2::fromMsg(a, b);
+  }
 
 namespace impl {
 

tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.h

@@ -381,19 +381,6 @@ geometry_msgs::QuaternionStamped toMsg(const tf2::Stamped<tf2::Quaternion>& in)
   return out;
 }
 
-template <>
-inline
-ROS_DEPRECATED geometry_msgs::QuaternionStamped toMsg(const tf2::Stamped<tf2::Quaternion>& in);
-
-
-//Backwards compatibility remove when forked for Lunar or newer
-template <>
-inline
-geometry_msgs::QuaternionStamped toMsg(const tf2::Stamped<tf2::Quaternion>& in)
-{
-  return toMsg(in);
-}
-
 /** \brief Convert a QuaternionStamped message to its equivalent tf2 representation.
  * This function is a specialization of the fromMsg template defined in tf2/convert.h.
  * \param in A QuaternionStamped message type.
@@ -409,18 +396,6 @@ void fromMsg(const geometry_msgs::QuaternionStamped& in, tf2::Stamped<tf2::Quate
   out.setData(tmp);
 }
 
-template<>
-inline
-ROS_DEPRECATED void fromMsg(const geometry_msgs::QuaternionStamped& in, tf2::Stamped<tf2::Quaternion>& out);
-
-//Backwards compatibility remove when forked for Lunar or newer
-template<>
-inline
-void fromMsg(const geometry_msgs::QuaternionStamped& in, tf2::Stamped<tf2::Quaternion>& out)
-{
-    fromMsg(in, out);
-}
-
 /**********/
 /** Pose **/
 /**********/
@@ -529,6 +504,18 @@ void fromMsg(const geometry_msgs::PoseStamped& msg, tf2::Stamped<tf2::Transform>
 /** PoseWithCovarianceStamped **/
 /*******************************/
 
+/** \brief Convert a geometry_msgs PoseWithCovariance message to an equivalent tf2 Transform type.
+ * \param in A PoseWithCovariance message.
+ * \param out The PoseWithCovariance converted to a tf2 Transform type.
+ */
+inline
+void fromMsg(const geometry_msgs::PoseWithCovariance& in, tf2::Transform& out)
+{
+  out.setOrigin(tf2::Vector3(in.pose.position.x, in.pose.position.y, in.pose.position.z));
+  // w at the end in the constructor
+  out.setRotation(tf2::Quaternion(in.pose.orientation.x, in.pose.orientation.y, in.pose.orientation.z, in.pose.orientation.w));
+}
+
 /** \brief Extract a timestamp from the header of a PoseWithCovarianceStamped message.
  * This function is a specialization of the getTimestamp template defined in tf2/convert.h.
  * \param t PoseWithCovarianceStamped message to extract the timestamp from.