From b5a7191944f090ec4367d8a0f0dd7e70d1a776de Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Sat, 28 Nov 2020 18:49:09 +0000 Subject: [PATCH 1/6] Add test for Vector/Point toMsg() --- test_tf2/test/test_convert.cpp | 74 +++++++++++++++++++++++++++++++++- 1 file changed, 73 insertions(+), 1 deletion(-) diff --git a/test_tf2/test/test_convert.cpp b/test_tf2/test/test_convert.cpp index a8b0dfe71..a53ef48ee 100644 --- a/test_tf2/test/test_convert.cpp +++ b/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 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 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(); } - From 2a31419776ca90ba06df370ac622615a6e96719a Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Sat, 28 Nov 2020 22:21:38 +0000 Subject: [PATCH 2/6] Rework of toMsg() and fromMsg() The conversion is moved into the `ImplDetails` struct to avoid link-time errors. As the return value of `toMsg()` can't be deduced, a default value depending on the first argument will be chosen from the `defaultMessage` struct. --- tf2/include/tf2/convert.h | 118 ++- tf2/include/tf2/impl/convert.h | 114 ++- tf2/include/tf2/impl/stamped_traits.h | 272 ++++++ tf2_bullet/include/tf2_bullet/tf2_bullet.h | 60 +- tf2_eigen/include/tf2_eigen/tf2_eigen.h | 484 ++++------- .../tf2_geometry_msgs/tf2_geometry_msgs.h | 813 ++++-------------- tf2_kdl/include/tf2_kdl/tf2_kdl.h | 297 +++---- .../include/tf2_sensor_msgs/tf2_sensor_msgs.h | 10 - 8 files changed, 923 insertions(+), 1245 deletions(-) create mode 100644 tf2/include/tf2/impl/stamped_traits.h diff --git a/tf2/include/tf2/convert.h b/tf2/include/tf2/convert.h index 36efd5be8..20abdb8f3 100644 --- a/tf2/include/tf2/convert.h +++ b/tf2/include/tf2/convert.h @@ -32,57 +32,68 @@ #ifndef TF2_CONVERT_H #define TF2_CONVERT_H - #include #include #include -#include -namespace tf2 { +namespace tf2 +{ +namespace impl +{ +template +struct defaultMessage +{ + // using type = ...; +}; +template +struct ImplDetails +{ + // void toMsg(const Datatype&, Message&); + // void fromMsg(const Message&, Datatype&); +}; + +template +struct DefaultStampedImpl; +template +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 - 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. * \return The timestamp associated with the data. The lifetime of the returned * reference is bound to the lifetime of the argument. */ template - const ros::Time& getTimestamp(const T& t); +inline +const ros::Time& getTimestamp(const T& t) +{ + return impl::DefaultStampedImpl::getTimestamp(t); +} -/**\brief Get the frame_id from data +/**\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 - const std::string& getFrameId(const T& t); - - - -/* An implementation for Stamped

datatypes */ -template - const ros::Time& getTimestamp(const tf2::Stamped

& t) - { - return t.stamp_; - } - -/* An implementation for Stamped

datatypes */ -template - const std::string& getFrameId(const tf2::Stamped

& t) - { - return t.frame_id_; - } +inline +const std::string& getFrameId(const T& t) +{ + return impl::DefaultStampedImpl::getFrameId(t); +} /** 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 @@ -90,8 +101,20 @@ template * \param a an object of whatever type * \return the conversion as a ROS message */ -template - B toMsg(const A& a); +template ::type> +inline B toMsg(const A& a) +{ + B b; + impl::ImplDetails::toMsg(a, b); + return b; +} + +template +inline B& toMsg(const A& a, B& b) +{ + impl::ImplDetails::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 @@ -99,8 +122,11 @@ template * \param a a ROS message to convert from * \param b the object to convert to */ -template - void fromMsg(const A&, B& b); +template +inline void fromMsg(const A& a, B& b) +{ + impl::ImplDetails::fromMsg(a, b); +} /** Function that converts any type to any type (messages or not). * Matching toMsg and from Msg conversion functions need to exist. @@ -110,21 +136,23 @@ template * \param b the object to convert to */ template - void convert(const A& a, B& b) - { - //printf("In double type convert\n"); - impl::Converter::value, ros::message_traits::IsMessage::value>::convert(a, b); - } +inline void convert(const A& a, B& b) +{ + // printf("In double type convert\n"); + impl::Converter::value, ros::message_traits::IsMessage::value>::convert(a, b); +} template - 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) +{ + // printf("In single type convert\n"); + if (&a1 != &a2) + a2 = a1; +} +} // namespace tf2 -} +#include +#include -#endif //TF2_CONVERT_H +#endif // TF2_CONVERT_H diff --git a/tf2/include/tf2/impl/convert.h b/tf2/include/tf2/impl/convert.h index 6b68ccdd4..f6e8941f6 100644 --- a/tf2/include/tf2/impl/convert.h +++ b/tf2/include/tf2/impl/convert.h @@ -30,13 +30,58 @@ #ifndef TF2_IMPL_CONVERT_H #define TF2_IMPL_CONVERT_H -namespace tf2 { -namespace impl { +namespace tf2 +{ +namespace impl +{ + +template +struct stampedMessageTraits +{ + // using unstampedType = ...; + // static unstampedType& accessMessage(StampedMsg &); + // static unstampedType getMessage(StampedMsg const&); +}; + +template +struct unstampedMessageTraits +{ + // using stampedType = ...; +}; + +template +struct defaultMessage> +{ + using type = typename unstampedMessageTraits::type>::stampedType; +}; + + +template +struct ImplDetails, StampedMessage> +{ + using traits = stampedMessageTraits; + using unstampedMessage = typename traits::unstampedType; + + static void toMsg(const tf2::Stamped& s, StampedMessage& msg) + { + tf2::toMsg<>(static_cast(s), traits::accessMessage(msg)); + msg.header.stamp = s.stamp_; + msg.header.frame_id = s.frame_id_; + } + + static void fromMsg(const StampedMessage& msg, tf2::Stamped& s) + { + tf2::fromMsg<>(traits::getMessage(msg), static_cast(s)); + s.stamp_ = msg.header.stamp; + s.frame_id_ = msg.header.frame_id; + } +}; template -class Converter { +class Converter +{ public: - template + template static void convert(const A& a, B& b); }; @@ -47,44 +92,63 @@ class Converter { // if B == A, the templated version of convert with only one argument will be // used. // -//template <> -//template -//inline void Converter::convert(const A& a, B& b); +// template <> +// template +// inline void Converter::convert(const A& a, B& b); template <> template inline void Converter::convert(const A& a, B& b) { -#ifdef _MSC_VER - tf2::fromMsg(a, b); -#else - fromMsg(a, b); -#endif + tf2::fromMsg<>(a, b); } template <> template inline void Converter::convert(const A& a, B& b) { -#ifdef _MSC_VER - b = tf2::toMsg(a); -#else - b = toMsg(a); -#endif + b = tf2::toMsg<>(a); } template <> template inline void Converter::convert(const A& a, B& b) { -#ifdef _MSC_VER - tf2::fromMsg(tf2::toMsg(a), b); -#else - fromMsg(toMsg(a), b); -#endif + tf2::fromMsg<>(tf2::toMsg<>(a), b); } -} -} +template +using void_t = void; + +template +struct DefaultStampedImpl +{ + static const ros::Time& getTimestamp(const T& t, void_t::unstampedType>* = nullptr) + { + return t.header.stamp; + } + + static const std::string& getFrameId(const T& t, void_t::unstampedType>* = nullptr) + { + return t.header.frame_id; + } +}; + +template +struct DefaultStampedImpl> +{ + static const ros::Time& getTimestamp(const tf2::Stamped& t) + { + return t.stamp_; + } + static const std::string& getFrameId(const tf2::Stamped& t) + { + return t.frame_id_; + } +}; + + +} // namespace impl +} // namespace tf2 -#endif //TF2_IMPL_CONVERT_H +#endif // TF2_IMPL_CONVERT_H diff --git a/tf2/include/tf2/impl/stamped_traits.h b/tf2/include/tf2/impl/stamped_traits.h new file mode 100644 index 000000000..5369c3677 --- /dev/null +++ b/tf2/include/tf2/impl/stamped_traits.h @@ -0,0 +1,272 @@ + +/* + * Copyright (c) 2013, Open Source Robotics Foundation + * All rights reserved. + * + * 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 OWNER 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_IMPL_STAMPED_TRAITS_H_ +#define TF2_IMPL_STAMPED_TRAITS_H_ + +// forward declarations +namespace geometry_msgs +{ +template +class Point_; +template +class Vector_; +template +class Quaternion_; +template +class Pose_; +template +class Twist_; +template +class PoseWithCovariance_; +template +class Wrench_; +template +class PointStamped_; +template +class VectorStamped_; +template +class QuaternionStamped_; +template +class PoseStamped_; +template +class TwistStamped_; +template +class PoseWithCovarianceStamped_; +template +class WrenchStamped_; +template +class TransformStamped_; +template +class Transform_; +template +class Vector3_; +template +class Vector3Stamped_; +} // namespace geometry_msgs + +namespace tf2 +{ +namespace impl +{ +template +struct unstampedMessageTraits; + +template +struct unstampedMessageTraits> +{ + using stampedType = geometry_msgs::PointStamped_; +}; + +template +struct unstampedMessageTraits> +{ + using stampedType = geometry_msgs::VectorStamped_; +}; + +template +struct unstampedMessageTraits> +{ + using stampedType = geometry_msgs::QuaternionStamped_; +}; + +template +struct unstampedMessageTraits> +{ + using stampedType = geometry_msgs::PoseStamped_; +}; + +template +struct unstampedMessageTraits> +{ + using stampedType = geometry_msgs::TwistStamped_; +}; + +template +struct unstampedMessageTraits> +{ + using stampedType = geometry_msgs::PoseWithCovarianceStamped_; +}; + +template +struct unstampedMessageTraits> +{ + using stampedType = geometry_msgs::WrenchStamped_; +}; + +template +struct unstampedMessageTraits> +{ + using stampedType = geometry_msgs::TransformStamped_; +}; + +template +struct unstampedMessageTraits> +{ + using stampedType = geometry_msgs::Vector3Stamped_; +}; + +template +struct stampedMessageTraits; + +template +struct stampedMessageTraits> +{ + using unstampedType = geometry_msgs::Point_; + static geometry_msgs::Point_& accessMessage(geometry_msgs::PointStamped_& smsg) + { + return smsg.point; + } + static geometry_msgs::Point_ getMessage(geometry_msgs::PointStamped_ const& smsg) + { + return smsg.point; + } +}; + +template +struct stampedMessageTraits> +{ + using unstampedType = geometry_msgs::Vector_; + static geometry_msgs::Vector_& accessMessage(geometry_msgs::VectorStamped_& smsg) + { + return smsg.vector; + } + static geometry_msgs::Vector_ getMessage(geometry_msgs::VectorStamped_ const& smsg) + { + return smsg.vector; + } +}; + +template +struct stampedMessageTraits> +{ + using unstampedType = geometry_msgs::Quaternion_; + static geometry_msgs::Quaternion_& accessMessage(geometry_msgs::QuaternionStamped_& smsg) + { + return smsg.quaternion; + } + static geometry_msgs::Quaternion_ getMessage(geometry_msgs::QuaternionStamped_ const& smsg) + { + return smsg.quaternion; + } +}; + +template +struct stampedMessageTraits> +{ + using unstampedType = geometry_msgs::Pose_; + static geometry_msgs::Pose_& accessMessage(geometry_msgs::PoseStamped_& smsg) + { + return smsg.pose; + } + static geometry_msgs::Pose_ getMessage(geometry_msgs::PoseStamped_ const& smsg) + { + return smsg.pose; + } +}; + +template +struct stampedMessageTraits> +{ + using unstampedType = geometry_msgs::Twist_; + static geometry_msgs::Twist_& accessMessage(geometry_msgs::TwistStamped_& smsg) + { + return smsg.twist; + } + static geometry_msgs::Twist_ getMessage(geometry_msgs::TwistStamped_ const& smsg) + { + return smsg.twist; + } +}; + +template +struct stampedMessageTraits> +{ + using unstampedType = geometry_msgs::PoseWithCovariance_; + static geometry_msgs::PoseWithCovariance_& + accessMessage(geometry_msgs::PoseWithCovarianceStamped_& smsg) + { + return smsg.pose; + } + static geometry_msgs::PoseWithCovariance_ + getMessage(geometry_msgs::PoseWithCovarianceStamped_ const& smsg) + { + return smsg.pose; + } +}; + +template +struct stampedMessageTraits> +{ + using unstampedType = geometry_msgs::Wrench_; + static geometry_msgs::Wrench_& accessMessage(geometry_msgs::WrenchStamped_& smsg) + { + return smsg.wrench; + } + static geometry_msgs::Wrench_ getMessage(geometry_msgs::WrenchStamped_ const& smsg) + { + return smsg.wrench; + } +}; + +template +struct stampedMessageTraits> +{ + using unstampedType = geometry_msgs::Transform_; + static geometry_msgs::Transform_& accessMessage(geometry_msgs::TransformStamped_& smsg) + { + return smsg.transform; + } + + static geometry_msgs::Transform_ getMessage(geometry_msgs::TransformStamped_ const& smsg) + { + return smsg.transform; + } +}; + +template +struct stampedMessageTraits> +{ + using unstampedType = geometry_msgs::Vector3_; + static geometry_msgs::Vector3_& accessMessage(geometry_msgs::Vector3Stamped_& smsg) + { + return smsg.vector; + } + + static geometry_msgs::Vector3_ getMessage(geometry_msgs::Vector3Stamped_ const& smsg) + { + return smsg.vector; + } +}; + +} // namespace impl +} // namespace tf2 + +#endif // TF2_IMPL_STAMPED_TRAITS_H_ diff --git a/tf2_bullet/include/tf2_bullet/tf2_bullet.h b/tf2_bullet/include/tf2_bullet/tf2_bullet.h index 63e88f5ac..8f4b99a0b 100644 --- a/tf2_bullet/include/tf2_bullet/tf2_bullet.h +++ b/tf2_bullet/include/tf2_bullet/tf2_bullet.h @@ -65,38 +65,42 @@ inline t_out = tf2::Stamped(transformToBullet(transform) * t_in, transform.header.stamp, transform.header.frame_id); } -/** \brief Convert a stamped Bullet Vector3 type to a PointStamped message. - * This function is a specialization of the toMsg template defined in tf2/convert.h - * \param in The timestamped Bullet btVector3 to convert. - * \return The vector converted to a PointStamped message. - */ -inline -geometry_msgs::PointStamped toMsg(const tf2::Stamped& in) +namespace impl +{ +template <> +struct defaultMessage { - geometry_msgs::PointStamped msg; - msg.header.stamp = in.stamp_; - msg.header.frame_id = in.frame_id_; - msg.point.x = in[0]; - msg.point.y = in[1]; - msg.point.z = in[2]; - return msg; -} + using type = geometry_msgs::Point; +}; -/** \brief Convert a PointStamped message type to a stamped Bullet-specific Vector3 type. - * This function is a specialization of the fromMsg template defined in tf2/convert.h - * \param msg The PointStamped message to convert. - * \param out The point converted to a timestamped Bullet Vector3. - */ -inline -void fromMsg(const geometry_msgs::PointStamped& msg, tf2::Stamped& out) +template <> +struct ImplDetails { - out.stamp_ = msg.header.stamp; - out.frame_id_ = msg.header.frame_id; - out[0] = msg.point.x; - out[1] = msg.point.y; - out[2] = msg.point.z; -} + /** \brief Convert a stamped Bullet Vector3 type to a PointStamped message. + * This function is a specialization of the toMsg template defined in tf2/convert.h + * \param in The timestamped Bullet btVector3 to convert. + * \return The vector converted to a PointStamped message. + */ + static void toMsg(const btVector3& in, geometry_msgs::Point& msg) + { + msg.x = in[0]; + msg.y = in[1]; + msg.z = in[2]; + } + /** \brief Convert a PointStamped message type to a stamped Bullet-specific Vector3 type. + * This function is a specialization of the fromMsg template defined in tf2/convert.h + * \param msg The PointStamped message to convert. + * \param out The point converted to a timestamped Bullet Vector3. + */ + static void fromMsg(const geometry_msgs::Point& msg, btVector3& out) + { + out[0] = msg.x; + out[1] = msg.y; + out[2] = msg.z; + } +}; +} // namespace impl /** \brief Apply a geometry_msgs TransformStamped to a Bullet-specific Transform data type. * This function is a specialization of the doTransform template defined in tf2/convert.h diff --git a/tf2_eigen/include/tf2_eigen/tf2_eigen.h b/tf2_eigen/include/tf2_eigen/tf2_eigen.h index 00c789ba7..7180eeaad 100644 --- a/tf2_eigen/include/tf2_eigen/tf2_eigen.h +++ b/tf2_eigen/include/tf2_eigen/tf2_eigen.h @@ -34,6 +34,7 @@ #include #include #include +#include #include @@ -117,60 +118,52 @@ void doTransform(const Eigen::Vector3d& t_in, Eigen::Vector3d& t_out, const geom t_out = Eigen::Vector3d(transformToEigen(transform) * t_in); } -/** \brief Convert a Eigen Vector3d type to a Point message. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param in The timestamped Eigen Vector3d to convert. - * \return The vector converted to a Point message. - */ -inline -geometry_msgs::Point toMsg(const Eigen::Vector3d& in) +namespace impl { - geometry_msgs::Point msg; - msg.x = in.x(); - msg.y = in.y(); - msg.z = in.z(); - return msg; -} +template +struct Vector3ImplDetails +{ + /** \brief Convert a Eigen Vector3d type to a Point message. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param in The timestamped Eigen Vector3d to convert. + * \return The vector converted to a Point message. + */ + static void toMsg(const Eigen::Vector3d& in, Message& msg) + { + msg.x = in.x(); + msg.y = in.y(); + msg.z = in.z(); + } -/** \brief Convert a Point message type to a Eigen-specific Vector3d type. - * This function is a specialization of the fromMsg template defined in tf2/convert.h - * \param msg The Point message to convert. - * \param out The point converted to a Eigen Vector3d. - */ -inline -void fromMsg(const geometry_msgs::Point& msg, Eigen::Vector3d& out) + /** \brief Convert a Point message type to a Eigen-specific Vector3d type. + * This function is a specialization of the fromMsg template defined in tf2/convert.h + * \param msg The Point message to convert. + * \param out The point converted to a Eigen Vector3d. + */ + static void fromMsg(const Message& msg, Eigen::Vector3d& out) + { + out.x() = msg.x; + out.y() = msg.y; + out.z() = msg.z; + } +}; + +template <> +struct ImplDetails : public Vector3ImplDetails { - out.x() = msg.x; - out.y() = msg.y; - out.z() = msg.z; -} +}; -/** \brief Convert an Eigen Vector3d type to a Vector3 message. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param in The Eigen Vector3d to convert. - * \return The vector converted to a Vector3 message. - */ -inline -geometry_msgs::Vector3& toMsg(const Eigen::Vector3d& in, geometry_msgs::Vector3& out) +template <> +struct ImplDetails : public Vector3ImplDetails { - out.x = in.x(); - out.y = in.y(); - out.z = in.z(); - return out; -} +}; -/** \brief Convert a Vector3 message type to a Eigen-specific Vector3d type. - * This function is a specialization of the fromMsg template defined in tf2/convert.h - * \param msg The Vector3 message to convert. - * \param out The vector converted to a Eigen Vector3d. - */ -inline -void fromMsg(const geometry_msgs::Vector3& msg, Eigen::Vector3d& out) +template <> +struct defaultMessage { - out.x() = msg.x; - out.y() = msg.y; - out.z() = msg.z; -} + using type = geometry_msgs::Point; +}; +} // namespace impl /** \brief Apply a geometry_msgs TransformStamped to an Eigen-specific Vector3d type. * This function is a specialization of the doTransform template defined in tf2/convert.h. @@ -188,33 +181,6 @@ void doTransform(const tf2::Stamped& t_in, transform.header.frame_id); } -/** \brief Convert a stamped Eigen Vector3d type to a PointStamped message. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param in The timestamped Eigen Vector3d to convert. - * \return The vector converted to a PointStamped message. - */ -inline -geometry_msgs::PointStamped toMsg(const tf2::Stamped& in) -{ - geometry_msgs::PointStamped msg; - msg.header.stamp = in.stamp_; - msg.header.frame_id = in.frame_id_; - msg.point = toMsg(static_cast(in)); - return msg; -} - -/** \brief Convert a PointStamped message type to a stamped Eigen-specific Vector3d type. - * This function is a specialization of the fromMsg template defined in tf2/convert.h - * \param msg The PointStamped message to convert. - * \param out The point converted to a timestamped Eigen Vector3d. - */ -inline -void fromMsg(const geometry_msgs::PointStamped& msg, tf2::Stamped& out) { - out.stamp_ = msg.header.stamp; - out.frame_id_ = msg.header.frame_id; - fromMsg(msg.point, static_cast(out)); -} - /** \brief Apply a geometry_msgs Transform to an Eigen Affine3d transform. * This function is a specialization of the doTransform template defined in tf2/convert.h, * although it can not be used in tf2_ros::BufferInterface::transform because this @@ -240,30 +206,41 @@ void doTransform(const Eigen::Isometry3d& t_in, t_out = Eigen::Isometry3d(transformToEigen(transform) * t_in); } +namespace impl +{ /** \brief Convert a Eigen Quaterniond type to a Quaternion message. * This function is a specialization of the toMsg template defined in tf2/convert.h. * \param in The Eigen Quaterniond to convert. * \return The quaternion converted to a Quaterion message. */ -inline -geometry_msgs::Quaternion toMsg(const Eigen::Quaterniond& in) { - geometry_msgs::Quaternion msg; - msg.w = in.w(); - msg.x = in.x(); - msg.y = in.y(); - msg.z = in.z(); - return msg; -} +template <> +struct ImplDetails +{ + static void toMsg(const Eigen::Quaterniond& in, geometry_msgs::Quaternion& msg) + { + msg.w = in.w(); + msg.x = in.x(); + msg.y = in.y(); + msg.z = in.z(); + } -/** \brief Convert a Quaternion message type to a Eigen-specific Quaterniond type. - * This function is a specialization of the fromMsg template defined in tf2/convert.h - * \param msg The Quaternion message to convert. - * \param out The quaternion converted to a Eigen Quaterniond. - */ -inline -void fromMsg(const geometry_msgs::Quaternion& msg, Eigen::Quaterniond& out) { - out = Eigen::Quaterniond(msg.w, msg.x, msg.y, msg.z); -} + /** \brief Convert a Quaternion message type to a Eigen-specific Quaterniond type. + * This function is a specialization of the fromMsg template defined in tf2/convert.h + * \param msg The Quaternion message to convert. + * \param out The quaternion converted to a Eigen Quaterniond. + */ + static void fromMsg(const geometry_msgs::Quaternion& msg, Eigen::Quaterniond& out) + { + out = Eigen::Quaterniond(msg.w, msg.x, msg.y, msg.z); + } +}; + +template <> +struct defaultMessage +{ + using type = geometry_msgs::Quaternion; +}; +} // namespace impl /** \brief Apply a geometry_msgs TransformStamped to an Eigen-specific Quaterniond type. * This function is a specialization of the doTransform template defined in tf2/convert.h, @@ -284,32 +261,6 @@ void doTransform(const Eigen::Quaterniond& t_in, t_out = t.inverse() * t_in * t; } -/** \brief Convert a stamped Eigen Quaterniond type to a QuaternionStamped message. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param in The timestamped Eigen Quaterniond to convert. - * \return The quaternion converted to a QuaternionStamped message. - */ -inline -geometry_msgs::QuaternionStamped toMsg(const Stamped& in) { - geometry_msgs::QuaternionStamped msg; - msg.header.stamp = in.stamp_; - msg.header.frame_id = in.frame_id_; - msg.quaternion = toMsg(static_cast(in)); - return msg; -} - -/** \brief Convert a QuaternionStamped message type to a stamped Eigen-specific Quaterniond type. - * This function is a specialization of the fromMsg template defined in tf2/convert.h - * \param msg The QuaternionStamped message to convert. - * \param out The quaternion converted to a timestamped Eigen Quaterniond. - */ -inline -void fromMsg(const geometry_msgs::QuaternionStamped& msg, Stamped& out) { - out.frame_id_ = msg.header.frame_id; - out.stamp_ = msg.header.stamp; - fromMsg(msg.quaternion, static_cast(out)); -} - /** \brief Apply a geometry_msgs TransformStamped to an Eigen-specific Quaterniond type. * This function is a specialization of the doTransform template defined in tf2/convert.h. * \param t_in The vector to transform, as a timestamped Eigen Quaterniond data type. @@ -326,117 +277,109 @@ void doTransform(const tf2::Stamped& t_in, doTransform(static_cast(t_in), static_cast(t_out), transform); } -/** \brief Convert a Eigen Affine3d transform type to a Pose message. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param in The Eigen Affine3d to convert. - * \return The Eigen transform converted to a Pose message. - */ -inline -geometry_msgs::Pose toMsg(const Eigen::Affine3d& in) { - geometry_msgs::Pose msg; - msg.position.x = in.translation().x(); - msg.position.y = in.translation().y(); - msg.position.z = in.translation().z(); - Eigen::Quaterniond q(in.linear()); - msg.orientation.x = q.x(); - msg.orientation.y = q.y(); - msg.orientation.z = q.z(); - msg.orientation.w = q.w(); - if (msg.orientation.w < 0) { - msg.orientation.x *= -1; - msg.orientation.y *= -1; - msg.orientation.z *= -1; - msg.orientation.w *= -1; +namespace impl +{ +template +struct PoseImplDetails +{ + /** \brief Convert a Eigen Affine3d transform type to a Pose message. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param in The Eigen Affine3d to convert. + * \return The Eigen transform converted to a Pose message. + */ + static void toMsg(const T& in, geometry_msgs::Pose& msg) + { + msg.position.x = in.translation().x(); + msg.position.y = in.translation().y(); + msg.position.z = in.translation().z(); + const Eigen::Quaterniond q(in.linear()); + msg.orientation.x = q.x(); + msg.orientation.y = q.y(); + msg.orientation.z = q.z(); + msg.orientation.w = q.w(); + if (msg.orientation.w < 0) + { + msg.orientation.x *= -1; + msg.orientation.y *= -1; + msg.orientation.z *= -1; + msg.orientation.w *= -1; + } } - return msg; -} -/** \brief Convert a Eigen Isometry3d transform type to a Pose message. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param in The Eigen Isometry3d to convert. - * \return The Eigen transform converted to a Pose message. - */ -inline -geometry_msgs::Pose toMsg(const Eigen::Isometry3d& in) { - geometry_msgs::Pose msg; - msg.position.x = in.translation().x(); - msg.position.y = in.translation().y(); - msg.position.z = in.translation().z(); - Eigen::Quaterniond q(in.linear()); - msg.orientation.x = q.x(); - msg.orientation.y = q.y(); - msg.orientation.z = q.z(); - msg.orientation.w = q.w(); - if (msg.orientation.w < 0) { - msg.orientation.x *= -1; - msg.orientation.y *= -1; - msg.orientation.z *= -1; - msg.orientation.w *= -1; + /** \brief Convert a Pose message transform type to a Eigen Affine3d. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param msg The Pose message to convert. + * \param out The pose converted to a Eigen Affine3d. + */ + static void fromMsg(const geometry_msgs::Pose& msg, T& out) + { + out = T(Eigen::Translation3d(msg.position.x, msg.position.y, msg.position.z) * + Eigen::Quaterniond(msg.orientation.w, msg.orientation.x, msg.orientation.y, msg.orientation.z)); } - return msg; -} +}; -/** \brief Convert a Pose message transform type to a Eigen Affine3d. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param msg The Pose message to convert. - * \param out The pose converted to a Eigen Affine3d. - */ -inline -void fromMsg(const geometry_msgs::Pose& msg, Eigen::Affine3d& out) { - out = Eigen::Affine3d( - Eigen::Translation3d(msg.position.x, msg.position.y, msg.position.z) * - Eigen::Quaterniond(msg.orientation.w, - msg.orientation.x, - msg.orientation.y, - msg.orientation.z)); -} +template <> +struct ImplDetails : public PoseImplDetails +{ +}; -/** \brief Convert a Pose message transform type to a Eigen Isometry3d. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param msg The Pose message to convert. - * \param out The pose converted to a Eigen Isometry3d. - */ -inline -void fromMsg(const geometry_msgs::Pose& msg, Eigen::Isometry3d& out) { - out = Eigen::Isometry3d( - Eigen::Translation3d(msg.position.x, msg.position.y, msg.position.z) * - Eigen::Quaterniond(msg.orientation.w, - msg.orientation.x, - msg.orientation.y, - msg.orientation.z)); -} +template <> +struct ImplDetails : public PoseImplDetails +{ +}; +template <> +struct defaultMessage +{ + using type = geometry_msgs::Pose; +}; + +template <> +struct defaultMessage +{ + using type = geometry_msgs::Pose; +}; /** \brief Convert a Eigen 6x1 Matrix type to a Twist message. * This function is a specialization of the toMsg template defined in tf2/convert.h. * \param in The 6x1 Eigen Matrix to convert. * \return The Eigen Matrix converted to a Twist message. */ -inline -geometry_msgs::Twist toMsg(const Eigen::Matrix& in) { - geometry_msgs::Twist msg; - msg.linear.x = in[0]; - msg.linear.y = in[1]; - msg.linear.z = in[2]; - msg.angular.x = in[3]; - msg.angular.y = in[4]; - msg.angular.z = in[5]; - return msg; -} +template <> +struct ImplDetails, geometry_msgs::Twist> +{ + static void toMsg(const Eigen::Matrix& in, geometry_msgs::Twist& msg) + { + msg.linear.x = in[0]; + msg.linear.y = in[1]; + msg.linear.z = in[2]; + msg.angular.x = in[3]; + msg.angular.y = in[4]; + msg.angular.z = in[5]; + } -/** \brief Convert a Twist message transform type to a Eigen 6x1 Matrix. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param msg The Twist message to convert. - * \param out The twist converted to a Eigen 6x1 Matrix. - */ -inline -void fromMsg(const geometry_msgs::Twist &msg, Eigen::Matrix& out) { - out[0] = msg.linear.x; - out[1] = msg.linear.y; - out[2] = msg.linear.z; - out[3] = msg.angular.x; - out[4] = msg.angular.y; - out[5] = msg.angular.z; -} + /** \brief Convert a Twist message transform type to a Eigen 6x1 Matrix. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param msg The Twist message to convert. + * \param out The twist converted to a Eigen 6x1 Matrix. + */ + static void fromMsg(const geometry_msgs::Twist& msg, Eigen::Matrix& out) + { + out[0] = msg.linear.x; + out[1] = msg.linear.y; + out[2] = msg.linear.z; + out[3] = msg.angular.x; + out[4] = msg.angular.y; + out[5] = msg.angular.z; + } +}; + +template <> +struct defaultMessage> +{ + using type = geometry_msgs::Twist; +}; + +} // namespace impl /** \brief Apply a geometry_msgs TransformStamped to an Eigen Affine3d transform. * This function is a specialization of the doTransform template defined in tf2/convert.h, @@ -472,113 +415,6 @@ void doTransform(const tf2::Stamped& t_in, t_out = tf2::Stamped(transformToEigen(transform) * t_in, transform.header.stamp, transform.header.frame_id); } -/** \brief Convert a stamped Eigen Affine3d transform type to a Pose message. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param in The timestamped Eigen Affine3d to convert. - * \return The Eigen transform converted to a PoseStamped message. - */ -inline -geometry_msgs::PoseStamped toMsg(const tf2::Stamped& in) -{ - geometry_msgs::PoseStamped msg; - msg.header.stamp = in.stamp_; - msg.header.frame_id = in.frame_id_; - msg.pose = toMsg(static_cast(in)); - return msg; -} - -inline -geometry_msgs::PoseStamped toMsg(const tf2::Stamped& in) -{ - geometry_msgs::PoseStamped msg; - msg.header.stamp = in.stamp_; - msg.header.frame_id = in.frame_id_; - msg.pose = toMsg(static_cast(in)); - return msg; -} - -/** \brief Convert a Pose message transform type to a stamped Eigen Affine3d. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param msg The PoseStamped message to convert. - * \param out The pose converted to a timestamped Eigen Affine3d. - */ -inline -void fromMsg(const geometry_msgs::PoseStamped& msg, tf2::Stamped& out) -{ - out.stamp_ = msg.header.stamp; - out.frame_id_ = msg.header.frame_id; - fromMsg(msg.pose, static_cast(out)); -} - -inline -void fromMsg(const geometry_msgs::PoseStamped& msg, tf2::Stamped& out) -{ - out.stamp_ = msg.header.stamp; - out.frame_id_ = msg.header.frame_id; - fromMsg(msg.pose, static_cast(out)); -} - -} // namespace - - -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& in) { - return tf2::toMsg(in); -} - -inline -void fromMsg(const geometry_msgs::Twist &msg, Eigen::Matrix& out) { - tf2::fromMsg(msg, out); -} } // namespace 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 717b5eb82..3fa86c031 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 @@ -68,648 +68,180 @@ KDL::Frame gmTransformToKDL(const geometry_msgs::TransformStamped& t) KDL::Vector(t.transform.translation.x, t.transform.translation.y, t.transform.translation.z)); } - +namespace impl +{ /*************/ /** Vector3 **/ /*************/ -/** \brief Convert a tf2 Vector3 type to its equivalent geometry_msgs representation. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param in A tf2 Vector3 object. - * \return The Vector3 converted to a geometry_msgs message type. - */ -inline -geometry_msgs::Vector3 toMsg(const tf2::Vector3& in) -{ - geometry_msgs::Vector3 out; - out.x = in.getX(); - out.y = in.getY(); - out.z = in.getZ(); - return out; -} - -/** \brief Convert a Vector3 message to its equivalent tf2 representation. - * This function is a specialization of the fromMsg template defined in tf2/convert.h. - * \param in A Vector3 message type. - * \param out The Vector3 converted to a tf2 type. - */ -inline -void fromMsg(const geometry_msgs::Vector3& in, tf2::Vector3& out) +template +struct tf2VectorImplDetails { - out = tf2::Vector3(in.x, in.y, in.z); -} - - -/********************/ -/** Vector3Stamped **/ -/********************/ + /** \brief Convert a tf2 Vector3 type to its equivalent geometry_msgs representation. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param in A tf2 Vector3 object. + * \return The Vector3 converted to a geometry_msgs message type. + */ + static void toMsg(const tf2::Vector3& in, Msg& out) + { + out.x = in.getX(); + out.y = in.getY(); + out.z = in.getZ(); + } -/** \brief Extract a timestamp from the header of a Vector message. - * This function is a specialization of the getTimestamp template defined in tf2/convert.h. - * \param t VectorStamped message to extract the timestamp from. - * \return The timestamp of the message. The lifetime of the returned reference - * is bound to the lifetime of the argument. - */ -template <> -inline - const ros::Time& getTimestamp(const geometry_msgs::Vector3Stamped& t) {return t.header.stamp;} + /** \brief Convert a Vector3 message to its equivalent tf2 representation. + * This function is a specialization of the fromMsg template defined in tf2/convert.h. + * \param in A Vector3 message type. + * \param out The Vector3 converted to a tf2 type. + */ + static void fromMsg(const Msg& in, tf2::Vector3& out) + { + out = tf2::Vector3(in.x, in.y, in.z); + } +}; -/** \brief Extract a frame ID from the header of a Vector message. - * This function is a specialization of the getFrameId template defined in tf2/convert.h. - * \param t VectorStamped message to extract the frame ID from. - * \return A string containing the frame ID of the message. The lifetime of the - * returned reference is bound to the lifetime of the argument. - */ template <> -inline - const std::string& getFrameId(const geometry_msgs::Vector3Stamped& t) {return t.header.frame_id;} - - -/** \brief Trivial "conversion" function for Vector3 message type. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param in A Vector3Stamped message. - * \return The input argument. - */ -inline -geometry_msgs::Vector3Stamped toMsg(const geometry_msgs::Vector3Stamped& in) +struct ImplDetails : tf2VectorImplDetails { - return in; -} +}; -/** \brief Trivial "conversion" function for Vector3 message type. - * This function is a specialization of the fromMsg template defined in tf2/convert.h. - * \param msg A Vector3Stamped message. - * \param out The input argument. - */ -inline -void fromMsg(const geometry_msgs::Vector3Stamped& msg, geometry_msgs::Vector3Stamped& out) -{ - out = msg; -} - -/** \brief Convert as stamped tf2 Vector3 type to its equivalent geometry_msgs representation. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param in An instance of the tf2::Vector3 specialization of the tf2::Stamped template. - * \return The Vector3Stamped converted to a geometry_msgs Vector3Stamped message type. - */ -inline -geometry_msgs::Vector3Stamped toMsg(const tf2::Stamped& in) -{ - geometry_msgs::Vector3Stamped out; - out.header.stamp = in.stamp_; - out.header.frame_id = in.frame_id_; - out.vector.x = in.getX(); - out.vector.y = in.getY(); - out.vector.z = in.getZ(); - return out; -} - -/** \brief Convert a Vector3Stamped message to its equivalent tf2 representation. - * This function is a specialization of the fromMsg template defined in tf2/convert.h. - * \param msg A Vector3Stamped message. - * \param out The Vector3Stamped converted to the equivalent tf2 type. - */ -inline -void fromMsg(const geometry_msgs::Vector3Stamped& msg, tf2::Stamped& out) -{ - out.stamp_ = msg.header.stamp; - out.frame_id_ = msg.header.frame_id; - out.setData(tf2::Vector3(msg.vector.x, msg.vector.y, msg.vector.z)); -} - - -/***********/ -/** Point **/ -/***********/ - -/** \brief Convert a tf2 Vector3 type to its equivalent geometry_msgs representation. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param in A tf2 Vector3 object. - * \return The Vector3 converted to a geometry_msgs message type. - */ -inline -geometry_msgs::Point& toMsg(const tf2::Vector3& in, geometry_msgs::Point& out) -{ - out.x = in.getX(); - out.y = in.getY(); - out.z = in.getZ(); - return out; -} - -/** \brief Convert a Vector3 message to its equivalent tf2 representation. - * This function is a specialization of the fromMsg template defined in tf2/convert.h. - * \param in A Vector3 message type. - * \param out The Vector3 converted to a tf2 type. - */ -inline -void fromMsg(const geometry_msgs::Point& in, tf2::Vector3& out) -{ - out = tf2::Vector3(in.x, in.y, in.z); -} - - -/******************/ -/** PointStamped **/ -/******************/ - -/** \brief Extract a timestamp from the header of a Point message. - * This function is a specialization of the getTimestamp template defined in tf2/convert.h. - * \param t PointStamped message to extract the timestamp from. - * \return The timestamp of the message. The lifetime of the returned reference - * is bound to the lifetime of the argument. - */ -template <> -inline - const ros::Time& getTimestamp(const geometry_msgs::PointStamped& t) {return t.header.stamp;} - -/** \brief Extract a frame ID from the header of a Point message. - * This function is a specialization of the getFrameId template defined in tf2/convert.h. - * \param t PointStamped message to extract the frame ID from. - * \return A string containing the frame ID of the message. The lifetime of the - * returned reference is bound to the lifetime of the argument. - */ template <> -inline - const std::string& getFrameId(const geometry_msgs::PointStamped& t) {return t.header.frame_id;} - -/** \brief Trivial "conversion" function for Point message type. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param in A PointStamped message. - * \return The input argument. - */ -inline -geometry_msgs::PointStamped toMsg(const geometry_msgs::PointStamped& in) -{ - return in; -} - -/** \brief Trivial "conversion" function for Point message type. - * This function is a specialization of the fromMsg template defined in tf2/convert.h. - * \param msg A PointStamped message. - * \param out The input argument. - */ -inline -void fromMsg(const geometry_msgs::PointStamped& msg, geometry_msgs::PointStamped& out) +struct ImplDetails : tf2VectorImplDetails { - out = msg; -} +}; -/** \brief Convert as stamped tf2 Vector3 type to its equivalent geometry_msgs representation. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param in An instance of the tf2::Vector3 specialization of the tf2::Stamped template. - * \return The Vector3Stamped converted to a geometry_msgs PointStamped message type. - */ -inline -geometry_msgs::PointStamped toMsg(const tf2::Stamped& in, geometry_msgs::PointStamped & out) -{ - out.header.stamp = in.stamp_; - out.header.frame_id = in.frame_id_; - out.point.x = in.getX(); - out.point.y = in.getY(); - out.point.z = in.getZ(); - return out; -} - -/** \brief Convert a PointStamped message to its equivalent tf2 representation. - * This function is a specialization of the fromMsg template defined in tf2/convert.h. - * \param msg A PointStamped message. - * \param out The PointStamped converted to the equivalent tf2 type. - */ -inline -void fromMsg(const geometry_msgs::PointStamped& msg, tf2::Stamped& out) +template <> +struct defaultMessage { - out.stamp_ = msg.header.stamp; - out.frame_id_ = msg.header.frame_id; - out.setData(tf2::Vector3(msg.point.x, msg.point.y, msg.point.z)); -} - + using type = geometry_msgs::Vector3; +}; /****************/ /** Quaternion **/ /****************/ -/** \brief Convert a tf2 Quaternion type to its equivalent geometry_msgs representation. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param in A tf2 Quaternion object. - * \return The Quaternion converted to a geometry_msgs message type. - */ -inline -geometry_msgs::Quaternion toMsg(const tf2::Quaternion& in) -{ - geometry_msgs::Quaternion out; - out.w = in.getW(); - out.x = in.getX(); - out.y = in.getY(); - out.z = in.getZ(); - return out; -} - -/** \brief Convert a Quaternion message to its equivalent tf2 representation. - * This function is a specialization of the fromMsg template defined in tf2/convert.h. - * \param in A Quaternion message type. - * \param out The Quaternion converted to a tf2 type. - */ -inline -void fromMsg(const geometry_msgs::Quaternion& in, tf2::Quaternion& out) -{ - // w at the end in the constructor - out = tf2::Quaternion(in.x, in.y, in.z, in.w); -} - - -/***********************/ -/** QuaternionStamped **/ -/***********************/ - -/** \brief Extract a timestamp from the header of a Quaternion message. - * This function is a specialization of the getTimestamp template defined in tf2/convert.h. - * \param t QuaternionStamped message to extract the timestamp from. - * \return The timestamp of the message. The lifetime of the returned reference - * is bound to the lifetime of the argument. - */ template <> -inline -const ros::Time& getTimestamp(const geometry_msgs::QuaternionStamped& t) {return t.header.stamp;} - -/** \brief Extract a frame ID from the header of a Quaternion message. - * This function is a specialization of the getFrameId template defined in tf2/convert.h. - * \param t QuaternionStamped message to extract the frame ID from. - * \return A string containing the frame ID of the message. The lifetime of the - * returned reference is bound to the lifetime of the argument. - */ -template <> -inline -const std::string& getFrameId(const geometry_msgs::QuaternionStamped& t) {return t.header.frame_id;} - -/** \brief Trivial "conversion" function for Quaternion message type. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param in A QuaternionStamped message. - * \return The input argument. - */ -inline -geometry_msgs::QuaternionStamped toMsg(const geometry_msgs::QuaternionStamped& in) -{ - return in; -} - -/** \brief Trivial "conversion" function for Quaternion message type. - * This function is a specialization of the fromMsg template defined in tf2/convert.h. - * \param msg A QuaternionStamped message. - * \param out The input argument. - */ -inline -void fromMsg(const geometry_msgs::QuaternionStamped& msg, geometry_msgs::QuaternionStamped& out) -{ - out = msg; -} - -/** \brief Convert as stamped tf2 Quaternion type to its equivalent geometry_msgs representation. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param in An instance of the tf2::Quaternion specialization of the tf2::Stamped template. - * \return The QuaternionStamped converted to a geometry_msgs QuaternionStamped message type. - */ -inline -geometry_msgs::QuaternionStamped toMsg(const tf2::Stamped& in) -{ - geometry_msgs::QuaternionStamped out; - out.header.stamp = 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; -} - -template <> -inline -ROS_DEPRECATED geometry_msgs::QuaternionStamped toMsg(const tf2::Stamped& in); +struct ImplDetails +{ + /** \brief Convert a tf2 Quaternion type to its equivalent geometry_msgs representation. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param in A tf2 Quaternion object. + * \return The Quaternion converted to a geometry_msgs message type. + */ + static void toMsg(const tf2::Quaternion& in, geometry_msgs::Quaternion& out) + { + out.w = in.getW(); + out.x = in.getX(); + out.y = in.getY(); + out.z = in.getZ(); + } + /** \brief Convert a Quaternion message to its equivalent tf2 representation. + * This function is a specialization of the fromMsg template defined in tf2/convert.h. + * \param in A Quaternion message type. + * \param out The Quaternion converted to a tf2 type. + */ + static void fromMsg(const geometry_msgs::Quaternion& in, tf2::Quaternion& out) + { + // w at the end in the constructor + out = tf2::Quaternion(in.x, in.y, in.z, in.w); + } +}; -//Backwards compatibility remove when forked for Lunar or newer template <> -inline -geometry_msgs::QuaternionStamped toMsg(const tf2::Stamped& 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. - * \param out The QuaternionStamped converted to the equivalent tf2 type. - */ -inline -void fromMsg(const geometry_msgs::QuaternionStamped& in, tf2::Stamped& out) -{ - out.stamp_ = in.header.stamp; - out.frame_id_ = in.header.frame_id; - tf2::Quaternion tmp; - fromMsg(in.quaternion, tmp); - out.setData(tmp); -} - -template<> -inline -ROS_DEPRECATED void fromMsg(const geometry_msgs::QuaternionStamped& in, tf2::Stamped& out); - -//Backwards compatibility remove when forked for Lunar or newer -template<> -inline -void fromMsg(const geometry_msgs::QuaternionStamped& in, tf2::Stamped& out) +struct defaultMessage { - fromMsg(in, out); -} + using type = geometry_msgs::Quaternion; +}; /**********/ /** Pose **/ /**********/ -/** \brief Convert a tf2 Transform type to an equivalent geometry_msgs Pose message. - * \param in A tf2 Transform object. - * \param out The Transform converted to a geometry_msgs Pose message type. - */ -inline -geometry_msgs::Pose& toMsg(const tf2::Transform& in, geometry_msgs::Pose& out) -{ - toMsg(in.getOrigin(), out.position); - out.orientation = toMsg(in.getRotation()); - return out; -} - -/** \brief Convert a geometry_msgs Pose message to an equivalent tf2 Transform type. - * \param in A Pose message. - * \param out The Pose converted to a tf2 Transform type. - */ -inline -void fromMsg(const geometry_msgs::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)); -} - - - -/*****************/ -/** PoseStamped **/ -/*****************/ - -/** \brief Extract a timestamp from the header of a Pose message. - * This function is a specialization of the getTimestamp template defined in tf2/convert.h. - * \param t PoseStamped message to extract the timestamp from. - * \return The timestamp of the message. - */ -template <> -inline - const ros::Time& getTimestamp(const geometry_msgs::PoseStamped& t) {return t.header.stamp;} - -/** \brief Extract a frame ID from the header of a Pose message. - * This function is a specialization of the getFrameId template defined in tf2/convert.h. - * \param t PoseStamped message to extract the frame ID from. - * \return A string containing the frame ID of the message. - */ template <> -inline - const std::string& getFrameId(const geometry_msgs::PoseStamped& t) {return t.header.frame_id;} - -/** \brief Trivial "conversion" function for Pose message type. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param in A PoseStamped message. - * \return The input argument. - */ -inline -geometry_msgs::PoseStamped toMsg(const geometry_msgs::PoseStamped& in) +struct ImplDetails { - return in; -} - -/** \brief Trivial "conversion" function for Pose message type. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param msg A PoseStamped message. - * \param out The input argument. - */ -inline -void fromMsg(const geometry_msgs::PoseStamped& msg, geometry_msgs::PoseStamped& out) -{ - out = msg; -} - -/** \brief Convert as stamped tf2 Pose type to its equivalent geometry_msgs representation. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param in An instance of the tf2::Pose specialization of the tf2::Stamped template. - * \return The PoseStamped converted to a geometry_msgs PoseStamped message type. - */ -inline -geometry_msgs::PoseStamped toMsg(const tf2::Stamped& in, geometry_msgs::PoseStamped & out) -{ - out.header.stamp = in.stamp_; - out.header.frame_id = in.frame_id_; - toMsg(in.getOrigin(), out.pose.position); - out.pose.orientation = toMsg(in.getRotation()); - return out; -} + /** \brief Convert a tf2 Transform type to an equivalent geometry_msgs Pose message. + * \param in A tf2 Transform object. + * \param out The Transform converted to a geometry_msgs Pose message type. + */ + static void toMsg(const tf2::Transform& in, geometry_msgs::Pose& out) + { + tf2::toMsg(in.getOrigin(), out.position); + tf2::toMsg(in.getRotation(), out.orientation); + } -/** \brief Convert a PoseStamped message to its equivalent tf2 representation. - * This function is a specialization of the fromMsg template defined in tf2/convert.h. - * \param msg A PoseStamped message. - * \param out The PoseStamped converted to the equivalent tf2 type. - */ -inline -void fromMsg(const geometry_msgs::PoseStamped& msg, tf2::Stamped& out) -{ - out.stamp_ = msg.header.stamp; - out.frame_id_ = msg.header.frame_id; - tf2::Transform tmp; - fromMsg(msg.pose, tmp); - out.setData(tmp); -} + /** \brief Convert a geometry_msgs Pose message to an equivalent tf2 Transform type. + * \param in A Pose message. + * \param out The Pose converted to a tf2 Transform type. + */ + static void fromMsg(const geometry_msgs::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)); + } +}; /*******************************/ /** PoseWithCovarianceStamped **/ /*******************************/ -/** \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. - * \return The timestamp of the message. - */ template <> -inline - const ros::Time& getTimestamp(const geometry_msgs::PoseWithCovarianceStamped& t) {return t.header.stamp;} - -/** \brief Extract a frame ID from the header of a PoseWithCovarianceStamped message. - * This function is a specialization of the getFrameId template defined in tf2/convert.h. - * \param t PoseWithCovarianceStamped message to extract the frame ID from. - * \return A string containing the frame ID of the message. - */ -template <> -inline - const std::string& getFrameId(const geometry_msgs::PoseWithCovarianceStamped& t) {return t.header.frame_id;} - -/** \brief Trivial "conversion" function for PoseWithCovarianceStamped message type. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param in A PoseWithCovarianceStamped message. - * \return The input argument. - */ -inline -geometry_msgs::PoseWithCovarianceStamped toMsg(const geometry_msgs::PoseWithCovarianceStamped& in) -{ - return in; -} - -/** \brief Trivial "conversion" function for PoseWithCovarianceStamped message type. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param msg A PoseWithCovarianceStamped message. - * \param out The input argument. - */ -inline -void fromMsg(const geometry_msgs::PoseWithCovarianceStamped& msg, geometry_msgs::PoseWithCovarianceStamped& out) -{ - out = msg; -} - -/** \brief Convert as stamped tf2 PoseWithCovarianceStamped type to its equivalent geometry_msgs representation. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param in An instance of the tf2::Pose specialization of the tf2::Stamped template. - * \return The PoseWithCovarianceStamped converted to a geometry_msgs PoseWithCovarianceStamped message type. - */ -inline -geometry_msgs::PoseWithCovarianceStamped toMsg(const tf2::Stamped& in, geometry_msgs::PoseWithCovarianceStamped & out) -{ - out.header.stamp = in.stamp_; - out.header.frame_id = in.frame_id_; - toMsg(in.getOrigin(), out.pose.pose.position); - out.pose.pose.orientation = toMsg(in.getRotation()); - return out; -} - -/** \brief Convert a PoseWithCovarianceStamped message to its equivalent tf2 representation. - * This function is a specialization of the fromMsg template defined in tf2/convert.h. - * \param msg A PoseWithCovarianceStamped message. - * \param out The PoseWithCovarianceStamped converted to the equivalent tf2 type. - */ -inline -void fromMsg(const geometry_msgs::PoseWithCovarianceStamped& msg, tf2::Stamped& out) -{ - out.stamp_ = msg.header.stamp; - out.frame_id_ = msg.header.frame_id; - tf2::Transform tmp; - fromMsg(msg.pose, tmp); - out.setData(tmp); -} +struct ImplDetails +{ + /** \brief Convert a PoseWithCovarianceStamped message to its equivalent tf2 representation. + * This function is a specialization of the fromMsg template defined in tf2/convert.h. + * \param msg A PoseWithCovarianceStamped message. + * \param out The PoseWithCovarianceStamped converted to the equivalent tf2 type. + */ + static void fromMsg(const geometry_msgs::PoseWithCovariance& msg, tf2::Transform& out) + { + tf2::fromMsg<>(msg.pose, out); + } +}; /***************/ /** Transform **/ /***************/ -/** \brief Convert a tf2 Transform type to its equivalent geometry_msgs representation. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param in A tf2 Transform object. - * \return The Transform converted to a geometry_msgs message type. - */ -inline -geometry_msgs::Transform toMsg(const tf2::Transform& in) -{ - geometry_msgs::Transform out; - out.translation = toMsg(in.getOrigin()); - out.rotation = toMsg(in.getRotation()); - return out; -} - -/** \brief Convert a Transform message to its equivalent tf2 representation. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param in A Transform message type. - * \param out The Transform converted to a tf2 type. - */ -inline -void fromMsg(const geometry_msgs::Transform& in, tf2::Transform& out) -{ - tf2::Vector3 v; - fromMsg(in.translation, v); - out.setOrigin(v); - // w at the end in the constructor - tf2::Quaternion q; - fromMsg(in.rotation, q); - out.setRotation(q); -} - - -/**********************/ -/** TransformStamped **/ -/**********************/ - -/** \brief Extract a timestamp from the header of a Transform message. - * This function is a specialization of the getTimestamp template defined in tf2/convert.h. - * \param t TransformStamped message to extract the timestamp from. - * \return The timestamp of the message. - */ template <> -inline -const ros::Time& getTimestamp(const geometry_msgs::TransformStamped& t) {return t.header.stamp;} - -/** \brief Extract a frame ID from the header of a Transform message. - * This function is a specialization of the getFrameId template defined in tf2/convert.h. - * \param t TransformStamped message to extract the frame ID from. - * \return A string containing the frame ID of the message. - */ -template <> -inline -const std::string& getFrameId(const geometry_msgs::TransformStamped& t) {return t.header.frame_id;} - -/** \brief Trivial "conversion" function for Transform message type. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param in A TransformStamped message. - * \return The input argument. - */ -inline -geometry_msgs::TransformStamped toMsg(const geometry_msgs::TransformStamped& in) -{ - return in; -} - -/** \brief Convert a TransformStamped message to its equivalent tf2 representation. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param msg A TransformStamped message type. - * \param out The TransformStamped converted to the equivalent tf2 type. - */ -inline -void fromMsg(const geometry_msgs::TransformStamped& msg, geometry_msgs::TransformStamped& out) -{ - out = msg; -} - -/** \brief Convert as stamped tf2 Transform type to its equivalent geometry_msgs representation. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param in An instance of the tf2::Transform specialization of the tf2::Stamped template. - * \return The tf2::Stamped converted to a geometry_msgs TransformStamped message type. - */ -inline -geometry_msgs::TransformStamped toMsg(const tf2::Stamped& in) -{ - geometry_msgs::TransformStamped out; - out.header.stamp = in.stamp_; - out.header.frame_id = in.frame_id_; - out.transform.translation = toMsg(in.getOrigin()); - out.transform.rotation = toMsg(in.getRotation()); - return out; -} +struct ImplDetails +{ + /** \brief Convert a tf2 Transform type to its equivalent geometry_msgs representation. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param in A tf2 Transform object. + * \return The Transform converted to a geometry_msgs message type. + */ + static void toMsg(const tf2::Transform& in, geometry_msgs::Transform& out) + { + tf2::toMsg(in.getOrigin(), out.translation); + tf2::toMsg(in.getRotation(), out.rotation); + } + /** \brief Convert a Transform message to its equivalent tf2 representation. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param in A Transform message type. + * \param out The Transform converted to a tf2 type. + */ + static void fromMsg(const geometry_msgs::Transform& in, tf2::Transform& out) + { + tf2::Vector3 v; + tf2::fromMsg(in.translation, v); + out.setOrigin(v); + // w at the end in the constructor + tf2::Quaternion q; + tf2::fromMsg(in.rotation, q); + out.setRotation(q); + } +}; -/** \brief Convert a TransformStamped message to its equivalent tf2 representation. - * This function is a specialization of the fromMsg template defined in tf2/convert.h. - * \param msg A TransformStamped message. - * \param out The TransformStamped converted to the equivalent tf2 type. - */ -inline -void fromMsg(const geometry_msgs::TransformStamped& msg, tf2::Stamped& out) +template <> +struct defaultMessage { - out.stamp_ = msg.header.stamp; - out.frame_id_ = msg.header.frame_id; - tf2::Transform tmp; - fromMsg(msg.transform, tmp); - out.setData(tmp); -} + using type = geometry_msgs::Transform; +}; +} // namespace impl /** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Point type. * This function is a specialization of the doTransform template defined in tf2/convert.h. @@ -721,12 +253,12 @@ template <> inline void doTransform(const geometry_msgs::Point& t_in, geometry_msgs::Point& t_out, const geometry_msgs::TransformStamped& transform) { - tf2::Transform t; - fromMsg(transform.transform, t); - tf2::Vector3 v_in; - fromMsg(t_in, v_in); - tf2::Vector3 v_out = t * v_in; - toMsg(v_out, t_out); + tf2::Transform t; + tf2::fromMsg<>(transform.transform, t); + tf2::Vector3 v_in; + tf2::fromMsg<>(t_in, v_in); + tf2::Vector3 v_out = t * v_in; + tf2::toMsg<>(v_out, t_out); } /** \brief Apply a geometry_msgs TransformStamped to an stamped geometry_msgs Point type. @@ -755,11 +287,11 @@ inline void doTransform(const geometry_msgs::Quaternion& t_in, geometry_msgs::Quaternion& t_out, const geometry_msgs::TransformStamped& transform) { tf2::Quaternion t, q_in; - fromMsg(transform.transform.rotation, t); - fromMsg(t_in, q_in); + tf2::fromMsg<>(transform.transform.rotation, t); + tf2::fromMsg<>(t_in, q_in); tf2::Quaternion q_out = t * q_in; - t_out = toMsg(q_out); + tf2::toMsg<>(q_out, t_out); } /** \brief Apply a geometry_msgs TransformStamped to an stamped geometry_msgs Quaternion type. @@ -789,14 +321,14 @@ inline void doTransform(const geometry_msgs::Pose& t_in, geometry_msgs::Pose& t_out, const geometry_msgs::TransformStamped& transform) { tf2::Vector3 v; - fromMsg(t_in.position, v); + tf2::fromMsg<>(t_in.position, v); tf2::Quaternion r; - fromMsg(t_in.orientation, r); + tf2::fromMsg<>(t_in.orientation, r); tf2::Transform t; - fromMsg(transform.transform, t); + tf2::fromMsg<>(transform.transform, t); tf2::Transform v_out = t * tf2::Transform(r, v); - toMsg(v_out, t_out); + tf2::toMsg<>(v_out, t_out); } /** \brief Apply a geometry_msgs TransformStamped to an stamped geometry_msgs Pose type. @@ -913,14 +445,14 @@ inline void doTransform(const geometry_msgs::PoseWithCovarianceStamped& t_in, geometry_msgs::PoseWithCovarianceStamped& t_out, const geometry_msgs::TransformStamped& transform) { tf2::Vector3 v; - fromMsg(t_in.pose.pose.position, v); + tf2::fromMsg<>(t_in.pose.pose.position, v); tf2::Quaternion r; - fromMsg(t_in.pose.pose.orientation, r); + tf2::fromMsg<>(t_in.pose.pose.orientation, r); tf2::Transform t; - fromMsg(transform.transform, t); + tf2::fromMsg<>(transform.transform, t); tf2::Transform v_out = t * tf2::Transform(r, v); - toMsg(v_out, t_out.pose.pose); + tf2::toMsg<>(v_out, t_out.pose.pose); t_out.header.stamp = transform.header.stamp; t_out.header.frame_id = transform.header.frame_id; @@ -938,10 +470,10 @@ inline void doTransform(const geometry_msgs::TransformStamped& t_in, geometry_msgs::TransformStamped& t_out, const geometry_msgs::TransformStamped& transform) { tf2::Transform input; - fromMsg(t_in.transform, input); + tf2::fromMsg<>(t_in.transform, input); tf2::Transform t; - fromMsg(transform.transform, t); + tf2::fromMsg<>(transform.transform, t); tf2::Transform v_out = t * input; t_out.transform = toMsg(v_out); @@ -960,7 +492,7 @@ inline void doTransform(const geometry_msgs::Vector3& t_in, geometry_msgs::Vector3& t_out, const geometry_msgs::TransformStamped& transform) { tf2::Transform t; - fromMsg(transform.transform, t); + tf2::fromMsg<>(transform.transform, t); tf2::Vector3 v_out = t.getBasis() * tf2::Vector3(t_in.x, t_in.y, t_in.z); t_out.x = v_out[0]; t_out.y = v_out[1]; @@ -986,54 +518,25 @@ inline /**********************/ /*** WrenchStamped ****/ /**********************/ -template <> -inline -const ros::Time& getTimestamp(const geometry_msgs::WrenchStamped& t) {return t.header.stamp;} - -template <> -inline -const std::string& getFrameId(const geometry_msgs::WrenchStamped& t) {return t.header.frame_id;} - - -inline -geometry_msgs::WrenchStamped toMsg(const geometry_msgs::WrenchStamped& in) +namespace impl { - return in; -} - -inline -void fromMsg(const geometry_msgs::WrenchStamped& msg, geometry_msgs::WrenchStamped& out) +template <> +struct ImplDetails, geometry_msgs::Wrench> { - out = msg; -} - - -inline -geometry_msgs::WrenchStamped toMsg(const tf2::Stamped>& in, geometry_msgs::WrenchStamped & out) + static void toMsg(const std::array& in, geometry_msgs::Wrench& out) { - out.header.stamp = in.stamp_; - out.header.frame_id = in.frame_id_; - out.wrench.force = toMsg(in[0]); - out.wrench.torque = toMsg(in[1]); - return out; + tf2::toMsg(std::get<0>(in), out.force); + tf2::toMsg(std::get<1>(in), out.torque); } - -inline -void fromMsg(const geometry_msgs::WrenchStamped& msg, tf2::Stamped>& out) + static void fromMsg(const geometry_msgs::Wrench& msg, std::array& out) { - out.stamp_ = msg.header.stamp; - out.frame_id_ = msg.header.frame_id; - tf2::Vector3 tmp; - fromMsg(msg.wrench.force, tmp); - tf2::Vector3 tmp1; - fromMsg(msg.wrench.torque, tmp1); - std::array tmp_array; - tmp_array[0] = tmp; - tmp_array[1] = tmp1; - out.setData(tmp_array); + tf2::fromMsg<>(msg.force, std::get<0>(out)); + tf2::fromMsg<>(msg.torque, std::get<1>(out)); } +}; +} // namespace impl template<> inline diff --git a/tf2_kdl/include/tf2_kdl/tf2_kdl.h b/tf2_kdl/include/tf2_kdl/tf2_kdl.h index e1f28bfb1..7cbd89655 100644 --- a/tf2_kdl/include/tf2_kdl/tf2_kdl.h +++ b/tf2_kdl/include/tf2_kdl/tf2_kdl.h @@ -86,37 +86,53 @@ inline t_out = tf2::Stamped(transformToKDL(transform) * t_in, transform.header.stamp, transform.header.frame_id); } -/** \brief Convert a stamped KDL Vector type to a PointStamped message. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param in The timestamped KDL Vector to convert. - * \return The vector converted to a PointStamped message. - */ -inline -geometry_msgs::PointStamped toMsg(const tf2::Stamped& in) +namespace impl { - geometry_msgs::PointStamped msg; - msg.header.stamp = in.stamp_; - msg.header.frame_id = in.frame_id_; - msg.point.x = in[0]; - msg.point.y = in[1]; - msg.point.z = in[2]; - return msg; -} +template +struct KDLVectorImplDetails +{ + /** \brief Convert a stamped KDL Vector type to a PointStamped message. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param in The timestamped KDL Vector to convert. + * \return The vector converted to a PointStamped message. + */ + static void toMsg(const KDL::Vector& in, Msg& msg) + { + msg.x = in[0]; + msg.y = in[1]; + msg.z = in[2]; + } -/** \brief Convert a PointStamped message type to a stamped KDL-specific Vector type. - * This function is a specialization of the fromMsg template defined in tf2/convert.h - * \param msg The PointStamped message to convert. - * \param out The point converted to a timestamped KDL Vector. - */ -inline -void fromMsg(const geometry_msgs::PointStamped& msg, tf2::Stamped& out) + /** \brief Convert a PointStamped message type to a stamped KDL-specific Vector type. + * This function is a specialization of the fromMsg template defined in tf2/convert.h + * \param msg The PointStamped message to convert. + * \param out The point converted to a timestamped KDL Vector. + */ + static void fromMsg(const Msg& msg, KDL::Vector& out) + { + out[0] = msg.x; + out[1] = msg.y; + out[2] = msg.z; + } +}; + +template <> +struct ImplDetails : KDLVectorImplDetails { - out.stamp_ = msg.header.stamp; - out.frame_id_ = msg.header.frame_id; - out[0] = msg.point.x; - out[1] = msg.point.y; - out[2] = msg.point.z; -} +}; + +template <> +struct ImplDetails : KDLVectorImplDetails +{ +}; + +template <> +struct defaultMessage +{ + using type = geometry_msgs::Point; +}; + +} // namespace impl // --------------------- // Twist @@ -133,46 +149,42 @@ inline { t_out = tf2::Stamped(transformToKDL(transform) * t_in, transform.header.stamp, transform.header.frame_id); } - -/** \brief Convert a stamped KDL Twist type to a TwistStamped message. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param in The timestamped KDL Twist to convert. - * \return The twist converted to a TwistStamped message. - */ -inline -geometry_msgs::TwistStamped toMsg(const tf2::Stamped& in) +namespace impl { - geometry_msgs::TwistStamped msg; - msg.header.stamp = in.stamp_; - msg.header.frame_id = in.frame_id_; - msg.twist.linear.x = in.vel[0]; - msg.twist.linear.y = in.vel[1]; - msg.twist.linear.z = in.vel[2]; - msg.twist.angular.x = in.rot[0]; - msg.twist.angular.y = in.rot[1]; - msg.twist.angular.z = in.rot[2]; - return msg; -} - -/** \brief Convert a TwistStamped message type to a stamped KDL-specific Twist type. - * This function is a specialization of the fromMsg template defined in tf2/convert.h - * \param msg The TwistStamped message to convert. - * \param out The twist converted to a timestamped KDL Twist. - */ -inline -void fromMsg(const geometry_msgs::TwistStamped& msg, tf2::Stamped& out) +template <> +struct ImplDetails { - out.stamp_ = msg.header.stamp; - out.frame_id_ = msg.header.frame_id; - out.vel[0] = msg.twist.linear.x; - out.vel[1] = msg.twist.linear.y; - out.vel[2] = msg.twist.linear.z; - out.rot[0] = msg.twist.angular.x; - out.rot[1] = msg.twist.angular.y; - out.rot[2] = msg.twist.angular.z; -} - + /** \brief Convert a stamped KDL Twist type to a TwistStamped message. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param in The timestamped KDL Twist to convert. + * \return The twist converted to a TwistStamped message. + */ + static void toMsg(const KDL::Twist& in, geometry_msgs::Twist& msg) + { + msg.linear.x = in.vel[0]; + msg.linear.y = in.vel[1]; + msg.linear.z = in.vel[2]; + msg.angular.x = in.rot[0]; + msg.angular.y = in.rot[1]; + msg.angular.z = in.rot[2]; + } + /** \brief Convert a TwistStamped message type to a stamped KDL-specific Twist type. + * This function is a specialization of the fromMsg template defined in tf2/convert.h + * \param msg The TwistStamped message to convert. + * \param out The twist converted to a timestamped KDL Twist. + */ + static void fromMsg(const geometry_msgs::Twist& msg, KDL::Twist& out) + { + out.vel[0] = msg.linear.x; + out.vel[1] = msg.linear.y; + out.vel[2] = msg.linear.z; + out.rot[0] = msg.angular.x; + out.rot[1] = msg.angular.y; + out.rot[2] = msg.angular.z; + } +}; +} // namespace impl // --------------------- // Wrench // --------------------- @@ -189,47 +201,42 @@ inline t_out = tf2::Stamped(transformToKDL(transform) * t_in, transform.header.stamp, transform.header.frame_id); } -/** \brief Convert a stamped KDL Wrench type to a WrenchStamped message. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param in The timestamped KDL Wrench to convert. - * \return The wrench converted to a WrenchStamped message. - */ -inline -geometry_msgs::WrenchStamped toMsg(const tf2::Stamped& in) +namespace impl { - geometry_msgs::WrenchStamped msg; - msg.header.stamp = in.stamp_; - msg.header.frame_id = in.frame_id_; - msg.wrench.force.x = in.force[0]; - msg.wrench.force.y = in.force[1]; - msg.wrench.force.z = in.force[2]; - msg.wrench.torque.x = in.torque[0]; - msg.wrench.torque.y = in.torque[1]; - msg.wrench.torque.z = in.torque[2]; - return msg; -} - -/** \brief Convert a WrenchStamped message type to a stamped KDL-specific Wrench type. - * This function is a specialization of the fromMsg template defined in tf2/convert.h - * \param msg The WrenchStamped message to convert. - * \param out The wrench converted to a timestamped KDL Wrench. - */ -inline -void fromMsg(const geometry_msgs::WrenchStamped& msg, tf2::Stamped& out) +template <> +struct ImplDetails { - out.stamp_ = msg.header.stamp; - out.frame_id_ = msg.header.frame_id; - out.force[0] = msg.wrench.force.x; - out.force[1] = msg.wrench.force.y; - out.force[2] = msg.wrench.force.z; - out.torque[0] = msg.wrench.torque.x; - out.torque[1] = msg.wrench.torque.y; - out.torque[2] = msg.wrench.torque.z; -} - - - + /** \brief Convert a stamped KDL Wrench type to a WrenchStamped message. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param in The timestamped KDL Wrench to convert. + * \return The wrench converted to a WrenchStamped message. + */ + static void toMsg(const KDL::Wrench& in, geometry_msgs::Wrench& msg) + { + msg.force.x = in.force[0]; + msg.force.y = in.force[1]; + msg.force.z = in.force[2]; + msg.torque.x = in.torque[0]; + msg.torque.y = in.torque[1]; + msg.torque.z = in.torque[2]; + } + /** \brief Convert a WrenchStamped message type to a stamped KDL-specific Wrench type. + * This function is a specialization of the fromMsg template defined in tf2/convert.h + * \param msg The WrenchStamped message to convert. + * \param out The wrench converted to a timestamped KDL Wrench. + */ + static void fromMsg(const geometry_msgs::Wrench& msg, KDL::Wrench& out) + { + out.force[0] = msg.force.x; + out.force[1] = msg.force.y; + out.force[2] = msg.force.z; + out.torque[0] = msg.torque.x; + out.torque[1] = msg.torque.y; + out.torque[2] = msg.torque.z; + } +}; +} // namespace impl // --------------------- // Frame // --------------------- @@ -245,64 +252,38 @@ inline { t_out = tf2::Stamped(transformToKDL(transform) * t_in, transform.header.stamp, transform.header.frame_id); } - -/** \brief Convert a stamped KDL Frame type to a Pose message. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param in The timestamped KDL Frame to convert. - * \return The frame converted to a Pose message. - */ -inline -geometry_msgs::Pose toMsg(const KDL::Frame& in) +namespace impl { - geometry_msgs::Pose msg; - msg.position.x = in.p[0]; - msg.position.y = in.p[1]; - msg.position.z = in.p[2]; - in.M.GetQuaternion(msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w); - return msg; -} - -/** \brief Convert a Pose message type to a KDL Frame. - * This function is a specialization of the fromMsg template defined in tf2/convert.h. - * \param msg The Pose message to convert. - * \param out The pose converted to a KDL Frame. - */ -inline -void fromMsg(const geometry_msgs::Pose& msg, KDL::Frame& out) -{ - out.p[0] = msg.position.x; - out.p[1] = msg.position.y; - out.p[2] = msg.position.z; - out.M = KDL::Rotation::Quaternion(msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w); -} - -/** \brief Convert a stamped KDL Frame type to a Pose message. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param in The timestamped KDL Frame to convert. - * \return The frame converted to a PoseStamped message. - */ -inline -geometry_msgs::PoseStamped toMsg(const tf2::Stamped& in) +template <> +struct ImplDetails { - geometry_msgs::PoseStamped msg; - msg.header.stamp = in.stamp_; - msg.header.frame_id = in.frame_id_; - msg.pose = toMsg(static_cast(in)); - return msg; -} + /** \brief Convert a stamped KDL Frame type to a Pose message. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param in The timestamped KDL Frame to convert. + * \return The frame converted to a Pose message. + */ + static void toMsg(const KDL::Frame& in, geometry_msgs::Pose& msg) + { + msg.position.x = in.p[0]; + msg.position.y = in.p[1]; + msg.position.z = in.p[2]; + in.M.GetQuaternion(msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w); + } -/** \brief Convert a Pose message transform type to a stamped KDL Frame. - * This function is a specialization of the fromMsg template defined in tf2/convert.h. - * \param msg The PoseStamped message to convert. - * \param out The pose converted to a timestamped KDL Frame. - */ -inline -void fromMsg(const geometry_msgs::PoseStamped& msg, tf2::Stamped& out) -{ - out.stamp_ = msg.header.stamp; - out.frame_id_ = msg.header.frame_id; - fromMsg(msg.pose, static_cast(out)); -} + /** \brief Convert a Pose message type to a KDL Frame. + * This function is a specialization of the fromMsg template defined in tf2/convert.h. + * \param msg The Pose message to convert. + * \param out The pose converted to a KDL Frame. + */ + static void fromMsg(const geometry_msgs::Pose& msg, KDL::Frame& out) + { + out.p[0] = msg.position.x; + out.p[1] = msg.position.y; + out.p[2] = msg.position.z; + out.M = KDL::Rotation::Quaternion(msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w); + } +}; +} // namespace impl } // namespace diff --git a/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.h b/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.h index 9e16e0d2e..f66102302 100644 --- a/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.h +++ b/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.h @@ -91,16 +91,6 @@ void doTransform(const sensor_msgs::PointCloud2 &p_in, sensor_msgs::PointCloud2 *z_out = point.z(); } } -inline -sensor_msgs::PointCloud2 toMsg(const sensor_msgs::PointCloud2 &in) -{ - return in; -} -inline -void fromMsg(const sensor_msgs::PointCloud2 &msg, sensor_msgs::PointCloud2 &out) -{ - out = msg; -} } // namespace From e45f5b8000283de2a8657478b0a0ff38af01add8 Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Sun, 29 Nov 2020 16:38:45 +0000 Subject: [PATCH 3/6] add missing defaultMessage specialization --- tf2_kdl/include/tf2_kdl/tf2_kdl.h | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/tf2_kdl/include/tf2_kdl/tf2_kdl.h b/tf2_kdl/include/tf2_kdl/tf2_kdl.h index 7cbd89655..150833e46 100644 --- a/tf2_kdl/include/tf2_kdl/tf2_kdl.h +++ b/tf2_kdl/include/tf2_kdl/tf2_kdl.h @@ -252,6 +252,7 @@ inline { t_out = tf2::Stamped(transformToKDL(transform) * t_in, transform.header.stamp, transform.header.frame_id); } + namespace impl { template <> @@ -283,6 +284,12 @@ struct ImplDetails out.M = KDL::Rotation::Quaternion(msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w); } }; + +template <> +struct defaultMessage +{ + using type = geometry_msgs::Pose; +}; } // namespace impl } // namespace From 332970b82d2c0ffe45b01fbe63186a9ff4ca4fd8 Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Fri, 11 Dec 2020 13:18:43 +0000 Subject: [PATCH 4/6] First doxygen steps --- tf2/include/tf2/convert.h | 81 +++++++++++++++++--- tf2/include/tf2/impl/convert.h | 104 +++++++++++++++++++++++++- tf2/include/tf2/impl/stamped_traits.h | 3 + 3 files changed, 176 insertions(+), 12 deletions(-) diff --git a/tf2/include/tf2/convert.h b/tf2/include/tf2/convert.h index 20abdb8f3..646cf696c 100644 --- a/tf2/include/tf2/convert.h +++ b/tf2/include/tf2/convert.h @@ -40,11 +40,40 @@ 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 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 + * { + * static void toMsg(const Datatype&, Message&); + * static void fromMsg(const Message&, Datatype&); + * }: + * \endcode + * Note that the conversion between tf2::Stamped\ and + * geometry_msgs::...Stamped is done automatically. + */ template struct ImplDetails { @@ -52,8 +81,10 @@ struct ImplDetails // void fromMsg(const Message&, Datatype&); }; +// Forward declaration for the extraction of timestamps and frame IDs template struct DefaultStampedImpl; +// Forward declaration for the tf2::convert() implementation template class Converter; @@ -73,8 +104,13 @@ void doTransform(const T& data_in, T& data_out, const geometry_msgs::TransformSt /**\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 inline @@ -85,8 +121,13 @@ const ros::Time& getTimestamp(const T& t) /**\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 inline @@ -95,10 +136,14 @@ const std::string& getFrameId(const T& t) return impl::DefaultStampedImpl::getFrameId(t); } -/** 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 + * \tparam A Non-message Datatype + * \tparam B ROS message Datatype. The default value will be taken from impl::defaultMessage\::type. * \return the conversion as a ROS message */ template ::type> @@ -109,6 +154,18 @@ inline B toMsg(const A& a) return b; } + +/** + * \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 + * \param b ROS message + * \tparam A Non-message Datatype + * \tparam B Type of the ROS Message + * \return Reference to the parameter b + */ template inline B& toMsg(const A& a, B& b) { @@ -116,11 +173,15 @@ inline B& toMsg(const A& a, B& 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 inline void fromMsg(const A& a, B& b) @@ -128,24 +189,26 @@ inline void fromMsg(const A& a, B& b) impl::ImplDetails::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 inline void convert(const A& a, B& b) { - // printf("In double type convert\n"); impl::Converter::value, ros::message_traits::IsMessage::value>::convert(a, b); } template inline void convert(const A& a1, A& a2) { - // printf("In single type convert\n"); if (&a1 != &a2) a2 = a1; } diff --git a/tf2/include/tf2/impl/convert.h b/tf2/include/tf2/impl/convert.h index f6e8941f6..5b0e9cd53 100644 --- a/tf2/include/tf2/impl/convert.h +++ b/tf2/include/tf2/impl/convert.h @@ -35,6 +35,35 @@ namespace tf2 namespace impl { +/** + * \brief Mapping of unstamped Messages for stamped Messages + * + * This struct contains utility methods to access the data member of a stamped ROS message + * and an alias (named \c unstampedType ) of the unstamped message type. + * It is needed for the conversion of stamped datatypes, + * so that only the conversions of unstamped datatypes has do be implemented. + * For example, a \c geometry_msgs::Vector3Stamped has two members, + * the \c header (which contains a timestamp and a frame ID) and the \c vector itself. + * For this class, the specialization should look like + * \code + * template<> + * struct stampedMessageTraits + * { + * using unstampedType = geometry_msgs::Vector3; + * static geometry_msgs::Vector3& accessMessage(geometry_msgs::Vector3Stamped& vs) + * { + * return vs.vector; + * } + * static geometry_msgs::Vector3 getMessage(const geometry_msgs::Vector3Stamped& vs) + * { + * return vs.vector; + * } + * }; + * \endcode + * The both almost identical methods are required to keep const-correctness. + * + * \tparam StampedMessage The datatype of the ros message + */ template struct stampedMessageTraits { @@ -43,24 +72,59 @@ struct stampedMessageTraits // static unstampedType getMessage(StampedMsg const&); }; +/** + * \brief Mapping of stamped Messages for unstamped Messages + * + * This struct is needed for the deduction of the return type of + * tf2::convert() for tf2::Stamped\<\> datatypes. + * Its specializations should contain an alias (named \c stampedType ) + * of the stamped type. + * Example: + * \code + * template<> + * struct unstampedMessageTraits + * { + * using stampedType = geometry_msgs::Vector3Stamped; + * }; + * \endcode + * + * \tparam UnstampedMessage Type of the ROS message which is not stamped + */ template struct unstampedMessageTraits { // using stampedType = ...; }; +/** + * \brief Partial specialization of impl::defaultMessage for stamped types + * + * The deduction of the default ROS message type of a tf2::Stamped\ type is + * based on the default ROS message type of \c T . + * \tparam T The unstamped datatype (not a ROS message) + */ + template struct defaultMessage> { using type = typename unstampedMessageTraits::type>::stampedType; }; - +/** + * \brief Partial specialization of impl::ImplDetails for stamped types + * + * This partial specialization provides the conversion implementation ( \c toMsg() and \c fromMsg() ) + * between stamped types ( non-message types of tf2::Stamped\ and ROS message datatypes with a \c header member). + * The timestamp and the frame ID are preserved during the conversion. + * The implementation of tf2::toMsg() and tf2::fromMsg() for the unstamped types are required, + * as well as a specialization of stampedMessageTraits. + * \tparam Datatype Unstamped non-message type + * \tparam StampedMessage Stamped ROS message type + */ template struct ImplDetails, StampedMessage> { using traits = stampedMessageTraits; - using unstampedMessage = typename traits::unstampedType; static void toMsg(const tf2::Stamped& s, StampedMessage& msg) { @@ -120,27 +184,61 @@ inline void Converter::convert(const A& a, B& b) template using void_t = void; +/** + * \brief Default implementation for extracting timestamps and frame IDs. + * + * Both static member functions are for stamped ROS messages. + * They are SFINAE'd out if T is not a stamped ROS message. + * + * \tparam T Arbitrary datatype + */ template struct DefaultStampedImpl { + /**\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. + * + * The second parameter is needed to hide the default implementation if T is not a stamped ROS message. + */ static const ros::Time& getTimestamp(const T& t, void_t::unstampedType>* = nullptr) { return t.header.stamp; } - + /**\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. + * + * The second parameter is needed to hide the default implementation if T is not a stamped ROS message. + */ static const std::string& getFrameId(const T& t, void_t::unstampedType>* = nullptr) { return t.header.frame_id; } }; +/** + * \brief Partial specialization of DefaultStampedImpl for tf2::Stamped\<\> types + */ template struct DefaultStampedImpl> { + /**\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. + */ static const ros::Time& getTimestamp(const tf2::Stamped& t) { return t.stamp_; } + /** 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. + */ static const std::string& getFrameId(const tf2::Stamped& t) { return t.frame_id_; diff --git a/tf2/include/tf2/impl/stamped_traits.h b/tf2/include/tf2/impl/stamped_traits.h index 5369c3677..69a21879d 100644 --- a/tf2/include/tf2/impl/stamped_traits.h +++ b/tf2/include/tf2/impl/stamped_traits.h @@ -136,6 +136,9 @@ struct unstampedMessageTraits> template struct stampedMessageTraits; +// we use partial specializations (with the allocator as template parameter) +// to avoid including all the message definitons + template struct stampedMessageTraits> { From 32dd5a51fa145956fab63bffcbd080ca32cc5439 Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Fri, 11 Dec 2020 13:28:15 +0000 Subject: [PATCH 5/6] Fix -Wcatch-value --- tf2_ros/src/buffer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tf2_ros/src/buffer.cpp b/tf2_ros/src/buffer.cpp index 803030165..d081c77ee 100644 --- a/tf2_ros/src/buffer.cpp +++ b/tf2_ros/src/buffer.cpp @@ -78,7 +78,7 @@ ros::Time now_fallback_to_wall() { return ros::Time::now(); } - catch (ros::TimeNotInitializedException ex) + catch (const ros::TimeNotInitializedException&) { ros::WallTime wt = ros::WallTime::now(); return ros::Time(wt.sec, wt.nsec); @@ -96,7 +96,7 @@ void sleep_fallback_to_wall(const ros::Duration& d) { d.sleep(); } - catch (ros::TimeNotInitializedException ex) + catch (const ros::TimeNotInitializedException&) { ros::WallDuration wd = ros::WallDuration(d.sec, d.nsec); wd.sleep(); From 20979ee292b75bfe1c9b0c34f844552e6742c094 Mon Sep 17 00:00:00 2001 From: Bjar Ne <43565432+gleichdick@users.noreply.github.com> Date: Mon, 25 Jan 2021 18:19:10 +0000 Subject: [PATCH 6/6] Fix KDL wrench and twist --- tf2_kdl/include/tf2_kdl/tf2_kdl.h | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/tf2_kdl/include/tf2_kdl/tf2_kdl.h b/tf2_kdl/include/tf2_kdl/tf2_kdl.h index 150833e46..f39cb9176 100644 --- a/tf2_kdl/include/tf2_kdl/tf2_kdl.h +++ b/tf2_kdl/include/tf2_kdl/tf2_kdl.h @@ -152,7 +152,7 @@ inline namespace impl { template <> -struct ImplDetails +struct ImplDetails { /** \brief Convert a stamped KDL Twist type to a TwistStamped message. * This function is a specialization of the toMsg template defined in tf2/convert.h. @@ -184,6 +184,13 @@ struct ImplDetails out.rot[2] = msg.angular.z; } }; + +template <> +struct defaultMessage +{ + using type = geometry_msgs::Twist; +}; + } // namespace impl // --------------------- // Wrench @@ -204,7 +211,7 @@ inline namespace impl { template <> -struct ImplDetails +struct ImplDetails { /** \brief Convert a stamped KDL Wrench type to a WrenchStamped message. * This function is a specialization of the toMsg template defined in tf2/convert.h. @@ -236,6 +243,12 @@ struct ImplDetails out.torque[2] = msg.torque.z; } }; + +template <> +struct defaultMessage +{ + using type = geometry_msgs::Wrench; +}; } // namespace impl // --------------------- // Frame