.. _program_listing_file_include_tf2_geometry_msgs_tf2_geometry_msgs.hpp:

Program Listing for File tf2_geometry_msgs.hpp
==============================================

|exhale_lsh| :ref:`Return to documentation for file <file_include_tf2_geometry_msgs_tf2_geometry_msgs.hpp>` (``include/tf2_geometry_msgs/tf2_geometry_msgs.hpp``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   // Copyright 2008 Willow Garage, Inc.
   //
   // Redistribution and use in source and binary forms, with or without
   // modification, are permitted provided that the following conditions are met:
   //
   //    * Redistributions of source code must retain the above copyright
   //      notice, this list of conditions and the following disclaimer.
   //
   //    * Redistributions in binary form must reproduce the above copyright
   //      notice, this list of conditions and the following disclaimer in the
   //      documentation and/or other materials provided with the distribution.
   //
   //    * Neither the name of the Willow Garage, Inc. nor the names of its
   //      contributors may be used to endorse or promote products derived from
   //      this software without specific prior written permission.
   //
   // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
   // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
   // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
   // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
   // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
   // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
   // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
   // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
   // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
   // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
   // POSSIBILITY OF SUCH DAMAGE.
   
   
   #ifndef TF2_GEOMETRY_MSGS__TF2_GEOMETRY_MSGS_HPP_
   #define TF2_GEOMETRY_MSGS__TF2_GEOMETRY_MSGS_HPP_
   
   #include <array>
   #include <string>
   
   #include "geometry_msgs/msg/point_stamped.hpp"
   #include "geometry_msgs/msg/quaternion_stamped.hpp"
   #include "geometry_msgs/msg/transform.hpp"
   #include "geometry_msgs/msg/transform_stamped.hpp"
   #include "geometry_msgs/msg/vector3.hpp"
   #include "geometry_msgs/msg/vector3_stamped.hpp"
   #include "geometry_msgs/msg/pose.hpp"
   #include "geometry_msgs/msg/point.hpp"
   #include "geometry_msgs/msg/point32.hpp"
   #include "geometry_msgs/msg/pose_stamped.hpp"
   #include "geometry_msgs/msg/pose_with_covariance.hpp"
   #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
   #include "geometry_msgs/msg/polygon_stamped.hpp"
   #include "geometry_msgs/msg/wrench.hpp"
   #include "geometry_msgs/msg/wrench_stamped.hpp"
   #include "kdl/frames.hpp"
   
   #include "tf2/convert.h"
   #include "tf2/LinearMath/Quaternion.h"
   #include "tf2/LinearMath/Transform.h"
   #include "tf2/LinearMath/Vector3.h"
   #include "tf2_ros/buffer_interface.h"
   
   namespace tf2
   {
   
   inline
   KDL::Frame gmTransformToKDL(const geometry_msgs::msg::TransformStamped & t)
   {
     return KDL::Frame(
       KDL::Rotation::Quaternion(
         t.transform.rotation.x, t.transform.rotation.y,
         t.transform.rotation.z, t.transform.rotation.w),
       KDL::Vector(
         t.transform.translation.x, t.transform.translation.y,
         t.transform.translation.z));
   }
   
   /*************/
   /*************/
   
   template<>
   inline
   void doTransform(
     const geometry_msgs::msg::Vector3 & t_in,
     geometry_msgs::msg::Vector3 & t_out,
     const geometry_msgs::msg::TransformStamped & transform)
   {
     KDL::Vector v_out = gmTransformToKDL(transform).M * KDL::Vector(t_in.x, t_in.y, t_in.z);
     t_out.x = v_out[0];
     t_out.y = v_out[1];
     t_out.z = v_out[2];
   }
   
   inline
   geometry_msgs::msg::Vector3 toMsg(const tf2::Vector3 & in)
   {
     geometry_msgs::msg::Vector3 out;
     out.x = in.getX();
     out.y = in.getY();
     out.z = in.getZ();
     return out;
   }
   
   inline
   void fromMsg(const geometry_msgs::msg::Vector3 & in, tf2::Vector3 & out)
   {
     out = tf2::Vector3(in.x, in.y, in.z);
   }
   
   /********************/
   /********************/
   
   template<>
   inline
   tf2::TimePoint getTimestamp(const geometry_msgs::msg::Vector3Stamped & t)
   {
     return tf2_ros::fromMsg(t.header.stamp);
   }
   
   template<>
   inline
   std::string getFrameId(const geometry_msgs::msg::Vector3Stamped & t)
   {
     return t.header.frame_id;
   }
   
   template<>
   inline
   void doTransform(
     const geometry_msgs::msg::Vector3Stamped & t_in,
     geometry_msgs::msg::Vector3Stamped & t_out,
     const geometry_msgs::msg::TransformStamped & transform)
   {
     doTransform(t_in.vector, t_out.vector, transform);
     t_out.header.stamp = transform.header.stamp;
     t_out.header.frame_id = transform.header.frame_id;
   }
   
   inline
   geometry_msgs::msg::Vector3Stamped toMsg(const geometry_msgs::msg::Vector3Stamped & in)
   {
     return in;
   }
   
   inline
   void fromMsg(
     const geometry_msgs::msg::Vector3Stamped & msg,
     geometry_msgs::msg::Vector3Stamped & out)
   {
     out = msg;
   }
   
   
   /***********/
   /***********/
   
   template<>
   inline
   void doTransform(
     const geometry_msgs::msg::Point & t_in,
     geometry_msgs::msg::Point & t_out,
     const geometry_msgs::msg::TransformStamped & transform)
   {
     KDL::Vector v_out = gmTransformToKDL(transform) * KDL::Vector(t_in.x, t_in.y, t_in.z);
     t_out.x = v_out[0];
     t_out.y = v_out[1];
     t_out.z = v_out[2];
   }
   
   inline
   geometry_msgs::msg::Point & toMsg(const tf2::Vector3 & in, geometry_msgs::msg::Point & out)
   {
     out.x = in.getX();
     out.y = in.getY();
     out.z = in.getZ();
     return out;
   }
   
   inline
   void fromMsg(const geometry_msgs::msg::Point & in, tf2::Vector3 & out)
   {
     out = tf2::Vector3(in.x, in.y, in.z);
   }
   
   /*************/
   /*************/
   
   template<>
   inline
   void doTransform(
     const geometry_msgs::msg::Point32 & t_in,
     geometry_msgs::msg::Point32 & t_out,
     const geometry_msgs::msg::TransformStamped & transform)
   {
     KDL::Vector v_out = gmTransformToKDL(transform) * KDL::Vector(t_in.x, t_in.y, t_in.z);
     t_out.x = static_cast<float>(v_out[0]);
     t_out.y = static_cast<float>(v_out[1]);
     t_out.z = static_cast<float>(v_out[2]);
   }
   
   inline
   geometry_msgs::msg::Point32 & toMsg(const tf2::Vector3 & in, geometry_msgs::msg::Point32 & out)
   {
     out.x = static_cast<float>(in.getX());
     out.y = static_cast<float>(in.getY());
     out.z = static_cast<float>(in.getZ());
     return out;
   }
   
   inline
   void fromMsg(const geometry_msgs::msg::Point32 & in, tf2::Vector3 & out)
   {
     out = tf2::Vector3(in.x, in.y, in.z);
   }
   
   /******************/
   /******************/
   
   template<>
   inline
   tf2::TimePoint getTimestamp(const geometry_msgs::msg::PointStamped & t)
   {
     return tf2_ros::fromMsg(t.header.stamp);
   }
   
   template<>
   inline
   std::string getFrameId(const geometry_msgs::msg::PointStamped & t)
   {
     return t.header.frame_id;
   }
   
   template<>
   inline
   void doTransform(
     const geometry_msgs::msg::PointStamped & t_in,
     geometry_msgs::msg::PointStamped & t_out,
     const geometry_msgs::msg::TransformStamped & transform)
   {
     KDL::Vector v_out = gmTransformToKDL(transform) * KDL::Vector(
       t_in.point.x, t_in.point.y, t_in.point.z);
     t_out.point.x = v_out[0];
     t_out.point.y = v_out[1];
     t_out.point.z = v_out[2];
     t_out.header.stamp = transform.header.stamp;
     t_out.header.frame_id = transform.header.frame_id;
   }
   
   inline
   geometry_msgs::msg::PointStamped toMsg(const geometry_msgs::msg::PointStamped & in)
   {
     return in;
   }
   
   inline
   void fromMsg(const geometry_msgs::msg::PointStamped & msg, geometry_msgs::msg::PointStamped & out)
   {
     out = msg;
   }
   
   
   /*****************/
   /*****************/
   
   template<>
   inline
   tf2::TimePoint getTimestamp(const geometry_msgs::msg::PoseStamped & t)
   {
     return tf2_ros::fromMsg(t.header.stamp);
   }
   
   template<>
   inline
   std::string getFrameId(const geometry_msgs::msg::PoseStamped & t)
   {
     return t.header.frame_id;
   }
   
   template<>
   inline
   void doTransform(
     const geometry_msgs::msg::PoseStamped & t_in,
     geometry_msgs::msg::PoseStamped & t_out,
     const geometry_msgs::msg::TransformStamped & transform)
   {
     KDL::Vector v(t_in.pose.position.x, t_in.pose.position.y, t_in.pose.position.z);
     KDL::Rotation r = KDL::Rotation::Quaternion(
       t_in.pose.orientation.x, t_in.pose.orientation.y,
       t_in.pose.orientation.z, t_in.pose.orientation.w);
   
     KDL::Frame v_out = gmTransformToKDL(transform) * KDL::Frame(r, v);
     t_out.pose.position.x = v_out.p[0];
     t_out.pose.position.y = v_out.p[1];
     t_out.pose.position.z = v_out.p[2];
     v_out.M.GetQuaternion(
       t_out.pose.orientation.x, t_out.pose.orientation.y,
       t_out.pose.orientation.z, t_out.pose.orientation.w);
     t_out.header.stamp = transform.header.stamp;
     t_out.header.frame_id = transform.header.frame_id;
   }
   
   inline
   geometry_msgs::msg::PoseStamped toMsg(const geometry_msgs::msg::PoseStamped & in)
   {
     return in;
   }
   
   inline
   void fromMsg(const geometry_msgs::msg::PoseStamped & msg, geometry_msgs::msg::PoseStamped & out)
   {
     out = msg;
   }
   
   /*************/
   /*************/
   
   template<>
   inline
   void doTransform(
     const geometry_msgs::msg::Polygon & poly_in,
     geometry_msgs::msg::Polygon & poly_out,
     const geometry_msgs::msg::TransformStamped & transform)
   {
     poly_out.points.clear();
     for (auto & point : poly_in.points) {
       geometry_msgs::msg::Point32 point_transformed;
       doTransform(point, point_transformed, transform);
       poly_out.points.push_back(point_transformed);
     }
   }
   
   inline
   geometry_msgs::msg::Polygon toMsg(const geometry_msgs::msg::Polygon & in)
   {
     return in;
   }
   
   inline
   void fromMsg(
     const geometry_msgs::msg::Polygon & msg,
     geometry_msgs::msg::Polygon & out)
   {
     out = msg;
   }
   
   /********************/
   /********************/
   
   template<>
   inline
   tf2::TimePoint getTimestamp(const geometry_msgs::msg::PolygonStamped & t)
   {
     return tf2_ros::fromMsg(t.header.stamp);
   }
   
   template<>
   inline
   std::string getFrameId(const geometry_msgs::msg::PolygonStamped & t)
   {
     return t.header.frame_id;
   }
   
   template<>
   inline
   void doTransform(
     const geometry_msgs::msg::PolygonStamped & poly_in,
     geometry_msgs::msg::PolygonStamped & poly_out,
     const geometry_msgs::msg::TransformStamped & transform)
   {
     doTransform(poly_in.polygon, poly_out.polygon, transform);
   
     poly_out.header.stamp = transform.header.stamp;
     poly_out.header.frame_id = transform.header.frame_id;
   }
   
   inline
   geometry_msgs::msg::PolygonStamped toMsg(const geometry_msgs::msg::PolygonStamped & in)
   {
     return in;
   }
   
   inline
   void fromMsg(
     const geometry_msgs::msg::PolygonStamped & msg,
     geometry_msgs::msg::PolygonStamped & out)
   {
     out = msg;
   }
   
   /************************/
   /************************/
   
   template<>
   inline
   std::array<std::array<double, 6>, 6> getCovarianceMatrix(
     const geometry_msgs::msg::PoseWithCovariance & t)
   {
     return covarianceRowMajorToNested(t.covariance);
   }
   
   inline
   geometry_msgs::msg::PoseWithCovariance::_covariance_type transformCovariance(
     const geometry_msgs::msg::PoseWithCovariance::_covariance_type & cov_in,
     const tf2::Transform & transform)
   {
     // get rotation matrix (and transpose)
     const tf2::Matrix3x3 R = transform.getBasis();
     const tf2::Matrix3x3 R_transpose = R.transpose();
   
     // convert covariance matrix into four 3x3 blocks
     // *INDENT-OFF*
     const tf2::Matrix3x3 cov_11(cov_in[0], cov_in[1], cov_in[2],
                                 cov_in[6], cov_in[7], cov_in[8],
                                 cov_in[12], cov_in[13], cov_in[14]);
     const tf2::Matrix3x3 cov_12(cov_in[3], cov_in[4], cov_in[5],
                                 cov_in[9], cov_in[10], cov_in[11],
                                 cov_in[15], cov_in[16], cov_in[17]);
     const tf2::Matrix3x3 cov_21(cov_in[18], cov_in[19], cov_in[20],
                                 cov_in[24], cov_in[25], cov_in[26],
                                 cov_in[30], cov_in[31], cov_in[32]);
     const tf2::Matrix3x3 cov_22(cov_in[21], cov_in[22], cov_in[23],
                                 cov_in[27], cov_in[28], cov_in[29],
                                 cov_in[33], cov_in[34], cov_in[35]);
     // *INDENT-ON*
   
     // perform blockwise matrix multiplication
     const tf2::Matrix3x3 result_11 = R * cov_11 * R_transpose;
     const tf2::Matrix3x3 result_12 = R * cov_12 * R_transpose;
     const tf2::Matrix3x3 result_21 = R * cov_21 * R_transpose;
     const tf2::Matrix3x3 result_22 = R * cov_22 * R_transpose;
   
     // form the output
     geometry_msgs::msg::PoseWithCovariance::_covariance_type cov_out;
     cov_out[0] = result_11[0][0];
     cov_out[1] = result_11[0][1];
     cov_out[2] = result_11[0][2];
     cov_out[6] = result_11[1][0];
     cov_out[7] = result_11[1][1];
     cov_out[8] = result_11[1][2];
     cov_out[12] = result_11[2][0];
     cov_out[13] = result_11[2][1];
     cov_out[14] = result_11[2][2];
   
     cov_out[3] = result_12[0][0];
     cov_out[4] = result_12[0][1];
     cov_out[5] = result_12[0][2];
     cov_out[9] = result_12[1][0];
     cov_out[10] = result_12[1][1];
     cov_out[11] = result_12[1][2];
     cov_out[15] = result_12[2][0];
     cov_out[16] = result_12[2][1];
     cov_out[17] = result_12[2][2];
   
     cov_out[18] = result_21[0][0];
     cov_out[19] = result_21[0][1];
     cov_out[20] = result_21[0][2];
     cov_out[24] = result_21[1][0];
     cov_out[25] = result_21[1][1];
     cov_out[26] = result_21[1][2];
     cov_out[30] = result_21[2][0];
     cov_out[31] = result_21[2][1];
     cov_out[32] = result_21[2][2];
   
     cov_out[21] = result_22[0][0];
     cov_out[22] = result_22[0][1];
     cov_out[23] = result_22[0][2];
     cov_out[27] = result_22[1][0];
     cov_out[28] = result_22[1][1];
     cov_out[29] = result_22[1][2];
     cov_out[33] = result_22[2][0];
     cov_out[34] = result_22[2][1];
     cov_out[35] = result_22[2][2];
   
     return cov_out;
   }
   
   // Forward declaration
   void fromMsg(const geometry_msgs::msg::Transform & in, tf2::Transform & out);
   
   template<>
   inline
   void doTransform(
     const geometry_msgs::msg::PoseWithCovariance & t_in,
     geometry_msgs::msg::PoseWithCovariance & t_out,
     const geometry_msgs::msg::TransformStamped & transform)
   {
     KDL::Vector v(t_in.pose.position.x, t_in.pose.position.y, t_in.pose.position.z);
     KDL::Rotation r = KDL::Rotation::Quaternion(
       t_in.pose.orientation.x, t_in.pose.orientation.y,
       t_in.pose.orientation.z, t_in.pose.orientation.w);
   
     KDL::Frame v_out = gmTransformToKDL(transform) * KDL::Frame(r, v);
     t_out.pose.position.x = v_out.p[0];
     t_out.pose.position.y = v_out.p[1];
     t_out.pose.position.z = v_out.p[2];
     v_out.M.GetQuaternion(
       t_out.pose.orientation.x, t_out.pose.orientation.y,
       t_out.pose.orientation.z, t_out.pose.orientation.w);
   
     tf2::Transform tf_transform;
     fromMsg(transform.transform, tf_transform);
     t_out.covariance = transformCovariance(t_in.covariance, tf_transform);
   }
   
   inline
   geometry_msgs::msg::PoseWithCovariance toMsg(const geometry_msgs::msg::PoseWithCovariance & in)
   {
     return in;
   }
   
   inline
   void fromMsg(
     const geometry_msgs::msg::PoseWithCovariance & msg,
     geometry_msgs::msg::PoseWithCovariance & out)
   {
     out = msg;
   }
   
   
   /*******************************/
   /*******************************/
   
   template<>
   inline
   tf2::TimePoint getTimestamp(const geometry_msgs::msg::PoseWithCovarianceStamped & t)
   {
     return tf2_ros::fromMsg(t.header.stamp);
   }
   
   template<>
   inline
   std::string getFrameId(const geometry_msgs::msg::PoseWithCovarianceStamped & t)
   {
     return t.header.frame_id;
   }
   
   template<>
   inline
   std::array<std::array<double, 6>, 6> getCovarianceMatrix(
     const geometry_msgs::msg::PoseWithCovarianceStamped & t)
   {
     return covarianceRowMajorToNested(t.pose.covariance);
   }
   
   template<>
   inline
   void doTransform(
     const geometry_msgs::msg::PoseWithCovarianceStamped & t_in,
     geometry_msgs::msg::PoseWithCovarianceStamped & t_out,
     const geometry_msgs::msg::TransformStamped & transform)
   {
     KDL::Vector v(t_in.pose.pose.position.x, t_in.pose.pose.position.y, t_in.pose.pose.position.z);
     KDL::Rotation r = KDL::Rotation::Quaternion(
       t_in.pose.pose.orientation.x, t_in.pose.pose.orientation.y,
       t_in.pose.pose.orientation.z, t_in.pose.pose.orientation.w);
   
     KDL::Frame v_out = gmTransformToKDL(transform) * KDL::Frame(r, v);
     t_out.pose.pose.position.x = v_out.p[0];
     t_out.pose.pose.position.y = v_out.p[1];
     t_out.pose.pose.position.z = v_out.p[2];
     v_out.M.GetQuaternion(
       t_out.pose.pose.orientation.x, t_out.pose.pose.orientation.y,
       t_out.pose.pose.orientation.z, t_out.pose.pose.orientation.w);
     t_out.header.stamp = transform.header.stamp;
     t_out.header.frame_id = transform.header.frame_id;
   
     tf2::Transform tf_transform;
     fromMsg(transform.transform, tf_transform);
     t_out.pose.covariance = transformCovariance(t_in.pose.covariance, tf_transform);
   }
   
   inline
   geometry_msgs::msg::PoseWithCovarianceStamped toMsg(
     const geometry_msgs::msg::PoseWithCovarianceStamped & in)
   {
     return in;
   }
   
   inline
   void fromMsg(
     const geometry_msgs::msg::PoseWithCovarianceStamped & msg,
     geometry_msgs::msg::PoseWithCovarianceStamped & out)
   {
     out = msg;
   }
   
   template<>
   inline
   geometry_msgs::msg::PoseWithCovarianceStamped toMsg(
     const tf2::WithCovarianceStamped<tf2::Transform> & in)
   {
     geometry_msgs::msg::PoseWithCovarianceStamped out;
     out.header.stamp = tf2_ros::toMsg(in.stamp_);
     out.header.frame_id = in.frame_id_;
     out.pose.covariance = covarianceNestedToRowMajor(in.cov_mat_);
     out.pose.pose.orientation.x = in.getRotation().getX();
     out.pose.pose.orientation.y = in.getRotation().getY();
     out.pose.pose.orientation.z = in.getRotation().getZ();
     out.pose.pose.orientation.w = in.getRotation().getW();
     out.pose.pose.position.x = in.getOrigin().getX();
     out.pose.pose.position.y = in.getOrigin().getY();
     out.pose.pose.position.z = in.getOrigin().getZ();
     return out;
   }
   
   template<>
   inline
   void fromMsg(
     const geometry_msgs::msg::PoseWithCovarianceStamped & in,
     tf2::WithCovarianceStamped<tf2::Transform> & out)
   {
     out.stamp_ = tf2_ros::fromMsg(in.header.stamp);
     out.frame_id_ = in.header.frame_id;
     out.cov_mat_ = covarianceRowMajorToNested(in.pose.covariance);
     tf2::Transform tmp;
     fromMsg(in.pose.pose, tmp);
     out.setData(tmp);
   }
   
   
   /****************/
   /****************/
   
   // Forward declaration
   geometry_msgs::msg::Quaternion toMsg(const tf2::Quaternion & in);
   
   template<>
   inline
   void doTransform(
     const geometry_msgs::msg::Quaternion & t_in,
     geometry_msgs::msg::Quaternion & t_out,
     const geometry_msgs::msg::TransformStamped & transform)
   {
     tf2::Quaternion q_out = tf2::Quaternion(
       transform.transform.rotation.x, transform.transform.rotation.y,
       transform.transform.rotation.z, transform.transform.rotation.w) *
       tf2::Quaternion(t_in.x, t_in.y, t_in.z, t_in.w);
     t_out = toMsg(q_out);
   }
   
   inline
   geometry_msgs::msg::Quaternion toMsg(const tf2::Quaternion & in)
   {
     geometry_msgs::msg::Quaternion out;
     out.w = in.getW();
     out.x = in.getX();
     out.y = in.getY();
     out.z = in.getZ();
     return out;
   }
   
   inline
   void fromMsg(const geometry_msgs::msg::Quaternion & in, tf2::Quaternion & out)
   {
     // w at the end in the constructor
     out = tf2::Quaternion(in.x, in.y, in.z, in.w);
   }
   
   
   /***********************/
   /***********************/
   
   template<>
   inline
   tf2::TimePoint getTimestamp(const geometry_msgs::msg::QuaternionStamped & t)
   {
     return tf2_ros::fromMsg(t.header.stamp);
   }
   
   template<>
   inline
   std::string getFrameId(const geometry_msgs::msg::QuaternionStamped & t)
   {
     return t.header.frame_id;
   }
   
   template<>
   inline
   void doTransform(
     const geometry_msgs::msg::QuaternionStamped & t_in,
     geometry_msgs::msg::QuaternionStamped & t_out,
     const geometry_msgs::msg::TransformStamped & transform)
   {
     tf2::Quaternion q_out = tf2::Quaternion(
       transform.transform.rotation.x, transform.transform.rotation.y,
       transform.transform.rotation.z, transform.transform.rotation.w) *
       tf2::Quaternion(t_in.quaternion.x, t_in.quaternion.y, t_in.quaternion.z, t_in.quaternion.w);
     t_out.quaternion = toMsg(q_out);
     t_out.header.stamp = transform.header.stamp;
     t_out.header.frame_id = transform.header.frame_id;
   }
   
   inline
   geometry_msgs::msg::QuaternionStamped toMsg(const geometry_msgs::msg::QuaternionStamped & in)
   {
     return in;
   }
   
   inline
   void fromMsg(
     const geometry_msgs::msg::QuaternionStamped & msg,
     geometry_msgs::msg::QuaternionStamped & out)
   {
     out = msg;
   }
   
   inline
   geometry_msgs::msg::QuaternionStamped toMsg(const tf2::Stamped<tf2::Quaternion> & in)
   {
     geometry_msgs::msg::QuaternionStamped out;
     out.header.stamp = tf2_ros::toMsg(in.stamp_);
     out.header.frame_id = in.frame_id_;
     out.quaternion.w = in.getW();
     out.quaternion.x = in.getX();
     out.quaternion.y = in.getY();
     out.quaternion.z = in.getZ();
     return out;
   }
   
   inline
   void fromMsg(const geometry_msgs::msg::QuaternionStamped & in, tf2::Stamped<tf2::Quaternion> & out)
   {
     out.stamp_ = tf2_ros::fromMsg(in.header.stamp);
     out.frame_id_ = in.header.frame_id;
     tf2::Quaternion tmp;
     fromMsg(in.quaternion, tmp);
     out.setData(tmp);
   }
   
   
   /***************/
   /***************/
   
   inline
   geometry_msgs::msg::Transform toMsg(const tf2::Transform & in)
   {
     geometry_msgs::msg::Transform out;
     out.translation.x = in.getOrigin().getX();
     out.translation.y = in.getOrigin().getY();
     out.translation.z = in.getOrigin().getZ();
     out.rotation = toMsg(in.getRotation());
     return out;
   }
   
   inline
   void toMsg(const tf2::Transform & in, geometry_msgs::msg::Transform & out)
   {
     out = toMsg(in);
   }
   
   inline
   void fromMsg(const geometry_msgs::msg::Transform & in, tf2::Transform & out)
   {
     out.setOrigin(tf2::Vector3(in.translation.x, in.translation.y, in.translation.z));
     // w at the end in the constructor
     out.setRotation(tf2::Quaternion(in.rotation.x, in.rotation.y, in.rotation.z, in.rotation.w));
   }
   
   template<>
   inline
   void doTransform(
     const geometry_msgs::msg::Transform & t_in,
     geometry_msgs::msg::Transform & t_out,
     const geometry_msgs::msg::TransformStamped & transform)
   {
     KDL::Vector v(t_in.translation.x, t_in.translation.y,
       t_in.translation.z);
     KDL::Rotation r = KDL::Rotation::Quaternion(
       t_in.rotation.x, t_in.rotation.y,
       t_in.rotation.z, t_in.rotation.w);
   
     KDL::Frame v_out = gmTransformToKDL(transform) * KDL::Frame(r, v);
     t_out.translation.x = v_out.p[0];
     t_out.translation.y = v_out.p[1];
     t_out.translation.z = v_out.p[2];
     v_out.M.GetQuaternion(
       t_out.rotation.x, t_out.rotation.y,
       t_out.rotation.z, t_out.rotation.w);
   }
   
   
   /**********************/
   /**********************/
   
   template<>
   inline
   tf2::TimePoint getTimestamp(const geometry_msgs::msg::TransformStamped & t)
   {
     return tf2_ros::fromMsg(t.header.stamp);
   }
   
   template<>
   inline
   std::string getFrameId(const geometry_msgs::msg::TransformStamped & t)
   {
     return t.header.frame_id;
   }
   
   template<>
   inline
   void doTransform(
     const geometry_msgs::msg::TransformStamped & t_in,
     geometry_msgs::msg::TransformStamped & t_out,
     const geometry_msgs::msg::TransformStamped & transform)
   {
     KDL::Vector v(t_in.transform.translation.x, t_in.transform.translation.y,
       t_in.transform.translation.z);
     KDL::Rotation r = KDL::Rotation::Quaternion(
       t_in.transform.rotation.x, t_in.transform.rotation.y,
       t_in.transform.rotation.z, t_in.transform.rotation.w);
   
     KDL::Frame v_out = gmTransformToKDL(transform) * KDL::Frame(r, v);
     t_out.transform.translation.x = v_out.p[0];
     t_out.transform.translation.y = v_out.p[1];
     t_out.transform.translation.z = v_out.p[2];
     v_out.M.GetQuaternion(
       t_out.transform.rotation.x, t_out.transform.rotation.y,
       t_out.transform.rotation.z, t_out.transform.rotation.w);
     t_out.header.stamp = transform.header.stamp;
     t_out.header.frame_id = transform.header.frame_id;
   }
   
   inline
   geometry_msgs::msg::TransformStamped toMsg(const geometry_msgs::msg::TransformStamped & in)
   {
     return in;
   }
   
   inline
   void fromMsg(
     const geometry_msgs::msg::TransformStamped & msg,
     geometry_msgs::msg::TransformStamped & out)
   {
     out = msg;
   }
   
   inline
   void fromMsg(const geometry_msgs::msg::TransformStamped & in, tf2::Stamped<tf2::Transform> & out)
   {
     out.stamp_ = tf2_ros::fromMsg(in.header.stamp);
     out.frame_id_ = in.header.frame_id;
     tf2::Transform tmp;
     fromMsg(in.transform, tmp);
     out.setData(tmp);
   }
   
   inline
   geometry_msgs::msg::TransformStamped toMsg(const tf2::Stamped<tf2::Transform> & in)
   {
     geometry_msgs::msg::TransformStamped out;
     out.header.stamp = tf2_ros::toMsg(in.stamp_);
     out.header.frame_id = in.frame_id_;
     out.transform.translation.x = in.getOrigin().getX();
     out.transform.translation.y = in.getOrigin().getY();
     out.transform.translation.z = in.getOrigin().getZ();
     out.transform.rotation = toMsg(in.getRotation());
     return out;
   }
   
   /**********/
   /**********/
   
   template<>
   inline
   void doTransform(
     const geometry_msgs::msg::Pose & t_in,
     geometry_msgs::msg::Pose & t_out,
     const geometry_msgs::msg::TransformStamped & transform)
   {
     KDL::Vector v(t_in.position.x, t_in.position.y, t_in.position.z);
     KDL::Rotation r = KDL::Rotation::Quaternion(
       t_in.orientation.x, t_in.orientation.y,
       t_in.orientation.z, t_in.orientation.w);
   
     KDL::Frame v_out = gmTransformToKDL(transform) * KDL::Frame(r, v);
     t_out.position.x = v_out.p[0];
     t_out.position.y = v_out.p[1];
     t_out.position.z = v_out.p[2];
     v_out.M.GetQuaternion(
       t_out.orientation.x, t_out.orientation.y,
       t_out.orientation.z, t_out.orientation.w);
   }
   
   inline
   geometry_msgs::msg::Pose toMsg(const geometry_msgs::msg::Pose & in)
   {
     return in;
   }
   
   inline
   void fromMsg(const geometry_msgs::msg::Pose & msg, geometry_msgs::msg::Pose & out)
   {
     out = msg;
   }
   
   inline
   void toMsg(const tf2::Transform & in, geometry_msgs::msg::Pose & out)
   {
     out.position.x = in.getOrigin().getX();
     out.position.y = in.getOrigin().getY();
     out.position.z = in.getOrigin().getZ();
     out.orientation = toMsg(in.getRotation());
   }
   
   inline
   void fromMsg(const geometry_msgs::msg::Pose & in, tf2::Transform & out)
   {
     out.setOrigin(tf2::Vector3(in.position.x, in.position.y, in.position.z));
     // w at the end in the constructor
     out.setRotation(
       tf2::Quaternion(in.orientation.x, in.orientation.y, in.orientation.z, in.orientation.w));
   }
   
   /**********************/
   /*** WrenchStamped ****/
   /**********************/
   
   template<>
   inline
   tf2::TimePoint getTimestamp(const geometry_msgs::msg::WrenchStamped & t)
   {
     return tf2_ros::fromMsg(t.header.stamp);
   }
   
   template<>
   inline
   std::string getFrameId(const geometry_msgs::msg::WrenchStamped & t) {return t.header.frame_id;}
   
   
   template<>
   inline
   void doTransform(
     const geometry_msgs::msg::Wrench & t_in, geometry_msgs::msg::Wrench & t_out,
     const geometry_msgs::msg::TransformStamped & transform)
   {
     doTransform(t_in.force, t_out.force, transform);
     doTransform(t_in.torque, t_out.torque, transform);
     // add additional torque created by translating the force
     Vector3 offset = {transform.transform.translation.x, transform.transform.translation.y,
       transform.transform.translation.z};
     auto added_torque = offset.cross({t_out.force.x, t_out.force.y, t_out.force.z});
     t_out.torque.x += added_torque.getX();
     t_out.torque.y += added_torque.getY();
     t_out.torque.z += added_torque.getZ();
   }
   
   template<>
   inline
   void doTransform(
     const geometry_msgs::msg::WrenchStamped & t_in,
     geometry_msgs::msg::WrenchStamped & t_out,
     const geometry_msgs::msg::TransformStamped & transform)
   {
     doTransform(t_in.wrench, t_out.wrench, transform);
     t_out.header.stamp = transform.header.stamp;
     t_out.header.frame_id = transform.header.frame_id;
   }
   
   inline
   geometry_msgs::msg::WrenchStamped toMsg(const geometry_msgs::msg::WrenchStamped & in)
   {
     return in;
   }
   
   inline
   void fromMsg(const geometry_msgs::msg::WrenchStamped & msg, geometry_msgs::msg::WrenchStamped & out)
   {
     out = msg;
   }
   
   }  // namespace tf2
   
   #endif  // TF2_GEOMETRY_MSGS__TF2_GEOMETRY_MSGS_HPP_