From 92427036cbfb48c0d1d0ac2ceeab85125225e332 Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Mon, 25 Jan 2021 15:12:22 +0000 Subject: [PATCH 01/51] Add test for Vector/Point toMsg() --- test_tf2/test/test_convert.cpp | 86 ++++++++++++++++++++++++++++++++++ 1 file changed, 86 insertions(+) diff --git a/test_tf2/test/test_convert.cpp b/test_tf2/test/test_convert.cpp index 0a022b009..b68d10ae7 100644 --- a/test_tf2/test/test_convert.cpp +++ b/test_tf2/test/test_convert.cpp @@ -38,6 +38,7 @@ #include #include #include +#include #include TEST(tf2Convert, kdlToBullet) @@ -96,6 +97,91 @@ TEST(tf2Convert, kdlBulletROSConversions) EXPECT_NEAR(b1.z(), b4.z(), epsilon); } +TEST(tf2Convert, ConvertTf2Quaternion) +{ + tf2::Quaternion tq(1,2,3,4); + Eigen::Quaterniond eq; + tf2::convert(tq, eq); + + EXPECT_EQ(tq.w(), eq.w()); + EXPECT_EQ(tq.x(), eq.x()); + EXPECT_EQ(tq.y(), eq.y()); + 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); From c095835dacaa255eeaeeeeb38ee48c0c67128d88 Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Fri, 12 Feb 2021 00:13:08 +0000 Subject: [PATCH 02/51] Complete rework of toMsg()/fromMsg() --- tf2/include/tf2/convert.h | 178 ++++-- tf2/include/tf2/impl/convert.h | 230 +++++++- tf2/include/tf2/impl/stamped_traits.h | 232 ++++++++ tf2_bullet/include/tf2_bullet/tf2_bullet.h | 60 +- tf2_eigen/include/tf2_eigen/tf2_eigen.h | 522 ++++++----------- .../include/tf2_eigen_kdl/tf2_eigen_kdl.hpp | 43 +- .../tf2_geometry_msgs/tf2_geometry_msgs.h | 551 ++++++------------ tf2_kdl/include/tf2_kdl/tf2_kdl.h | 313 +++++----- 8 files changed, 1123 insertions(+), 1006 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 3ea9618ab..dc15c704c 100644 --- a/tf2/include/tf2/convert.h +++ b/tf2/include/tf2/convert.h @@ -26,24 +26,74 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. -/** \author Tully Foote */ +/** \author Tully Foote, Bjarne von Horn */ #ifndef TF2__CONVERT_H_ #define TF2__CONVERT_H_ #include +#include +#include +#include #include -#include "builtin_interfaces/msg/time.hpp" -#include "geometry_msgs/msg/transform_stamped.hpp" -#include "rosidl_runtime_cpp/traits.hpp" -#include "tf2/exceptions.h" -#include "tf2/impl/convert.h" -#include "tf2/transform_datatypes.h" -#include "tf2/visibility_control.h" +#include "exceptions.h" +#include "transform_datatypes.h" +#include "visibility_control.h" 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 +{ + // void toMsg(const Datatype&, Message&); + // 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; + +} // namespace impl /**\brief The templated function expected to be able to do a transform * @@ -63,15 +113,21 @@ void doTransform( * \param[in] t The data input. * \return The timestamp associated with the data. */ -template -tf2::TimePoint getTimestamp(const T & t); +template +inline tf2::TimePoint getTimestamp(const T & t) +{ + return impl::DefaultStampedImpl::getTimestamp(t); +} /**\brief Get the frame_id from data * \param[in] t The data input. * \return The frame_id associated with the data. */ -template -std::string getFrameId(const T & t); +template +inline std::string getFrameId(const T & t) +{ + return impl::DefaultStampedImpl::getFrameId(t); +} /**\brief Get the covariance matrix from data * \param[in] t The data input. @@ -80,32 +136,6 @@ std::string getFrameId(const T & t); template std::array, 6> getCovarianceMatrix(const T & t); -/**\brief Get the frame_id from data - * - * An implementation for Stamped

datatypes. - * - * \param[in] t The data input. - * \return The frame_id associated with the data. - */ -template -tf2::TimePoint getTimestamp(const tf2::Stamped

& t) -{ - return t.stamp_; -} - -/**\brief Get the frame_id from data - * - * An implementation for Stamped

datatypes. - * - * \param[in] t The data input. - * \return The frame_id associated with the data. - */ -template -std::string getFrameId(const tf2::Stamped

& t) -{ - return t.frame_id_; -} - /**\brief Get the covariance matrix from data * * An implementation for WithCovarianceStamped

datatypes. @@ -119,39 +149,78 @@ std::array, 6> getCovarianceMatrix(const tf2::WithCovarian return t.cov_mat_; } -/**\brief 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 -B toMsg(const A & a); +template ::type> +inline B toMsg(const A & a) +{ + B b; + impl::ImplDetails::toMsg(a, b); + 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) +{ + impl::ImplDetails::toMsg(a, b); + return b; +} -/**\brief 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 -void fromMsg(const A &, B & b); +template +inline void fromMsg(const A & a, B & b) +{ + impl::ImplDetails::fromMsg(a, b); +} -/**\brief 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 -void convert(const A & a, B & b) +template +inline void convert(const A & a, B & b) { - impl::Converter::value, + impl::Converter< + rosidl_generator_traits::is_message::value, rosidl_generator_traits::is_message::value>::convert(a, b); } -template +template void convert(const A & a1, A & a2) { if (&a1 != &a2) { @@ -202,4 +271,7 @@ std::array covarianceNestedToRowMajor(const std::array + +#include "../convert.h" +#include "../time.h" + +namespace builtin_interfaces +{ +namespace msg +{ +template +class Time_; +} +} // namespace builtin_interfaces + 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 +{ + // using unstampedType = ...; + // static unstampedType& accessMessage(StampedMsg &); + // static unstampedType getMessage(StampedMsg const&); +}; -template +/** + * \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; + + static void toMsg(const tf2::Stamped & s, StampedMessage & msg) + { + tf2::toMsg<>(static_cast(s), traits::accessMessage(msg)); + tf2::toMsg<>(s.stamp_, msg.header.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)); + tf2::fromMsg<>(msg.header.stamp, s.stamp_); + s.frame_id_ = msg.header.frame_id; + } +}; + +template <> +struct ImplDetails +{ + static void toMsg(const tf2::TimePoint & t, builtin_interfaces::msg::Time & time_msg) + { + std::chrono::nanoseconds ns = + std::chrono::duration_cast(t.time_since_epoch()); + std::chrono::seconds s = std::chrono::duration_cast(t.time_since_epoch()); + + time_msg.sec = static_cast(s.count()); + time_msg.nanosec = static_cast(ns.count() % 1000000000ull); + } + + static void fromMsg(const builtin_interfaces::msg::Time & time_msg, tf2::TimePoint & t) + { + int64_t d = time_msg.sec * 1000000000ull + time_msg.nanosec; + std::chrono::nanoseconds ns(d); + t = tf2::TimePoint(std::chrono::duration_cast(ns)); + } +}; + +template class Converter { public: - template + template static void convert(const A & a, B & b); }; @@ -49,31 +191,93 @@ class Converter // if B == A, the templated version of convert with only one argument will be // used. // -template< > -template +template <> +template inline void Converter::convert(const A & a, B & b); -template< > -template +template <> +template inline void Converter::convert(const A & a, B & b) { - fromMsg(a, b); + fromMsg<>(a, b); } -template< > -template +template <> +template inline void Converter::convert(const A & a, B & b) { - b = toMsg(a); + b = toMsg<>(a); } -template< > -template +template <> +template inline void Converter::convert(const A & a, B & b) { - fromMsg(toMsg(a), b); + fromMsg<>(toMsg<>(a), 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 tf2::TimePoint getTimestamp( + const T & t, void_t::unstampedType> * = nullptr) + { + tf2::TimePoint timestamp; + tf2::fromMsg<>(t.header.stamp, timestamp); + return timestamp; + } + /**\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 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 tf2::TimePoint 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 std::string getFrameId(const tf2::Stamped & t) { return t.frame_id_; } +}; + } // namespace impl } // namespace tf2 diff --git a/tf2/include/tf2/impl/stamped_traits.h b/tf2/include/tf2/impl/stamped_traits.h new file mode 100644 index 000000000..0b9d6123f --- /dev/null +++ b/tf2/include/tf2/impl/stamped_traits.h @@ -0,0 +1,232 @@ + +/* + * Copyright (c) 2021, Bjarne von Horn + * 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 Bjarne von Horn 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. + */ + +/** \author Bjarne von Horn */ + + +#ifndef TF2_IMPL_STAMPED_TRAITS_H_ +#define TF2_IMPL_STAMPED_TRAITS_H_ + +// forward declarations +namespace geometry_msgs +{ +namespace msg +{ +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 msg +} // namespace geometry_msgs + +namespace tf2 +{ +namespace impl +{ +template +struct unstampedMessageTraits; + +template +struct unstampedMessageTraits> +{ + using stampedType = geometry_msgs::msg::PointStamped_; +}; + +template +struct unstampedMessageTraits> +{ + using stampedType = geometry_msgs::msg::VectorStamped_; +}; + +template +struct unstampedMessageTraits> +{ + using stampedType = geometry_msgs::msg::QuaternionStamped_; +}; + +template +struct unstampedMessageTraits> +{ + using stampedType = geometry_msgs::msg::PoseStamped_; +}; + +template +struct unstampedMessageTraits> +{ + using stampedType = geometry_msgs::msg::TwistStamped_; +}; + +template +struct unstampedMessageTraits> +{ + using stampedType = geometry_msgs::msg::PoseWithCovarianceStamped_; +}; + +template +struct unstampedMessageTraits> +{ + using stampedType = geometry_msgs::msg::WrenchStamped_; +}; + +template +struct unstampedMessageTraits> +{ + using stampedType = geometry_msgs::msg::TransformStamped_; +}; + +template +struct unstampedMessageTraits> +{ + using stampedType = geometry_msgs::msg::Vector3Stamped_; +}; + +template +struct stampedMessageTraits; + +template +struct defaultStampedMessageTraits +{ + using unstampedType = UnstampedMessage; + static unstampedType & accessMessage(StampedMessage & stamped) { return stamped.*member; } + static unstampedType getMessage(StampedMessage const & stamped) { return stamped.*member; } +}; + +// we use partial specializations (with the allocator as template parameter) +// to avoid including all the message definitons + +template +struct stampedMessageTraits> +: defaultStampedMessageTraits< + geometry_msgs::msg::PointStamped_, geometry_msgs::msg::Point_, + &geometry_msgs::msg::PointStamped_::point> +{ +}; + +template +struct stampedMessageTraits> +: defaultStampedMessageTraits< + geometry_msgs::msg::VectorStamped_, geometry_msgs::msg::Vector_, + &geometry_msgs::msg::VectorStamped_::vector> +{ +}; + +template +struct stampedMessageTraits> +: defaultStampedMessageTraits< + geometry_msgs::msg::QuaternionStamped_, geometry_msgs::msg::Quaternion_, + &geometry_msgs::msg::QuaternionStamped_::quaternion> +{ +}; + +template +struct stampedMessageTraits> +: defaultStampedMessageTraits< + geometry_msgs::msg::PoseStamped_, geometry_msgs::msg::Pose_, + &geometry_msgs::msg::PoseStamped_::pose> +{ +}; + +template +struct stampedMessageTraits> +: defaultStampedMessageTraits< + geometry_msgs::msg::TwistStamped_, geometry_msgs::msg::Twist_, + &geometry_msgs::msg::TwistStamped_::twist> +{ +}; + +template +struct stampedMessageTraits> +: defaultStampedMessageTraits< + geometry_msgs::msg::PoseWithCovarianceStamped_, + geometry_msgs::msg::PoseWithCovariance_, + &geometry_msgs::msg::PoseWithCovarianceStamped_::pose> +{ +}; + +template +struct stampedMessageTraits> +: defaultStampedMessageTraits< + geometry_msgs::msg::WrenchStamped_, geometry_msgs::msg::Wrench_, + &geometry_msgs::msg::WrenchStamped_::wrench> +{ +}; + +template +struct stampedMessageTraits> +: defaultStampedMessageTraits< + geometry_msgs::msg::TransformStamped_, geometry_msgs::msg::Transform_, + &geometry_msgs::msg::TransformStamped_::transform> +{ +}; + +template +struct stampedMessageTraits> +: defaultStampedMessageTraits< + geometry_msgs::msg::Vector3Stamped_, geometry_msgs::msg::Vector3_, + &geometry_msgs::msg::Vector3Stamped_::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 2057ab810..3a83bbd24 100644 --- a/tf2_bullet/include/tf2_bullet/tf2_bullet.h +++ b/tf2_bullet/include/tf2_bullet/tf2_bullet.h @@ -26,7 +26,7 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. -/** \author Wim Meeussen */ +/** \author Wim Meeussen, Bjarne von Horn */ #ifndef TF2_BULLET__TF2_BULLET_H_ #define TF2_BULLET__TF2_BULLET_H_ @@ -88,38 +88,42 @@ namespace tf2 tf2_ros::fromMsg(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::msg::PointStamped toMsg(const tf2::Stamped < btVector3 > & in) +namespace impl +{ +template <> +struct defaultMessage +{ + using type = geometry_msgs::msg::Point; +}; + +template <> +struct ImplDetails +{ + /** \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::msg::Point & msg) { - geometry_msgs::msg::PointStamped msg; - msg.header.stamp = tf2_ros::toMsg(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; + 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. - */ - inline - void fromMsg(const geometry_msgs::msg::PointStamped & msg, tf2::Stamped < btVector3 > & out) + /** \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::msg::Point & msg, btVector3 & out) { - out.stamp_ = tf2_ros::fromMsg(msg.header.stamp); - out.frame_id_ = msg.header.frame_id; - out[0] = static_cast < float > (msg.point.x); - out[1] = static_cast < float > (msg.point.y); - out[2] = static_cast < float > (msg.point.z); + 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 58cd5170a..bc573ae4a 100644 --- a/tf2_eigen/include/tf2_eigen/tf2_eigen.h +++ b/tf2_eigen/include/tf2_eigen/tf2_eigen.h @@ -24,7 +24,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ -/** \author Koji Terada */ +/** \author Koji Terada, Bjarne von Horn */ #ifndef TF2_EIGEN__TF2_EIGEN_H_ #define TF2_EIGEN__TF2_EIGEN_H_ @@ -127,60 +127,54 @@ void doTransform( 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::msg::Point toMsg(const Eigen::Vector3d & in) +namespace impl { - geometry_msgs::msg::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::msg::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::msg::Vector3 & toMsg(const Eigen::Vector3d & in, geometry_msgs::msg::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::msg::Vector3 & msg, Eigen::Vector3d & out) +template <> +struct defaultMessage { - out.x() = msg.x; - out.y() = msg.y; - out.z() = msg.z; -} + using type = geometry_msgs::msg::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. @@ -201,34 +195,6 @@ void doTransform( 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::msg::PointStamped toMsg(const tf2::Stamped & in) -{ - geometry_msgs::msg::PointStamped msg; - msg.header.stamp = tf2_ros::toMsg(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::msg::PointStamped & msg, tf2::Stamped & out) -{ - out.stamp_ = tf2_ros::fromMsg(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 @@ -258,32 +224,41 @@ void doTransform( 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::msg::Quaternion toMsg(const Eigen::Quaterniond & in) -{ - geometry_msgs::msg::Quaternion msg; - msg.w = in.w(); - msg.x = in.x(); - msg.y = in.y(); - msg.z = in.z(); - return msg; -} - -/** \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::msg::Quaternion & msg, Eigen::Quaterniond & out) -{ - out = Eigen::Quaterniond(msg.w, msg.x, msg.y, msg.z); -} +template <> +struct ImplDetails +{ + static void toMsg(const Eigen::Quaterniond & in, geometry_msgs::msg::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. + */ + static void fromMsg(const geometry_msgs::msg::Quaternion & msg, Eigen::Quaterniond & out) + { + out = Eigen::Quaterniond(msg.w, msg.x, msg.y, msg.z); + } +}; + +template <> +struct defaultMessage +{ + using type = geometry_msgs::msg::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, @@ -306,34 +281,6 @@ void doTransform( t_out = t * t_in; } -/** \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::msg::QuaternionStamped toMsg(const Stamped & in) -{ - geometry_msgs::msg::QuaternionStamped msg; - msg.header.stamp = tf2_ros::toMsg(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::msg::QuaternionStamped & msg, Stamped & out) -{ - out.frame_id_ = msg.header.frame_id; - out.stamp_ = tf2_ros::fromMsg(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. @@ -354,45 +301,113 @@ void doTransform( 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::msg::Pose toMsg(const Eigen::Affine3d & in) -{ - geometry_msgs::msg::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(); - return msg; -} - -/** \brief Convert a Eigen Isometry3d transform type to a Pose message. +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::msg::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; + } + } + + /** \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::msg::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)); + } +}; + +template <> +struct ImplDetails +: public PoseImplDetails +{ +}; + +template <> +struct ImplDetails +: public PoseImplDetails +{ +}; + +template <> +struct defaultMessage +{ + using type = geometry_msgs::msg::Pose; +}; + +template <> +struct defaultMessage +{ + using type = geometry_msgs::msg::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 Eigen Isometry3d to convert. - * \return The Eigen transform converted to a Pose message. + * \param in The 6x1 Eigen Matrix to convert. + * \return The Eigen Matrix converted to a Twist message. */ -inline -geometry_msgs::msg::Pose toMsg(const Eigen::Isometry3d & in) -{ - geometry_msgs::msg::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(); - return msg; -} +template <> +struct ImplDetails, geometry_msgs::msg::Twist> +{ + static void toMsg(const Eigen::Matrix & in, geometry_msgs::msg::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. + */ + static void fromMsg(const geometry_msgs::msg::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::msg::Twist; +}; + +} // namespace impl /** \brief Convert a Eigen::Vector3d type to a geometry_msgs::msg::Vector3. * This function is a specialization of the toMsg template defined in tf2/convert.h. @@ -411,74 +426,6 @@ geometry_msgs::msg::Vector3 toMsg2(const Eigen::Vector3d & in) 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::msg::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)); -} - -/** \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::msg::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)); -} - -/** \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::msg::Twist toMsg(const Eigen::Matrix & in) -{ - geometry_msgs::msg::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; -} - -/** \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::msg::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 Apply a geometry_msgs TransformStamped 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 @@ -521,119 +468,6 @@ void doTransform( 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::msg::PoseStamped toMsg(const tf2::Stamped & in) -{ - geometry_msgs::msg::PoseStamped msg; - msg.header.stamp = tf2_ros::toMsg(in.stamp_); - msg.header.frame_id = in.frame_id_; - msg.pose = toMsg(static_cast(in)); - return msg; -} - -inline -geometry_msgs::msg::PoseStamped toMsg(const tf2::Stamped & in) -{ - geometry_msgs::msg::PoseStamped msg; - msg.header.stamp = tf2_ros::toMsg(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::msg::PoseStamped & msg, tf2::Stamped & out) -{ - out.stamp_ = tf2_ros::fromMsg(msg.header.stamp); - out.frame_id_ = msg.header.frame_id; - fromMsg(msg.pose, static_cast(out)); -} - -inline -void fromMsg(const geometry_msgs::msg::PoseStamped & msg, tf2::Stamped & out) -{ - out.stamp_ = tf2_ros::fromMsg(msg.header.stamp); - out.frame_id_ = msg.header.frame_id; - fromMsg(msg.pose, static_cast(out)); -} - } // namespace tf2 - -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::msg::Pose toMsg(const Eigen::Affine3d & in) -{ - return tf2::toMsg(in); -} - -inline -geometry_msgs::msg::Pose toMsg(const Eigen::Isometry3d & in) -{ - return tf2::toMsg(in); -} - -inline -void fromMsg(const geometry_msgs::msg::Point & msg, Eigen::Vector3d & out) -{ - tf2::fromMsg(msg, out); -} - -inline -geometry_msgs::msg::Point toMsg(const Eigen::Vector3d & in) -{ - return tf2::toMsg(in); -} - -inline -void fromMsg(const geometry_msgs::msg::Pose & msg, Eigen::Affine3d & out) -{ - tf2::fromMsg(msg, out); -} - -inline -void fromMsg(const geometry_msgs::msg::Pose & msg, Eigen::Isometry3d & out) -{ - tf2::fromMsg(msg, out); -} - -inline -geometry_msgs::msg::Quaternion toMsg(const Eigen::Quaterniond & in) -{ - return tf2::toMsg(in); -} - -inline -geometry_msgs::msg::Twist toMsg(const Eigen::Matrix & in) -{ - return tf2::toMsg(in); -} - -inline -void fromMsg(const geometry_msgs::msg::Twist & msg, Eigen::Matrix & out) -{ - tf2::fromMsg(msg, out); -} - -} // namespace Eigen - -#endif // TF2_EIGEN__TF2_EIGEN_H_ +#endif // TF2_EIGEN_H diff --git a/tf2_eigen_kdl/include/tf2_eigen_kdl/tf2_eigen_kdl.hpp b/tf2_eigen_kdl/include/tf2_eigen_kdl/tf2_eigen_kdl.hpp index 8154c385e..718cb7eeb 100644 --- a/tf2_eigen_kdl/include/tf2_eigen_kdl/tf2_eigen_kdl.hpp +++ b/tf2_eigen_kdl/include/tf2_eigen_kdl/tf2_eigen_kdl.hpp @@ -42,7 +42,7 @@ #include -#include +#include #include "tf2_eigen_kdl/visibility_control.h" @@ -97,94 +97,77 @@ void wrenchKDLToEigen(const KDL::Wrench & k, Eigen::Matrix & e); TF2_EIGEN_KDL_PUBLIC void wrenchEigenToKDL(const Eigen::Matrix & e, KDL::Wrench & k); -namespace impl -{ - template<> -template<> -inline void Converter::convert(const KDL::Rotation & a, Eigen::Quaterniond & b) +void tf2::convert(const KDL::Rotation & a, Eigen::Quaterniond & b) { quaternionKDLToEigen(a, b); } template<> -template<> -inline void Converter::convert(const Eigen::Quaterniond & a, KDL::Rotation & b) +void tf2::convert(const Eigen::Quaterniond & a, KDL::Rotation & b) { quaternionEigenToKDL(a, b); } template<> -template<> -inline void Converter::convert(const KDL::Frame & a, Eigen::Affine3d & b) +void tf2::convert(const KDL::Frame & a, Eigen::Affine3d & b) { transformKDLToEigen(a, b); } template<> -template<> -inline void Converter::convert(const KDL::Frame & a, Eigen::Isometry3d & b) +void tf2::convert(const KDL::Frame & a, Eigen::Isometry3d & b) { transformKDLToEigen(a, b); } template<> -template<> -inline void Converter::convert(const Eigen::Affine3d & a, KDL::Frame & b) +void tf2::convert(const Eigen::Affine3d & a, KDL::Frame & b) { transformEigenToKDL(a, b); } template<> -template<> -inline void Converter::convert(const Eigen::Isometry3d & a, KDL::Frame & b) +void tf2::convert(const Eigen::Isometry3d & a, KDL::Frame & b) { transformEigenToKDL(a, b); } template<> -template<> -inline void Converter::convert(const KDL::Twist & a, Eigen::Matrix & b) +void tf2::convert(const KDL::Twist & a, Eigen::Matrix & b) { twistKDLToEigen(a, b); } template<> -template<> -inline void Converter::convert(const Eigen::Matrix & a, KDL::Twist & b) +void tf2::convert(const Eigen::Matrix & a, KDL::Twist & b) { twistEigenToKDL(a, b); } template<> -template<> -inline void Converter::convert(const KDL::Vector & a, Eigen::Matrix & b) +void tf2::convert(const KDL::Vector & a, Eigen::Matrix & b) { vectorKDLToEigen(a, b); } template<> -template<> -inline void Converter::convert(const Eigen::Matrix & a, KDL::Vector & b) +void tf2::convert(const Eigen::Matrix & a, KDL::Vector & b) { vectorEigenToKDL(a, b); } template<> -template<> -inline void Converter::convert(const KDL::Wrench & a, Eigen::Matrix & b) +void tf2::convert(const KDL::Wrench & a, Eigen::Matrix & b) { wrenchKDLToEigen(a, b); } template<> -template<> -inline void Converter::convert(const Eigen::Matrix & a, KDL::Wrench & b) +void tf2::convert(const Eigen::Matrix & a, KDL::Wrench & b) { wrenchEigenToKDL(a, b); } -} // namespace impl - } // namespace tf2 #endif // TF2_EIGEN_KDL__TF2_EIGEN_KDL_HPP_ 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 ec8a7bef3..590e8ea13 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 @@ -27,11 +27,13 @@ * POSSIBILITY OF SUCH DAMAGE. */ -/** \author Wim Meeussen */ +/** \author Wim Meeussen, Bjarne von Horn */ #ifndef TF2_GEOMETRY_MSGS_H #define TF2_GEOMETRY_MSGS_H +#include + #include #include #include @@ -43,6 +45,7 @@ #include #include #include +#include #include namespace tf2 @@ -60,28 +63,58 @@ KDL::Frame gmTransformToKDL(const geometry_msgs::msg::TransformStamped& t) KDL::Vector(t.transform.translation.x, t.transform.translation.y, t.transform.translation.z)); } +namespace impl +{ +/*************/ +/** Vector3 **/ +/*************/ -/********************/ -/** Vector3Stamped **/ -/********************/ +template +struct tf2VectorImplDetails +{ + /** \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 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 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. - */ template <> -inline - tf2::TimePoint getTimestamp(const geometry_msgs::msg::Vector3Stamped& t) {return tf2_ros::fromMsg(t.header.stamp);} +struct ImplDetails +: tf2VectorImplDetails +{ +}; -/** \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. - */ template <> -inline - std::string getFrameId(const geometry_msgs::msg::Vector3Stamped& t) {return t.header.frame_id;} +struct ImplDetails +: tf2VectorImplDetails +{ +}; + +template <> +struct defaultMessage +{ + using type = geometry_msgs::msg::Vector3; +}; + +} // namespace impl + +/********************/ +/** Vector3Stamped **/ +/********************/ /** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Vector type. * This function is a specialization of the doTransform template defined in tf2/convert.h. @@ -101,52 +134,11 @@ inline t_out.header.frame_id = transform.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::msg::Vector3Stamped toMsg(const geometry_msgs::msg::Vector3Stamped& in) -{ - return in; -} - -/** \brief Trivial "conversion" function for Vector3 message type. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param msg A Vector3Stamped message. - * \param out The input argument. - */ -inline -void fromMsg(const geometry_msgs::msg::Vector3Stamped& msg, geometry_msgs::msg::Vector3Stamped& out) -{ - out = msg; -} - - /******************/ /** 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. - */ -template <> -inline - tf2::TimePoint getTimestamp(const geometry_msgs::msg::PointStamped& t) {return tf2_ros::fromMsg(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. - */ -template <> -inline - std::string getFrameId(const geometry_msgs::msg::PointStamped& t) {return t.header.frame_id;} - /** \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. * \param t_in The point to transform, as a timestamped Point3 message. @@ -165,51 +157,10 @@ inline t_out.header.frame_id = transform.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::msg::PointStamped toMsg(const geometry_msgs::msg::PointStamped& in) -{ - return in; -} - -/** \brief Trivial "conversion" function for Point message type. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param msg A PointStamped message. - * \param out The input argument. - */ -inline -void fromMsg(const geometry_msgs::msg::PointStamped& msg, geometry_msgs::msg::PointStamped& out) -{ - out = msg; -} - - /*****************/ /** 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 - tf2::TimePoint getTimestamp(const geometry_msgs::msg::PoseStamped& t) {return tf2_ros::fromMsg(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 - std::string getFrameId(const geometry_msgs::msg::PoseStamped& t) {return t.header.frame_id;} - /** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Pose type. * This function is a specialization of the doTransform template defined in tf2/convert.h. * \param t_in The pose to transform, as a timestamped Pose3 message. @@ -232,51 +183,11 @@ inline t_out.header.frame_id = transform.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::msg::PoseStamped toMsg(const geometry_msgs::msg::PoseStamped& in) -{ - 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::msg::PoseStamped& msg, geometry_msgs::msg::PoseStamped& out) -{ - out = msg; -} - /*******************************/ /** PoseWithCovarianceStamped **/ /*******************************/ -/** \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 PoseWithCovarianceStamped message to extract the timestamp from. - * \return The timestamp of the message. - */ -template <> -inline - tf2::TimePoint getTimestamp(const geometry_msgs::msg::PoseWithCovarianceStamped& t) {return tf2_ros::fromMsg(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 PoseWithCovarianceStamped message to extract the frame ID from. - * \return A string containing the frame ID of the message. - */ -template <> -inline - std::string getFrameId(const geometry_msgs::msg::PoseWithCovarianceStamped& t) {return t.header.frame_id;} - /** \brief Extract a covariance matrix from a PoseWithCovarianceStamped message. * This function is a specialization of the getCovarianceMatrix template defined in tf2/convert.h. * \param t PoseWithCovarianceStamped message to extract the covariance matrix from. @@ -309,27 +220,6 @@ inline t_out.pose.covariance = t_in.pose.covariance; } -/** \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 PoseWithCovarianceStamped message. - * \return The input argument. - */ -inline -geometry_msgs::msg::PoseWithCovarianceStamped toMsg(const geometry_msgs::msg::PoseWithCovarianceStamped& in) -{ - 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 PoseWithCovarianceStamped message. - * \param out The input argument. - */ -inline -void fromMsg(const geometry_msgs::msg::PoseWithCovarianceStamped& msg, geometry_msgs::msg::PoseWithCovarianceStamped& out) -{ - out = msg; -} /** \brief Convert a tf2 TransformWithCovarianceStamped type to its equivalent geometry_msgs representation. * This function is a specialization of the toMsg template defined in tf2/convert.h. @@ -367,7 +257,7 @@ void fromMsg(const geometry_msgs::msg::PoseWithCovarianceStamped& in, tf2::WithC out.frame_id_ = in.header.frame_id; out.cov_mat_ = covarianceRowMajorToNested(in.pose.covariance); tf2::Transform tmp; - fromMsg(in.pose.pose, tmp); + fromMsg<>(in.pose.pose, tmp); out.setData(tmp); } @@ -375,57 +265,48 @@ void fromMsg(const geometry_msgs::msg::PoseWithCovarianceStamped& in, tf2::WithC /** 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::msg::Quaternion toMsg(const tf2::Quaternion& in) +namespace impl { - geometry_msgs::msg::Quaternion out; - out.w = in.getW(); - out.x = in.getX(); - out.y = in.getY(); - out.z = in.getZ(); - return out; -} +template <> +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::msg::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 toMsg 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::msg::Quaternion& in, tf2::Quaternion& 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. + */ + static void fromMsg(const geometry_msgs::msg::Quaternion & in, tf2::Quaternion & out) + { + // w at the end in the constructor + out = tf2::Quaternion(in.x, in.y, in.z, in.w); + } +}; + +template <> +struct defaultMessage { - // w at the end in the constructor - out = tf2::Quaternion(in.x, in.y, in.z, in.w); -} + using type = geometry_msgs::msg::Quaternion; +}; +} // namespace impl /***********************/ /** 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. - */ -template <> -inline -tf2::TimePoint getTimestamp(const geometry_msgs::msg::QuaternionStamped& t) {return tf2_ros::fromMsg(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. - */ -template <> -inline -std::string getFrameId(const geometry_msgs::msg::QuaternionStamped& t) {return t.header.frame_id;} - /** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Quaternion type. * This function is a specialization of the doTransform template defined in tf2/convert.h. * \param t_in The quaternion to transform, as a timestamped Quaternion3 message. @@ -439,122 +320,54 @@ void doTransform(const geometry_msgs::msg::QuaternionStamped& t_in, geometry_msg tf2::Quaternion q_out = tf2::Quaternion(transform.transform.rotation.x, transform.transform.rotation.y, transform.transform.rotation.z, transform.transform.rotation.w)* tf2::Quaternion(t_in.quaternion.x, t_in.quaternion.y, t_in.quaternion.z, t_in.quaternion.w); - t_out.quaternion = toMsg(q_out); + toMsg<>(q_out, t_out.quaternion); t_out.header.stamp = transform.header.stamp; t_out.header.frame_id = transform.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::msg::QuaternionStamped toMsg(const geometry_msgs::msg::QuaternionStamped& in) -{ - return in; -} - -/** \brief Trivial "conversion" function for Quaternion message type. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param msg A QuaternionStamped message. - * \param out The input argument. - */ -inline -void fromMsg(const geometry_msgs::msg::QuaternionStamped& msg, geometry_msgs::msg::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 A 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::msg::QuaternionStamped toMsg(const tf2::Stamped& in) -{ - geometry_msgs::msg::QuaternionStamped out; - out.header.stamp = tf2_ros::toMsg(in.stamp_); - out.header.frame_id = in.frame_id_; - out.quaternion.w = in.getW(); - out.quaternion.x = in.getX(); - out.quaternion.y = in.getY(); - out.quaternion.z = in.getZ(); - return out; -} - -/** \brief Convert a QuaternionStamped message to its equivalent tf2 representation. - * This function is a specialization of the toMsg 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::msg::QuaternionStamped& in, tf2::Stamped& out) -{ - out.stamp_ = tf2_ros::fromMsg(in.header.stamp); - out.frame_id_ = in.header.frame_id; - tf2::Quaternion tmp; - fromMsg(in.quaternion, tmp); - out.setData(tmp); -} - - /***************/ /** 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::msg::Transform toMsg(const tf2::Transform& in) +namespace impl { - geometry_msgs::msg::Transform out; - out.translation.x = in.getOrigin().getX(); - out.translation.y = in.getOrigin().getY(); - out.translation.z = in.getOrigin().getZ(); - out.rotation = toMsg(in.getRotation()); - return out; -} - -/** \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::msg::Transform& in, tf2::Transform& out) +template <> +struct ImplDetails { - out.setOrigin(tf2::Vector3(in.translation.x, in.translation.y, in.translation.z)); - // w at the end in the constructor - out.setRotation(tf2::Quaternion(in.rotation.x, in.rotation.y, in.rotation.z, in.rotation.w)); -} - - -/**********************/ -/** TransformStamped **/ -/**********************/ + /** \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::msg::Transform & out) + { + tf2::toMsg<>(in.getOrigin(), out.translation); + tf2::toMsg<>(in.getRotation(), out.rotation); + } -/** \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 -tf2::TimePoint getTimestamp(const geometry_msgs::msg::TransformStamped& t) {return tf2_ros::fromMsg(t.header.stamp);} + /** \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::msg::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 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 -std::string getFrameId(const geometry_msgs::msg::TransformStamped& t) {return t.header.frame_id;} +struct defaultMessage +{ + using type = geometry_msgs::msg::Transform; +}; +} // namespace impl /** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Transform type. * This function is a specialization of the doTransform template defined in tf2/convert.h. @@ -581,91 +394,59 @@ void doTransform(const geometry_msgs::msg::TransformStamped& t_in, geometry_msgs t_out.header.frame_id = transform.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::msg::TransformStamped toMsg(const geometry_msgs::msg::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::msg::TransformStamped& msg, geometry_msgs::msg::TransformStamped& out) -{ - out = msg; -} - -/** \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::msg::TransformStamped& in, tf2::Stamped & out) -{ - out.stamp_ = tf2_ros::fromMsg(in.header.stamp); - out.frame_id_ = in.header.frame_id; - tf2::Transform tmp; - fromMsg(in.transform, tmp); - out.setData(tmp); -} - -/** \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 TransformStamped converted to a geometry_msgs TransformStamped message type. - */ -inline -geometry_msgs::msg::TransformStamped toMsg(const tf2::Stamped& in) +namespace impl { - geometry_msgs::msg::TransformStamped out; - out.header.stamp = tf2_ros::toMsg(in.stamp_); - out.header.frame_id = in.frame_id_; - out.transform.translation.x = in.getOrigin().getX(); - out.transform.translation.y = in.getOrigin().getY(); - out.transform.translation.z = in.getOrigin().getZ(); - out.transform.rotation = toMsg(in.getRotation()); - return out; -} - /**********/ /** 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 -/** This section is about converting */ -void toMsg(const tf2::Transform& in, geometry_msgs::msg::Pose& out ) +template <> +struct ImplDetails { - out.position.x = in.getOrigin().getX(); - out.position.y = in.getOrigin().getY(); - out.position.z = in.getOrigin().getZ(); - out.orientation = toMsg(in.getRotation()); -} + /** \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::msg::Pose & out) + { + tf2::toMsg<>(in.getOrigin(), out.position); + tf2::toMsg<>(in.getRotation(), out.orientation); + } -/** \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::msg::Pose& in, tf2::Transform& 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. + */ + static void fromMsg(const geometry_msgs::msg::Pose & in, tf2::Transform & out) + { + out.setOrigin(tf2::Vector3(in.position.x, in.position.y, in.position.z)); + // w at the end in the constructor + out.setRotation( + tf2::Quaternion(in.orientation.x, in.orientation.y, in.orientation.z, in.orientation.w)); + } +}; + +/************/ +/** Wrench **/ +/************/ + +template <> +struct ImplDetails, geometry_msgs::msg::Wrench> { - 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)); -} + static void toMsg(const std::array & in, geometry_msgs::msg::Wrench & out) + { + tf2::toMsg<>(std::get<0>(in), out.force); + tf2::toMsg<>(std::get<1>(in), out.torque); + } + + static void fromMsg(const geometry_msgs::msg::Wrench & msg, std::array & out) + { + tf2::fromMsg<>(msg.force, std::get<0>(out)); + tf2::fromMsg<>(msg.torque, std::get<1>(out)); + } +}; -} // namespace +} // namespace impl +} // namespace tf2 -#endif // TF2_GEOMETRY_MSGS_H +#endif // TF2_GEOMETRY_MSGS_H diff --git a/tf2_kdl/include/tf2_kdl/tf2_kdl.h b/tf2_kdl/include/tf2_kdl/tf2_kdl.h index d3f4ef913..31c095896 100644 --- a/tf2_kdl/include/tf2_kdl/tf2_kdl.h +++ b/tf2_kdl/include/tf2_kdl/tf2_kdl.h @@ -27,12 +27,13 @@ * POSSIBILITY OF SUCH DAMAGE. */ -/** \author Wim Meeussen */ +/** \author Wim Meeussen, Bjarne von Horn */ #ifndef TF2_KDL_H #define TF2_KDL_H #include +#include #include #include #include @@ -88,37 +89,56 @@ inline t_out = tf2::Stamped(transformToKDL(transform) * t_in, tf2_ros::fromMsg(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::msg::PointStamped toMsg(const tf2::Stamped& in) + +namespace impl { - geometry_msgs::msg::PointStamped msg; - msg.header.stamp = tf2_ros::toMsg(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::msg::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_ = tf2_ros::fromMsg(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::msg::Point; +}; + +} // namespace impl // --------------------- // Twist @@ -135,45 +155,40 @@ inline { t_out = tf2::Stamped(transformToKDL(transform) * t_in, tf2_ros::fromMsg(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::msg::TwistStamped toMsg(const tf2::Stamped& in) +namespace impl { - geometry_msgs::msg::TwistStamped msg; - msg.header.stamp = tf2_ros::toMsg(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::msg::TwistStamped& msg, tf2::Stamped& out) +template <> +struct ImplDetails { - out.stamp_ = tf2_ros::fromMsg(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::msg::Twist & msg) + { + tf2::toMsg<>(in.vel, msg.linear); + tf2::toMsg<>(in.rot, msg.angular); + } + + /** \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::msg::Twist & msg, KDL::Twist & out) + { + tf2::fromMsg<>(msg.linear, out.vel); + tf2::fromMsg<>(msg.angular, out.rot); + } +}; +template <> +struct defaultMessage +{ + using type = geometry_msgs::msg::Twist; +}; +} // namespace impl // --------------------- // Wrench @@ -191,46 +206,40 @@ inline t_out = tf2::Stamped(transformToKDL(transform) * t_in, tf2_ros::fromMsg(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::msg::WrenchStamped toMsg(const tf2::Stamped& in) +namespace impl { - geometry_msgs::msg::WrenchStamped msg; - msg.header.stamp = tf2_ros::toMsg(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::msg::WrenchStamped& msg, tf2::Stamped& out) +template <> +struct ImplDetails { - out.stamp_ = tf2_ros::fromMsg(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::msg::Wrench & msg) + { + tf2::toMsg<>(in.force, msg.force); + tf2::toMsg<>(in.torque, msg.torque); + } + /** \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::msg::Wrench & msg, KDL::Wrench & out) + { + tf2::fromMsg<>(msg.force, out.force); + tf2::fromMsg<>(msg.torque, out.torque); + } +}; +template <> +struct defaultMessage +{ + using type = geometry_msgs::msg::Wrench; +}; +} // namespace impl // --------------------- // Frame @@ -248,64 +257,62 @@ inline t_out = tf2::Stamped(transformToKDL(transform) * t_in, tf2_ros::fromMsg(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::msg::Pose toMsg(const KDL::Frame& in) +namespace impl { - geometry_msgs::msg::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; -} +template <> +struct ImplDetails +{ + /** \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::msg::Pose & msg) + { + tf2::toMsg<>(in.p, msg.position); + tf2::toMsg<>(in.M, msg.orientation); + } -/** \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::msg::Pose& msg, KDL::Frame& 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::msg::Pose & msg, KDL::Frame & out) + { + tf2::fromMsg<>(msg.position, out.p); + tf2::fromMsg<>(msg.orientation, out.M); + } +}; + +template <> +struct defaultMessage { - 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); -} + using type = geometry_msgs::msg::Pose; +}; -/** \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::msg::PoseStamped toMsg(const tf2::Stamped& in) +template <> +struct ImplDetails { - geometry_msgs::msg::PoseStamped msg; - msg.header.stamp = tf2_ros::toMsg(in.stamp_); - msg.header.frame_id = in.frame_id_; - msg.pose = toMsg(static_cast(in)); - return msg; -} + static void toMsg(KDL::Rotation const & r, geometry_msgs::msg::Quaternion & q) + { + r.GetQuaternion(q.x, q.y, q.z, q.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::msg::PoseStamped& msg, tf2::Stamped& out) + static void fromMsg(geometry_msgs::msg::Quaternion const & q, KDL::Rotation & r) + { + r = KDL::Rotation::Quaternion(q.x, q.y, q.z, q.w); + } +}; + +template <> +struct defaultMessage { - out.stamp_ = tf2_ros::fromMsg(msg.header.stamp); - out.frame_id_ = msg.header.frame_id; - fromMsg(msg.pose, static_cast(out)); -} + using type = geometry_msgs::msg::Quaternion; +}; + +} // namespace impl -} // namespace +} // namespace tf2 -#endif // TF2_KDL_H +#endif // TF2_KDL_H From 786fdd3571bfb1a1fc484b2abdecef40281ca113 Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Mon, 25 Jan 2021 21:03:05 +0000 Subject: [PATCH 03/51] Reorganize some headers --- tf2/CMakeLists.txt | 4 +- tf2/include/tf2/convert.h | 58 ++------------------ tf2/include/tf2/impl/convert.h | 76 +++++++++++++++------------ tf2/include/tf2/impl/forward.h | 71 +++++++++++++++++++++++++ tf2/include/tf2/impl/stamped_traits.h | 7 +-- tf2/include/tf2/impl/time_convert.h | 74 ++++++++++++++++++++++++++ tf2/include/tf2/transform_datatypes.h | 8 +++ tf2/package.xml | 1 + 8 files changed, 205 insertions(+), 94 deletions(-) create mode 100644 tf2/include/tf2/impl/forward.h create mode 100644 tf2/include/tf2/impl/time_convert.h diff --git a/tf2/CMakeLists.txt b/tf2/CMakeLists.txt index ab9cdaba2..56c26703a 100644 --- a/tf2/CMakeLists.txt +++ b/tf2/CMakeLists.txt @@ -11,6 +11,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() find_package(ament_cmake) +find_package(builtin_interfaces REQUIRED) find_package(console_bridge_vendor REQUIRED) # Provides console_bridge 0.4.0 on platforms without it. find_package(console_bridge REQUIRED) find_package(geometry_msgs REQUIRED) @@ -25,6 +26,7 @@ target_include_directories(tf2 PUBLIC "$" "$") ament_target_dependencies(tf2 + "builtin_interfaces" "console_bridge" "geometry_msgs" "rcutils" @@ -89,7 +91,7 @@ if(BUILD_TESTING) endif() -ament_export_dependencies(console_bridge geometry_msgs rcutils) +ament_export_dependencies(builtin_interfaces console_bridge geometry_msgs rcutils) ament_export_include_directories(include) ament_export_libraries(tf2) ament_export_targets(tf2) diff --git a/tf2/include/tf2/convert.h b/tf2/include/tf2/convert.h index dc15c704c..80be78818 100644 --- a/tf2/include/tf2/convert.h +++ b/tf2/include/tf2/convert.h @@ -38,62 +38,13 @@ #include #include "exceptions.h" -#include "transform_datatypes.h" +#include "impl/convert.h" +#include "impl/stamped_traits.h" +#include "time.h" #include "visibility_control.h" 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 -{ - // void toMsg(const Datatype&, Message&); - // 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; - -} // namespace impl /**\brief The templated function expected to be able to do a transform * @@ -271,7 +222,4 @@ std::array covarianceNestedToRowMajor(const std::array - -#include "../convert.h" #include "../time.h" - -namespace builtin_interfaces -{ -namespace msg -{ -template -class Time_; -} -} // namespace builtin_interfaces +#include "forward.h" 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 +{ + // void toMsg(const Datatype&, Message&); + // void fromMsg(const Message&, Datatype&); +}; + + /** * \brief Mapping of unstamped Messages for stamped Messages * @@ -155,27 +186,6 @@ struct ImplDetails, StampedMessage> } }; -template <> -struct ImplDetails -{ - static void toMsg(const tf2::TimePoint & t, builtin_interfaces::msg::Time & time_msg) - { - std::chrono::nanoseconds ns = - std::chrono::duration_cast(t.time_since_epoch()); - std::chrono::seconds s = std::chrono::duration_cast(t.time_since_epoch()); - - time_msg.sec = static_cast(s.count()); - time_msg.nanosec = static_cast(ns.count() % 1000000000ull); - } - - static void fromMsg(const builtin_interfaces::msg::Time & time_msg, tf2::TimePoint & t) - { - int64_t d = time_msg.sec * 1000000000ull + time_msg.nanosec; - std::chrono::nanoseconds ns(d); - t = tf2::TimePoint(std::chrono::duration_cast(ns)); - } -}; - template class Converter { diff --git a/tf2/include/tf2/impl/forward.h b/tf2/include/tf2/impl/forward.h new file mode 100644 index 000000000..371d77797 --- /dev/null +++ b/tf2/include/tf2/impl/forward.h @@ -0,0 +1,71 @@ +// Copyright 2021, Bjarne von Horn. 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 Open Source Robotics Foundation nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +/** \author Bjarne von Horn */ + +#ifndef TF2__IMPL__FORWARD_H_ +#define TF2__IMPL__FORWARD_H_ + +namespace tf2 +{ +template +class Stamped; + +template +class WithCovarianceStamped; + +template +B & toMsg(const A & a, B & b); + +template +void fromMsg(const A & a, B & b); + +namespace impl +{ +template +struct ImplDetails; + +template +struct stampedMessageTraits; + +template +struct unstampedMessageTraits; + +template +struct defaultMessage; + +template +struct DefaultStampedImpl; + +template +class Converter; + +} // namespace impl +} // namespace tf2 + +#endif // TF2__IMPL__FORWARD_H_ diff --git a/tf2/include/tf2/impl/stamped_traits.h b/tf2/include/tf2/impl/stamped_traits.h index 0b9d6123f..9bf94ef16 100644 --- a/tf2/include/tf2/impl/stamped_traits.h +++ b/tf2/include/tf2/impl/stamped_traits.h @@ -34,6 +34,8 @@ #ifndef TF2_IMPL_STAMPED_TRAITS_H_ #define TF2_IMPL_STAMPED_TRAITS_H_ +#include "forward.h" + // forward declarations namespace geometry_msgs { @@ -82,8 +84,6 @@ namespace tf2 { namespace impl { -template -struct unstampedMessageTraits; template struct unstampedMessageTraits> @@ -139,9 +139,6 @@ struct unstampedMessageTraits> using stampedType = geometry_msgs::msg::Vector3Stamped_; }; -template -struct stampedMessageTraits; - template struct defaultStampedMessageTraits { diff --git a/tf2/include/tf2/impl/time_convert.h b/tf2/include/tf2/impl/time_convert.h new file mode 100644 index 000000000..4c4b2b7af --- /dev/null +++ b/tf2/include/tf2/impl/time_convert.h @@ -0,0 +1,74 @@ +// Copyright 2021, Bjarne von Horn. 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 Open Source Robotics Foundation nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +/** \author dhood, Bjarne von Horn */ + +#ifndef TF2__IMPL__TIME_CONVERT_H_ +#define TF2__IMPL__TIME_CONVERT_H_ + +#include + +#include "../time.h" +#include "forward.h" + +namespace tf2 +{ +namespace impl +{ + +template <> +struct ImplDetails +{ + static void toMsg(const tf2::TimePoint & t, builtin_interfaces::msg::Time & time_msg) + { + std::chrono::nanoseconds ns = + std::chrono::duration_cast(t.time_since_epoch()); + std::chrono::seconds s = std::chrono::duration_cast(t.time_since_epoch()); + + time_msg.sec = static_cast(s.count()); + time_msg.nanosec = static_cast(ns.count() % 1000000000ull); + } + + static void fromMsg(const builtin_interfaces::msg::Time & time_msg, tf2::TimePoint & t) + { + int64_t d = time_msg.sec * 1000000000ull + time_msg.nanosec; + std::chrono::nanoseconds ns(d); + t = tf2::TimePoint(std::chrono::duration_cast(ns)); + } +}; +} // namespace impl + +inline tf2::TimePoint TimePointFromMsg(const builtin_interfaces::msg::Time & time_msg) +{ + TimePoint tp; + impl::ImplDetails::fromMsg(time_msg, tp); + return tp; +} +} // namespace tf2 + +#endif // TF2__IMPL__TIME_CONVERT_H_ diff --git a/tf2/include/tf2/transform_datatypes.h b/tf2/include/tf2/transform_datatypes.h index 9e57e2258..3c1b2dbf1 100644 --- a/tf2/include/tf2/transform_datatypes.h +++ b/tf2/include/tf2/transform_datatypes.h @@ -36,6 +36,8 @@ #include #include "tf2/time.h" +#include "impl/time_convert.h" +#include namespace tf2 { @@ -61,6 +63,12 @@ class Stamped : public T { } + /** Full constructor */ + Stamped(const T & input, const builtin_interfaces::msg::Time & timestamp, const std::string & frame_id) + : T(input), stamp_(TimePointFromMsg(timestamp)), frame_id_(frame_id) + { + } + /** Copy Constructor */ Stamped(const Stamped & s) : T(s), diff --git a/tf2/package.xml b/tf2/package.xml index f9307334b..eb51910c1 100644 --- a/tf2/package.xml +++ b/tf2/package.xml @@ -20,6 +20,7 @@ ament_cmake + builtin_interfaces console_bridge_vendor geometry_msgs libconsole-bridge-dev From 01e8c0231cc2ecb52f3594eb39653441802657b3 Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Fri, 12 Feb 2021 00:16:37 +0000 Subject: [PATCH 04/51] Cleanup of Time/Duration conversion --- tf2/include/tf2/convert.h | 1 + tf2/include/tf2/impl/time_convert.h | 44 +++++++++++++- tf2_bullet/include/tf2_bullet/tf2_bullet.h | 46 ++++++--------- tf2_eigen/CMakeLists.txt | 1 + tf2_eigen/include/tf2_eigen/tf2_eigen.h | 33 +++++------ .../tf2_geometry_msgs/tf2_geometry_msgs.h | 6 +- tf2_kdl/include/tf2_kdl/tf2_kdl.h | 58 ++++++++++--------- tf2_ros/include/tf2_ros/buffer_interface.h | 26 ++------- .../include/tf2_sensor_msgs/tf2_sensor_msgs.h | 13 +---- tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp | 51 ++++++++-------- 10 files changed, 144 insertions(+), 135 deletions(-) diff --git a/tf2/include/tf2/convert.h b/tf2/include/tf2/convert.h index 80be78818..c8e41bd96 100644 --- a/tf2/include/tf2/convert.h +++ b/tf2/include/tf2/convert.h @@ -40,6 +40,7 @@ #include "exceptions.h" #include "impl/convert.h" #include "impl/stamped_traits.h" +#include "impl/time_convert.h" #include "time.h" #include "visibility_control.h" diff --git a/tf2/include/tf2/impl/time_convert.h b/tf2/include/tf2/impl/time_convert.h index 4c4b2b7af..da28f5349 100644 --- a/tf2/include/tf2/impl/time_convert.h +++ b/tf2/include/tf2/impl/time_convert.h @@ -31,7 +31,9 @@ #ifndef TF2__IMPL__TIME_CONVERT_H_ #define TF2__IMPL__TIME_CONVERT_H_ +#include #include +#include #include "../time.h" #include "forward.h" @@ -40,7 +42,6 @@ namespace tf2 { namespace impl { - template <> struct ImplDetails { @@ -61,14 +62,53 @@ struct ImplDetails t = tf2::TimePoint(std::chrono::duration_cast(ns)); } }; + +template <> +struct ImplDetails +{ + static void toMsg(const tf2::Duration & t, builtin_interfaces::msg::Duration & duration_msg) + { + std::chrono::nanoseconds ns = std::chrono::duration_cast(t); + std::chrono::seconds s = std::chrono::duration_cast(t); + duration_msg.sec = static_cast(s.count()); + duration_msg.nanosec = static_cast(ns.count() % 1000000000ull); + } + + static void fromMsg( + const builtin_interfaces::msg::Duration & duration_msg, tf2::Duration & duration) + { + int64_t d = duration_msg.sec * 1000000000ull + duration_msg.nanosec; + std::chrono::nanoseconds ns(d); + duration = tf2::Duration(std::chrono::duration_cast(ns)); + } +}; + +template <> +struct defaultMessage +{ + using type = builtin_interfaces::msg::Time; +}; + +template <> +struct defaultMessage +{ + using type = builtin_interfaces::msg::Duration; +}; } // namespace impl inline tf2::TimePoint TimePointFromMsg(const builtin_interfaces::msg::Time & time_msg) { TimePoint tp; - impl::ImplDetails::fromMsg(time_msg, tp); + tf2::fromMsg<>(time_msg, tp); return tp; } + +inline tf2::Duration DurationFromMsg(const builtin_interfaces::msg::Duration & duration_msg) +{ + Duration d; + tf2::fromMsg<>(duration_msg, d); + return d; +} } // namespace tf2 #endif // TF2__IMPL__TIME_CONVERT_H_ diff --git a/tf2_bullet/include/tf2_bullet/tf2_bullet.h b/tf2_bullet/include/tf2_bullet/tf2_bullet.h index 3a83bbd24..5c3a06920 100644 --- a/tf2_bullet/include/tf2_bullet/tf2_bullet.h +++ b/tf2_bullet/include/tf2_bullet/tf2_bullet.h @@ -31,13 +31,12 @@ #ifndef TF2_BULLET__TF2_BULLET_H_ #define TF2_BULLET__TF2_BULLET_H_ -#include #include #include -#include -#include +#include +#include -#include +#include #if (BT_BULLET_VERSION <= 282) // Suppress compilation warning on older versions of Bullet. @@ -76,17 +75,14 @@ namespace tf2 * \param t_out The transformed vector, as a timestamped Bullet btVector3 data type. * \param transform The timestamped transform to apply, as a TransformStamped message. */ - template < > - inline - void doTransform( - const tf2::Stamped < btVector3 > & t_in, tf2::Stamped < btVector3 > & t_out, - const geometry_msgs::msg::TransformStamped & transform) - { - t_out = - tf2::Stamped < btVector3 > ( - transformToBullet(transform) * t_in, - tf2_ros::fromMsg(transform.header.stamp), transform.header.frame_id); - } +template <> +inline void doTransform( + const tf2::Stamped & t_in, tf2::Stamped & t_out, + const geometry_msgs::msg::TransformStamped & transform) +{ + t_out = tf2::Stamped( + transformToBullet(transform) * t_in, transform.header.stamp, transform.header.frame_id); +} namespace impl { @@ -131,18 +127,14 @@ struct ImplDetails * \param t_out The transformed frame, as a timestamped Bullet btTransform. * \param transform The timestamped transform to apply, as a TransformStamped message. */ - template < > - inline - void doTransform( - const tf2::Stamped < btTransform > & t_in, tf2::Stamped < btTransform > & t_out, - const geometry_msgs::msg::TransformStamped & transform) - { - t_out = - tf2::Stamped < btTransform > ( - transformToBullet(transform) * t_in, - tf2_ros::fromMsg(transform.header.stamp), transform.header.frame_id); - } - +template <> +inline void doTransform( + const tf2::Stamped & t_in, tf2::Stamped & t_out, + const geometry_msgs::msg::TransformStamped & transform) +{ + t_out = tf2::Stamped( + transformToBullet(transform) * t_in, transform.header.stamp, transform.header.frame_id); +} } // namespace tf2 diff --git a/tf2_eigen/CMakeLists.txt b/tf2_eigen/CMakeLists.txt index 2bd4852e9..5dbd59ad5 100644 --- a/tf2_eigen/CMakeLists.txt +++ b/tf2_eigen/CMakeLists.txt @@ -49,5 +49,6 @@ ament_export_include_directories(include) ament_export_dependencies( eigen3_cmake_module Eigen3 + tf2 tf2_ros) ament_package() diff --git a/tf2_eigen/include/tf2_eigen/tf2_eigen.h b/tf2_eigen/include/tf2_eigen/tf2_eigen.h index bc573ae4a..1545cc965 100644 --- a/tf2_eigen/include/tf2_eigen/tf2_eigen.h +++ b/tf2_eigen/include/tf2_eigen/tf2_eigen.h @@ -30,16 +30,15 @@ #define TF2_EIGEN__TF2_EIGEN_H_ #include -#include +#include + #include +#include #include #include +#include #include #include -#include -#include -#include - namespace tf2 { @@ -183,15 +182,13 @@ struct defaultMessage * \param transform The timestamped transform to apply, as a TransformStamped message. */ template<> -inline -void doTransform( - const tf2::Stamped & t_in, - tf2::Stamped & t_out, +inline void doTransform( + const tf2::Stamped & t_in, tf2::Stamped & t_out, const geometry_msgs::msg::TransformStamped & transform) { t_out = tf2::Stamped( transformToEigen(transform) * t_in, - tf2_ros::fromMsg(transform.header.stamp), + transform.header.stamp, transform.header.frame_id); } @@ -295,7 +292,7 @@ void doTransform( const geometry_msgs::msg::TransformStamped & transform) { t_out.frame_id_ = transform.header.frame_id; - t_out.stamp_ = tf2_ros::fromMsg(transform.header.stamp); + tf2::fromMsg(transform.header.stamp, t_out.stamp_); doTransform( static_cast(t_in), static_cast(t_out), transform); @@ -436,15 +433,14 @@ geometry_msgs::msg::Vector3 toMsg2(const Eigen::Vector3d & in) * \param transform The timestamped transform to apply, as a TransformStamped message. */ template<> -inline -void doTransform( +inline void doTransform( const tf2::Stamped & t_in, tf2::Stamped & t_out, const geometry_msgs::msg::TransformStamped & transform) { t_out = tf2::Stamped( - transformToEigen(transform) * t_in, tf2_ros::fromMsg( - transform.header.stamp), transform.header.frame_id); + transformToEigen(transform) * t_in, + transform.header.stamp,transform.header.frame_id); } /** \brief Apply a geometry_msgs TransformStamped to an Eigen Isometry transform. @@ -457,15 +453,14 @@ void doTransform( * \param transform The timestamped transform to apply, as a TransformStamped message. */ template<> -inline -void doTransform( +inline void doTransform( const tf2::Stamped & t_in, tf2::Stamped & t_out, const geometry_msgs::msg::TransformStamped & transform) { t_out = tf2::Stamped( - transformToEigen(transform) * t_in, tf2_ros::fromMsg( - transform.header.stamp), transform.header.frame_id); + transformToEigen(transform) * t_in, transform.header.stamp, + transform.header.frame_id); } } // namespace tf2 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 590e8ea13..3479fd52e 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 @@ -35,9 +35,9 @@ #include #include +#include #include #include -#include #include #include #include @@ -231,7 +231,7 @@ inline geometry_msgs::msg::PoseWithCovarianceStamped toMsg(const tf2::WithCovarianceStamped& in) { geometry_msgs::msg::PoseWithCovarianceStamped out; - out.header.stamp = tf2_ros::toMsg(in.stamp_); + tf2::toMsg<>(in.stamp_, out.header.stamp); out.header.frame_id = in.frame_id_; out.pose.covariance = covarianceNestedToRowMajor(in.cov_mat_); out.pose.pose.orientation.x = in.getRotation().getX(); @@ -253,7 +253,7 @@ template<> inline void fromMsg(const geometry_msgs::msg::PoseWithCovarianceStamped& in, tf2::WithCovarianceStamped& out) { - out.stamp_ = tf2_ros::fromMsg(in.header.stamp); + tf2::fromMsg<>(in.header.stamp, out.stamp_); out.frame_id_ = in.header.frame_id; out.cov_mat_ = covarianceRowMajorToNested(in.pose.covariance); tf2::Transform tmp; diff --git a/tf2_kdl/include/tf2_kdl/tf2_kdl.h b/tf2_kdl/include/tf2_kdl/tf2_kdl.h index 31c095896..a2ecc2da6 100644 --- a/tf2_kdl/include/tf2_kdl/tf2_kdl.h +++ b/tf2_kdl/include/tf2_kdl/tf2_kdl.h @@ -34,15 +34,14 @@ #include #include -#include -#include + #include -#include -#include #include #include -#include #include +#include +#include +#include namespace tf2 { @@ -83,12 +82,13 @@ geometry_msgs::msg::TransformStamped kdlToTransform(const KDL::Frame& k) * \param transform The timestamped transform to apply, as a TransformStamped message. */ template <> -inline - void doTransform(const tf2::Stamped& t_in, tf2::Stamped& t_out, const geometry_msgs::msg::TransformStamped& transform) - { - t_out = tf2::Stamped(transformToKDL(transform) * t_in, tf2_ros::fromMsg(transform.header.stamp), transform.header.frame_id); - } - +inline void doTransform( + const tf2::Stamped & t_in, tf2::Stamped & t_out, + const geometry_msgs::msg::TransformStamped & transform) +{ + t_out = tf2::Stamped( + transformToKDL(transform) * t_in, transform.header.stamp, transform.header.frame_id); +} namespace impl { @@ -150,11 +150,13 @@ struct defaultMessage * \param transform The timestamped transform to apply, as a TransformStamped message. */ template <> -inline - void doTransform(const tf2::Stamped& t_in, tf2::Stamped& t_out, const geometry_msgs::msg::TransformStamped& transform) - { - t_out = tf2::Stamped(transformToKDL(transform) * t_in, tf2_ros::fromMsg(transform.header.stamp), transform.header.frame_id); - } +inline void doTransform( + const tf2::Stamped & t_in, tf2::Stamped & t_out, + const geometry_msgs::msg::TransformStamped & transform) +{ + t_out = tf2::Stamped( + transformToKDL(transform) * t_in, transform.header.stamp, transform.header.frame_id); +} namespace impl { template <> @@ -200,11 +202,13 @@ struct defaultMessage * \param transform The timestamped transform to apply, as a TransformStamped message. */ template <> -inline - void doTransform(const tf2::Stamped& t_in, tf2::Stamped& t_out, const geometry_msgs::msg::TransformStamped& transform) - { - t_out = tf2::Stamped(transformToKDL(transform) * t_in, tf2_ros::fromMsg(transform.header.stamp), transform.header.frame_id); - } +inline void doTransform( + const tf2::Stamped & t_in, tf2::Stamped & t_out, + const geometry_msgs::msg::TransformStamped & transform) +{ + t_out = tf2::Stamped( + transformToKDL(transform) * t_in, transform.header.stamp, transform.header.frame_id); +} namespace impl { @@ -251,11 +255,13 @@ struct defaultMessage * \param transform The timestamped transform to apply, as a TransformStamped message. */ template <> -inline - void doTransform(const tf2::Stamped& t_in, tf2::Stamped& t_out, const geometry_msgs::msg::TransformStamped& transform) - { - t_out = tf2::Stamped(transformToKDL(transform) * t_in, tf2_ros::fromMsg(transform.header.stamp), transform.header.frame_id); - } +inline void doTransform( + const tf2::Stamped & t_in, tf2::Stamped & t_out, + const geometry_msgs::msg::TransformStamped & transform) +{ + t_out = tf2::Stamped( + transformToKDL(transform) * t_in, transform.header.stamp, transform.header.frame_id); +} namespace impl { diff --git a/tf2_ros/include/tf2_ros/buffer_interface.h b/tf2_ros/include/tf2_ros/buffer_interface.h index 80a038886..88223ab91 100644 --- a/tf2_ros/include/tf2_ros/buffer_interface.h +++ b/tf2_ros/include/tf2_ros/buffer_interface.h @@ -54,40 +54,22 @@ using TransformReadyCallback = std::function(t.time_since_epoch()); - std::chrono::seconds s = - std::chrono::duration_cast(t.time_since_epoch()); - builtin_interfaces::msg::Time time_msg; - time_msg.sec = static_cast(s.count()); - time_msg.nanosec = static_cast(ns.count() % 1000000000ull); - return time_msg; + return tf2::toMsg(t); } inline tf2::TimePoint fromMsg(const builtin_interfaces::msg::Time & time_msg) { - int64_t d = time_msg.sec * 1000000000ull + time_msg.nanosec; - std::chrono::nanoseconds ns(d); - return tf2::TimePoint(std::chrono::duration_cast(ns)); + return tf2::TimePointFromMsg(time_msg); } inline builtin_interfaces::msg::Duration toMsg(const tf2::Duration & t) { - std::chrono::nanoseconds ns = - std::chrono::duration_cast(t); - std::chrono::seconds s = - std::chrono::duration_cast(t); - builtin_interfaces::msg::Duration duration_msg; - duration_msg.sec = static_cast(s.count()); - duration_msg.nanosec = static_cast(ns.count() % 1000000000ull); - return duration_msg; + return tf2::toMsg(t); } inline tf2::Duration fromMsg(const builtin_interfaces::msg::Duration & duration_msg) { - int64_t d = duration_msg.sec * 1000000000ull + duration_msg.nanosec; - std::chrono::nanoseconds ns(d); - return tf2::Duration(std::chrono::duration_cast(ns)); + return tf2::DurationFromMsg(duration_msg); } inline double timeToSec(const builtin_interfaces::msg::Time & time_msg) 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 ec5872011..5224899a3 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 @@ -36,7 +36,6 @@ #include #include #include -#include namespace tf2 { @@ -48,7 +47,7 @@ namespace tf2 // method to extract timestamp from object template <> inline -tf2::TimePoint getTimestamp(const sensor_msgs::msg::PointCloud2& p) {return tf2_ros::fromMsg(p.header.stamp);} +tf2::TimePoint getTimestamp(const sensor_msgs::msg::PointCloud2& p) {return tf2::TimePointFromMsg(p.header.stamp);} // method to extract frame id from object template <> @@ -83,16 +82,6 @@ void doTransform(const sensor_msgs::msg::PointCloud2 &p_in, sensor_msgs::msg::Po *z_out = point.z(); } } -inline -sensor_msgs::msg::PointCloud2 toMsg(const sensor_msgs::msg::PointCloud2 &in) -{ - return in; -} -inline -void fromMsg(const sensor_msgs::msg::PointCloud2 &msg, sensor_msgs::msg::PointCloud2 &out) -{ - out = msg; -} } // namespace diff --git a/tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp b/tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp index a7e283b2e..cc87da4cf 100644 --- a/tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp +++ b/tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp @@ -27,60 +27,62 @@ * POSSIBILITY OF SUCH DAMAGE. */ - -#include -#include -#include -#include #include #include +#include +#include + +#include +#include +#include +#include std::unique_ptr tf_buffer = nullptr; static const double EPS = 1e-3; - TEST(Tf2Sensor, PointCloud2) { sensor_msgs::msg::PointCloud2 cloud; - sensor_msgs::msg::PointCloud2Modifier modifier(cloud); + sensor_msgs::PointCloud2Modifier modifier(cloud); modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); modifier.resize(1); - sensor_msgs::msg::PointCloud2Iterator iter_x(cloud, "x"); - sensor_msgs::msg::PointCloud2Iterator iter_y(cloud, "y"); - sensor_msgs::msg::PointCloud2Iterator iter_z(cloud, "z"); + sensor_msgs::PointCloud2Iterator iter_x(cloud, "x"); + sensor_msgs::PointCloud2Iterator iter_y(cloud, "y"); + sensor_msgs::PointCloud2Iterator iter_z(cloud, "z"); *iter_x = 1; *iter_y = 2; *iter_z = 3; - cloud.header.stamp = builtin_interfaces::msg::Time(2); + cloud.header.stamp.sec = 2; + cloud.header.stamp.nanosec = 0; cloud.header.frame_id = "A"; // simple api - sensor_msgs::msg::PointCloud2 cloud_simple = tf_buffer->transform(cloud, "B", tf2::durationFromSec(2.0)); - sensor_msgs::msg::PointCloud2Iterator iter_x_after(cloud_simple, "x"); - sensor_msgs::msg::PointCloud2Iterator iter_y_after(cloud_simple, "y"); - sensor_msgs::msg::PointCloud2Iterator iter_z_after(cloud_simple, "z"); + sensor_msgs::msg::PointCloud2 cloud_simple = + tf_buffer->transform(cloud, "B", tf2::durationFromSec(2.0)); + sensor_msgs::PointCloud2Iterator iter_x_after(cloud_simple, "x"); + sensor_msgs::PointCloud2Iterator iter_y_after(cloud_simple, "y"); + sensor_msgs::PointCloud2Iterator iter_z_after(cloud_simple, "z"); EXPECT_NEAR(*iter_x_after, -9, EPS); EXPECT_NEAR(*iter_y_after, 18, EPS); EXPECT_NEAR(*iter_z_after, 27, EPS); // advanced api - sensor_msgs::msg::PointCloud2 cloud_advanced = tf_buffer->transform(cloud, "B", builtin_interfaces::msg::Time(2.0), - "A", tf2::durationFromSec(3.0)); - sensor_msgs::msg::PointCloud2Iterator iter_x_advanced(cloud_advanced, "x"); - sensor_msgs::msg::PointCloud2Iterator iter_y_advanced(cloud_advanced, "y"); - sensor_msgs::msg::PointCloud2Iterator iter_z_advanced(cloud_advanced, "z"); + sensor_msgs::msg::PointCloud2 cloud_advanced = tf_buffer->transform( + cloud, "B", tf2::TimePoint(tf2::durationFromSec(2.0)), "A", tf2::durationFromSec(3.0)); + sensor_msgs::PointCloud2Iterator iter_x_advanced(cloud_advanced, "x"); + sensor_msgs::PointCloud2Iterator iter_y_advanced(cloud_advanced, "y"); + sensor_msgs::PointCloud2Iterator iter_z_advanced(cloud_advanced, "z"); EXPECT_NEAR(*iter_x_advanced, -9, EPS); EXPECT_NEAR(*iter_y_advanced, 18, EPS); EXPECT_NEAR(*iter_z_advanced, 27, EPS); } -int main(int argc, char **argv){ +int main(int argc, char ** argv) +{ testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "test"); - ros::NodeHandle n; rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); tf_buffer = std::make_unique(clock); @@ -94,7 +96,8 @@ int main(int argc, char **argv){ t.transform.rotation.y = 0; t.transform.rotation.z = 0; t.transform.rotation.w = 0; - t.header.stamp = builtin_interfaces::msg::Time(2.0); + t.header.stamp.sec = 2; + t.header.stamp.nanosec = 0; t.header.frame_id = "A"; t.child_frame_id = "B"; tf_buffer->setTransform(t, "test"); From 3904bd0922d48a3dfe7a06a5066d09ab39ef6afb Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Mon, 25 Jan 2021 22:29:17 +0000 Subject: [PATCH 05/51] Fix toMsg forward --- tf2/include/tf2/convert.h | 2 +- tf2/include/tf2/impl/convert.h | 2 +- tf2/include/tf2/impl/forward.h | 12 +++++++++--- 3 files changed, 11 insertions(+), 5 deletions(-) diff --git a/tf2/include/tf2/convert.h b/tf2/include/tf2/convert.h index c8e41bd96..eb3975029 100644 --- a/tf2/include/tf2/convert.h +++ b/tf2/include/tf2/convert.h @@ -111,7 +111,7 @@ std::array, 6> getCovarianceMatrix(const tf2::WithCovarian * \tparam B ROS message Datatype. The default value will be taken from impl::defaultMessage\::type. * \return the conversion as a ROS message */ -template ::type> +template inline B toMsg(const A & a) { B b; diff --git a/tf2/include/tf2/impl/convert.h b/tf2/include/tf2/impl/convert.h index c2fd465e0..506f86a21 100644 --- a/tf2/include/tf2/impl/convert.h +++ b/tf2/include/tf2/impl/convert.h @@ -216,7 +216,7 @@ template <> template inline void Converter::convert(const A & a, B & b) { - b = toMsg<>(a); + b = toMsg(a); } template <> diff --git a/tf2/include/tf2/impl/forward.h b/tf2/include/tf2/impl/forward.h index 371d77797..4717b8552 100644 --- a/tf2/include/tf2/impl/forward.h +++ b/tf2/include/tf2/impl/forward.h @@ -33,6 +33,12 @@ namespace tf2 { +namespace impl +{ +template +struct defaultMessage; +} + template class Stamped; @@ -42,6 +48,9 @@ class WithCovarianceStamped; template B & toMsg(const A & a, B & b); +template ::type> +B toMsg(const A & a); + template void fromMsg(const A & a, B & b); @@ -56,9 +65,6 @@ struct stampedMessageTraits; template struct unstampedMessageTraits; -template -struct defaultMessage; - template struct DefaultStampedImpl; From 2499ae97feec002e6e26ec002d3bfcb65f0a6500 Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Fri, 12 Feb 2021 00:17:04 +0000 Subject: [PATCH 06/51] Reenable Eigen tests --- tf2_eigen/test/tf2_eigen-test.cpp | 109 ++++++++++++++---------------- 1 file changed, 52 insertions(+), 57 deletions(-) diff --git a/tf2_eigen/test/tf2_eigen-test.cpp b/tf2_eigen/test/tf2_eigen-test.cpp index f03c25445..6382b7925 100644 --- a/tf2_eigen/test/tf2_eigen-test.cpp +++ b/tf2_eigen/test/tf2_eigen-test.cpp @@ -44,63 +44,58 @@ #include #include -// TODO(clalancette) Re-enable these tests once we have tf2/convert.h:convert(A, B) implemented -// TEST(TfEigen, ConvertVector3dStamped) -// { -// const tf2::Stamped v(Eigen::Vector3d(1,2,3), -// tf2::TimePoint(std::chrono::seconds(5)), "test"); - -// tf2::Stamped v1; -// geometry_msgs::msg::PointStamped p1; -// tf2::convert(v, p1); -// tf2::convert(p1, v1); - -// EXPECT_EQ(v, v1); -// } - -// TEST(TfEigen, ConvertVector3d) -// { -// const Eigen::Vector3d v(1,2,3); - -// Eigen::Vector3d v1; -// geometry_msgs::msg::Point p1; -// tf2::convert(v, p1); -// tf2::convert(p1, v1); - -// EXPECT_EQ(v, v1); -// } - -// TEST(TfEigen, ConvertAffine3dStamped) -// { -// const Eigen::Affine3d v_nonstamped( -// Eigen::Translation3d(1,2,3) * Eigen::AngleAxis(1, Eigen::Vector3d::UnitX())); -// const tf2::Stamped v( -// v_nonstamped, tf2::TimePoint(std::chrono::seconds(42)), "test_frame"); - -// tf2::Stamped v1; -// geometry_msgs::msg::PoseStamped p1; -// tf2::convert(v, p1); -// tf2::convert(p1, v1); - -// EXPECT_EQ(v.translation(), v1.translation()); -// EXPECT_EQ(v.rotation(), v1.rotation()); -// EXPECT_EQ(v.frame_id_, v1.frame_id_); -// EXPECT_EQ(v.stamp_, v1.stamp_); -// } - -// TEST(TfEigen, ConvertAffine3d) -// { -// const Eigen::Affine3d v( -// Eigen::Translation3d(1,2,3) * Eigen::AngleAxis(1, Eigen::Vector3d::UnitX())); - -// Eigen::Affine3d v1; -// geometry_msgs::msg::Pose p1; -// tf2::convert(v, p1); -// tf2::convert(p1, v1); - -// EXPECT_EQ(v.translation(), v1.translation()); -// EXPECT_EQ(v.rotation(), v1.rotation()); -// } +TEST(TfEigen, ConvertVector3dStamped) +{ + const tf2::Stamped v(Eigen::Vector3d(1,2,3), tf2::TimePoint(std::chrono::seconds(5)), "test"); + + tf2::Stamped v1; + geometry_msgs::msg::PointStamped p1; + tf2::convert(v, p1); + tf2::convert(p1, v1); + + EXPECT_EQ(v, v1); +} + +TEST(TfEigen, ConvertVector3d) +{ + const Eigen::Vector3d v(1,2,3); + + Eigen::Vector3d v1; + geometry_msgs::msg::Point p1; + tf2::convert(v, p1); + tf2::convert(p1, v1); + + EXPECT_EQ(v, v1); +} + +TEST(TfEigen, ConvertAffine3dStamped) +{ + const Eigen::Affine3d v_nonstamped(Eigen::Translation3d(1,2,3) * Eigen::AngleAxis(1, Eigen::Vector3d::UnitX())); + const tf2::Stamped v(v_nonstamped, tf2::TimePoint(std::chrono::seconds(42)), "test_frame"); + + tf2::Stamped v1; + geometry_msgs::msg::PoseStamped p1; + tf2::convert(v, p1); + tf2::convert(p1, v1); + + EXPECT_EQ(v.translation(), v1.translation()); + EXPECT_EQ(v.rotation(), v1.rotation()); + EXPECT_EQ(v.frame_id_, v1.frame_id_); + EXPECT_EQ(v.stamp_, v1.stamp_); +} + +TEST(TfEigen, ConvertAffine3d) +{ + const Eigen::Affine3d v(Eigen::Translation3d(1,2,3) * Eigen::AngleAxis(1, Eigen::Vector3d::UnitX())); + + Eigen::Affine3d v1; + geometry_msgs::msg::Pose p1; + tf2::convert(v, p1); + tf2::convert(p1, v1); + + EXPECT_EQ(v.translation(), v1.translation()); + EXPECT_EQ(v.rotation(), v1.rotation()); +} TEST(TfEigen, ConvertTransform) { From 592056a82b9ff3bfd77c77fcada382ce92ae3bd6 Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Mon, 25 Jan 2021 22:46:22 +0000 Subject: [PATCH 07/51] Reenable sensor_msgs cpp test --- tf2_sensor_msgs/CMakeLists.txt | 38 +++++++++++++++++----------------- 1 file changed, 19 insertions(+), 19 deletions(-) diff --git a/tf2_sensor_msgs/CMakeLists.txt b/tf2_sensor_msgs/CMakeLists.txt index 45243b083..f05148f04 100644 --- a/tf2_sensor_msgs/CMakeLists.txt +++ b/tf2_sensor_msgs/CMakeLists.txt @@ -24,24 +24,24 @@ find_package(Eigen3 REQUIRED) ament_export_dependencies(eigen3_cmake_module) ament_export_dependencies(Eigen3) - -# TODO enable tests -#if(BUILD_TESTING) -# catkin_add_nosetests(test/test_tf2_sensor_msgs.py) - -#find_package(catkin REQUIRED COMPONENTS -# sensor_msgs -# rostest -# tf2_ros -# tf2 -#) -#include_directories(${EIGEN_INCLUDE_DIRS}) -#add_executable(test_tf2_sensor_msgs_cpp EXCLUDE_FROM_ALL test/test_tf2_sensor_msgs.cpp) -#target_link_libraries(test_tf2_sensor_msgs_cpp ${catkin_LIBRARIES} ${GTEST_LIBRARIES}) -#if(TARGET tests) -# add_dependencies(tests test_tf2_sensor_msgs_cpp) -#endif() -#add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test.launch) -#endif() +ament_export_dependencies(tf2) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_pytest REQUIRED) + + #ament_add_pytest_test(test_tf2_sensor_msgs_py test/test_tf2_sensor_msgs.py) + + ament_add_gtest(test_tf2_sensor_msgs_cpp test/test_tf2_sensor_msgs.cpp) + if(TARGET test_tf2_sensor_msgs_cpp) + target_include_directories(test_tf2_sensor_msgs_cpp PUBLIC include) + target_link_libraries(test_tf2_sensor_msgs_cpp Eigen3::Eigen) + ament_target_dependencies(test_tf2_sensor_msgs_cpp + "rclcpp" + "tf2" + "tf2_ros" + ) + endif() +endif() ament_auto_package() From 402f4bddd09447b863cc31905342d29add16c727 Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Mon, 25 Jan 2021 23:14:48 +0000 Subject: [PATCH 08/51] Reenable conversion test --- test_tf2/CMakeLists.txt | 29 +++++++++++++++-------------- test_tf2/package.xml | 2 ++ test_tf2/test/test_convert.cpp | 24 ++++++++++++------------ 3 files changed, 29 insertions(+), 26 deletions(-) diff --git a/test_tf2/CMakeLists.txt b/test_tf2/CMakeLists.txt index ff0c9be04..a29ddbe59 100644 --- a/test_tf2/CMakeLists.txt +++ b/test_tf2/CMakeLists.txt @@ -24,18 +24,17 @@ find_package(geometry_msgs REQUIRED) find_package(launch_testing_ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(tf2 REQUIRED) -# TODO (ahcorde): activate when tf2_bullet is merged -# find_package(tf2_bullet REQUIRED) +find_package(tf2_bullet REQUIRED) +find_package(tf2_eigen REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_kdl REQUIRED) find_package(tf2_ros REQUIRED) ament_find_gtest() -# TODO (ahcorde): activate when tf2_bullet is merged -# find_package(PkgConfig REQUIRED) -# pkg_check_modules(bullet REQUIRED bullet) -# include_directories(include ${bullet_INCLUDE_DIRS}) +find_package(PkgConfig REQUIRED) +pkg_check_modules(bullet REQUIRED bullet) +include_directories(include ${bullet_INCLUDE_DIRS}) ament_add_gtest(buffer_core_test test/buffer_core_test.cpp) if(TARGET buffer_core_test) @@ -60,14 +59,16 @@ if(TARGET test_message_filter) ) endif() -# TODO (ahcorde): activate when tf2_bullet is merged -# ament_add_gtest(test_convert test/test_convert.cpp) -# if(TARGET test_convert) -# ament_target_dependencies(test_convert -# tf2 -# tf2_bullet -# ) -# endif() +ament_add_gtest(test_convert test/test_convert.cpp) +if(TARGET test_convert) + ament_target_dependencies(test_convert + tf2 + tf2_bullet + tf2_eigen + tf2_geometry_msgs + tf2_kdl + ) +endif() ament_add_gtest(test_utils test/test_utils.cpp) if(TARGET test_utils) diff --git a/test_tf2/package.xml b/test_tf2/package.xml index ff0bf3730..501158518 100644 --- a/test_tf2/package.xml +++ b/test_tf2/package.xml @@ -17,6 +17,8 @@ geometry_msgs rclcpp tf2 + tf2_bullet + tf2_eigen tf2_geometry_msgs tf2_kdl tf2_ros diff --git a/test_tf2/test/test_convert.cpp b/test_tf2/test/test_convert.cpp index b68d10ae7..614a33a16 100644 --- a/test_tf2/test/test_convert.cpp +++ b/test_tf2/test/test_convert.cpp @@ -115,19 +115,19 @@ TEST(tf2Convert, PointVectorDefaultMessagetype) // 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); + const tf2::Stamped b1{btVector3{1.0, 3.0, 4.0}, tf2::get_now(), "my_frame" }; + const geometry_msgs::msg::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_); + EXPECT_EQ(msg.header.stamp, tf2::toMsg<>(b1.stamp_)); } { // Eigen const Eigen::Vector3d e1{2.0, 4.0, 5.0}; - const geometry_msgs::Point msg = tf2::toMsg(e1); + const geometry_msgs::msg::Point msg = tf2::toMsg(e1); EXPECT_EQ(msg.x, 2.0); EXPECT_EQ(msg.y, 4.0); @@ -136,7 +136,7 @@ TEST(tf2Convert, PointVectorDefaultMessagetype) { // tf2 const tf2::Vector3 t1{2.0, 4.0, 5.0}; - const geometry_msgs::Vector3 msg = tf2::toMsg(t1); + const geometry_msgs::msg::Vector3 msg = tf2::toMsg(t1); EXPECT_EQ(msg.x, 2.0); EXPECT_EQ(msg.y, 4.0); @@ -144,14 +144,14 @@ TEST(tf2Convert, PointVectorDefaultMessagetype) } { // 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); + const tf2::Stamped k1{KDL::Vector{1.0, 3.0, 4.0}, tf2::get_now(), "my_frame"}; + const geometry_msgs::msg::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_); + EXPECT_EQ(msg.header.stamp, tf2::toMsg<>(k1.stamp_)); } } @@ -159,8 +159,8 @@ 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); + geometry_msgs::msg::Point msg; + const geometry_msgs::msg::Point& msg2 = tf2::toMsg(t1, msg); // returned reference is second argument EXPECT_EQ(&msg2, &msg); @@ -171,8 +171,8 @@ TEST(tf2Convert, PointVectorOtherMessagetype) { // Eigen const Eigen::Vector3d e1{2.0, 4.0, 5.0}; - geometry_msgs::Vector3 msg; - const geometry_msgs::Vector3& msg2 = tf2::toMsg(e1, msg); + geometry_msgs::msg::Vector3 msg; + const geometry_msgs::msg::Vector3& msg2 = tf2::toMsg(e1, msg); // returned reference is second argument EXPECT_EQ(&msg2, &msg); From 71513ec7e7ddc4d57686a9aa294d15916c66d499 Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Tue, 26 Jan 2021 12:08:03 +0000 Subject: [PATCH 09/51] Add testcases from tf2_eigen_kdl to tf2_test --- test_tf2/test/test_convert.cpp | 74 ++++++++++++++++++++++++++++++++++ 1 file changed, 74 insertions(+) diff --git a/test_tf2/test/test_convert.cpp b/test_tf2/test/test_convert.cpp index 614a33a16..96a99d476 100644 --- a/test_tf2/test/test_convert.cpp +++ b/test_tf2/test/test_convert.cpp @@ -182,6 +182,80 @@ TEST(tf2Convert, PointVectorOtherMessagetype) } } +TEST(TfEigenKdl, TestRotationQuaternion) +{ + const auto kdl_v = KDL::Rotation::RPY(1.5, 0.2, 0.3); + Eigen::Quaterniond eigen_v = Eigen::Quaterniond::Identity(); + tf2::convert(kdl_v, eigen_v); + KDL::Rotation kdl_v1; + tf2::convert(eigen_v, kdl_v1); + EXPECT_EQ(kdl_v, kdl_v1); +} + +TEST(TfEigenKdl, TestQuaternionRotation) +{ + const Eigen::Quaterniond eigen_v = Eigen::Quaterniond(1, 2, 1.5, 3).normalized(); + KDL::Rotation kdl_v; + tf2::convert(eigen_v, kdl_v); + Eigen::Quaterniond eigen_v1; + tf2::convert(kdl_v, eigen_v1); + EXPECT_TRUE(eigen_v.isApprox(eigen_v1)); +} + +TEST(TfEigenKdl, TestFrameIsometry3d) +{ + const auto kdl_v = KDL::Frame(KDL::Rotation::RPY(1.2, 0.2, 0), KDL::Vector(1, 2, 3)); + Eigen::Isometry3d eigen_v = Eigen::Isometry3d::Identity(); + tf2::convert(kdl_v, eigen_v); + KDL::Frame kdl_v1; + tf2::convert(eigen_v, kdl_v1); + EXPECT_EQ(kdl_v, kdl_v1); +} + +TEST(TfEigenKdl, TestIsometry3dFrame) +{ + const Eigen::Isometry3d eigen_v( + Eigen::Translation3d(1, 2, 3) * Eigen::AngleAxisd(1, Eigen::Vector3d::UnitX())); + KDL::Frame kdl_v; + tf2::convert(eigen_v, kdl_v); + Eigen::Isometry3d eigen_v1; + tf2::convert(kdl_v, eigen_v1); + EXPECT_EQ(eigen_v.translation(), eigen_v1.translation()); + EXPECT_EQ(eigen_v.rotation(), eigen_v1.rotation()); +} + +TEST(TfEigenKdl, TestFrameAffine3d) +{ + const auto kdl_v = KDL::Frame(KDL::Rotation::RPY(1.2, 0.2, 0), KDL::Vector(1, 2, 3)); + Eigen::Affine3d eigen_v = Eigen::Affine3d::Identity(); + tf2::convert(kdl_v, eigen_v); + KDL::Frame kdl_v1; + tf2::convert(eigen_v, kdl_v1); + EXPECT_EQ(kdl_v, kdl_v1); +} + + +TEST(TfEigenKdl, TestVectorVector3d) +{ + const auto kdl_v = KDL::Vector(1, 2, 3); + Eigen::Vector3d eigen_v; + tf2::convert(kdl_v, eigen_v); + KDL::Vector kdl_v1; + tf2::convert(eigen_v, kdl_v1); + EXPECT_EQ(kdl_v, kdl_v1); +} + +TEST(TfEigenKdl, TestVector3dVector) +{ + Eigen::Vector3d eigen_v; + eigen_v << 1, 2, 3; + KDL::Vector kdl_v; + tf2::convert(eigen_v, kdl_v); + Eigen::Vector3d eigen_v1; + tf2::convert(kdl_v, eigen_v1); + EXPECT_EQ(eigen_v, eigen_v1); +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); From 4dc54537d78bcb2a24608bc38eeb4effd8d8867a Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Tue, 26 Jan 2021 12:12:08 +0000 Subject: [PATCH 10/51] Add Wrench conversion --- test_tf2/test/test_convert.cpp | 30 +++++++++++++++++++ tf2_eigen/include/tf2_eigen/tf2_eigen.h | 30 +++++++++++++++++++ .../tf2_geometry_msgs/tf2_geometry_msgs.h | 21 ++++++++++++- tf2_kdl/include/tf2_kdl/tf2_kdl.h | 14 +++++++++ 4 files changed, 94 insertions(+), 1 deletion(-) diff --git a/test_tf2/test/test_convert.cpp b/test_tf2/test/test_convert.cpp index 96a99d476..cd79efee3 100644 --- a/test_tf2/test/test_convert.cpp +++ b/test_tf2/test/test_convert.cpp @@ -41,6 +41,8 @@ #include #include +using Vector6d = Eigen::Matrix; + TEST(tf2Convert, kdlToBullet) { double epsilon = 1e-9; @@ -234,6 +236,34 @@ TEST(TfEigenKdl, TestFrameAffine3d) EXPECT_EQ(kdl_v, kdl_v1); } +TEST(TfEigenKdl, TestTwistMatrix) +{ + const auto kdl_v = KDL::Twist(KDL::Vector(1, 2, 3), KDL::Vector(4, 5, 6)); + Vector6d eigen_v; + tf2::convert(kdl_v, eigen_v); + KDL::Twist kdl_v1; + tf2::convert(eigen_v, kdl_v1); + EXPECT_EQ(kdl_v, kdl_v1); +} + + +TEST(TfEigenKdl, TestMatrixWrench) +{ + Vector6d eigen_v; + eigen_v << 1, 2, 3, 3, 2, 1; + KDL::Wrench kdl_v; + tf2::convert(eigen_v, kdl_v); + std::array tf2_v; + tf2::convert(kdl_v, tf2_v); + Vector6d eigen_v1; + tf2::convert(tf2_v, eigen_v1); + std::array tf2_v1; + tf2::convert(eigen_v1, tf2_v1); + Vector6d eigen_v2; + tf2::convert(tf2_v1, eigen_v2); + EXPECT_EQ(eigen_v, eigen_v2); +} + TEST(TfEigenKdl, TestVectorVector3d) { diff --git a/tf2_eigen/include/tf2_eigen/tf2_eigen.h b/tf2_eigen/include/tf2_eigen/tf2_eigen.h index 1545cc965..21e604507 100644 --- a/tf2_eigen/include/tf2_eigen/tf2_eigen.h +++ b/tf2_eigen/include/tf2_eigen/tf2_eigen.h @@ -39,6 +39,7 @@ #include #include #include +#include namespace tf2 { @@ -404,6 +405,35 @@ struct defaultMessage> using type = geometry_msgs::msg::Twist; }; +template <> +struct ImplDetails, geometry_msgs::msg::Wrench> +{ + static void toMsg(const Eigen::Matrix & in, geometry_msgs::msg::Wrench & msg) + { + msg.force.x = in[0]; + msg.force.y = in[1]; + msg.force.z = in[2]; + msg.torque.x = in[3]; + msg.torque.y = in[4]; + msg.torque.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. + */ + static void fromMsg(const geometry_msgs::msg::Wrench & msg, Eigen::Matrix & out) + { + out[0] = msg.force.x; + out[1] = msg.force.y; + out[2] = msg.force.z; + out[3] = msg.torque.x; + out[4] = msg.torque.y; + out[5] = msg.torque.z; + } +}; + } // namespace impl /** \brief Convert a Eigen::Vector3d type to a geometry_msgs::msg::Vector3. 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 3479fd52e..3a2614780 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 @@ -48,9 +48,14 @@ #include #include -namespace tf2 +namespace Eigen { +template +class Matrix; +} +namespace tf2 +{ /** \brief Convert a TransformStamped message to a KDL frame. * \param t TransformStamped message to convert. * \return The converted KDL Frame. @@ -446,7 +451,21 @@ struct ImplDetails, geometry_msgs::msg::Wrench> } }; +template <> +struct defaultMessage > +{ + using type = geometry_msgs::msg::Wrench; +}; } // namespace impl + +template +void convert( + const Eigen::Matrix & in, std::array & out) +{ + out[0] = tf2::Vector3{in(0), in(1), in(2)}; + out[1] = tf2::Vector3{in(3), in(4), in(5)}; +} + } // namespace tf2 #endif // TF2_GEOMETRY_MSGS_H diff --git a/tf2_kdl/include/tf2_kdl/tf2_kdl.h b/tf2_kdl/include/tf2_kdl/tf2_kdl.h index a2ecc2da6..4c0347759 100644 --- a/tf2_kdl/include/tf2_kdl/tf2_kdl.h +++ b/tf2_kdl/include/tf2_kdl/tf2_kdl.h @@ -43,6 +43,12 @@ #include #include +namespace Eigen +{ +template +class Matrix; +} + namespace tf2 { /** \brief Convert a timestamped transform to the equivalent KDL data type. @@ -245,6 +251,14 @@ struct defaultMessage }; } // namespace impl +template +void convert(Eigen::Matrix const & in, KDL::Wrench & out) +{ + const auto msg = + toMsg, geometry_msgs::msg::Wrench>(in); + fromMsg<>(msg, out); +} + // --------------------- // Frame // --------------------- From 1f69cb074350f18a716f2dddc199fddc7b830c5a Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Tue, 26 Jan 2021 13:17:03 +0000 Subject: [PATCH 11/51] Enable Transform and Quaternion for bullet --- tf2_bullet/include/tf2_bullet/tf2_bullet.h | 132 +++++++++++++++------ tf2_bullet/test/test_tf2_bullet.cpp | 16 +++ 2 files changed, 110 insertions(+), 38 deletions(-) diff --git a/tf2_bullet/include/tf2_bullet/tf2_bullet.h b/tf2_bullet/include/tf2_bullet/tf2_bullet.h index 5c3a06920..f5d877783 100644 --- a/tf2_bullet/include/tf2_bullet/tf2_bullet.h +++ b/tf2_bullet/include/tf2_bullet/tf2_bullet.h @@ -37,6 +37,7 @@ #include #include +#include #if (BT_BULLET_VERSION <= 282) // Suppress compilation warning on older versions of Bullet. @@ -49,41 +50,6 @@ inline int bullet_btInfinityMask() namespace tf2 { -/** \brief Convert a timestamped transform to the equivalent Bullet data type. - * \param t The transform to convert, as a geometry_msgs TransformedStamped message. - * \return The transform message converted to a Bullet btTransform. - */ - inline - btTransform transformToBullet(const geometry_msgs::msg::TransformStamped & t) - { - return btTransform( - btQuaternion( - static_cast < float > (t.transform.rotation.x), - static_cast < float > (t.transform.rotation.y), - static_cast < float > (t.transform.rotation.z), - static_cast < float > (t.transform.rotation.w)), - btVector3( - static_cast < float > (t.transform.translation.x), - static_cast < float > (t.transform.translation.y), - static_cast < float > (t.transform.translation.z))); - } - - -/** \brief Apply a geometry_msgs TransformStamped to a Bullet-specific Vector3 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 Bullet btVector3 data type. - * \param t_out The transformed vector, as a timestamped Bullet btVector3 data type. - * \param transform The timestamped transform to apply, as a TransformStamped message. - */ -template <> -inline void doTransform( - const tf2::Stamped & t_in, tf2::Stamped & t_out, - const geometry_msgs::msg::TransformStamped & transform) -{ - t_out = tf2::Stamped( - transformToBullet(transform) * t_in, transform.header.stamp, transform.header.frame_id); -} - namespace impl { template <> @@ -93,14 +59,26 @@ struct defaultMessage }; template <> -struct ImplDetails +struct defaultMessage +{ + using type = geometry_msgs::msg::Quaternion; +}; + +template <> +struct defaultMessage +{ + using type = geometry_msgs::msg::Transform; +}; + +template +struct BulletVectorImpl { /** \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::msg::Point & msg) + static void toMsg(const btVector3 & in, Message & msg) { msg.x = in[0]; msg.y = in[1]; @@ -112,15 +90,93 @@ struct ImplDetails * \param msg The PointStamped message to convert. * \param out The point converted to a timestamped Bullet Vector3. */ - static void fromMsg(const geometry_msgs::msg::Point & msg, btVector3 & out) + static void fromMsg(const Message & msg, btVector3 & out) + { + out[0] = msg.x; + out[1] = msg.y; + out[2] = msg.z; + } +}; + +template <> +struct ImplDetails +: BulletVectorImpl +{ +}; + +template <> +struct ImplDetails +: BulletVectorImpl +{ +}; + +template <> +struct ImplDetails +{ + static void toMsg(const btQuaternion & in, geometry_msgs::msg::Quaternion & msg) + { + msg.x = in[0]; + msg.y = in[1]; + msg.z = in[2]; + msg.w = in[3]; + } + + static void fromMsg(const geometry_msgs::msg::Quaternion & msg, btQuaternion & out) { out[0] = msg.x; out[1] = msg.y; out[2] = msg.z; + out[3] = msg.w; + if (msg.w < 0) out *= -1; + } +}; + +template <> +struct ImplDetails +{ + static void fromMsg(geometry_msgs::msg::Transform const & in, btTransform & out) + { + btVector3 trans; + btQuaternion rot; + tf2::fromMsg<>(in.rotation, rot); + tf2::fromMsg<>(in.translation, trans); + out = btTransform(rot, trans); + } + + static void toMsg(btTransform const & in, geometry_msgs::msg::Transform & out) + { + tf2::toMsg<>(in.getRotation(), out.rotation); + tf2::toMsg<>(in.getOrigin(), out.translation); } }; } // namespace impl +/** \brief Convert a timestamped transform to the equivalent Bullet data type. + * \param t The transform to convert, as a geometry_msgs TransformedStamped message. + * \return The transform message converted to a Bullet btTransform. + */ +inline btTransform transformToBullet(const geometry_msgs::msg::TransformStamped & t) +{ + btTransform ans; + fromMsg<>(t.transform, ans); + return ans; +} + +/** \brief Apply a geometry_msgs TransformStamped to a Bullet-specific Vector3 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 Bullet btVector3 data type. + * \param t_out The transformed vector, as a timestamped Bullet btVector3 data type. + * \param transform The timestamped transform to apply, as a TransformStamped message. + */ +template <> +inline void doTransform( + const tf2::Stamped & t_in, tf2::Stamped & t_out, + const geometry_msgs::msg::TransformStamped & transform) +{ + t_out = tf2::Stamped( + transformToBullet(transform) * t_in, transform.header.stamp, transform.header.frame_id); +} + /** \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 * \param t_in The frame to transform, as a timestamped Bullet btTransform. diff --git a/tf2_bullet/test/test_tf2_bullet.cpp b/tf2_bullet/test/test_tf2_bullet.cpp index 08a735e51..03a957727 100644 --- a/tf2_bullet/test/test_tf2_bullet.cpp +++ b/tf2_bullet/test/test_tf2_bullet.cpp @@ -51,6 +51,22 @@ TEST(TfBullet, ConvertVector) EXPECT_EQ(v1, v2); } +TEST(TfBullet, ConvertTransform) +{ + geometry_msgs::msg::Transform transform; + transform.rotation.x = 1.0; + transform.rotation.w = 0.0; + transform.translation.z = 2.0; + + btTransform transform_bt; + tf2::convert(transform, transform_bt); + EXPECT_EQ(transform_bt.getRotation(), btQuaternion(1.0f, 0.0f, 0.0f, 0.0f)); + EXPECT_EQ(transform_bt.getOrigin(), btVector3(0.0f, 0.0f, 2.0f)); + + geometry_msgs::msg::Transform transform2; + tf2::convert(transform_bt, transform2); + EXPECT_EQ(transform2, transform); +} int main(int argc, char ** argv) { From 8288bc4bb7b08837369f54bc0ab653f6c91358db Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Tue, 26 Jan 2021 14:08:08 +0000 Subject: [PATCH 12/51] fix bullet uncrustify --- tf2_bullet/CMakeLists.txt | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/tf2_bullet/CMakeLists.txt b/tf2_bullet/CMakeLists.txt index c423d5062..025c2b1e9 100644 --- a/tf2_bullet/CMakeLists.txt +++ b/tf2_bullet/CMakeLists.txt @@ -28,13 +28,16 @@ install(DIRECTORY include/${PROJECT_NAME}/ ) if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - find_package(ament_cmake_cppcheck REQUIRED) list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_cppcheck + ament_cmake_uncrustify ) + find_package(ament_lint_auto REQUIRED) + find_package(ament_cmake_uncrustify REQUIRED) + find_package(ament_cmake_cppcheck REQUIRED) ament_lint_auto_find_test_dependencies() - ament_cppcheck(LANGUAGE c++) + ament_cppcheck(LANGUAGE "c++") + ament_uncrustify(LANGUAGE "c++") find_package(ament_cmake_gtest REQUIRED) ament_add_gtest(test_bullet test/test_tf2_bullet.cpp) From 185433a9fc65e3683c448a16ab65de3514bbeda9 Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Tue, 26 Jan 2021 14:34:28 +0000 Subject: [PATCH 13/51] bullet uncrustify format --- tf2_bullet/include/tf2_bullet/tf2_bullet.h | 26 +++++++++++----------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/tf2_bullet/include/tf2_bullet/tf2_bullet.h b/tf2_bullet/include/tf2_bullet/tf2_bullet.h index f5d877783..6e82f34bf 100644 --- a/tf2_bullet/include/tf2_bullet/tf2_bullet.h +++ b/tf2_bullet/include/tf2_bullet/tf2_bullet.h @@ -52,25 +52,25 @@ namespace tf2 { namespace impl { -template <> +template<> struct defaultMessage { using type = geometry_msgs::msg::Point; }; -template <> +template<> struct defaultMessage { using type = geometry_msgs::msg::Quaternion; }; -template <> +template<> struct defaultMessage { using type = geometry_msgs::msg::Transform; }; -template +template struct BulletVectorImpl { /** \brief Convert a stamped Bullet Vector3 type to a PointStamped message. @@ -98,19 +98,19 @@ struct BulletVectorImpl } }; -template <> +template<> struct ImplDetails -: BulletVectorImpl + : BulletVectorImpl { }; -template <> +template<> struct ImplDetails -: BulletVectorImpl + : BulletVectorImpl { }; -template <> +template<> struct ImplDetails { static void toMsg(const btQuaternion & in, geometry_msgs::msg::Quaternion & msg) @@ -127,11 +127,11 @@ struct ImplDetails out[1] = msg.y; out[2] = msg.z; out[3] = msg.w; - if (msg.w < 0) out *= -1; + if (msg.w < 0) {out *= -1;} } }; -template <> +template<> struct ImplDetails { static void fromMsg(geometry_msgs::msg::Transform const & in, btTransform & out) @@ -168,7 +168,7 @@ inline btTransform transformToBullet(const geometry_msgs::msg::TransformStamped * \param t_out The transformed vector, as a timestamped Bullet btVector3 data type. * \param transform The timestamped transform to apply, as a TransformStamped message. */ -template <> +template<> inline void doTransform( const tf2::Stamped & t_in, tf2::Stamped & t_out, const geometry_msgs::msg::TransformStamped & transform) @@ -183,7 +183,7 @@ inline void doTransform( * \param t_out The transformed frame, as a timestamped Bullet btTransform. * \param transform The timestamped transform to apply, as a TransformStamped message. */ -template <> +template<> inline void doTransform( const tf2::Stamped & t_in, tf2::Stamped & t_out, const geometry_msgs::msg::TransformStamped & transform) From 938d9082adc3d4a253520df94cc87e1b8698e50a Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Tue, 26 Jan 2021 15:46:34 +0000 Subject: [PATCH 14/51] Fix bullet test and cmake --- tf2_bullet/CMakeLists.txt | 1 - tf2_bullet/test/test_tf2_bullet.cpp | 6 ++++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/tf2_bullet/CMakeLists.txt b/tf2_bullet/CMakeLists.txt index 025c2b1e9..6cc257ec0 100644 --- a/tf2_bullet/CMakeLists.txt +++ b/tf2_bullet/CMakeLists.txt @@ -49,5 +49,4 @@ if(BUILD_TESTING) endif() ament_export_include_directories(include) -ament_export_libraries(${PROJECT_NAME}) ament_package() diff --git a/tf2_bullet/test/test_tf2_bullet.cpp b/tf2_bullet/test/test_tf2_bullet.cpp index 03a957727..7ea274a8c 100644 --- a/tf2_bullet/test/test_tf2_bullet.cpp +++ b/tf2_bullet/test/test_tf2_bullet.cpp @@ -60,8 +60,10 @@ TEST(TfBullet, ConvertTransform) btTransform transform_bt; tf2::convert(transform, transform_bt); - EXPECT_EQ(transform_bt.getRotation(), btQuaternion(1.0f, 0.0f, 0.0f, 0.0f)); - EXPECT_EQ(transform_bt.getOrigin(), btVector3(0.0f, 0.0f, 2.0f)); + EXPECT_EQ(transform_bt.getRotation(), btQuaternion(1, 0, 0, 0)); + // vector has 4 entries, set last one to 0 to make comparsion more stable + transform_bt.getOrigin().setW(0); + EXPECT_EQ(transform_bt.getOrigin(), btVector3(0, 0, 2)); geometry_msgs::msg::Transform transform2; tf2::convert(transform_bt, transform2); From 9f39eb76a1a98efa1eb521339c75a65680a244dc Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Tue, 26 Jan 2021 16:51:11 +0000 Subject: [PATCH 15/51] make Vector and Quaternion conversion reuseable --- tf2/include/tf2/impl/convert.h | 56 ++++++++++++++++++- tf2_bullet/include/tf2_bullet/tf2_bullet.h | 51 +---------------- tf2_eigen/include/tf2_eigen/tf2_eigen.h | 32 +---------- .../tf2_geometry_msgs/tf2_geometry_msgs.h | 53 +----------------- tf2_kdl/include/tf2_kdl/tf2_kdl.h | 32 +---------- 5 files changed, 65 insertions(+), 159 deletions(-) diff --git a/tf2/include/tf2/impl/convert.h b/tf2/include/tf2/impl/convert.h index 506f86a21..2b76ea0dd 100644 --- a/tf2/include/tf2/impl/convert.h +++ b/tf2/include/tf2/impl/convert.h @@ -31,6 +31,8 @@ #ifndef TF2__IMPL__CONVERT_H_ #define TF2__IMPL__CONVERT_H_ +#include + #include "../time.h" #include "forward.h" @@ -79,7 +81,6 @@ struct ImplDetails // void fromMsg(const Message&, Datatype&); }; - /** * \brief Mapping of unstamped Messages for stamped Messages * @@ -288,6 +289,59 @@ struct DefaultStampedImpl> static std::string getFrameId(const tf2::Stamped & t) { return t.frame_id_; } }; +template +struct DefaultVectorImpl +{ + /** \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 VectorType & in, Message & 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 Message & msg, VectorType & out) + { + out = VectorType(msg.x, msg.y, msg.z); + } +}; + +template +struct DefaultQuaternionImpl +{ + /** \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 QuaternionType & in, geometry_msgs::msg::Quaternion & msg) + { + msg.x = in[0]; + msg.y = in[1]; + msg.z = in[2]; + msg.w = in[3]; + } + + /** \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::msg::Quaternion & msg, QuaternionType & out) + { + out = QuaternionType(msg.x, msg.y, msg.z, msg.w); + } +}; + } // namespace impl } // namespace tf2 diff --git a/tf2_bullet/include/tf2_bullet/tf2_bullet.h b/tf2_bullet/include/tf2_bullet/tf2_bullet.h index 6e82f34bf..39760b239 100644 --- a/tf2_bullet/include/tf2_bullet/tf2_bullet.h +++ b/tf2_bullet/include/tf2_bullet/tf2_bullet.h @@ -70,66 +70,21 @@ struct defaultMessage using type = geometry_msgs::msg::Transform; }; -template -struct BulletVectorImpl -{ - /** \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, Message & 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 Message & msg, btVector3 & out) - { - out[0] = msg.x; - out[1] = msg.y; - out[2] = msg.z; - } -}; - template<> struct ImplDetails - : BulletVectorImpl + : DefaultVectorImpl { }; template<> struct ImplDetails - : BulletVectorImpl + : DefaultVectorImpl { }; template<> struct ImplDetails -{ - static void toMsg(const btQuaternion & in, geometry_msgs::msg::Quaternion & msg) - { - msg.x = in[0]; - msg.y = in[1]; - msg.z = in[2]; - msg.w = in[3]; - } - - static void fromMsg(const geometry_msgs::msg::Quaternion & msg, btQuaternion & out) - { - out[0] = msg.x; - out[1] = msg.y; - out[2] = msg.z; - out[3] = msg.w; - if (msg.w < 0) {out *= -1;} - } -}; +: DefaultQuaternionImpl {}; template<> struct ImplDetails diff --git a/tf2_eigen/include/tf2_eigen/tf2_eigen.h b/tf2_eigen/include/tf2_eigen/tf2_eigen.h index 21e604507..30ad2805f 100644 --- a/tf2_eigen/include/tf2_eigen/tf2_eigen.h +++ b/tf2_eigen/include/tf2_eigen/tf2_eigen.h @@ -129,43 +129,15 @@ void doTransform( namespace impl { -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. - */ - 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 +: DefaultVectorImpl { }; template <> struct ImplDetails -: public Vector3ImplDetails +: DefaultVectorImpl { }; 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 3a2614780..8dd8b0ae2 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 @@ -74,38 +74,15 @@ namespace impl /** Vector3 **/ /*************/ -template -struct tf2VectorImplDetails -{ - /** \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 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); } -}; - template <> struct ImplDetails -: tf2VectorImplDetails +: DefaultVectorImpl { }; template <> struct ImplDetails -: tf2VectorImplDetails +: DefaultVectorImpl { }; @@ -274,31 +251,7 @@ namespace impl { template <> 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::msg::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::msg::Quaternion & in, tf2::Quaternion & out) - { - // w at the end in the constructor - out = tf2::Quaternion(in.x, in.y, in.z, in.w); - } -}; +: DefaultQuaternionImpl {}; template <> struct defaultMessage diff --git a/tf2_kdl/include/tf2_kdl/tf2_kdl.h b/tf2_kdl/include/tf2_kdl/tf2_kdl.h index 4c0347759..e2a91ca7c 100644 --- a/tf2_kdl/include/tf2_kdl/tf2_kdl.h +++ b/tf2_kdl/include/tf2_kdl/tf2_kdl.h @@ -98,43 +98,15 @@ inline void doTransform( namespace impl { -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. - */ - 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 +: DefaultVectorImpl { }; template <> struct ImplDetails -: KDLVectorImplDetails +: DefaultVectorImpl { }; From d4d17047133f23421672481156023f0b056daa44 Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Fri, 12 Feb 2021 00:17:55 +0000 Subject: [PATCH 16/51] Cleanup of Transform conversions --- tf2_eigen/include/tf2_eigen/tf2_eigen.h | 94 ++++++++++++++----------- tf2_eigen/test/tf2_eigen-test.cpp | 5 +- tf2_kdl/include/tf2_kdl/tf2_kdl.h | 44 ++++++++---- 3 files changed, 83 insertions(+), 60 deletions(-) diff --git a/tf2_eigen/include/tf2_eigen/tf2_eigen.h b/tf2_eigen/include/tf2_eigen/tf2_eigen.h index 30ad2805f..fa0908e05 100644 --- a/tf2_eigen/include/tf2_eigen/tf2_eigen.h +++ b/tf2_eigen/include/tf2_eigen/tf2_eigen.h @@ -43,7 +43,6 @@ namespace tf2 { - /** \brief Convert a timestamped transform to the equivalent Eigen data type. * \param t The transform to convert, as a geometry_msgs Transform message. * \return The transform message converted to an Eigen Isometry3d transform. @@ -51,9 +50,9 @@ namespace tf2 inline Eigen::Isometry3d transformToEigen(const geometry_msgs::msg::Transform & t) { - return Eigen::Isometry3d( - Eigen::Translation3d(t.translation.x, t.translation.y, t.translation.z) * - Eigen::Quaterniond(t.rotation.w, t.rotation.x, t.rotation.y, t.rotation.z)); + Eigen::Isometry3d res; + tf2::fromMsg<>(t, res); + return res; } /** \brief Convert a timestamped transform to the equivalent Eigen data type. @@ -74,16 +73,7 @@ inline geometry_msgs::msg::TransformStamped eigenToTransform(const Eigen::Affine3d & T) { geometry_msgs::msg::TransformStamped t; - t.transform.translation.x = T.translation().x(); - t.transform.translation.y = T.translation().y(); - t.transform.translation.z = T.translation().z(); - - Eigen::Quaterniond q(T.linear()); // assuming that upper 3x3 matrix is orthonormal - t.transform.rotation.x = q.x(); - t.transform.rotation.y = q.y(); - t.transform.rotation.z = q.z(); - t.transform.rotation.w = q.w(); - + tf2::toMsg<>(T, t.transform); return t; } @@ -95,19 +85,47 @@ inline geometry_msgs::msg::TransformStamped eigenToTransform(const Eigen::Isometry3d & T) { geometry_msgs::msg::TransformStamped t; - t.transform.translation.x = T.translation().x(); - t.transform.translation.y = T.translation().y(); - t.transform.translation.z = T.translation().z(); - - Eigen::Quaterniond q(T.rotation()); - t.transform.rotation.x = q.x(); - t.transform.rotation.y = q.y(); - t.transform.rotation.z = q.z(); - t.transform.rotation.w = q.w(); - + tf2::toMsg<>(T, t.transform); return t; } +namespace impl +{ +template +struct EigenTransformImpl +{ + static void fromMsg( + const geometry_msgs::msg::Transform & msg, Eigen::Transform & out) + { + Eigen::Quaterniond q; + Eigen::Vector3d v; + tf2::fromMsg<>(msg.rotation, q); + tf2::fromMsg<>(msg.translation, v); + out = Eigen::Translation3d(v) * q; + } + + static void toMsg( + const Eigen::Transform & in, geometry_msgs::msg::Transform & msg) + { + tf2::toMsg<>(Eigen::Quaterniond(in.linear()), msg.rotation); + tf2::toMsg<>(Eigen::Vector3d(in.translation()), msg.translation); + } +}; + +template <> +struct ImplDetails +: EigenTransformImpl +{ +}; + +template <> +struct ImplDetails +: EigenTransformImpl +{ +}; + +} // 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, * although it can not be used in tf2_ros::BufferInterface::transform because this @@ -283,21 +301,10 @@ struct PoseImplDetails */ static void toMsg(const T & in, geometry_msgs::msg::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; - } + tf2::toMsg<>(Eigen::Vector3d(in.translation()), msg.position); + Eigen::Quaterniond q(in.linear()); + if (q.w() < 0) q.coeffs() *= -1; + tf2::toMsg<>(q, msg.orientation); } /** \brief Convert a Pose message transform type to a Eigen Affine3d. @@ -307,10 +314,11 @@ struct PoseImplDetails */ static void fromMsg(const geometry_msgs::msg::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)); + Eigen::Quaterniond q; + Eigen::Vector3d v; + tf2::fromMsg<>(msg.orientation, q); + tf2::fromMsg<>(msg.position, v); + out = Eigen::Translation3d(v) * q; } }; diff --git a/tf2_eigen/test/tf2_eigen-test.cpp b/tf2_eigen/test/tf2_eigen-test.cpp index 6382b7925..41e1338b3 100644 --- a/tf2_eigen/test/tf2_eigen-test.cpp +++ b/tf2_eigen/test/tf2_eigen-test.cpp @@ -114,7 +114,8 @@ TEST(TfEigen, ConvertTransform) Eigen::Affine3d T(tm); - geometry_msgs::msg::TransformStamped msg = tf2::eigenToTransform(T); + geometry_msgs::msg::Transform msg; + tf2::toMsg(T, msg); Eigen::Affine3d Tback = tf2::transformToEigen(msg); EXPECT_TRUE(T.isApprox(Tback)); @@ -123,7 +124,7 @@ TEST(TfEigen, ConvertTransform) // same for Isometry Eigen::Isometry3d I(tm); - msg = tf2::eigenToTransform(T); + tf2::toMsg(T, msg); Eigen::Isometry3d Iback = tf2::transformToEigen(msg); EXPECT_TRUE(I.isApprox(Iback)); diff --git a/tf2_kdl/include/tf2_kdl/tf2_kdl.h b/tf2_kdl/include/tf2_kdl/tf2_kdl.h index e2a91ca7c..6e15cc697 100644 --- a/tf2_kdl/include/tf2_kdl/tf2_kdl.h +++ b/tf2_kdl/include/tf2_kdl/tf2_kdl.h @@ -55,27 +55,22 @@ namespace tf2 * \param t The transform to convert, as a geometry_msgs TransformedStamped message. * \return The transform message converted to an KDL Frame. */ -inline -KDL::Frame transformToKDL(const geometry_msgs::msg::TransformStamped& t) - { - return KDL::Frame(KDL::Rotation::Quaternion(t.transform.rotation.x, t.transform.rotation.y, - t.transform.rotation.z, t.transform.rotation.w), - KDL::Vector(t.transform.translation.x, t.transform.translation.y, t.transform.translation.z)); - } +inline KDL::Frame transformToKDL(const geometry_msgs::msg::TransformStamped & t) +{ + KDL::Frame ans; + tf2::fromMsg<>(t.transform, ans); + return ans; +} /** \brief Convert an KDL Frame to the equivalent geometry_msgs message type. * \param k The transform to convert, as an KDL Frame. * \return The transform converted to a TransformStamped message. */ -inline -geometry_msgs::msg::TransformStamped kdlToTransform(const KDL::Frame& k) +inline geometry_msgs::msg::TransformStamped kdlToTransform(const KDL::Frame & k) { - geometry_msgs::msg::TransformStamped t; - t.transform.translation.x = k.p.x(); - t.transform.translation.y = k.p.y(); - t.transform.translation.z = k.p.z(); - k.M.GetQuaternion(t.transform.rotation.x, t.transform.rotation.y, t.transform.rotation.z, t.transform.rotation.w); - return t; + geometry_msgs::msg::TransformStamped ans; + tf2::toMsg<>(k, ans.transform); + return ans; } // --------------------- @@ -98,6 +93,25 @@ inline void doTransform( namespace impl { +template <> +struct ImplDetails +{ + static void fromMsg(geometry_msgs::msg::Transform const & in, KDL::Frame & out) + { + KDL::Rotation r; + KDL::Vector v; + tf2::fromMsg<>(in.translation, v); + tf2::fromMsg<>(in.rotation, r); + out = KDL::Frame(r, v); + } + + static void toMsg(KDL::Frame const & in, geometry_msgs::msg::Transform & out) + { + tf2::toMsg<>(in.p, out.translation); + tf2::toMsg<>(in.M, out.rotation); + } +}; + template <> struct ImplDetails : DefaultVectorImpl From 97cd92a26107329fd6db1ac4d1f3b069e496ff35 Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Fri, 12 Feb 2021 00:18:47 +0000 Subject: [PATCH 17/51] Unify doTransform() (except for tf2_geometry_msgs) --- tf2/include/tf2/convert.h | 23 +++- tf2/include/tf2/impl/convert.h | 6 + tf2/include/tf2/impl/forward.h | 4 +- tf2_bullet/include/tf2_bullet/tf2_bullet.h | 51 +++----- tf2_eigen/include/tf2_eigen/tf2_eigen.h | 133 ++++----------------- tf2_kdl/include/tf2_kdl/tf2_kdl.h | 91 ++++---------- 6 files changed, 91 insertions(+), 217 deletions(-) diff --git a/tf2/include/tf2/convert.h b/tf2/include/tf2/convert.h index eb3975029..facb6033d 100644 --- a/tf2/include/tf2/convert.h +++ b/tf2/include/tf2/convert.h @@ -42,11 +42,11 @@ #include "impl/stamped_traits.h" #include "impl/time_convert.h" #include "time.h" +#include "transform_datatypes.h" #include "visibility_control.h" namespace tf2 { - /**\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. @@ -56,10 +56,25 @@ namespace tf2 * * This method needs to be implemented by client library developers */ -template +template +void doTransform( + const T & data_in, T & data_out, const geometry_msgs::msg::TransformStamped & transform) +{ + using TransformType = typename impl::DefaultTransformType::type; + TransformType t; + tf2::fromMsg<>(transform.transform, t); + data_out = t * data_in; +} + +template void doTransform( - const T & data_in, T & data_out, - const geometry_msgs::msg::TransformStamped & transform); + tf2::Stamped const & data_in, tf2::Stamped & data_out, + const geometry_msgs::msg::TransformStamped & transform) +{ + T tmp; + doTransform(static_cast(data_in), tmp, transform); + data_out = tf2::Stamped{tmp, transform.header.stamp, transform.header.frame_id}; +} /**\brief Get the timestamp from data * \param[in] t The data input. diff --git a/tf2/include/tf2/impl/convert.h b/tf2/include/tf2/impl/convert.h index 2b76ea0dd..cd26b37c5 100644 --- a/tf2/include/tf2/impl/convert.h +++ b/tf2/include/tf2/impl/convert.h @@ -55,6 +55,12 @@ struct defaultMessage // using type = ...; }; +template +struct DefaultTransformType +{ + // using type = ...; +}; + /** * \brief Conversion details between a Message and a non-Message datatype. * \tparam Datatype Non-Message datatype like \c Vector3d diff --git a/tf2/include/tf2/impl/forward.h b/tf2/include/tf2/impl/forward.h index 4717b8552..5c3d64f1f 100644 --- a/tf2/include/tf2/impl/forward.h +++ b/tf2/include/tf2/impl/forward.h @@ -37,7 +37,9 @@ namespace impl { template struct defaultMessage; -} +template +struct DefaultTransformType; +} // namespace impl template class Stamped; diff --git a/tf2_bullet/include/tf2_bullet/tf2_bullet.h b/tf2_bullet/include/tf2_bullet/tf2_bullet.h index 39760b239..e8461b844 100644 --- a/tf2_bullet/include/tf2_bullet/tf2_bullet.h +++ b/tf2_bullet/include/tf2_bullet/tf2_bullet.h @@ -84,7 +84,7 @@ struct ImplDetails template<> struct ImplDetails -: DefaultQuaternionImpl {}; + : DefaultQuaternionImpl {}; template<> struct ImplDetails @@ -104,6 +104,24 @@ struct ImplDetails tf2::toMsg<>(in.getOrigin(), out.translation); } }; + +template<> +struct DefaultTransformType +{ + using type = btTransform; +}; + +template<> +struct DefaultTransformType +{ + using type = btTransform; +}; + +template<> +struct DefaultTransformType +{ + using type = btTransform; +}; } // namespace impl /** \brief Convert a timestamped transform to the equivalent Bullet data type. @@ -116,37 +134,6 @@ inline btTransform transformToBullet(const geometry_msgs::msg::TransformStamped fromMsg<>(t.transform, ans); return ans; } - -/** \brief Apply a geometry_msgs TransformStamped to a Bullet-specific Vector3 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 Bullet btVector3 data type. - * \param t_out The transformed vector, as a timestamped Bullet btVector3 data type. - * \param transform The timestamped transform to apply, as a TransformStamped message. - */ -template<> -inline void doTransform( - const tf2::Stamped & t_in, tf2::Stamped & t_out, - const geometry_msgs::msg::TransformStamped & transform) -{ - t_out = tf2::Stamped( - transformToBullet(transform) * t_in, transform.header.stamp, transform.header.frame_id); -} - -/** \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 - * \param t_in The frame to transform, as a timestamped Bullet btTransform. - * \param t_out The transformed frame, as a timestamped Bullet btTransform. - * \param transform The timestamped transform to apply, as a TransformStamped message. - */ -template<> -inline void doTransform( - const tf2::Stamped & t_in, tf2::Stamped & t_out, - const geometry_msgs::msg::TransformStamped & transform) -{ - t_out = tf2::Stamped( - transformToBullet(transform) * t_in, transform.header.stamp, transform.header.frame_id); -} - } // namespace tf2 #endif // TF2_BULLET__TF2_BULLET_H_ diff --git a/tf2_eigen/include/tf2_eigen/tf2_eigen.h b/tf2_eigen/include/tf2_eigen/tf2_eigen.h index fa0908e05..947d3e138 100644 --- a/tf2_eigen/include/tf2_eigen/tf2_eigen.h +++ b/tf2_eigen/include/tf2_eigen/tf2_eigen.h @@ -124,29 +124,6 @@ struct ImplDetails { }; -} // 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, - * although it can not be used in tf2_ros::BufferInterface::transform because this - * functions rely on the existence of a time stamp and a frame id in the type which should - * get transformed. - * \param t_in The vector to transform, as a Eigen Vector3d data type. - * \param t_out The transformed vector, as a Eigen Vector3d data type. - * \param transform The timestamped transform to apply, as a TransformStamped message. - */ -template<> -inline -void doTransform( - const Eigen::Vector3d & t_in, - Eigen::Vector3d & t_out, - const geometry_msgs::msg::TransformStamped & transform) -{ - t_out = Eigen::Vector3d(transformToEigen(transform) * t_in); -} - -namespace impl -{ template <> struct ImplDetails : DefaultVectorImpl @@ -159,61 +136,19 @@ struct ImplDetails { }; -template <> -struct defaultMessage -{ - using type = geometry_msgs::msg::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. - * \param t_in The vector to transform, as a timestamped Eigen Vector3d data type. - * \param t_out The transformed vector, as a timestamped Eigen Vector3d data type. - * \param transform The timestamped transform to apply, as a TransformStamped message. - */ -template<> -inline void doTransform( - const tf2::Stamped & t_in, tf2::Stamped & t_out, - const geometry_msgs::msg::TransformStamped & transform) -{ - t_out = tf2::Stamped( - transformToEigen(transform) * t_in, - transform.header.stamp, - transform.header.frame_id); -} -/** \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 - * function relies on the existence of a time stamp and a frame id in the type which should - * get transformed. - * \param t_in The frame to transform, as a Eigen Affine3d transform. - * \param t_out The transformed frame, as a Eigen Affine3d transform. - * \param transform The timestamped transform to apply, as a TransformStamped message. - */ template<> -inline -void doTransform( - const Eigen::Affine3d & t_in, - Eigen::Affine3d & t_out, - const geometry_msgs::msg::TransformStamped & transform) +struct DefaultTransformType { - t_out = Eigen::Affine3d(transformToEigen(transform) * t_in); -} + using type = Eigen::Isometry3d; +}; -template<> -inline -void doTransform( - const Eigen::Isometry3d & t_in, - Eigen::Isometry3d & t_out, - const geometry_msgs::msg::TransformStamped & transform) +template <> +struct defaultMessage { - t_out = Eigen::Isometry3d(transformToEigen(transform) * t_in); -} + using type = geometry_msgs::msg::Point; +}; -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. @@ -414,6 +349,19 @@ struct ImplDetails, geometry_msgs::msg::Wrench> } }; +template<> +struct DefaultTransformType +{ + using type = Eigen::Isometry3d; +}; + + +template<> +struct DefaultTransformType +{ + using type = Eigen::Isometry3d; +}; + } // namespace impl /** \brief Convert a Eigen::Vector3d type to a geometry_msgs::msg::Vector3. @@ -432,47 +380,6 @@ geometry_msgs::msg::Vector3 toMsg2(const Eigen::Vector3d & in) msg.z = in(2); return msg; } - -/** \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, - * although it can not be used in tf2_ros::BufferInterface::transform because this - * function relies on the existence of a time stamp and a frame id in the type which should - * get transformed. - * \param t_in The frame to transform, as a timestamped Eigen Affine3d transform. - * \param t_out The transformed frame, as a timestamped Eigen Affine3d transform. - * \param transform The timestamped transform to apply, as a TransformStamped message. - */ -template<> -inline void doTransform( - const tf2::Stamped & t_in, - tf2::Stamped & t_out, - const geometry_msgs::msg::TransformStamped & transform) -{ - t_out = tf2::Stamped( - transformToEigen(transform) * t_in, - transform.header.stamp,transform.header.frame_id); -} - -/** \brief Apply a geometry_msgs TransformStamped to an Eigen Isometry 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 - * function relies on the existence of a time stamp and a frame id in the type which should - * get transformed. - * \param t_in The frame to transform, as a timestamped Eigen Isometry transform. - * \param t_out The transformed frame, as a timestamped Eigen Isometry transform. - * \param transform The timestamped transform to apply, as a TransformStamped message. - */ -template<> -inline void doTransform( - const tf2::Stamped & t_in, - tf2::Stamped & t_out, - const geometry_msgs::msg::TransformStamped & transform) -{ - t_out = tf2::Stamped( - transformToEigen(transform) * t_in, transform.header.stamp, - transform.header.frame_id); -} - } // namespace tf2 #endif // TF2_EIGEN_H diff --git a/tf2_kdl/include/tf2_kdl/tf2_kdl.h b/tf2_kdl/include/tf2_kdl/tf2_kdl.h index 6e15cc697..fbd4e34a3 100644 --- a/tf2_kdl/include/tf2_kdl/tf2_kdl.h +++ b/tf2_kdl/include/tf2_kdl/tf2_kdl.h @@ -73,24 +73,6 @@ inline geometry_msgs::msg::TransformStamped kdlToTransform(const KDL::Frame & k) return ans; } -// --------------------- -// Vector -// --------------------- -/** \brief Apply a geometry_msgs TransformStamped to an KDL-specific Vector 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 KDL Vector data type. - * \param t_out The transformed vector, as a timestamped KDL Vector data type. - * \param transform The timestamped transform to apply, as a TransformStamped message. - */ -template <> -inline void doTransform( - const tf2::Stamped & t_in, tf2::Stamped & t_out, - const geometry_msgs::msg::TransformStamped & transform) -{ - t_out = tf2::Stamped( - transformToKDL(transform) * t_in, transform.header.stamp, transform.header.frame_id); -} - namespace impl { template <> @@ -130,27 +112,16 @@ struct defaultMessage using type = geometry_msgs::msg::Point; }; -} // namespace impl +template <> +struct DefaultTransformType +{ + using type = KDL::Frame; +}; // --------------------- // Twist // --------------------- -/** \brief Apply a geometry_msgs TransformStamped to an KDL-specific Twist type. - * This function is a specialization of the doTransform template defined in tf2/convert.h. - * \param t_in The twist to transform, as a timestamped KDL Twist data type. - * \param t_out The transformed Twist, as a timestamped KDL Frame data type. - * \param transform The timestamped transform to apply, as a TransformStamped message. - */ -template <> -inline void doTransform( - const tf2::Stamped & t_in, tf2::Stamped & t_out, - const geometry_msgs::msg::TransformStamped & transform) -{ - t_out = tf2::Stamped( - transformToKDL(transform) * t_in, transform.header.stamp, transform.header.frame_id); -} -namespace impl -{ + template <> struct ImplDetails { @@ -177,33 +148,22 @@ struct ImplDetails } }; +template <> +struct DefaultTransformType +{ + using type = KDL::Frame; +}; + template <> struct defaultMessage { using type = geometry_msgs::msg::Twist; }; -} // namespace impl // --------------------- // Wrench // --------------------- -/** \brief Apply a geometry_msgs TransformStamped to an KDL-specific Wrench type. - * This function is a specialization of the doTransform template defined in tf2/convert.h. - * \param t_in The wrench to transform, as a timestamped KDL Wrench data type. - * \param t_out The transformed Wrench, as a timestamped KDL Frame data type. - * \param transform The timestamped transform to apply, as a TransformStamped message. - */ -template <> -inline void doTransform( - const tf2::Stamped & t_in, tf2::Stamped & t_out, - const geometry_msgs::msg::TransformStamped & transform) -{ - t_out = tf2::Stamped( - transformToKDL(transform) * t_in, transform.header.stamp, transform.header.frame_id); -} -namespace impl -{ template <> struct ImplDetails { @@ -235,6 +195,12 @@ struct defaultMessage { using type = geometry_msgs::msg::Wrench; }; + +template <> +struct DefaultTransformType +{ + using type = KDL::Frame; +}; } // namespace impl template @@ -248,21 +214,6 @@ void convert(Eigen::Matrix const & in, KDL::W // --------------------- // Frame // --------------------- -/** \brief Apply a geometry_msgs TransformStamped to a KDL-specific Frame data type. - * This function is a specialization of the doTransform template defined in tf2/convert.h. - * \param t_in The frame to transform, as a timestamped KDL Frame. - * \param t_out The transformed frame, as a timestamped KDL Frame. - * \param transform The timestamped transform to apply, as a TransformStamped message. - */ -template <> -inline void doTransform( - const tf2::Stamped & t_in, tf2::Stamped & t_out, - const geometry_msgs::msg::TransformStamped & transform) -{ - t_out = tf2::Stamped( - transformToKDL(transform) * t_in, transform.header.stamp, transform.header.frame_id); -} - namespace impl { template <> @@ -297,6 +248,12 @@ struct defaultMessage using type = geometry_msgs::msg::Pose; }; +template <> +struct DefaultTransformType +{ + using type = KDL::Frame; +}; + template <> struct ImplDetails { From b0504654e5e3b3ec5460e027f369868b8888f7b7 Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Tue, 26 Jan 2021 22:29:53 +0000 Subject: [PATCH 18/51] doTransformation for tf2_geometry_msgs --- .../tf2_geometry_msgs/tf2_geometry_msgs.h | 199 +++++++++++------- 1 file changed, 126 insertions(+), 73 deletions(-) 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 8dd8b0ae2..e60e1441a 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 @@ -92,6 +92,21 @@ struct defaultMessage using type = geometry_msgs::msg::Vector3; }; +template <> +struct DefaultTransformType +{ + using type = tf2::Transform; +}; + +template +inline void doTransformWithTf( + const Message & in, Message & out, const geometry_msgs::msg::TransformStamped & transform) +{ + TFDatatype in_tf, out_tf; + tf2::convert(in, in_tf); + tf2::doTransform(in_tf, out_tf, transform); + tf2::convert(out_tf, out); +} } // namespace impl /********************/ @@ -105,17 +120,28 @@ struct defaultMessage * \param transform The timestamped transform to apply, as a TransformStamped message. */ template <> -inline - void doTransform(const geometry_msgs::msg::Vector3Stamped& t_in, geometry_msgs::msg::Vector3Stamped& t_out, const geometry_msgs::msg::TransformStamped& transform) - { - KDL::Vector v_out = gmTransformToKDL(transform).M * KDL::Vector(t_in.vector.x, t_in.vector.y, t_in.vector.z); - t_out.vector.x = v_out[0]; - t_out.vector.y = v_out[1]; - t_out.vector.z = v_out[2]; - t_out.header.stamp = transform.header.stamp; - t_out.header.frame_id = transform.header.frame_id; - } +inline void doTransform( + const geometry_msgs::msg::Vector3Stamped & t_in, geometry_msgs::msg::Vector3Stamped & t_out, + const geometry_msgs::msg::TransformStamped & transform) +{ + impl::doTransformWithTf(t_in.vector, t_out.vector, transform); + t_out.header.stamp = transform.header.stamp; + t_out.header.frame_id = transform.header.frame_id; +} +/** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Vector 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 Vector3 message. + * \param t_out The transformed vector, as a timestamped Vector3 message. + * \param transform The timestamped transform to apply, as a TransformStamped message. + */ +template <> +inline void doTransform( + const geometry_msgs::msg::Vector3 & t_in, geometry_msgs::msg::Vector3 & t_out, + const geometry_msgs::msg::TransformStamped & transform) +{ + impl::doTransformWithTf(t_in, t_out, transform); +} /******************/ /** PointStamped **/ @@ -128,16 +154,28 @@ inline * \param transform The timestamped transform to apply, as a TransformStamped message. */ template <> -inline - void doTransform(const geometry_msgs::msg::PointStamped& t_in, geometry_msgs::msg::PointStamped& t_out, const geometry_msgs::msg::TransformStamped& transform) - { - KDL::Vector v_out = gmTransformToKDL(transform) * KDL::Vector(t_in.point.x, t_in.point.y, t_in.point.z); - t_out.point.x = v_out[0]; - t_out.point.y = v_out[1]; - t_out.point.z = v_out[2]; - t_out.header.stamp = transform.header.stamp; - t_out.header.frame_id = transform.header.frame_id; - } +inline void doTransform( + const geometry_msgs::msg::PointStamped & t_in, geometry_msgs::msg::PointStamped & t_out, + const geometry_msgs::msg::TransformStamped & transform) +{ + impl::doTransformWithTf(t_in.point, t_out.point, transform); + t_out.header.stamp = transform.header.stamp; + t_out.header.frame_id = transform.header.frame_id; +} + +/** \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. + * \param t_in The point to transform, as a timestamped Point3 message. + * \param t_out The transformed point, as a timestamped Point3 message. + * \param transform The timestamped transform to apply, as a TransformStamped message. + */ +template <> +inline void doTransform( + const geometry_msgs::msg::Point & t_in, geometry_msgs::msg::Point & t_out, + const geometry_msgs::msg::TransformStamped & transform) +{ + impl::doTransformWithTf(t_in, t_out, transform); +} /*****************/ /** PoseStamped **/ @@ -150,21 +188,28 @@ inline * \param transform The timestamped transform to apply, as a TransformStamped message. */ template <> -inline - void doTransform(const geometry_msgs::msg::PoseStamped& t_in, geometry_msgs::msg::PoseStamped& t_out, const geometry_msgs::msg::TransformStamped& transform) - { - KDL::Vector v(t_in.pose.position.x, t_in.pose.position.y, t_in.pose.position.z); - KDL::Rotation r = KDL::Rotation::Quaternion(t_in.pose.orientation.x, t_in.pose.orientation.y, t_in.pose.orientation.z, t_in.pose.orientation.w); - - KDL::Frame v_out = gmTransformToKDL(transform) * KDL::Frame(r, v); - t_out.pose.position.x = v_out.p[0]; - t_out.pose.position.y = v_out.p[1]; - t_out.pose.position.z = v_out.p[2]; - v_out.M.GetQuaternion(t_out.pose.orientation.x, t_out.pose.orientation.y, t_out.pose.orientation.z, t_out.pose.orientation.w); - t_out.header.stamp = transform.header.stamp; - t_out.header.frame_id = transform.header.frame_id; - } +inline void doTransform( + const geometry_msgs::msg::PoseStamped & t_in, geometry_msgs::msg::PoseStamped & t_out, + const geometry_msgs::msg::TransformStamped & transform) +{ + impl::doTransformWithTf(t_in.pose, t_out.pose, transform); + t_out.header.stamp = transform.header.stamp; + t_out.header.frame_id = transform.header.frame_id; +} +/** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Pose type. + * This function is a specialization of the doTransform template defined in tf2/convert.h. + * \param t_in The pose to transform, as a timestamped Pose3 message. + * \param t_out The transformed pose, as a timestamped Pose3 message. + * \param transform The timestamped transform to apply, as a TransformStamped message. + */ +template <> +inline void doTransform( + const geometry_msgs::msg::Pose & t_in, geometry_msgs::msg::Pose & t_out, + const geometry_msgs::msg::TransformStamped & transform) +{ + impl::doTransformWithTf(t_in, t_out, transform); +} /*******************************/ /** PoseWithCovarianceStamped **/ @@ -186,22 +231,16 @@ inline * \param transform The timestamped transform to apply, as a TransformStamped message. */ template <> -inline - void doTransform(const geometry_msgs::msg::PoseWithCovarianceStamped& t_in, geometry_msgs::msg::PoseWithCovarianceStamped& t_out, const geometry_msgs::msg::TransformStamped& transform) - { - KDL::Vector v(t_in.pose.pose.position.x, t_in.pose.pose.position.y, t_in.pose.pose.position.z); - KDL::Rotation r = KDL::Rotation::Quaternion(t_in.pose.pose.orientation.x, t_in.pose.pose.orientation.y, t_in.pose.pose.orientation.z, t_in.pose.pose.orientation.w); - - KDL::Frame v_out = gmTransformToKDL(transform) * KDL::Frame(r, v); - t_out.pose.pose.position.x = v_out.p[0]; - t_out.pose.pose.position.y = v_out.p[1]; - t_out.pose.pose.position.z = v_out.p[2]; - v_out.M.GetQuaternion(t_out.pose.pose.orientation.x, t_out.pose.pose.orientation.y, t_out.pose.pose.orientation.z, t_out.pose.pose.orientation.w); - t_out.header.stamp = transform.header.stamp; - t_out.header.frame_id = transform.header.frame_id; - t_out.pose.covariance = t_in.pose.covariance; - } - +inline void doTransform( + const geometry_msgs::msg::PoseWithCovarianceStamped & t_in, + geometry_msgs::msg::PoseWithCovarianceStamped & t_out, + const geometry_msgs::msg::TransformStamped & transform) +{ + impl::doTransformWithTf(t_in.pose.pose, t_out.pose.pose, transform); + t_out.header.stamp = transform.header.stamp; + t_out.header.frame_id = transform.header.frame_id; + t_out.pose.covariance = t_in.pose.covariance; +} /** \brief Convert a tf2 TransformWithCovarianceStamped type to its equivalent geometry_msgs representation. * This function is a specialization of the toMsg template defined in tf2/convert.h. @@ -259,11 +298,30 @@ struct defaultMessage using type = geometry_msgs::msg::Quaternion; }; +template <> +struct DefaultTransformType +{ + using type = tf2::Transform; +}; + } // namespace impl /***********************/ /** QuaternionStamped **/ /***********************/ +/** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Quaternion type. + * This function is a specialization of the doTransform template defined in tf2/convert.h. + * \param t_in The quaternion to transform, as a timestamped Quaternion3 message. + * \param t_out The transformed quaternion, as a timestamped Quaternion3 message. + * \param transform The timestamped transform to apply, as a TransformStamped message. + */ +template <> +inline void doTransform( + const geometry_msgs::msg::Quaternion & t_in, geometry_msgs::msg::Quaternion & t_out, + const geometry_msgs::msg::TransformStamped & transform) +{ + impl::doTransformWithTf(t_in, t_out, transform); +} /** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Quaternion type. * This function is a specialization of the doTransform template defined in tf2/convert.h. @@ -272,13 +330,11 @@ struct defaultMessage * \param transform The timestamped transform to apply, as a TransformStamped message. */ template <> -inline -void doTransform(const geometry_msgs::msg::QuaternionStamped& t_in, geometry_msgs::msg::QuaternionStamped& t_out, const geometry_msgs::msg::TransformStamped& transform) +inline void doTransform( + const geometry_msgs::msg::QuaternionStamped & t_in, geometry_msgs::msg::QuaternionStamped & t_out, + const geometry_msgs::msg::TransformStamped & transform) { - tf2::Quaternion q_out = tf2::Quaternion(transform.transform.rotation.x, transform.transform.rotation.y, - transform.transform.rotation.z, transform.transform.rotation.w)* - tf2::Quaternion(t_in.quaternion.x, t_in.quaternion.y, t_in.quaternion.z, t_in.quaternion.w); - toMsg<>(q_out, t_out.quaternion); + impl::doTransformWithTf(t_in.quaternion, t_out.quaternion, transform); t_out.header.stamp = transform.header.stamp; t_out.header.frame_id = transform.header.frame_id; } @@ -334,23 +390,14 @@ struct defaultMessage * \param transform The timestamped transform to apply, as a TransformStamped message. */ template <> -inline -void doTransform(const geometry_msgs::msg::TransformStamped& t_in, geometry_msgs::msg::TransformStamped& t_out, const geometry_msgs::msg::TransformStamped& transform) - { - KDL::Vector v(t_in.transform.translation.x, t_in.transform.translation.y, - t_in.transform.translation.z); - KDL::Rotation r = KDL::Rotation::Quaternion(t_in.transform.rotation.x, - t_in.transform.rotation.y, t_in.transform.rotation.z, t_in.transform.rotation.w); - - KDL::Frame v_out = gmTransformToKDL(transform) * KDL::Frame(r, v); - t_out.transform.translation.x = v_out.p[0]; - t_out.transform.translation.y = v_out.p[1]; - t_out.transform.translation.z = v_out.p[2]; - v_out.M.GetQuaternion(t_out.transform.rotation.x, t_out.transform.rotation.y, - t_out.transform.rotation.z, t_out.transform.rotation.w); - t_out.header.stamp = transform.header.stamp; - t_out.header.frame_id = transform.header.frame_id; - } +inline void doTransform( + const geometry_msgs::msg::TransformStamped & t_in, geometry_msgs::msg::TransformStamped & t_out, + const geometry_msgs::msg::TransformStamped & transform) +{ + impl::doTransformWithTf(t_in.transform, t_out.transform, transform); + t_out.header.stamp = transform.header.stamp; + t_out.header.frame_id = transform.header.frame_id; +} namespace impl { @@ -384,6 +431,12 @@ struct ImplDetails } }; +template <> +struct DefaultTransformType +{ + using type = tf2::Transform; +}; + /************/ /** Wrench **/ /************/ From e68416310366a326ad56eb53938ab3080ff31262 Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Tue, 26 Jan 2021 22:59:01 +0000 Subject: [PATCH 19/51] Fix Vector transform --- .../tf2_geometry_msgs/tf2_geometry_msgs.h | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) 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 e60e1441a..34bbb3de5 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 @@ -121,12 +121,15 @@ inline void doTransformWithTf( */ template <> inline void doTransform( - const geometry_msgs::msg::Vector3Stamped & t_in, geometry_msgs::msg::Vector3Stamped & t_out, + const geometry_msgs::msg::Vector3 & t_in, geometry_msgs::msg::Vector3 & t_out, const geometry_msgs::msg::TransformStamped & transform) { - impl::doTransformWithTf(t_in.vector, t_out.vector, transform); - t_out.header.stamp = transform.header.stamp; - t_out.header.frame_id = transform.header.frame_id; + tf2::Vector3 in_tf; + tf2::Transform transform_tf; + tf2::convert(t_in, in_tf); + tf2::convert(transform.transform, transform_tf); + const tf2::Vector3 out_tf = transform_tf.getBasis() * in_tf; + tf2::convert(out_tf, t_out); } /** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Vector type. @@ -137,10 +140,12 @@ inline void doTransform( */ template <> inline void doTransform( - const geometry_msgs::msg::Vector3 & t_in, geometry_msgs::msg::Vector3 & t_out, + const geometry_msgs::msg::Vector3Stamped & t_in, geometry_msgs::msg::Vector3Stamped & t_out, const geometry_msgs::msg::TransformStamped & transform) { - impl::doTransformWithTf(t_in, t_out, transform); + doTransform<>(t_in.vector, t_out.vector, transform); + t_out.header.stamp = transform.header.stamp; + t_out.header.frame_id = transform.header.frame_id; } /******************/ From 2ed6b8f004217f2d20290bb8e57d4cb20e02f39a Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Wed, 27 Jan 2021 11:15:36 +0000 Subject: [PATCH 20/51] Add WithCovarianceStamped test --- .../test/test_tf2_geometry_msgs.cpp | 46 +++++++++++++++++++ 1 file changed, 46 insertions(+) diff --git a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp index 2746d0b46..dfd4c5e56 100644 --- a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp +++ b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp @@ -105,6 +105,52 @@ TEST(TfGeometry, Conversions) EXPECT_NEAR(tf_from_msg.getOrigin().getZ(), tf_stamped_msg.transform.translation.z, EPS); EXPECT_EQ(tf_from_msg.frame_id_, tf_stamped_msg.header.frame_id); } + { + geometry_msgs::msg::PoseWithCovarianceStamped v1; + v1.pose.pose.position.x = 1; + v1.pose.pose.position.y = 2; + v1.pose.pose.position.z = 3; + v1.pose.pose.orientation.w = 0; + v1.pose.pose.orientation.x = 1; + v1.pose.pose.orientation.y = 0; + v1.pose.pose.orientation.z = 0; + v1.header.stamp = tf2_ros::toMsg(tf2::timeFromSec(2)); + v1.header.frame_id = "A"; + v1.pose.covariance = { + 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, + 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, + 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, + 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, + 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, + 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, + }; + const std::array, 6> cov{ + std::array{1.0, 2.0, 3.0, 4.0, 5.0, 6.0}, + std::array {1.0, 2.0, 3.0, 4.0, 5.0, 6.0}, + std::array {1.0, 2.0, 3.0, 4.0, 5.0, 6.0}, + std::array {1.0, 2.0, 3.0, 4.0, 5.0, 6.0}, + std::array {1.0, 2.0, 3.0, 4.0, 5.0, 6.0}, + std::array {1.0, 2.0, 3.0, 4.0, 5.0, 6.0}, + }; + + EXPECT_EQ(tf2::getCovarianceMatrix(v1), cov); + + tf2::WithCovarianceStamped tf_from_msg; + tf2::convert(v1, tf_from_msg); + EXPECT_EQ(tf_from_msg.getOrigin(), tf2::Vector3(1.0, 2.0, 3.0)); + EXPECT_EQ(tf_from_msg.getRotation(), tf2::Quaternion(1.0, 0.0, 0.0, 0.0)); + EXPECT_EQ(tf_from_msg.cov_mat_, cov); + EXPECT_EQ(tf_from_msg.frame_id_, "A"); + EXPECT_EQ(tf_from_msg.stamp_, tf2::timeFromSec(2)); + + tf_from_msg.setRotation({0.0, 0.0, 0.0, 1.0}); + v1.pose.pose.orientation.w = 1; + v1.pose.pose.orientation.x = 0; + + geometry_msgs::msg::PoseWithCovarianceStamped v2; + tf2::convert(tf_from_msg, v2); + EXPECT_EQ(v1, v2); + } } TEST(TfGeometry, Frame) From bf2c151d2167a457040e6e045005be6f339fcef1 Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Wed, 27 Jan 2021 11:41:42 +0000 Subject: [PATCH 21/51] Generalize WithCovariance conversion --- tf2/include/tf2/convert.h | 6 ++- tf2/include/tf2/impl/convert.h | 33 +++++++++++++ tf2/include/tf2/impl/forward.h | 7 +++ tf2/include/tf2/impl/stamped_traits.h | 39 +++++++++++---- .../tf2_geometry_msgs/tf2_geometry_msgs.h | 49 ------------------- 5 files changed, 75 insertions(+), 59 deletions(-) diff --git a/tf2/include/tf2/convert.h b/tf2/include/tf2/convert.h index facb6033d..9a87d5f0e 100644 --- a/tf2/include/tf2/convert.h +++ b/tf2/include/tf2/convert.h @@ -101,7 +101,11 @@ inline std::string getFrameId(const T & t) * \return The covariance matrix associated with the data. */ template -std::array, 6> getCovarianceMatrix(const T & t); +std::array, 6> getCovarianceMatrix(const T & t) +{ + using traits = impl::stampedMessageTraits; + return covarianceRowMajorToNested(traits::getCovariance(t)); +} /**\brief Get the covariance matrix from data * diff --git a/tf2/include/tf2/impl/convert.h b/tf2/include/tf2/impl/convert.h index cd26b37c5..aac076c3a 100644 --- a/tf2/include/tf2/impl/convert.h +++ b/tf2/include/tf2/impl/convert.h @@ -162,6 +162,13 @@ struct defaultMessage> using type = typename unstampedMessageTraits::type>::stampedType; }; +template +struct defaultMessage> +{ + using type = + typename unstampedMessageTraits::type>::stampedTypeWithCovariance; +}; + /** * \brief Partial specialization of impl::ImplDetails for stamped types * @@ -193,6 +200,32 @@ struct ImplDetails, StampedMessage> } }; +template +struct ImplDetails, CovarianceStampedMessage> +{ + using traits = stampedMessageTraits; + static void toMsg(tf2::WithCovarianceStamped const & in, CovarianceStampedMessage & out) + { + CovarianceStampedMessage tmp; + tf2::toMsg<>(in.stamp_, tmp.header.stamp); + tmp.header.frame_id = in.frame_id_; + traits::accessCovariance(tmp) = tf2::covarianceNestedToRowMajor(in.cov_mat_); + tf2::toMsg<>(static_cast(in), traits::accessMessage(tmp)); + out = std::move(tmp); + } + + static void fromMsg( + CovarianceStampedMessage const & in, tf2::WithCovarianceStamped & out) + { + tf2::WithCovarianceStamped tmp; + tf2::fromMsg<>(in.header.stamp, tmp.stamp_); + tf2::fromMsg<>(traits::getMessage(in), static_cast(tmp)); + tmp.frame_id_ = in.header.frame_id; + tmp.cov_mat_ = tf2::covarianceRowMajorToNested(traits::getCovariance(in)); + out = std::move(tmp); + } +}; + template class Converter { diff --git a/tf2/include/tf2/impl/forward.h b/tf2/include/tf2/impl/forward.h index 5c3d64f1f..d3728b8df 100644 --- a/tf2/include/tf2/impl/forward.h +++ b/tf2/include/tf2/impl/forward.h @@ -31,6 +31,8 @@ #ifndef TF2__IMPL__FORWARD_H_ #define TF2__IMPL__FORWARD_H_ +#include + namespace tf2 { namespace impl @@ -56,6 +58,11 @@ B toMsg(const A & a); template void fromMsg(const A & a, B & b); +std::array, 6> covarianceRowMajorToNested( + const std::array & row_major); +std::array covarianceNestedToRowMajor( + const std::array, 6> & nested_array); + namespace impl { template diff --git a/tf2/include/tf2/impl/stamped_traits.h b/tf2/include/tf2/impl/stamped_traits.h index 9bf94ef16..c6363adda 100644 --- a/tf2/include/tf2/impl/stamped_traits.h +++ b/tf2/include/tf2/impl/stamped_traits.h @@ -77,6 +77,8 @@ template class Vector3_; template class Vector3Stamped_; +template +class TwistWithCovarianceStamped_; } // namespace msg } // namespace geometry_msgs @@ -107,12 +109,14 @@ template struct unstampedMessageTraits> { using stampedType = geometry_msgs::msg::PoseStamped_; + using stampedTypeWithCovariance = geometry_msgs::msg::PoseWithCovarianceStamped_; }; template struct unstampedMessageTraits> { using stampedType = geometry_msgs::msg::TwistStamped_; + using stampedTypeWithCovariance = geometry_msgs::msg::TwistWithCovarianceStamped_; }; template @@ -190,15 +194,6 @@ struct stampedMessageTraits> { }; -template -struct stampedMessageTraits> -: defaultStampedMessageTraits< - geometry_msgs::msg::PoseWithCovarianceStamped_, - geometry_msgs::msg::PoseWithCovariance_, - &geometry_msgs::msg::PoseWithCovarianceStamped_::pose> -{ -}; - template struct stampedMessageTraits> : defaultStampedMessageTraits< @@ -223,6 +218,32 @@ struct stampedMessageTraits> { }; +template +struct stampedMessageTraits> +{ + using unstampedType = geometry_msgs::msg::Pose_; + using stampedTypeWithCovariance = geometry_msgs::msg::PoseWithCovarianceStamped_; + using covarianceType = std::array; + + static unstampedType & accessMessage(stampedTypeWithCovariance & stamped) { return stamped.pose.pose; } + static unstampedType getMessage(stampedTypeWithCovariance const & stamped) { return stamped.pose.pose; } + static covarianceType & accessCovariance(stampedTypeWithCovariance & stamped) { return stamped.pose.covariance; } + static covarianceType getCovariance(stampedTypeWithCovariance const & stamped) { return stamped.pose.covariance; } +}; + +template +struct stampedMessageTraits> +{ + using unstampedType = geometry_msgs::msg::Twist_; + using stampedTypeWithCovariance = geometry_msgs::msg::TwistWithCovarianceStamped_; + using covarianceType = std::array; + + static unstampedType & accessMessage(stampedTypeWithCovariance & stamped) { return stamped.twist.twist; } + static unstampedType getMessage(stampedTypeWithCovariance const & stamped) { return stamped.twist.twist; } + static covarianceType & accessCovariance(stampedTypeWithCovariance & stamped) { return stamped.twist.covariance; } + static covarianceType getCovariance(stampedTypeWithCovariance const & stamped) { return stamped.twist.covariance; } +}; + } // namespace impl } // namespace tf2 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 34bbb3de5..38d141667 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 @@ -220,15 +220,6 @@ inline void doTransform( /** PoseWithCovarianceStamped **/ /*******************************/ -/** \brief Extract a covariance matrix from a PoseWithCovarianceStamped message. - * This function is a specialization of the getCovarianceMatrix template defined in tf2/convert.h. - * \param t PoseWithCovarianceStamped message to extract the covariance matrix from. - * \return A nested-array representation of the covariance matrix from the message. - */ -template <> -inline - std::array, 6> getCovarianceMatrix(const geometry_msgs::msg::PoseWithCovarianceStamped& t) {return covarianceRowMajorToNested(t.pose.covariance);} - /** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Pose type. * This function is a specialization of the doTransform template defined in tf2/convert.h. * \param t_in The pose to transform, as a timestamped Pose3 message with covariance. @@ -247,46 +238,6 @@ inline void doTransform( t_out.pose.covariance = t_in.pose.covariance; } -/** \brief Convert a tf2 TransformWithCovarianceStamped type to its equivalent geometry_msgs representation. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param in A instance of the tf2::Transform specialization of the tf2::WithCovarianceStamped template. - * \return The TransformWithCovarianceStamped converted to a geometry_msgs PoseStamped message type. - */ -template<> -inline -geometry_msgs::msg::PoseWithCovarianceStamped toMsg(const tf2::WithCovarianceStamped& in) -{ - geometry_msgs::msg::PoseWithCovarianceStamped out; - tf2::toMsg<>(in.stamp_, out.header.stamp); - out.header.frame_id = in.frame_id_; - out.pose.covariance = covarianceNestedToRowMajor(in.cov_mat_); - out.pose.pose.orientation.x = in.getRotation().getX(); - out.pose.pose.orientation.y = in.getRotation().getY(); - out.pose.pose.orientation.z = in.getRotation().getZ(); - out.pose.pose.orientation.w = in.getRotation().getW(); - out.pose.pose.position.x = in.getOrigin().getX(); - out.pose.pose.position.y = in.getOrigin().getY(); - out.pose.pose.position.z = in.getOrigin().getZ(); - return out; -} - -/** \brief Convert a PoseWithCovarianceStamped message to its equivalent tf2 representation. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param in A PoseWithCovarianceStamped message type. - * \param out The PoseWithCovarianceStamped converted to the equivalent tf2 type. - */ -template<> -inline -void fromMsg(const geometry_msgs::msg::PoseWithCovarianceStamped& in, tf2::WithCovarianceStamped& out) -{ - tf2::fromMsg<>(in.header.stamp, out.stamp_); - out.frame_id_ = in.header.frame_id; - out.cov_mat_ = covarianceRowMajorToNested(in.pose.covariance); - tf2::Transform tmp; - fromMsg<>(in.pose.pose, tmp); - out.setData(tmp); -} - /****************/ /** Quaternion **/ /****************/ From 983a6058b651a80f220ca66b0169b998e86d52d8 Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Thu, 28 Jan 2021 17:36:28 +0000 Subject: [PATCH 22/51] doTransform() for KDL::Rotation --- tf2_kdl/include/tf2_kdl/tf2_kdl.h | 17 +++++++++++++++++ tf2_kdl/test/test_tf2_kdl.cpp | 27 +++++++++++++++++++++++++++ 2 files changed, 44 insertions(+) diff --git a/tf2_kdl/include/tf2_kdl/tf2_kdl.h b/tf2_kdl/include/tf2_kdl/tf2_kdl.h index fbd4e34a3..1a349ee36 100644 --- a/tf2_kdl/include/tf2_kdl/tf2_kdl.h +++ b/tf2_kdl/include/tf2_kdl/tf2_kdl.h @@ -276,6 +276,23 @@ struct defaultMessage } // namespace impl +/** + * \brief Transform a KDL::Rotation + * + * \param[in] data_in The data to be transformed. + * \param[in,out] data_out A reference to the output data. + * \param[in] transform The transform to apply to data_in to fill data_out. + */ +template<> +void doTransform( + const KDL::Rotation & in, KDL::Rotation & out, + const geometry_msgs::msg::TransformStamped & transform) +{ + KDL::Rotation t; + tf2::fromMsg<>(transform.transform.rotation, t); + out = t * in; +} + } // namespace tf2 #endif // TF2_KDL_H diff --git a/tf2_kdl/test/test_tf2_kdl.cpp b/tf2_kdl/test/test_tf2_kdl.cpp index d8d89c0ff..b8923e261 100644 --- a/tf2_kdl/test/test_tf2_kdl.cpp +++ b/tf2_kdl/test/test_tf2_kdl.cpp @@ -96,6 +96,32 @@ TEST(TfKDL, Vector) EXPECT_NEAR(v_advanced[2], 27, EPS); } +TEST(TfKDL, Quaternion) +{ + const tf2::Stamped q1{KDL::Rotation::Rot({0, 1, 0}, -0.5 * M_PI), tf2::timeFromSec( + 2.0), "A"}; + + // simple api + const tf2::Stamped q_simple = + tf_buffer->transform(q1, "B", tf2::durationFromSec(2.0)); + double x, y, z, w; + q_simple.GetQuaternion(x, y, z, w); + EXPECT_NEAR(x, 0.707107, EPS); + EXPECT_NEAR(y, 0, EPS); + EXPECT_NEAR(z, -0.707107, EPS); + EXPECT_NEAR(w, 0, EPS); + + // advanced api + const tf2::Stamped q_advanced = tf_buffer->transform( + q1, "B", tf2::timeFromSec(2.0), + "A", tf2::durationFromSec(3.0)); + q_advanced.GetQuaternion(x, y, z, w); + EXPECT_NEAR(x, 0.707107, EPS); + EXPECT_NEAR(y, 0, EPS); + EXPECT_NEAR(z, -0.707107, EPS); + EXPECT_NEAR(w, 0, EPS); +} + TEST(TfKDL, ConvertVector) { tf2::Stamped v( @@ -219,6 +245,7 @@ int main(int argc, char **argv){ rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); tf_buffer = std::make_unique(clock); + tf_buffer->setUsingDedicatedThread(true); // populate buffer geometry_msgs::msg::TransformStamped t; From 2d477664de7b74afcf671b7724b4913347105d87 Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Thu, 28 Jan 2021 18:23:59 +0000 Subject: [PATCH 23/51] eigen doxy WIP --- tf2_eigen/include/tf2_eigen/tf2_eigen.h | 47 ++++++++++++++++--------- 1 file changed, 31 insertions(+), 16 deletions(-) diff --git a/tf2_eigen/include/tf2_eigen/tf2_eigen.h b/tf2_eigen/include/tf2_eigen/tf2_eigen.h index 947d3e138..ad9c62c96 100644 --- a/tf2_eigen/include/tf2_eigen/tf2_eigen.h +++ b/tf2_eigen/include/tf2_eigen/tf2_eigen.h @@ -91,9 +91,16 @@ geometry_msgs::msg::TransformStamped eigenToTransform(const Eigen::Isometry3d & namespace impl { -template +/** \brief Template for Eigen::Isometry3d and Eigen::Affine3d conversion + * \tparam mode Mode argument for Eigen::Transform template + */ +template struct EigenTransformImpl { + /** \brief Convert a Transform message to an Eigen type. + * \param msg The message to convert + * \param out The converted message, as an Eigen type + */ static void fromMsg( const geometry_msgs::msg::Transform & msg, Eigen::Transform & out) { @@ -104,6 +111,10 @@ struct EigenTransformImpl out = Eigen::Translation3d(v) * q; } + /** \brief Convert an Eigen Transform to a Transform message + * \param in The Eigen Transform to convert + * \param msg The converted Transform, as a message + */ static void toMsg( const Eigen::Transform & in, geometry_msgs::msg::Transform & msg) { @@ -112,27 +123,31 @@ struct EigenTransformImpl } }; -template <> +/// \brief Conversion implementation for geometry_msgs::msg::Transform and Eigen::Affine3d +template<> struct ImplDetails -: EigenTransformImpl + : EigenTransformImpl { }; -template <> +/// \brief Conversion implementation for geometry_msgs::msg::Transform and Eigen::Isometry3d +template<> struct ImplDetails -: EigenTransformImpl + : EigenTransformImpl { }; -template <> +/// \brief Conversion implementation for geometry_msgs::msg::Point and Eigen::Vector3d +template<> struct ImplDetails -: DefaultVectorImpl + : DefaultVectorImpl { }; -template <> +/// \brief Conversion implementation for geometry_msgs::msg::Vector3d and Eigen::Vector3d +template<> struct ImplDetails -: DefaultVectorImpl + : DefaultVectorImpl { }; @@ -149,14 +164,15 @@ struct defaultMessage using type = geometry_msgs::msg::Point; }; -/** \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. - */ -template <> +/// \brief Conversion implementation for geometry_msgs::msg::Quaternion and Eigen::Quaterniond +template<> struct ImplDetails { + + /** \brief Convert a Eigen Quaterniond type to a Quaternion message. + * \param in The Eigen Quaterniond to convert. + * \param msg The quaternion converted to a Quaterion message. + */ static void toMsg(const Eigen::Quaterniond & in, geometry_msgs::msg::Quaternion & msg) { msg.w = in.w(); @@ -166,7 +182,6 @@ struct ImplDetails } /** \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. */ From a23dee7e32737d6020b70b1e565592510358ccda Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Thu, 28 Jan 2021 21:12:05 +0000 Subject: [PATCH 24/51] more eigen doxygen --- tf2_eigen/include/tf2_eigen/tf2_eigen.h | 98 +++++++++++++++---------- 1 file changed, 61 insertions(+), 37 deletions(-) diff --git a/tf2_eigen/include/tf2_eigen/tf2_eigen.h b/tf2_eigen/include/tf2_eigen/tf2_eigen.h index ad9c62c96..3f12ac8c0 100644 --- a/tf2_eigen/include/tf2_eigen/tf2_eigen.h +++ b/tf2_eigen/include/tf2_eigen/tf2_eigen.h @@ -91,15 +91,15 @@ geometry_msgs::msg::TransformStamped eigenToTransform(const Eigen::Isometry3d & namespace impl { -/** \brief Template for Eigen::Isometry3d and Eigen::Affine3d conversion +/** \brief Template for Eigen::Isometry3d and Eigen::Affine3d conversion. * \tparam mode Mode argument for Eigen::Transform template */ template struct EigenTransformImpl { /** \brief Convert a Transform message to an Eigen type. - * \param msg The message to convert - * \param out The converted message, as an Eigen type + * \param[in] msg The message to convert. + * \param[out] out The converted message, as an Eigen type. */ static void fromMsg( const geometry_msgs::msg::Transform & msg, Eigen::Transform & out) @@ -111,9 +111,9 @@ struct EigenTransformImpl out = Eigen::Translation3d(v) * q; } - /** \brief Convert an Eigen Transform to a Transform message - * \param in The Eigen Transform to convert - * \param msg The converted Transform, as a message + /** \brief Convert an Eigen Transform to a Transform message. + * \param[in] in The Eigen Transform to convert. + * \param[out] msg The converted Transform, as a message. */ static void toMsg( const Eigen::Transform & in, geometry_msgs::msg::Transform & msg) @@ -123,28 +123,28 @@ struct EigenTransformImpl } }; -/// \brief Conversion implementation for geometry_msgs::msg::Transform and Eigen::Affine3d +/// \brief Conversion implementation for geometry_msgs::msg::Transform and Eigen::Affine3d. template<> struct ImplDetails : EigenTransformImpl { }; -/// \brief Conversion implementation for geometry_msgs::msg::Transform and Eigen::Isometry3d +/// \brief Conversion implementation for geometry_msgs::msg::Transform and Eigen::Isometry3d. template<> struct ImplDetails : EigenTransformImpl { }; -/// \brief Conversion implementation for geometry_msgs::msg::Point and Eigen::Vector3d +/// \brief Conversion implementation for geometry_msgs::msg::Point and Eigen::Vector3d. template<> struct ImplDetails : DefaultVectorImpl { }; -/// \brief Conversion implementation for geometry_msgs::msg::Vector3d and Eigen::Vector3d +/// \brief Conversion implementation for geometry_msgs::msg::Vector3d and Eigen::Vector3d. template<> struct ImplDetails : DefaultVectorImpl @@ -152,26 +152,30 @@ struct ImplDetails }; +/// \brief Default Type for automatic tf2::doTransform() implementation for Eigen::Vector3d. template<> struct DefaultTransformType { + /// \brief Default Type for automatic tf2::doTransform() implementation for Eigen::Vector3d. using type = Eigen::Isometry3d; }; +/// \brief Default return type of tf2::toMsg() for Eigen::Vector3d. template <> struct defaultMessage { + /// \brief Default return type of tf2::toMsg() for Eigen::Vector3d. using type = geometry_msgs::msg::Point; }; -/// \brief Conversion implementation for geometry_msgs::msg::Quaternion and Eigen::Quaterniond +/// \brief Conversion implementation for geometry_msgs::msg::Quaternion and Eigen::Quaterniond. template<> struct ImplDetails { /** \brief Convert a Eigen Quaterniond type to a Quaternion message. - * \param in The Eigen Quaterniond to convert. - * \param msg The quaternion converted to a Quaterion message. + * \param[in] in The Eigen Quaterniond to convert. + * \param[out] msg The quaternion converted to a Quaterion message. */ static void toMsg(const Eigen::Quaterniond & in, geometry_msgs::msg::Quaternion & msg) { @@ -182,8 +186,8 @@ struct ImplDetails } /** \brief Convert a Quaternion message type to a Eigen-specific Quaterniond type. - * \param msg The Quaternion message to convert. - * \param out The quaternion converted to a Eigen Quaterniond. + * \param[in] msg The Quaternion message to convert. + * \param[out] out The quaternion converted to a Eigen Quaterniond. */ static void fromMsg(const geometry_msgs::msg::Quaternion & msg, Eigen::Quaterniond & out) { @@ -191,9 +195,11 @@ struct ImplDetails } }; +/// \brief Default return type of tf2::toMsg() for Eigen::Quaterniond. template <> struct defaultMessage { + /// \brief Default return type of tf2::toMsg() for Eigen::Quaterniond. using type = geometry_msgs::msg::Quaternion; }; } // namespace impl @@ -241,13 +247,16 @@ void doTransform( namespace impl { + +/** \brief Pose conversion template for Eigen types. + * \tparam T Eigen transform type. + */ template -struct PoseImplDetails +struct EigenPoseImpl { - /** \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. + /** \brief Convert a Eigen transform type to a Pose message. + * \param[in] in The Eigen Affine3d to convert. + * \param[out] msg The Eigen transform converted to a Pose message. */ static void toMsg(const T & in, geometry_msgs::msg::Pose & msg) { @@ -257,10 +266,9 @@ struct PoseImplDetails tf2::toMsg<>(q, msg.orientation); } - /** \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. + /** \brief Convert a Pose message transform type to a Eigen transform type. + * \param[in] msg The Pose message to convert. + * \param[out] out The pose converted to a Eigen transform. */ static void fromMsg(const geometry_msgs::msg::Pose & msg, T & out) { @@ -272,37 +280,44 @@ struct PoseImplDetails } }; +/// \brief Conversion implementation for geometry_msgs::msg::Pose and Eigen::Affine3d. template <> struct ImplDetails -: public PoseImplDetails +: public EigenPoseImpl { }; +/// \brief Conversion implementation for geometry_msgs::msg::Pose and Eigen::Isometry3d. template <> struct ImplDetails -: public PoseImplDetails +: public EigenPoseImpl { }; +/// \brief Default return type of tf2::toMsg() for Eigen::Affine3d. template <> struct defaultMessage { + /// \brief Default return type of tf2::toMsg() for Eigen::Affine3d. using type = geometry_msgs::msg::Pose; }; +/// \brief Default return type of tf2::toMsg() for Eigen::Affine3d. template <> struct defaultMessage { + /// \brief Default return type of tf2::toMsg() for Eigen::Affine3d. using type = geometry_msgs::msg::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. - */ + +/// \brief Conversion implementation for geometry_msgs::msg::Twist and Eigen::Matrix. template <> struct ImplDetails, geometry_msgs::msg::Twist> { + /** \brief Convert a Eigen 6x1 Matrix type to a Twist message. + * \param[in] in The 6x1 Eigen Matrix to convert. + * \param[out] msg The Eigen Matrix converted to a Twist message. + */ static void toMsg(const Eigen::Matrix & in, geometry_msgs::msg::Twist & msg) { msg.linear.x = in[0]; @@ -314,9 +329,8 @@ struct ImplDetails, geometry_msgs::msg::Twist> } /** \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. + * \param[in] msg The Twist message to convert. + * \param[out] out The twist converted to a Eigen 6x1 Matrix. */ static void fromMsg(const geometry_msgs::msg::Twist & msg, Eigen::Matrix & out) { @@ -329,15 +343,22 @@ struct ImplDetails, geometry_msgs::msg::Twist> } }; +/// \brief Default return type of tf2::toMsg() for Eigen::Matrix. template <> struct defaultMessage> { + /// \brief Default return type of tf2::toMsg() for Eigen::Matrix. using type = geometry_msgs::msg::Twist; }; +/// \brief Conversion implementation for geometry_msgs::msg::Wrench and Eigen::Matrix. template <> struct ImplDetails, geometry_msgs::msg::Wrench> { + /** \brief Convert a Eigen 6x1 Matrix type to a Wrench message. + * \param[in] in The 6x1 Eigen Matrix to convert. + * \param[out] msg The Eigen Matrix converted to a Wrench message. + */ static void toMsg(const Eigen::Matrix & in, geometry_msgs::msg::Wrench & msg) { msg.force.x = in[0]; @@ -348,9 +369,8 @@ struct ImplDetails, geometry_msgs::msg::Wrench> msg.torque.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. + /** \brief Convert a Wrench message transform type to a Eigen 6x1 Matrix. + * \param msg The Wrench message to convert. * \param out The twist converted to a Eigen 6x1 Matrix. */ static void fromMsg(const geometry_msgs::msg::Wrench & msg, Eigen::Matrix & out) @@ -364,16 +384,20 @@ struct ImplDetails, geometry_msgs::msg::Wrench> } }; +/// \brief Default Type for automatic tf2::doTransform() implementation for Eigen::Affine3d. template<> struct DefaultTransformType { + /// \brief Default Type for automatic tf2::doTransform() implementation for Eigen::Affine3d. using type = Eigen::Isometry3d; }; +/// \brief Default Type for automatic tf2::doTransform() implementation for Eigen::Isometry3d. template<> struct DefaultTransformType { + /// \brief Default Type for automatic tf2::doTransform() implementation for Eigen::Isometry3d. using type = Eigen::Isometry3d; }; From f9b74174ba78a8bfc1ebe81e7d3991ae503c882d Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Thu, 28 Jan 2021 22:39:12 +0000 Subject: [PATCH 25/51] doxygen + uncrustify for tf2_geometry_msgs --- .../tf2_geometry_msgs/tf2_geometry_msgs.h | 217 +++++++++++------- .../test/test_tf2_geometry_msgs.cpp | 40 ++-- 2 files changed, 161 insertions(+), 96 deletions(-) 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 38d141667..e22599ec1 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 @@ -29,10 +29,8 @@ /** \author Wim Meeussen, Bjarne von Horn */ -#ifndef TF2_GEOMETRY_MSGS_H -#define TF2_GEOMETRY_MSGS_H - -#include +#ifndef TF2_GEOMETRY_MSGS__TF2_GEOMETRY_MSGS_H_ +#define TF2_GEOMETRY_MSGS__TF2_GEOMETRY_MSGS_H_ #include #include @@ -48,9 +46,13 @@ #include #include +#include + + namespace Eigen { -template +/// \brief Forward declaration +template class Matrix; } @@ -61,12 +63,14 @@ namespace tf2 * \return The converted KDL Frame. */ inline -KDL::Frame gmTransformToKDL(const geometry_msgs::msg::TransformStamped& t) - { - return KDL::Frame(KDL::Rotation::Quaternion(t.transform.rotation.x, t.transform.rotation.y, - t.transform.rotation.z, t.transform.rotation.w), - KDL::Vector(t.transform.translation.x, t.transform.translation.y, t.transform.translation.z)); - } +KDL::Frame gmTransformToKDL(const geometry_msgs::msg::TransformStamped & t) +{ + return KDL::Frame( + KDL::Rotation::Quaternion( + t.transform.rotation.x, t.transform.rotation.y, + t.transform.rotation.z, t.transform.rotation.w), + KDL::Vector(t.transform.translation.x, t.transform.translation.y, t.transform.translation.z)); +} namespace impl { @@ -74,31 +78,46 @@ namespace impl /** Vector3 **/ /*************/ -template <> +/// \brief Conversion implementation for geometry_msgs::msg::Vector3 and tf2::Vector3. +template<> struct ImplDetails -: DefaultVectorImpl + : DefaultVectorImpl { }; -template <> +/// \brief Conversion implementation for geometry_msgs::msg::Point and tf2::Vector3. +template<> struct ImplDetails -: DefaultVectorImpl + : DefaultVectorImpl { }; -template <> +/// \brief Default return type of tf2::toMsg() for tf2::Vector3. +template<> struct defaultMessage { + /// \brief Default return type of tf2::toMsg() for tf2::Vector3. using type = geometry_msgs::msg::Vector3; }; -template <> +/// \brief Default Type for automatic tf2::doTransform() implementation for tf2::Vector3. +template<> struct DefaultTransformType { + /// \brief Default Type for automatic tf2::doTransform() implementation for tf2::Vector3. using type = tf2::Transform; }; -template +/** \brief Template for geometry_msgs transformation with corresponding tf2 types. + * This function is used to implement various tf2::doTransform() specializations for + * geometry_msgs Messages. + * \param[in] in The message to transform. + * \param[in,out] out The transformed message. + * \param[in] transform The timestamped transform to apply, as a TransformStamped message. + * \tparam TFDatatype The tf2 datatype which will be used to calculate the transformation. + * \tparam Message The type of the message to be transformed + */ +template inline void doTransformWithTf( const Message & in, Message & out, const geometry_msgs::msg::TransformStamped & transform) { @@ -115,11 +134,11 @@ inline void doTransformWithTf( /** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Vector 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 Vector3 message. - * \param t_out The transformed vector, as a timestamped Vector3 message. - * \param transform The timestamped transform to apply, as a TransformStamped message. + * \param[in] t_in The vector to transform, as a timestamped Vector3 message. + * \param[in,out] t_out The transformed vector, as a timestamped Vector3 message. + * \param[in] transform The timestamped transform to apply, as a TransformStamped message. */ -template <> +template<> inline void doTransform( const geometry_msgs::msg::Vector3 & t_in, geometry_msgs::msg::Vector3 & t_out, const geometry_msgs::msg::TransformStamped & transform) @@ -134,11 +153,11 @@ inline void doTransform( /** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Vector 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 Vector3 message. - * \param t_out The transformed vector, as a timestamped Vector3 message. - * \param transform The timestamped transform to apply, as a TransformStamped message. + * \param[in] t_in The vector to transform, as a timestamped Vector3 message. + * \param[in,out] t_out The transformed vector, as a timestamped Vector3 message. + * \param[in] transform The timestamped transform to apply, as a TransformStamped message. */ -template <> +template<> inline void doTransform( const geometry_msgs::msg::Vector3Stamped & t_in, geometry_msgs::msg::Vector3Stamped & t_out, const geometry_msgs::msg::TransformStamped & transform) @@ -154,11 +173,11 @@ inline void doTransform( /** \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. - * \param t_in The point to transform, as a timestamped Point3 message. - * \param t_out The transformed point, as a timestamped Point3 message. - * \param transform The timestamped transform to apply, as a TransformStamped message. + * \param[in] t_in The point to transform, as a timestamped Point3 message. + * \param[in,out] t_out The transformed point, as a timestamped Point3 message. + * \param[in] transform The timestamped transform to apply, as a TransformStamped message. */ -template <> +template<> inline void doTransform( const geometry_msgs::msg::PointStamped & t_in, geometry_msgs::msg::PointStamped & t_out, const geometry_msgs::msg::TransformStamped & transform) @@ -170,11 +189,11 @@ inline void doTransform( /** \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. - * \param t_in The point to transform, as a timestamped Point3 message. - * \param t_out The transformed point, as a timestamped Point3 message. - * \param transform The timestamped transform to apply, as a TransformStamped message. + * \param[in] t_in The point to transform, as a timestamped Point3 message. + * \param[in,out] t_out The transformed point, as a timestamped Point3 message. + * \param[in] transform The timestamped transform to apply, as a TransformStamped message. */ -template <> +template<> inline void doTransform( const geometry_msgs::msg::Point & t_in, geometry_msgs::msg::Point & t_out, const geometry_msgs::msg::TransformStamped & transform) @@ -188,11 +207,11 @@ inline void doTransform( /** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Pose type. * This function is a specialization of the doTransform template defined in tf2/convert.h. - * \param t_in The pose to transform, as a timestamped Pose3 message. - * \param t_out The transformed pose, as a timestamped Pose3 message. - * \param transform The timestamped transform to apply, as a TransformStamped message. + * \param[in] t_in The pose to transform, as a timestamped Pose3 message. + * \param[in,out] t_out The transformed pose, as a timestamped Pose3 message. + * \param[in] transform The timestamped transform to apply, as a TransformStamped message. */ -template <> +template<> inline void doTransform( const geometry_msgs::msg::PoseStamped & t_in, geometry_msgs::msg::PoseStamped & t_out, const geometry_msgs::msg::TransformStamped & transform) @@ -204,11 +223,11 @@ inline void doTransform( /** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Pose type. * This function is a specialization of the doTransform template defined in tf2/convert.h. - * \param t_in The pose to transform, as a timestamped Pose3 message. - * \param t_out The transformed pose, as a timestamped Pose3 message. - * \param transform The timestamped transform to apply, as a TransformStamped message. + * \param[in] t_in The pose to transform, as a timestamped Pose3 message. + * \param[in,out] t_out The transformed pose, as a timestamped Pose3 message. + * \param[in] transform The timestamped transform to apply, as a TransformStamped message. */ -template <> +template<> inline void doTransform( const geometry_msgs::msg::Pose & t_in, geometry_msgs::msg::Pose & t_out, const geometry_msgs::msg::TransformStamped & transform) @@ -222,11 +241,11 @@ inline void doTransform( /** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Pose type. * This function is a specialization of the doTransform template defined in tf2/convert.h. - * \param t_in The pose to transform, as a timestamped Pose3 message with covariance. - * \param t_out The transformed pose, as a timestamped Pose3 message with covariance. - * \param transform The timestamped transform to apply, as a TransformStamped message. + * \param[in] t_in The pose to transform, as a timestamped Pose3 message with covariance. + * \param[in,out] t_out The transformed pose, as a timestamped Pose3 message with covariance. + * \param[in] transform The timestamped transform to apply, as a TransformStamped message. */ -template <> +template<> inline void doTransform( const geometry_msgs::msg::PoseWithCovarianceStamped & t_in, geometry_msgs::msg::PoseWithCovarianceStamped & t_out, @@ -244,19 +263,24 @@ inline void doTransform( namespace impl { -template <> +/// \brief Conversion implementation for geometry_msgs::msg::Quaternion and tf2::Quaternion. +template<> struct ImplDetails -: DefaultQuaternionImpl {}; + : DefaultQuaternionImpl {}; -template <> +/// \brief Default return type of tf2::toMsg() for tf2::Quaternion. +template<> struct defaultMessage { + /// \brief Default return type of tf2::toMsg() for tf2::Quaternion. using type = geometry_msgs::msg::Quaternion; }; -template <> +/// \brief Default Type for automatic tf2::doTransform() implementation for tf2::Quaternion. +template<> struct DefaultTransformType { + /// \brief Default Type for automatic tf2::doTransform() implementation for tf2::Quaternion. using type = tf2::Transform; }; @@ -267,11 +291,11 @@ struct DefaultTransformType /***********************/ /** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Quaternion type. * This function is a specialization of the doTransform template defined in tf2/convert.h. - * \param t_in The quaternion to transform, as a timestamped Quaternion3 message. - * \param t_out The transformed quaternion, as a timestamped Quaternion3 message. - * \param transform The timestamped transform to apply, as a TransformStamped message. + * \param[in] t_in The quaternion to transform, as a timestamped Quaternion3 message. + * \param[in,out] t_out The transformed quaternion, as a timestamped Quaternion3 message. + * \param[in] transform The timestamped transform to apply, as a TransformStamped message. */ -template <> +template<> inline void doTransform( const geometry_msgs::msg::Quaternion & t_in, geometry_msgs::msg::Quaternion & t_out, const geometry_msgs::msg::TransformStamped & transform) @@ -281,11 +305,11 @@ inline void doTransform( /** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Quaternion type. * This function is a specialization of the doTransform template defined in tf2/convert.h. - * \param t_in The quaternion to transform, as a timestamped Quaternion3 message. - * \param t_out The transformed quaternion, as a timestamped Quaternion3 message. - * \param transform The timestamped transform to apply, as a TransformStamped message. + * \param[in] t_in The quaternion to transform, as a timestamped Quaternion3 message. + * \param[in,out] t_out The transformed quaternion, as a timestamped Quaternion3 message. + * \param[in] transform The timestamped transform to apply, as a TransformStamped message. */ -template <> +template<> inline void doTransform( const geometry_msgs::msg::QuaternionStamped & t_in, geometry_msgs::msg::QuaternionStamped & t_out, const geometry_msgs::msg::TransformStamped & transform) @@ -301,13 +325,13 @@ inline void doTransform( namespace impl { -template <> +/// \brief Conversion implementation for geometry_msgs::msg::Transform and tf2::Transform. +template<> 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. + * \param[in] in in A tf2 Transform object. + * \param[out] out The Transform converted to a geometry_msgs message type. */ static void toMsg(const tf2::Transform & in, geometry_msgs::msg::Transform & out) { @@ -316,9 +340,8 @@ struct ImplDetails } /** \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. + * \param[in] in A Transform message type. + * \param[out] out The Transform converted to a tf2 type. */ static void fromMsg(const geometry_msgs::msg::Transform & in, tf2::Transform & out) { @@ -332,20 +355,22 @@ struct ImplDetails } }; -template <> +/// \brief Default return type of tf2::toMsg() for tf2::Transform. +template<> struct defaultMessage { + /// \brief Default return type of tf2::toMsg() for tf2::Transform. using type = geometry_msgs::msg::Transform; }; } // namespace impl /** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Transform type. * This function is a specialization of the doTransform template defined in tf2/convert.h. - * \param t_in The frame to transform, as a timestamped Transform3 message. - * \param t_out The frame transform, as a timestamped Transform3 message. - * \param transform The timestamped transform to apply, as a TransformStamped message. + * \param[in] t_in The frame to transform, as a timestamped Transform3 message. + * \param[in,out] t_out The frame transform, as a timestamped Transform3 message. + * \param[in] transform The timestamped transform to apply, as a TransformStamped message. */ -template <> +template<> inline void doTransform( const geometry_msgs::msg::TransformStamped & t_in, geometry_msgs::msg::TransformStamped & t_out, const geometry_msgs::msg::TransformStamped & transform) @@ -361,12 +386,13 @@ namespace impl /** Pose **/ /**********/ -template <> +/// \brief Conversion implementation for geometry_msgs::msg::Pose and tf2::Transform. +template<> struct ImplDetails { /** \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. + * \param[in] in A tf2 Transform object. + * \param[out] out The Transform converted to a geometry_msgs Pose message type. */ static void toMsg(const tf2::Transform & in, geometry_msgs::msg::Pose & out) { @@ -375,8 +401,8 @@ struct ImplDetails } /** \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. + * \param[in] in A Pose message. + * \param[out] out The Pose converted to a tf2 Transform type. */ static void fromMsg(const geometry_msgs::msg::Pose & in, tf2::Transform & out) { @@ -387,9 +413,11 @@ struct ImplDetails } }; -template <> +/// \brief Default Type for automatic tf2::doTransform() implementation for tf2::Transform. +template<> struct DefaultTransformType { + /// \brief Default Type for automatic tf2::doTransform() implementation for tf2::Transform. using type = tf2::Transform; }; @@ -397,15 +425,24 @@ struct DefaultTransformType /** Wrench **/ /************/ -template <> +/// \brief Conversion implementation for geometry_msgs::msg::Wrench and tf2::Transform. +template<> struct ImplDetails, geometry_msgs::msg::Wrench> { + /** \brief Convert a tf2 Wrench to an equivalent geometry_msgs Wrench message. + * \param[in] in A tf2 Wrench object. + * \param[out] out The Wrench converted to a geometry_msgs Wrench message type. + */ static void toMsg(const std::array & in, geometry_msgs::msg::Wrench & out) { tf2::toMsg<>(std::get<0>(in), out.force); tf2::toMsg<>(std::get<1>(in), out.torque); } + /** \brief Convert a geometry_msgs Wrench message to an equivalent tf2 Wrench type. + * \param[in] msg A Wrench message. + * \param[out] out The Wrench converted to a tf2 Wrench type. + */ static void fromMsg(const geometry_msgs::msg::Wrench & msg, std::array & out) { tf2::fromMsg<>(msg.force, std::get<0>(out)); @@ -413,21 +450,35 @@ struct ImplDetails, geometry_msgs::msg::Wrench> } }; -template <> -struct defaultMessage > +/// \brief Default return type of tf2::toMsg() for std::array. +template<> +struct defaultMessage> { + /// \brief Default return type of tf2::toMsg() for std::array. using type = geometry_msgs::msg::Wrench; }; } // namespace impl -template +/** \brief Specialization of tf2::convert for Eigen::Matrix and std::array. + * + * This specialization of the template defined in tf2/convert.h + * is for a Wrench represented in an Eigen matrix. + * To avoid a dependency on Eigen, the Eigen::Matrix template is forward declared. + * \param[in] in Wrench, as Eigen matrix + * \param[out] out The Wrench as tf2 type + * \tparam options Eigen::Matrix template parameter + * \tparam row Eigen::Matrix template parameter + * \tparam cols Eigen::Matrix template parameter + */ +template void convert( const Eigen::Matrix & in, std::array & out) { - out[0] = tf2::Vector3{in(0), in(1), in(2)}; - out[1] = tf2::Vector3{in(3), in(4), in(5)}; + out[0] = tf2::Vector3{in[0], in[1], in[2]}; + out[1] = tf2::Vector3{in[3], in[4], in[5]}; } } // namespace tf2 -#endif // TF2_GEOMETRY_MSGS_H + +#endif // TF2_GEOMETRY_MSGS__TF2_GEOMETRY_MSGS_H_ diff --git a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp index dfd4c5e56..d009e9ebd 100644 --- a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp +++ b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp @@ -44,6 +44,9 @@ #include #include +#include +#include + std::unique_ptr tf_buffer = nullptr; static const double EPS = 1e-3; @@ -167,7 +170,8 @@ TEST(TfGeometry, Frame) v1.header.frame_id = "A"; // simple api - geometry_msgs::msg::PoseStamped v_simple = tf_buffer->transform(v1, "B", tf2::durationFromSec(2.0)); + geometry_msgs::msg::PoseStamped v_simple = + tf_buffer->transform(v1, "B", tf2::durationFromSec(2.0)); EXPECT_NEAR(v_simple.pose.position.x, -9, EPS); EXPECT_NEAR(v_simple.pose.position.y, 18, EPS); EXPECT_NEAR(v_simple.pose.position.z, 27, EPS); @@ -178,8 +182,9 @@ TEST(TfGeometry, Frame) // advanced api - geometry_msgs::msg::PoseStamped v_advanced = tf_buffer->transform(v1, "B", tf2::timeFromSec(2.0), - "A", tf2::durationFromSec(3.0)); + geometry_msgs::msg::PoseStamped v_advanced = tf_buffer->transform( + v1, "B", tf2::timeFromSec(2.0), + "A", tf2::durationFromSec(3.0)); EXPECT_NEAR(v_advanced.pose.position.x, -9, EPS); EXPECT_NEAR(v_advanced.pose.position.y, 18, EPS); EXPECT_NEAR(v_advanced.pose.position.z, 27, EPS); @@ -212,7 +217,9 @@ TEST(TfGeometry, FrameWithCovariance) }; // simple api - geometry_msgs::msg::PoseWithCovarianceStamped v_simple = tf_buffer->transform(v1, "B", tf2::durationFromSec(2.0)); + geometry_msgs::msg::PoseWithCovarianceStamped v_simple = tf_buffer->transform( + v1, "B", tf2::durationFromSec( + 2.0)); EXPECT_NEAR(v_simple.pose.pose.position.x, -9, EPS); EXPECT_NEAR(v_simple.pose.pose.position.y, 18, EPS); EXPECT_NEAR(v_simple.pose.pose.position.z, 27, EPS); @@ -224,8 +231,9 @@ TEST(TfGeometry, FrameWithCovariance) // advanced api - geometry_msgs::msg::PoseWithCovarianceStamped v_advanced = tf_buffer->transform(v1, "B", tf2::timeFromSec(2.0), - "A", tf2::durationFromSec(3.0)); + geometry_msgs::msg::PoseWithCovarianceStamped v_advanced = tf_buffer->transform( + v1, "B", tf2::timeFromSec(2.0), + "A", tf2::durationFromSec(3.0)); EXPECT_NEAR(v_advanced.pose.pose.position.x, -9, EPS); EXPECT_NEAR(v_advanced.pose.pose.position.y, 18, EPS); EXPECT_NEAR(v_advanced.pose.pose.position.z, 27, EPS); @@ -247,14 +255,16 @@ TEST(TfGeometry, Vector) v1.header.frame_id = "A"; // simple api - geometry_msgs::msg::Vector3Stamped v_simple = tf_buffer->transform(v1, "B", tf2::durationFromSec(2.0)); + geometry_msgs::msg::Vector3Stamped v_simple = + tf_buffer->transform(v1, "B", tf2::durationFromSec(2.0)); EXPECT_NEAR(v_simple.vector.x, 1, EPS); EXPECT_NEAR(v_simple.vector.y, -2, EPS); EXPECT_NEAR(v_simple.vector.z, -3, EPS); // advanced api - geometry_msgs::msg::Vector3Stamped v_advanced = tf_buffer->transform(v1, "B", tf2::timeFromSec(2.0), - "A", tf2::durationFromSec(3.0)); + geometry_msgs::msg::Vector3Stamped v_advanced = tf_buffer->transform( + v1, "B", tf2::timeFromSec(2.0), + "A", tf2::durationFromSec(3.0)); EXPECT_NEAR(v_advanced.vector.x, 1, EPS); EXPECT_NEAR(v_advanced.vector.y, -2, EPS); EXPECT_NEAR(v_advanced.vector.z, -3, EPS); @@ -271,14 +281,17 @@ TEST(TfGeometry, Point) v1.header.frame_id = "A"; // simple api - geometry_msgs::msg::PointStamped v_simple = tf_buffer->transform(v1, "B", tf2::durationFromSec(2.0)); + geometry_msgs::msg::PointStamped v_simple = tf_buffer->transform( + v1, "B", tf2::durationFromSec( + 2.0)); EXPECT_NEAR(v_simple.point.x, -9, EPS); EXPECT_NEAR(v_simple.point.y, 18, EPS); EXPECT_NEAR(v_simple.point.z, 27, EPS); // advanced api - geometry_msgs::msg::PointStamped v_advanced = tf_buffer->transform(v1, "B", tf2::timeFromSec(2.0), - "A", tf2::durationFromSec(3.0)); + geometry_msgs::msg::PointStamped v_advanced = tf_buffer->transform( + v1, "B", tf2::timeFromSec(2.0), + "A", tf2::durationFromSec(3.0)); EXPECT_NEAR(v_advanced.point.x, -9, EPS); EXPECT_NEAR(v_advanced.point.y, 18, EPS); EXPECT_NEAR(v_advanced.point.z, 27, EPS); @@ -318,7 +331,8 @@ TEST(TfGeometry, Quaternion) } -int main(int argc, char **argv){ +int main(int argc, char ** argv) +{ testing::InitGoogleTest(&argc, argv); rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); From ef67cbe178e9b4745b6279f18e0075ac57289813 Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Fri, 12 Feb 2021 00:22:49 +0000 Subject: [PATCH 26/51] tf2_eigen uncrustify --- tf2_eigen/include/tf2_eigen/tf2_eigen.h | 44 ++++++++++++------------- tf2_eigen/test/tf2_eigen-test.cpp | 22 +++++++++---- 2 files changed, 36 insertions(+), 30 deletions(-) diff --git a/tf2_eigen/include/tf2_eigen/tf2_eigen.h b/tf2_eigen/include/tf2_eigen/tf2_eigen.h index 3f12ac8c0..f379fc7c2 100644 --- a/tf2_eigen/include/tf2_eigen/tf2_eigen.h +++ b/tf2_eigen/include/tf2_eigen/tf2_eigen.h @@ -151,7 +151,6 @@ struct ImplDetails { }; - /// \brief Default Type for automatic tf2::doTransform() implementation for Eigen::Vector3d. template<> struct DefaultTransformType @@ -161,7 +160,7 @@ struct DefaultTransformType }; /// \brief Default return type of tf2::toMsg() for Eigen::Vector3d. -template <> +template<> struct defaultMessage { /// \brief Default return type of tf2::toMsg() for Eigen::Vector3d. @@ -172,7 +171,6 @@ struct defaultMessage template<> struct ImplDetails { - /** \brief Convert a Eigen Quaterniond type to a Quaternion message. * \param[in] in The Eigen Quaterniond to convert. * \param[out] msg The quaternion converted to a Quaterion message. @@ -196,7 +194,7 @@ struct ImplDetails }; /// \brief Default return type of tf2::toMsg() for Eigen::Quaterniond. -template <> +template<> struct defaultMessage { /// \brief Default return type of tf2::toMsg() for Eigen::Quaterniond. @@ -251,7 +249,7 @@ namespace impl /** \brief Pose conversion template for Eigen types. * \tparam T Eigen transform type. */ -template +template struct EigenPoseImpl { /** \brief Convert a Eigen transform type to a Pose message. @@ -262,7 +260,7 @@ struct EigenPoseImpl { tf2::toMsg<>(Eigen::Vector3d(in.translation()), msg.position); Eigen::Quaterniond q(in.linear()); - if (q.w() < 0) q.coeffs() *= -1; + if (q.w() < 0) {q.coeffs() *= -1;} tf2::toMsg<>(q, msg.orientation); } @@ -281,21 +279,21 @@ struct EigenPoseImpl }; /// \brief Conversion implementation for geometry_msgs::msg::Pose and Eigen::Affine3d. -template <> +template<> struct ImplDetails -: public EigenPoseImpl + : public EigenPoseImpl { }; /// \brief Conversion implementation for geometry_msgs::msg::Pose and Eigen::Isometry3d. -template <> +template<> struct ImplDetails -: public EigenPoseImpl + : public EigenPoseImpl { }; /// \brief Default return type of tf2::toMsg() for Eigen::Affine3d. -template <> +template<> struct defaultMessage { /// \brief Default return type of tf2::toMsg() for Eigen::Affine3d. @@ -303,7 +301,7 @@ struct defaultMessage }; /// \brief Default return type of tf2::toMsg() for Eigen::Affine3d. -template <> +template<> struct defaultMessage { /// \brief Default return type of tf2::toMsg() for Eigen::Affine3d. @@ -311,7 +309,7 @@ struct defaultMessage }; /// \brief Conversion implementation for geometry_msgs::msg::Twist and Eigen::Matrix. -template <> +template<> struct ImplDetails, geometry_msgs::msg::Twist> { /** \brief Convert a Eigen 6x1 Matrix type to a Twist message. @@ -344,7 +342,7 @@ struct ImplDetails, geometry_msgs::msg::Twist> }; /// \brief Default return type of tf2::toMsg() for Eigen::Matrix. -template <> +template<> struct defaultMessage> { /// \brief Default return type of tf2::toMsg() for Eigen::Matrix. @@ -352,13 +350,13 @@ struct defaultMessage> }; /// \brief Conversion implementation for geometry_msgs::msg::Wrench and Eigen::Matrix. -template <> +template<> struct ImplDetails, geometry_msgs::msg::Wrench> { - /** \brief Convert a Eigen 6x1 Matrix type to a Wrench message. - * \param[in] in The 6x1 Eigen Matrix to convert. - * \param[out] msg The Eigen Matrix converted to a Wrench message. - */ + /** \brief Convert a Eigen 6x1 Matrix type to a Wrench message. + * \param[in] in The 6x1 Eigen Matrix to convert. + * \param[out] msg The Eigen Matrix converted to a Wrench message. + */ static void toMsg(const Eigen::Matrix & in, geometry_msgs::msg::Wrench & msg) { msg.force.x = in[0]; @@ -414,11 +412,11 @@ inline geometry_msgs::msg::Vector3 toMsg2(const Eigen::Vector3d & in) { geometry_msgs::msg::Vector3 msg; - msg.x = in(0); - msg.y = in(1); - msg.z = in(2); + msg.x = in[0]; + msg.y = in[1]; + msg.z = in[2]; return msg; } } // namespace tf2 -#endif // TF2_EIGEN_H +#endif // TF2_EIGEN__TF2_EIGEN_H_ diff --git a/tf2_eigen/test/tf2_eigen-test.cpp b/tf2_eigen/test/tf2_eigen-test.cpp index 41e1338b3..d24555269 100644 --- a/tf2_eigen/test/tf2_eigen-test.cpp +++ b/tf2_eigen/test/tf2_eigen-test.cpp @@ -1,6 +1,5 @@ /* - * Copyright (c) Koji Terada - * All rights reserved. + * Copyright (c) Koji Terada. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: @@ -44,9 +43,12 @@ #include #include + TEST(TfEigen, ConvertVector3dStamped) { - const tf2::Stamped v(Eigen::Vector3d(1,2,3), tf2::TimePoint(std::chrono::seconds(5)), "test"); + const tf2::Stamped v(Eigen::Vector3d(1, 2, 3), tf2::TimePoint( + std::chrono::seconds( + 5)), "test"); tf2::Stamped v1; geometry_msgs::msg::PointStamped p1; @@ -58,7 +60,7 @@ TEST(TfEigen, ConvertVector3dStamped) TEST(TfEigen, ConvertVector3d) { - const Eigen::Vector3d v(1,2,3); + const Eigen::Vector3d v(1, 2, 3); Eigen::Vector3d v1; geometry_msgs::msg::Point p1; @@ -70,8 +72,12 @@ TEST(TfEigen, ConvertVector3d) TEST(TfEigen, ConvertAffine3dStamped) { - const Eigen::Affine3d v_nonstamped(Eigen::Translation3d(1,2,3) * Eigen::AngleAxis(1, Eigen::Vector3d::UnitX())); - const tf2::Stamped v(v_nonstamped, tf2::TimePoint(std::chrono::seconds(42)), "test_frame"); + const Eigen::Affine3d v_nonstamped(Eigen::Translation3d(1, 2, 3) * Eigen::AngleAxis( + 1, + Eigen::Vector3d::UnitX())); + const tf2::Stamped v(v_nonstamped, tf2::TimePoint( + std::chrono::seconds( + 42)), "test_frame"); tf2::Stamped v1; geometry_msgs::msg::PoseStamped p1; @@ -86,7 +92,9 @@ TEST(TfEigen, ConvertAffine3dStamped) TEST(TfEigen, ConvertAffine3d) { - const Eigen::Affine3d v(Eigen::Translation3d(1,2,3) * Eigen::AngleAxis(1, Eigen::Vector3d::UnitX())); + const Eigen::Affine3d v(Eigen::Translation3d(1, 2, 3) * Eigen::AngleAxis( + 1, + Eigen::Vector3d::UnitX())); Eigen::Affine3d v1; geometry_msgs::msg::Pose p1; From b7ccaefac79e46f0c27337f7954c3f914c7cb89d Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Thu, 28 Jan 2021 23:42:07 +0000 Subject: [PATCH 27/51] tf2_kdl doxygen and linting --- tf2_kdl/include/tf2_kdl/tf2_kdl.h | 147 ++++++++++++++++++++---------- tf2_kdl/test/test_tf2_kdl.cpp | 81 +++++++++------- 2 files changed, 143 insertions(+), 85 deletions(-) diff --git a/tf2_kdl/include/tf2_kdl/tf2_kdl.h b/tf2_kdl/include/tf2_kdl/tf2_kdl.h index 1a349ee36..0fffb9102 100644 --- a/tf2_kdl/include/tf2_kdl/tf2_kdl.h +++ b/tf2_kdl/include/tf2_kdl/tf2_kdl.h @@ -29,8 +29,8 @@ /** \author Wim Meeussen, Bjarne von Horn */ -#ifndef TF2_KDL_H -#define TF2_KDL_H +#ifndef TF2_KDL__TF2_KDL_H_ +#define TF2_KDL__TF2_KDL_H_ #include #include @@ -45,14 +45,15 @@ namespace Eigen { -template +/// Forward declaration. +template class Matrix; } namespace tf2 { /** \brief Convert a timestamped transform to the equivalent KDL data type. - * \param t The transform to convert, as a geometry_msgs TransformedStamped message. + * \param[in] t The transform to convert, as a geometry_msgs TransformedStamped message. * \return The transform message converted to an KDL Frame. */ inline KDL::Frame transformToKDL(const geometry_msgs::msg::TransformStamped & t) @@ -63,7 +64,7 @@ inline KDL::Frame transformToKDL(const geometry_msgs::msg::TransformStamped & t) } /** \brief Convert an KDL Frame to the equivalent geometry_msgs message type. - * \param k The transform to convert, as an KDL Frame. + * \param[in] k The transform to convert, as an KDL Frame. * \return The transform converted to a TransformStamped message. */ inline geometry_msgs::msg::TransformStamped kdlToTransform(const KDL::Frame & k) @@ -75,9 +76,14 @@ inline geometry_msgs::msg::TransformStamped kdlToTransform(const KDL::Frame & k) namespace impl { -template <> +/// \brief Conversion implementation for geometry_msgs::msg::Transform and KDL::Frame. +template<> struct ImplDetails { + /** \brief Convert a Transform message type to a KDL Frame. + * \param[in] in The Transform message to convert. + * \param[out] out The transform converted to a KDL Frame. + */ static void fromMsg(geometry_msgs::msg::Transform const & in, KDL::Frame & out) { KDL::Rotation r; @@ -87,6 +93,10 @@ struct ImplDetails out = KDL::Frame(r, v); } + /** \brief Convert a KDL Frame type to a Transform message. + * \param[in] in The KDL Frame to convert. + * \param[out] out The KDL Frame converted to a Transform message. + */ static void toMsg(KDL::Frame const & in, geometry_msgs::msg::Transform & out) { tf2::toMsg<>(in.p, out.translation); @@ -94,25 +104,30 @@ struct ImplDetails } }; -template <> +/// \brief Conversion implementation for geometry_msgs::msg::Vector3 and KDL::Vector. +template<> struct ImplDetails -: DefaultVectorImpl + : DefaultVectorImpl { }; -template <> +/// \brief Conversion implementation for geometry_msgs::msg::Point and KDL::Vector. +template<> struct ImplDetails -: DefaultVectorImpl + : DefaultVectorImpl { }; -template <> +/// \brief Default return type of tf2::toMsg() for KDL::Vector. +template<> struct defaultMessage { + /// \brief Default return type of tf2::toMsg() for KDL::Vector. using type = geometry_msgs::msg::Point; }; -template <> +/// \brief Default Type for automatic tf2::doTransform() implementation for KDL::Frame. +template<> struct DefaultTransformType { using type = KDL::Frame; @@ -122,13 +137,13 @@ struct DefaultTransformType // Twist // --------------------- -template <> +/// \brief Conversion implementation for geometry_msgs::msg::Twist and KDL::Twist. +template<> 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. - * \param in The timestamped KDL Twist to convert. - * \return The twist converted to a TwistStamped message. + /** \brief Convert a KDL Twist type to a Twist message. + * \param[in] in The KDL Twist to convert. + * \param[out] msg The twist converted to a Twist message. */ static void toMsg(const KDL::Twist & in, geometry_msgs::msg::Twist & msg) { @@ -136,10 +151,9 @@ struct ImplDetails tf2::toMsg<>(in.rot, msg.angular); } - /** \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. + /** \brief Convert a Twistmessage type to a KDL-specific Twist type. + * \param[in] msg The Twist message to convert. + * \param[out] out The twist converted to a KDL Twist. */ static void fromMsg(const geometry_msgs::msg::Twist & msg, KDL::Twist & out) { @@ -148,15 +162,19 @@ struct ImplDetails } }; -template <> +/// \brief Default Type for automatic tf2::doTransform() implementation for KDL::Twist. +template<> struct DefaultTransformType { + /// \brief Default Type for automatic tf2::doTransform() implementation for KDL::Twist. using type = KDL::Frame; }; -template <> +/// \brief Default return type of tf2::toMsg() for KDL::Twist. +template<> struct defaultMessage { + /// \brief Default return type of tf2::toMsg() for KDL::Twist. using type = geometry_msgs::msg::Twist; }; @@ -164,13 +182,13 @@ struct defaultMessage // Wrench // --------------------- -template <> +/// \brief Conversion implementation for geometry_msgs::msg::Wrench and KDL::Wrench. +template<> 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. - * \param in The timestamped KDL Wrench to convert. - * \return The wrench converted to a WrenchStamped message. + /** \brief Convert a KDL Wrench type to a Wrench message. + * \param[in] in The KDL Wrench to convert. + * \param[out] msg The wrench converted to a Wrench message. */ static void toMsg(const KDL::Wrench & in, geometry_msgs::msg::Wrench & msg) { @@ -178,10 +196,9 @@ struct ImplDetails tf2::toMsg<>(in.torque, msg.torque); } - /** \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. + /** \brief Convert a Wrench message type to a KDL-specific Wrench type. + * \param[in] msg The Wrench message to convert. + * \param[out] out The wrench converted to a KDL Wrench. */ static void fromMsg(const geometry_msgs::msg::Wrench & msg, KDL::Wrench & out) { @@ -190,20 +207,35 @@ struct ImplDetails } }; -template <> +/// \brief Default return type of tf2::toMsg() for KDL::Wrench. +template<> struct defaultMessage { + /// \brief Default return type of tf2::toMsg() for KDL::Wrench. using type = geometry_msgs::msg::Wrench; }; -template <> +/// \brief Default Type for automatic tf2::doTransform() implementation for KDL::Wrench. +template<> struct DefaultTransformType { + /// \brief Default Type for automatic tf2::doTransform() implementation for KDL::Wrench. using type = KDL::Frame; }; } // namespace impl -template +/** \brief Specialization of tf2::convert for Eigen::Matrix and KDL::Wrench. + * + * This specialization of the template defined in tf2/convert.h + * is for a Wrench represented in an Eigen matrix. + * To avoid a dependency on Eigen, the Eigen::Matrix template is forward declared. + * \param[in] in Wrench, as Eigen matrix + * \param[out] out The Wrench as KDL::Wrench type + * \tparam options Eigen::Matrix template parameter + * \tparam row Eigen::Matrix template parameter + * \tparam cols Eigen::Matrix template parameter + */ +template void convert(Eigen::Matrix const & in, KDL::Wrench & out) { const auto msg = @@ -216,13 +248,13 @@ void convert(Eigen::Matrix const & in, KDL::W // --------------------- namespace impl { -template <> +/// \brief Conversion implementation for geometry_msgs::msg::Pose and KDL::Frame. +template<> struct ImplDetails { - /** \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. + /** \brief Convert a KDL Frame type to a Pose message. + * \param[in] in The KDL Frame to convert. + * \param[out] msg The frame converted to a Pose message. */ static void toMsg(const KDL::Frame & in, geometry_msgs::msg::Pose & msg) { @@ -231,9 +263,8 @@ struct ImplDetails } /** \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. + * \param[in] msg The Pose message to convert. + * \param[out] out The pose converted to a KDL Frame. */ static void fromMsg(const geometry_msgs::msg::Pose & msg, KDL::Frame & out) { @@ -242,35 +273,50 @@ struct ImplDetails } }; -template <> +/// \brief Default return type of tf2::toMsg() for KDL::Frame. +template<> struct defaultMessage { + /// \brief Default return type of tf2::toMsg() for KDL::Frame. using type = geometry_msgs::msg::Pose; }; -template <> +/// \brief Default Type for automatic tf2::doTransform() implementation for KDL::Frame. +template<> struct DefaultTransformType { + /// \brief Default Type for automatic tf2::doTransform() implementation for KDL::Frame. using type = KDL::Frame; }; -template <> +/// \brief Conversion implementation for geometry_msgs::msg::Quaternion and KDL::Rotation. +template<> struct ImplDetails { + /** \brief Convert a KDL Rotation type to a Quaternion message. + * \param[in] r The KDL Rotation to convert. + * \param[out] q The frame converted to a Quaternion message. + */ static void toMsg(KDL::Rotation const & r, geometry_msgs::msg::Quaternion & q) { r.GetQuaternion(q.x, q.y, q.z, q.w); } + /** \brief Convert a Quaternion message type to a KDL Rotation. + * \param[in] q The Quaternion message to convert. + * \param[out] r The quaternion converted to a KDL Rotation. + */ static void fromMsg(geometry_msgs::msg::Quaternion const & q, KDL::Rotation & r) { r = KDL::Rotation::Quaternion(q.x, q.y, q.z, q.w); } }; -template <> +/// \brief Default return type of tf2::toMsg() for KDL::Rotation. +template<> struct defaultMessage { + /// \brief Default return type of tf2::toMsg() for KDL::Rotation. using type = geometry_msgs::msg::Quaternion; }; @@ -279,11 +325,12 @@ struct defaultMessage /** * \brief Transform a KDL::Rotation * - * \param[in] data_in The data to be transformed. - * \param[in,out] data_out A reference to the output data. + * \param[in] in The data to be transformed. + * \param[in,out] out A reference to the output data. * \param[in] transform The transform to apply to data_in to fill data_out. */ template<> +inline void doTransform( const KDL::Rotation & in, KDL::Rotation & out, const geometry_msgs::msg::TransformStamped & transform) @@ -295,4 +342,4 @@ void doTransform( } // namespace tf2 -#endif // TF2_KDL_H +#endif // TF2_KDL__TF2_KDL_H_ diff --git a/tf2_kdl/test/test_tf2_kdl.cpp b/tf2_kdl/test/test_tf2_kdl.cpp index b8923e261..78fbf25d8 100644 --- a/tf2_kdl/test/test_tf2_kdl.cpp +++ b/tf2_kdl/test/test_tf2_kdl.cpp @@ -38,19 +38,27 @@ // To get M_PI, especially on Windows. #include +#include #include +#include +#include + #include -#include #include -#include "tf2_ros/buffer.h" -#include + +#include std::unique_ptr tf_buffer; static const double EPS = 1e-3; TEST(TfKDL, Frame) { - tf2::Stamped v1(KDL::Frame(KDL::Rotation::RPY(M_PI, 0, 0), KDL::Vector(1,2,3)), tf2::TimePoint(tf2::durationFromSec(2.0)), "A"); + tf2::Stamped v1(KDL::Frame( + KDL::Rotation::RPY(M_PI, 0, 0), KDL::Vector( + 1, 2, + 3)), tf2::TimePoint( + tf2::durationFromSec( + 2.0)), "A"); // simple api KDL::Frame v_simple = tf_buffer->transform(v1, "B", tf2::durationFromSec(2.0)); @@ -65,8 +73,9 @@ TEST(TfKDL, Frame) // advanced api - KDL::Frame v_advanced = tf_buffer->transform(v1, "B", tf2::TimePoint(tf2::durationFromSec(2.0)), - "A", tf2::Duration(std::chrono::seconds(3))); + KDL::Frame v_advanced = tf_buffer->transform( + v1, "B", tf2::TimePoint(tf2::durationFromSec(2.0)), + "A", tf2::Duration(std::chrono::seconds(3))); EXPECT_NEAR(v_advanced.p[0], -9, EPS); EXPECT_NEAR(v_advanced.p[1], 18, EPS); EXPECT_NEAR(v_advanced.p[2], 27, EPS); @@ -74,12 +83,12 @@ TEST(TfKDL, Frame) EXPECT_NEAR(r, 0.0, EPS); EXPECT_NEAR(p, 0.0, EPS); EXPECT_NEAR(y, 0.0, EPS); - } TEST(TfKDL, Vector) { - tf2::Stamped v1(KDL::Vector(1,2,3), tf2::TimePoint(tf2::durationFromSec(2.0)), "A"); + tf2::Stamped v1(KDL::Vector(1, 2, 3), tf2::TimePoint(tf2::durationFromSec(2.0)), + "A"); // simple api @@ -89,8 +98,9 @@ TEST(TfKDL, Vector) EXPECT_NEAR(v_simple[2], 27, EPS); // advanced api - KDL::Vector v_advanced = tf_buffer->transform(v1, "B", tf2::TimePoint(tf2::durationFromSec(2.0)), - "A", tf2::Duration(std::chrono::seconds(3))); + KDL::Vector v_advanced = tf_buffer->transform( + v1, "B", tf2::TimePoint(tf2::durationFromSec(2.0)), + "A", tf2::Duration(std::chrono::seconds(3))); EXPECT_NEAR(v_advanced[0], -9, EPS); EXPECT_NEAR(v_advanced[1], 18, EPS); EXPECT_NEAR(v_advanced[2], 27, EPS); @@ -125,9 +135,9 @@ TEST(TfKDL, Quaternion) TEST(TfKDL, ConvertVector) { tf2::Stamped v( - KDL::Vector(1,2,3), - tf2::TimePoint(tf2::durationFromSec(1.0)), - "my_frame" + KDL::Vector(1, 2, 3), + tf2::TimePoint(tf2::durationFromSec(1.0)), + "my_frame" ); tf2::Stamped v1 = v; @@ -137,9 +147,9 @@ TEST(TfKDL, ConvertVector) // Convert on same type tf2::Stamped v2( - KDL::Vector(3,4,5), - tf2::TimePoint(tf2::durationFromSec(2.0)), - "my_frame2" + KDL::Vector(3, 4, 5), + tf2::TimePoint(tf2::durationFromSec(2.0)), + "my_frame2" ); tf2::convert(v1, v2); EXPECT_EQ(v, v2); @@ -156,9 +166,9 @@ TEST(TfKDL, ConvertVector) TEST(TfKDL, ConvertTwist) { tf2::Stamped t( - KDL::Twist(KDL::Vector(1,2,3), KDL::Vector(4,5,6)), - tf2::TimePoint(tf2::durationFromSec(1.0)), - "my_frame"); + KDL::Twist(KDL::Vector(1, 2, 3), KDL::Vector(4, 5, 6)), + tf2::TimePoint(tf2::durationFromSec(1.0)), + "my_frame"); tf2::Stamped t1 = t; // Test convert with duplicate arguments @@ -167,9 +177,9 @@ TEST(TfKDL, ConvertTwist) // Convert on same type tf2::Stamped t2( - KDL::Twist(KDL::Vector(3,4,5), KDL::Vector(6,7,8)), - tf2::TimePoint(tf2::durationFromSec(2.0)), - "my_frame2"); + KDL::Twist(KDL::Vector(3, 4, 5), KDL::Vector(6, 7, 8)), + tf2::TimePoint(tf2::durationFromSec(2.0)), + "my_frame2"); tf2::convert(t1, t2); EXPECT_EQ(t, t2); EXPECT_EQ(t1, t2); @@ -185,9 +195,9 @@ TEST(TfKDL, ConvertTwist) TEST(TfKDL, ConvertWrench) { tf2::Stamped w( - KDL::Wrench(KDL::Vector(1,2,3), KDL::Vector(4,5,6)), - tf2::TimePoint(tf2::durationFromSec(1.0)), - "my_frame"); + KDL::Wrench(KDL::Vector(1, 2, 3), KDL::Vector(4, 5, 6)), + tf2::TimePoint(tf2::durationFromSec(1.0)), + "my_frame"); tf2::Stamped w1 = w; // Test convert with duplicate arguments @@ -196,9 +206,9 @@ TEST(TfKDL, ConvertWrench) // Convert on same type tf2::Stamped w2( - KDL::Wrench(KDL::Vector(3,4,5), KDL::Vector(6,7,8)), - tf2::TimePoint(tf2::durationFromSec(2.0)), - "my_frame2"); + KDL::Wrench(KDL::Vector(3, 4, 5), KDL::Vector(6, 7, 8)), + tf2::TimePoint(tf2::durationFromSec(2.0)), + "my_frame2"); tf2::convert(w1, w2); EXPECT_EQ(w, w2); EXPECT_EQ(w1, w2); @@ -214,9 +224,9 @@ TEST(TfKDL, ConvertWrench) TEST(TfKDL, ConvertFrame) { tf2::Stamped f( - KDL::Frame(KDL::Rotation::RPY(M_PI, 0, 0), KDL::Vector(1,2,3)), - tf2::TimePoint(tf2::durationFromSec(1.0)), - "my_frame"); + KDL::Frame(KDL::Rotation::RPY(M_PI, 0, 0), KDL::Vector(1, 2, 3)), + tf2::TimePoint(tf2::durationFromSec(1.0)), + "my_frame"); tf2::Stamped f1 = f; // Test convert with duplicate arguments @@ -225,9 +235,9 @@ TEST(TfKDL, ConvertFrame) // Convert on same type tf2::Stamped f2( - KDL::Frame(KDL::Rotation::RPY(0, M_PI, 0), KDL::Vector(4,5,6)), - tf2::TimePoint(tf2::durationFromSec(2.0)), - "my_frame2"); + KDL::Frame(KDL::Rotation::RPY(0, M_PI, 0), KDL::Vector(4, 5, 6)), + tf2::TimePoint(tf2::durationFromSec(2.0)), + "my_frame2"); tf2::convert(f1, f2); EXPECT_EQ(f, f2); EXPECT_EQ(f1, f2); @@ -240,7 +250,8 @@ TEST(TfKDL, ConvertFrame) EXPECT_EQ(f, f3); } -int main(int argc, char **argv){ +int main(int argc, char ** argv) +{ testing::InitGoogleTest(&argc, argv); rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); From d1f5ceaff97b0005e7ed59be9a8912e0fd1c5c64 Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Thu, 28 Jan 2021 23:52:58 +0000 Subject: [PATCH 28/51] tf2_bullet doxygen --- tf2_bullet/include/tf2_bullet/tf2_bullet.h | 26 +++++++++++++++++++++- 1 file changed, 25 insertions(+), 1 deletion(-) diff --git a/tf2_bullet/include/tf2_bullet/tf2_bullet.h b/tf2_bullet/include/tf2_bullet/tf2_bullet.h index e8461b844..da02ab382 100644 --- a/tf2_bullet/include/tf2_bullet/tf2_bullet.h +++ b/tf2_bullet/include/tf2_bullet/tf2_bullet.h @@ -52,43 +52,57 @@ namespace tf2 { namespace impl { +/// \brief Default return type of tf2::toMsg() for btVector3. template<> struct defaultMessage { + /// \brief Default return type of tf2::toMsg() for btVector3. using type = geometry_msgs::msg::Point; }; +/// \brief Default return type of tf2::toMsg() for btQuaternion. template<> struct defaultMessage { + /// \brief Default return type of tf2::toMsg() for btQuaternion. using type = geometry_msgs::msg::Quaternion; }; +/// \brief Default return type of tf2::toMsg() for btTransform. template<> struct defaultMessage { + /// \brief Default return type of tf2::toMsg() for btTransform. using type = geometry_msgs::msg::Transform; }; +/// \brief Conversion implementation for geometry_msgs::msg::Point and btVector3. template<> struct ImplDetails : DefaultVectorImpl { }; +/// \brief Conversion implementation for geometry_msgs::msg::Vector3 and btVector3. template<> struct ImplDetails : DefaultVectorImpl { }; +/// \brief Conversion implementation for geometry_msgs::msg::Quaternion and Eigen::Quaterniond. template<> struct ImplDetails : DefaultQuaternionImpl {}; +/// \brief Conversion implementation for geometry_msgs::msg::Transform and btTransform. template<> struct ImplDetails { + /** \brief Convert a Transform message to a btTransform type. + * \param[in] in The message to convert. + * \param[out] out The converted message, as a btTransform type. + */ static void fromMsg(geometry_msgs::msg::Transform const & in, btTransform & out) { btVector3 trans; @@ -98,6 +112,10 @@ struct ImplDetails out = btTransform(rot, trans); } + /** \brief Convert a btTransform to a Transform message. + * \param[in] in The btTransform to convert. + * \param[out] out The converted Transform, as a message. + */ static void toMsg(btTransform const & in, geometry_msgs::msg::Transform & out) { tf2::toMsg<>(in.getRotation(), out.rotation); @@ -105,27 +123,33 @@ struct ImplDetails } }; +/// \brief Default Type for automatic tf2::doTransform() implementation for btTransform. template<> struct DefaultTransformType { + /// \brief Default Type for automatic tf2::doTransform() implementation for btTransform. using type = btTransform; }; +/// \brief Default Type for automatic tf2::doTransform() implementation for btVector3. template<> struct DefaultTransformType { + /// \brief Default Type for automatic tf2::doTransform() implementation for btVector3. using type = btTransform; }; +/// \brief Default Type for automatic tf2::doTransform() implementation for btQuaternion. template<> struct DefaultTransformType { + /// \brief Default Type for automatic tf2::doTransform() implementation for btTransform. using type = btTransform; }; } // namespace impl /** \brief Convert a timestamped transform to the equivalent Bullet data type. - * \param t The transform to convert, as a geometry_msgs TransformedStamped message. + * \param[in] t The transform to convert, as a geometry_msgs TransformedStamped message. * \return The transform message converted to a Bullet btTransform. */ inline btTransform transformToBullet(const geometry_msgs::msg::TransformStamped & t) From 747836a5405b285f12fc557e8be2d4c634aa8519 Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Fri, 12 Feb 2021 00:24:50 +0000 Subject: [PATCH 29/51] Enable Linters on geometry_msgs and KDL --- tf2_geometry_msgs/CMakeLists.txt | 11 +++++++++++ tf2_geometry_msgs/package.xml | 2 ++ tf2_kdl/CMakeLists.txt | 12 ++++++++++-- tf2_kdl/package.xml | 2 ++ 4 files changed, 25 insertions(+), 2 deletions(-) diff --git a/tf2_geometry_msgs/CMakeLists.txt b/tf2_geometry_msgs/CMakeLists.txt index 7d5fd6db7..b6d1429f7 100644 --- a/tf2_geometry_msgs/CMakeLists.txt +++ b/tf2_geometry_msgs/CMakeLists.txt @@ -30,6 +30,17 @@ ament_auto_find_build_dependencies(REQUIRED ${required_dependencies}) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) find_package(rclcpp REQUIRED) + + find_package(ament_cmake_cppcheck REQUIRED) + find_package(ament_cmake_cpplint REQUIRED) + find_package(ament_cmake_lint_cmake REQUIRED) + find_package(ament_cmake_uncrustify REQUIRED) + + ament_cppcheck(LANGUAGE "c++") + ament_cpplint() + ament_lint_cmake() + ament_uncrustify(LANGUAGE "c++") + ament_add_gtest(test_tf2_geometry_msgs test/test_tf2_geometry_msgs.cpp) if(TARGET test_tf2_geometry_msgs) target_include_directories(test_tf2_geometry_msgs PUBLIC include) diff --git a/tf2_geometry_msgs/package.xml b/tf2_geometry_msgs/package.xml index c5e3d4846..070652a3b 100644 --- a/tf2_geometry_msgs/package.xml +++ b/tf2_geometry_msgs/package.xml @@ -27,6 +27,8 @@ tf2_ros_py ament_cmake_gtest + ament_lint_auto + ament_lint_common rclcpp diff --git a/tf2_kdl/CMakeLists.txt b/tf2_kdl/CMakeLists.txt index c9934979a..3a058600f 100644 --- a/tf2_kdl/CMakeLists.txt +++ b/tf2_kdl/CMakeLists.txt @@ -18,10 +18,10 @@ find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) ament_python_install_package(${PROJECT_NAME} - PACKAGE_DIR src/${PROJECT_NAME}) + PACKAGE_DIR src/${PROJECT_NAME}) install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION include/${PROJECT_NAME} + DESTINATION include/${PROJECT_NAME} ) # TODO(ahcorde): Port python once https://github.com/ros2/orocos_kinematics_dynamics/pull/4 is merged @@ -34,6 +34,14 @@ if(BUILD_TESTING) find_package(rclcpp REQUIRED) find_package(tf2_msgs REQUIRED) + find_package(ament_cmake_cppcheck REQUIRED) + find_package(ament_cmake_cpplint REQUIRED) + find_package(ament_cmake_lint_cmake REQUIRED) + find_package(ament_cmake_uncrustify REQUIRED) + + ament_cppcheck(LANGUAGE "c++") + ament_uncrustify(LANGUAGE "c++") + ament_add_gtest(test_kdl test/test_tf2_kdl.cpp) target_include_directories(test_kdl PUBLIC include diff --git a/tf2_kdl/package.xml b/tf2_kdl/package.xml index 52a173a38..d48062383 100644 --- a/tf2_kdl/package.xml +++ b/tf2_kdl/package.xml @@ -22,6 +22,8 @@ tf2_ros_py ament_cmake_gtest + ament_lint_auto + ament_lint_common rclcpp tf2_msgs From f1469a0d4e1e32611388d30d387c66f228df4c14 Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Fri, 29 Jan 2021 13:05:51 +0000 Subject: [PATCH 30/51] more meaningful names --- tf2/include/tf2/convert.h | 91 +++-- tf2/include/tf2/impl/convert.h | 312 ++++++++++----- tf2/include/tf2/impl/forward.h | 12 +- tf2/include/tf2/impl/stamped_traits.h | 357 ++++++++++++------ tf2/include/tf2/impl/time_convert.h | 46 ++- tf2_bullet/include/tf2_bullet/tf2_bullet.h | 20 +- tf2_eigen/include/tf2_eigen/tf2_eigen.h | 32 +- .../tf2_geometry_msgs/tf2_geometry_msgs.h | 26 +- tf2_kdl/include/tf2_kdl/tf2_kdl.h | 28 +- 9 files changed, 618 insertions(+), 306 deletions(-) diff --git a/tf2/include/tf2/convert.h b/tf2/include/tf2/convert.h index 9a87d5f0e..2e5ada088 100644 --- a/tf2/include/tf2/convert.h +++ b/tf2/include/tf2/convert.h @@ -47,16 +47,17 @@ namespace tf2 { -/**\brief The templated function expected to be able to do a transform +/** \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. - * \param data_in[in] The data to be transformed. - * \param data_out[inout] A reference to the output data. Note this can point to data in and the method should be mutation safe. - * \param transform[in] The transform to apply to data_in to fill data_out. + * \param[in] data_in The data to be transformed. + * \param[in,out] data_out A reference to the output data. Note this can point to data in and the method should be mutation safe. + * \param[in] transform The transform to apply to data_in to fill data_out. + * \tparam T The type of the data to be transformed. * * This method needs to be implemented by client library developers */ -template +template void doTransform( const T & data_in, T & data_out, const geometry_msgs::msg::TransformStamped & transform) { @@ -66,7 +67,16 @@ void doTransform( data_out = t * data_in; } -template +/** \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. + * Overload for tf2::Stamped\ types, uses doTransform(const T&, T&, const geometry_msgs::msg::TransformStamped&). + * \param[in] data_in The data to be transformed. + * \param[in,out] data_out A reference to the output data. Note this can point to data in and the method should be mutation safe. + * \param[in] transform The transform to apply to data_in to fill data_out. + * \tparam T The type of the data to be transformed. + */ +template void doTransform( tf2::Stamped const & data_in, tf2::Stamped & data_out, const geometry_msgs::msg::TransformStamped & transform) @@ -80,20 +90,20 @@ void doTransform( * \param[in] t The data input. * \return The timestamp associated with the data. */ -template +template inline tf2::TimePoint getTimestamp(const T & t) { - return impl::DefaultStampedImpl::getTimestamp(t); + return impl::StampedAttributesHelper::getTimestamp(t); } /**\brief Get the frame_id from data * \param[in] t The data input. * \return The frame_id associated with the data. */ -template +template inline std::string getFrameId(const T & t) { - return impl::DefaultStampedImpl::getFrameId(t); + return impl::StampedAttributesHelper::getFrameId(t); } /**\brief Get the covariance matrix from data @@ -103,7 +113,7 @@ inline std::string getFrameId(const T & t) template std::array, 6> getCovarianceMatrix(const T & t) { - using traits = impl::stampedMessageTraits; + using traits = impl::StampedMessageTraits; return covarianceRowMajorToNested(traits::getCovariance(t)); } @@ -124,17 +134,17 @@ std::array, 6> getCovarianceMatrix(const tf2::WithCovarian * \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 + * for each datatypes. Preferably in a specialization of the impl::ConversionImplementation struct. + * \param[in] 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. + * \tparam B ROS message Datatype. The default value will be taken from impl::DefaultMessageForDatatype\::type. * \return the conversion as a ROS message */ -template +template inline B toMsg(const A & a) { B b; - impl::ImplDetails::toMsg(a, b); + impl::ConversionImplementation::toMsg(a, b); return b; } @@ -142,17 +152,17 @@ inline B toMsg(const A & a) * \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 + * for each datatypes. Preferably in a specialization of the impl::ConversionImplementation struct. + * \param[in] a an object of whatever type + * \param[out] b ROS message * \tparam A Non-message Datatype * \tparam B Type of the ROS Message * \return Reference to the parameter b */ -template +template inline B & toMsg(const A & a, B & b) { - impl::ImplDetails::toMsg(a, b); + impl::ConversionImplementation::toMsg(a, b); return b; } @@ -160,16 +170,16 @@ inline B & toMsg(const A & a, B & b) * \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 + * for each datatypes. Preferably in a specialization of the impl::ConversionImplementation struct. + * \param[in] a a ROS message to convert from + * \param[out] b the object to convert to * \tparam A ROS message type * \tparam B Arbitrary type */ -template +template inline void fromMsg(const A & a, B & b) { - impl::ImplDetails::fromMsg(a, b); + impl::ConversionImplementation::fromMsg(a, b); } /** @@ -178,12 +188,12 @@ inline void fromMsg(const A & a, B & b) * 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 + * \param[in] a an object to convert from + * \param[in,out] 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 +template inline void convert(const A & a, B & b) { impl::Converter< @@ -191,7 +201,12 @@ inline void convert(const A & a, B & b) rosidl_generator_traits::is_message::value>::convert(a, b); } -template +/** \brief Overload of tf2::convert() for the same types. + * \param[in] a1 an object to convert from + * \param[in,out] a2 the object to convert to + * \tparam A Type of the object to convert + */ +template void convert(const A & a1, A & a2) { if (&a1 != &a2) { @@ -199,13 +214,15 @@ void convert(const A & a1, A & a2) } } -/**\brief Function that converts from a row-major representation of a 6x6 +/** \brief Function that converts from a row-major representation of a 6x6 * covariance matrix to a nested array representation. - * \param row_major A row-major array of 36 covariance values. + * \param[in] row_major A row-major array of 36 covariance values. * \return A nested array representation of 6x6 covariance values. */ inline -std::array, 6> covarianceRowMajorToNested(const std::array & row_major) +std::array, 6> covarianceRowMajorToNested( + const std::array & row_major) { std::array, 6> nested_array = {}; size_t l1 = 0, l2 = 0; @@ -222,13 +239,15 @@ std::array, 6> covarianceRowMajorToNested(const std::array return nested_array; } -/**\brief Function that converts from a nested array representation of a 6x6 +/** \brief Function that converts from a nested array representation of a 6x6 * covariance matrix to a row-major representation. - * \param nested_array A nested array representation of 6x6 covariance values. + * \param[in] nested_array A nested array representation of 6x6 covariance values. * \return A row-major array of 36 covariance values. */ inline -std::array covarianceNestedToRowMajor(const std::array, 6> & nested_array) +std::array covarianceNestedToRowMajor( + const std::array, + 6> & nested_array) { std::array row_major = {}; size_t counter = 0; diff --git a/tf2/include/tf2/impl/convert.h b/tf2/include/tf2/impl/convert.h index aac076c3a..1a5f0a48c 100644 --- a/tf2/include/tf2/impl/convert.h +++ b/tf2/include/tf2/impl/convert.h @@ -47,15 +47,30 @@ namespace impl * 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 + * \tparam Datatype Non-Message datatype like \c Vector3d . */ -template -struct defaultMessage +template +struct DefaultMessageForDatatype { // using type = ...; }; -template +/** + * \brief Transform type for the default implementation of tf2::doTransform(). + * + * The default implementation of tf2::doTransform() needs to convert the + * TransformStamped parameter into a type to perform the transform operation + * \code + * tf2::convert(transform, converted_transform); + * out = converted_transform * in; + * \endcode + * So, this struct needs to be specialized for each type which is passed + * to the default implementation of tf2::doTransform(). + * It needs to contain an alias of the transform datatype named \c type . + * + * \tparam Datatype Datatype to be transformed. + */ +template struct DefaultTransformType { // using type = ...; @@ -77,11 +92,13 @@ struct DefaultTransformType * static void fromMsg(const Message&, Datatype&); * }: * \endcode - * Note that the conversion between tf2::Stamped\ and - * geometry_msgs::...Stamped is done automatically. + * Note that the conversion between ( tf2::Stamped\ and + * geometry_msgs::...Stamped ) and + * ( tf2::WithCovarianceStamped\ and geometry_msgs::...WithCovarianceStamped ) + * is done automatically. */ -template -struct ImplDetails +template +struct ConversionImplementation { // void toMsg(const Datatype&, Message&); // void fromMsg(const Message&, Datatype&); @@ -91,7 +108,7 @@ struct ImplDetails * \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. + * and an alias (named \c UntampedType ) 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, @@ -99,9 +116,9 @@ struct ImplDetails * For this class, the specialization should look like * \code * template<> - * struct stampedMessageTraits + * struct StampedMessageTraits * { - * using unstampedType = geometry_msgs::Vector3; + * using UntampedType = geometry_msgs::Vector3; * static geometry_msgs::Vector3& accessMessage(geometry_msgs::Vector3Stamped& vs) * { * return vs.vector; @@ -114,14 +131,20 @@ struct ImplDetails * \endcode * The both almost identical methods are required to keep const-correctness. * + * If the message is a stamped message with a covariance member, + * accessMessage() should return the underlying message (e.g. Pose) + * and accessCovariance() should return the covariance accordingly. + * * \tparam StampedMessage The datatype of the ros message */ -template -struct stampedMessageTraits +template +struct StampedMessageTraits { - // using unstampedType = ...; - // static unstampedType& accessMessage(StampedMsg &); - // static unstampedType getMessage(StampedMsg const&); + // using UntampedType = ...; + // static UntampedType& accessMessage(StampedMsg &); + // static UntampedType getMessage(StampedMsg const&); + // static CovarianceType & accessCovariance(MsgWithCovarianceStamped &); + // static CovarianceType getCovariance(MsgWithCovarianceStamped const&); }; /** @@ -129,62 +152,80 @@ struct stampedMessageTraits * * 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 ) + * Its specializations should contain an alias (named \c StampedType ) * of the stamped type. * Example: * \code * template<> - * struct unstampedMessageTraits + * struct UnstampedMessageTraits * { - * using stampedType = geometry_msgs::Vector3Stamped; + * using StampedType = geometry_msgs::Vector3Stamped; * }; * \endcode * + * If messages with covariance are also available, + * an alias with the name \c StampedTypeWithCovariance + * to the accoring message type should be declared. + * * \tparam UnstampedMessage Type of the ROS message which is not stamped */ -template -struct unstampedMessageTraits +template +struct UnstampedMessageTraits { - // using stampedType = ...; + // using StampedType = ...; + // using StampedTypeWithCovariance = ...; }; /** - * \brief Partial specialization of impl::defaultMessage for stamped types + * \brief Partial specialization of impl::DefaultMessageForDatatype 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> +template +struct DefaultMessageForDatatype> { - using type = typename unstampedMessageTraits::type>::stampedType; + using type = + typename UnstampedMessageTraits::type>::StampedType; }; -template -struct defaultMessage> +/** + * \brief Partial specialization of impl::DefaultMessageForDatatype for stamped types with covariance. + * + * The deduction of the default ROS message type of a tf2::WithCovarianceStamped\ type is + * based on the default ROS message type of \c T . + * \tparam T The unstamped datatype (not a ROS message) + */ +template +struct DefaultMessageForDatatype> { using type = - typename unstampedMessageTraits::type>::stampedTypeWithCovariance; + typename UnstampedMessageTraits::type>:: + StampedTypeWithCovariance; }; /** - * \brief Partial specialization of impl::ImplDetails for stamped types + * \brief Partial specialization of impl::ConversionImplementation 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. + * as well as a specialization of StampedMessageTraits. * \tparam Datatype Unstamped non-message type * \tparam StampedMessage Stamped ROS message type */ -template -struct ImplDetails, StampedMessage> +template +struct ConversionImplementation, StampedMessage> { - using traits = stampedMessageTraits; + /// Typedefs and utility functions for the given message + using traits = StampedMessageTraits; + /** \brief Convert a stamped datatype to a stamped message. + * \param[in] s Stamped datatype to covert. + * \param[out] msg The stamped datatype, as a stamped message. + */ static void toMsg(const tf2::Stamped & s, StampedMessage & msg) { tf2::toMsg<>(static_cast(s), traits::accessMessage(msg)); @@ -192,6 +233,10 @@ struct ImplDetails, StampedMessage> msg.header.frame_id = s.frame_id_; } + /** \brief Convert a stamped message to a stamped datatype. + * \param[in] msg Stamped message to covert + * \param[out] s The stamped message, as a stamped datatype. + */ static void fromMsg(const StampedMessage & msg, tf2::Stamped & s) { tf2::fromMsg<>(traits::getMessage(msg), static_cast(s)); @@ -200,10 +245,28 @@ struct ImplDetails, StampedMessage> } }; -template -struct ImplDetails, CovarianceStampedMessage> +/** + * \brief Partial specialization of impl::ConversionImplementation for stamped types with covariance. + * + * This partial specialization provides the conversion implementation ( \c toMsg() and \c fromMsg() ) + * between stamped types with covariance ( non-message types of tf2::WithCovarianceStamped\ + * and ROS message datatypes with a \c header member). + * The covariance, the timestamp and the frame ID are preserved during the conversion. + * The implementation of tf2::toMsg() and tf2::fromMsg() for the unstamped types without covariance + * are required, as well as a specialization of StampedMessageTraits. + * \tparam Datatype Unstamped non-message type + * \tparam CovarianceStampedMessage Stamped ROS message type with covariance + */ +template +struct ConversionImplementation, CovarianceStampedMessage> { - using traits = stampedMessageTraits; + /// Typedefs and utility functions for the given message + using traits = StampedMessageTraits; + + /** \brief Convert a stamped datatype to a stamped message. + * \param[in] in Stamped datatype to covert. + * \param[out] out The stamped datatype, as a stamped message. + */ static void toMsg(tf2::WithCovarianceStamped const & in, CovarianceStampedMessage & out) { CovarianceStampedMessage tmp; @@ -214,6 +277,10 @@ struct ImplDetails, CovarianceStampedMessag out = std::move(tmp); } + /** \brief Convert a stamped message to a stamped datatype. + * \param[in] in Stamped message to covert + * \param[out] out The stamped message, as a stamped datatype. + */ static void fromMsg( CovarianceStampedMessage const & in, tf2::WithCovarianceStamped & out) { @@ -226,47 +293,78 @@ struct ImplDetails, CovarianceStampedMessag } }; -template +/** \brief Helper for tf2::convert(). + * \tparam IS_MESSAGE_A True if first argument is a message type. + * \tparam IS_MESSAGE_B True if second argument is a message type. + */ +template class Converter { public: - template + /** \brief Implementation of tf2::convert() depending of the argument types. + * \param[in] a Source of conversion. + * \param[out] b Target of conversion. + * \tparam A Type of first argument. + * \tparam B Type of second argument. + */ + template static void convert(const A & a, B & b); -}; -// The case where both A and B are messages should not happen: if you have two -// messages that are interchangeable, well, that's against the ROS purpose: -// only use one type. Worst comes to worst, specialize the original convert -// function for your types. -// 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); + // The case where both A and B are messages should not happen: if you have two + // messages that are interchangeable, well, that's against the ROS purpose: + // only use one type. Worst comes to worst, specialize the original convert + // function for your types. + // if B == A, the templated version of convert with only one argument will be + // used. + // + static_assert( + !(IS_MESSAGE_A && IS_MESSAGE_B), + "Conversion between two Message types is not supported!"); +}; -template <> -template +/** \brief Implementation of tf2::convert() for message-to-datatype conversion. + * \param[in] a Message to convert. + * \param[out] b Datatype to convert to. + * \tparam A Message type. + * \tparam B Datatype. + */ +template<> +template inline void Converter::convert(const A & a, B & b) { fromMsg<>(a, b); } -template <> -template +/** \brief Implementation of tf2::convert() for datatype-to-message converiosn. + * \param[in] a Datatype to convert. + * \param[out] b Message to convert to. + * \tparam A Datatype. + * \tparam B Message. + */ +template<> +template inline void Converter::convert(const A & a, B & b) { b = toMsg(a); } -template <> -template +/** \brief Implementation of tf2::convert() for datatypes. + * Converts the first argument to a message + * (usually \c impl::DefaultMessageForDatatype::type ) + * and then converts the message to the second argument. + * \param[in] a Source of conversion. + * \param[out] b Target of conversion. + * \tparam A Datatype of first argument. + * \tparam B Datatype of second argument. + */ +template<> +template inline void Converter::convert(const A & a, B & b) { fromMsg<>(toMsg<>(a), b); } -template +template using void_t = void; /** @@ -277,18 +375,17 @@ using void_t = void; * * \tparam T Arbitrary datatype */ -template -struct DefaultStampedImpl +template +struct StampedAttributesHelper { /**\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. + * \return The timestamp associated with the data. * * The second parameter is needed to hide the default implementation if T is not a stamped ROS message. */ static tf2::TimePoint getTimestamp( - const T & t, void_t::unstampedType> * = nullptr) + const T & t, void_t::UntampedType> * = nullptr) { tf2::TimePoint timestamp; tf2::fromMsg<>(t.header.stamp, timestamp); @@ -296,45 +393,64 @@ struct DefaultStampedImpl } /**\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. + * \return The frame_id associated with the data. * * The second parameter is needed to hide the default implementation if T is not a stamped ROS message. */ static std::string getFrameId( - const T & t, void_t::unstampedType> * = nullptr) + const T & t, void_t::UntampedType> * = nullptr) { return t.header.frame_id; } }; /** - * \brief Partial specialization of DefaultStampedImpl for tf2::Stamped\<\> types + * \brief Partial specialization of StampedAttributesHelper for tf2::Stamped\<\> types */ -template -struct DefaultStampedImpl> +template +struct StampedAttributesHelper> { - /**\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. + * \return The timestamp associated with the data. */ - static tf2::TimePoint getTimestamp(const tf2::Stamped & t) { return t.stamp_; } - /** brief Get the frame_id from data + static tf2::TimePoint 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. + * \return The frame_id associated with the data. */ - static std::string getFrameId(const tf2::Stamped & t) { return t.frame_id_; } + static std::string getFrameId(const tf2::Stamped & t) {return t.frame_id_;} }; -template -struct DefaultVectorImpl +/** + * \brief Partial specialization of StampedAttributesHelper for tf2::WithCovarianceStamped\<\> types + */ +template +struct StampedAttributesHelper> { - /** \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. + /** \brief Get the timestamp from data + * \param t The data input. + * \return The timestamp associated with the data. + */ + static tf2::TimePoint getTimestamp(const tf2::WithCovarianceStamped & t) {return t.stamp_;} + /** \brief Get the frame_id from data + * \param t The data input. + * \return The frame_id associated with the data. + */ + static std::string getFrameId(const tf2::WithCovarianceStamped & t) {return t.frame_id_;} +}; + +/** \brief Generic conversion of a vector and a vector-like message. + * + * \tparam VectorType Datatype of the Vector. + * \tparam Message Message type, like geometry_msgs::msg::Vector3. + */ +template +struct DefaultVectorConversionImplementation +{ + /** \brief Convert a vector type to a vector-like message. + * \param[in] in The vector to convert. + * \param[out] msg The converted vector, as a message. */ static void toMsg(const VectorType & in, Message & msg) { @@ -343,10 +459,9 @@ struct DefaultVectorImpl 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. + /** \brief Convert a vector-like message type to a vector type. + * \param[in] msg The message to convert. + * \param[out] out The message converted to a vector type. */ static void fromMsg(const Message & msg, VectorType & out) { @@ -354,13 +469,17 @@ struct DefaultVectorImpl } }; -template -struct DefaultQuaternionImpl +/** \brief Generic conversion of a quaternion and + * a geometry_msgs::msg::Quaternion message. + * + * \tparam QuaternionType Datatype of the Vector. + */ +template +struct DefaultQuaternionConversionImplementation { - /** \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. + /** \brief Convert a quaternion type to a Quaternion message. + * \param[in] in The quaternion convert. + * \param[out] msg The quaternion converted to a Quaternion message. */ static void toMsg(const QuaternionType & in, geometry_msgs::msg::Quaternion & msg) { @@ -370,10 +489,9 @@ struct DefaultQuaternionImpl msg.w = in[3]; } - /** \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. + /** \brief Convert a Quaternion message type to a quaternion type. + * \param[in] msg The Quaternion message to convert. + * \param[out] out The Quaternion message converted to a quaternion type. */ static void fromMsg(const geometry_msgs::msg::Quaternion & msg, QuaternionType & out) { diff --git a/tf2/include/tf2/impl/forward.h b/tf2/include/tf2/impl/forward.h index d3728b8df..89c410095 100644 --- a/tf2/include/tf2/impl/forward.h +++ b/tf2/include/tf2/impl/forward.h @@ -38,7 +38,7 @@ namespace tf2 namespace impl { template -struct defaultMessage; +struct DefaultMessageForDatatype; template struct DefaultTransformType; } // namespace impl @@ -52,7 +52,7 @@ class WithCovarianceStamped; template B & toMsg(const A & a, B & b); -template ::type> +template ::type> B toMsg(const A & a); template @@ -66,16 +66,16 @@ std::array covarianceNestedToRowMajor( namespace impl { template -struct ImplDetails; +struct ConversionImplementation; template -struct stampedMessageTraits; +struct StampedMessageTraits; template -struct unstampedMessageTraits; +struct UnstampedMessageTraits; template -struct DefaultStampedImpl; +struct StampedAttributesHelper; template class Converter; diff --git a/tf2/include/tf2/impl/stamped_traits.h b/tf2/include/tf2/impl/stamped_traits.h index c6363adda..a8e35ffc0 100644 --- a/tf2/include/tf2/impl/stamped_traits.h +++ b/tf2/include/tf2/impl/stamped_traits.h @@ -1,4 +1,3 @@ - /* * Copyright (c) 2021, Bjarne von Horn * All rights reserved. @@ -41,43 +40,43 @@ namespace geometry_msgs { namespace msg { -template +template class Point_; -template +template class Vector_; -template +template class Quaternion_; -template +template class Pose_; -template +template class Twist_; -template +template class PoseWithCovariance_; -template +template class Wrench_; -template +template class PointStamped_; -template +template class VectorStamped_; -template +template class QuaternionStamped_; -template +template class PoseStamped_; -template +template class TwistStamped_; -template +template class PoseWithCovarianceStamped_; -template +template class WrenchStamped_; -template +template class TransformStamped_; -template +template class Transform_; -template +template class Vector3_; -template +template class Vector3Stamped_; -template +template class TwistWithCovarianceStamped_; } // namespace msg } // namespace geometry_msgs @@ -87,161 +86,307 @@ namespace tf2 namespace impl { -template -struct unstampedMessageTraits> +/** \brief Traits for geometry_msgs::msg::Point. + * \tparam Alloc Message Allocator +*/ +template +struct UnstampedMessageTraits> { - using stampedType = geometry_msgs::msg::PointStamped_; + /// The corresponding stamped message type. + using StampedType = geometry_msgs::msg::PointStamped_; }; -template -struct unstampedMessageTraits> +/** \brief Traits for geometry_msgs::msg::Vector. + * \tparam Alloc Message Allocator + */ +template +struct UnstampedMessageTraits> { - using stampedType = geometry_msgs::msg::VectorStamped_; + /// The corresponding stamped message type. + using StampedType = geometry_msgs::msg::VectorStamped_; }; -template -struct unstampedMessageTraits> +/** \brief Traits for geometry_msgs::msg::Quaternion. + * \tparam Alloc Message Allocator + */ +template +struct UnstampedMessageTraits> { - using stampedType = geometry_msgs::msg::QuaternionStamped_; + /// The corresponding stamped message type. + using StampedType = geometry_msgs::msg::QuaternionStamped_; }; -template -struct unstampedMessageTraits> +/** \brief Traits for geometry_msgs::msg::Pose. + * \tparam Alloc Message Allocator + */ +template +struct UnstampedMessageTraits> { - using stampedType = geometry_msgs::msg::PoseStamped_; - using stampedTypeWithCovariance = geometry_msgs::msg::PoseWithCovarianceStamped_; + /// The corresponding stamped message type. + using StampedType = geometry_msgs::msg::PoseStamped_; + /// The corresponding stamped message type with covariance. + using StampedTypeWithCovariance = geometry_msgs::msg::PoseWithCovarianceStamped_; }; -template -struct unstampedMessageTraits> +/** \brief Traits for geometry_msgs::msg::Twist. + * \tparam Alloc Message Allocator + */ +template +struct UnstampedMessageTraits> { - using stampedType = geometry_msgs::msg::TwistStamped_; - using stampedTypeWithCovariance = geometry_msgs::msg::TwistWithCovarianceStamped_; + /// The corresponding stamped message type. + using StampedType = geometry_msgs::msg::TwistStamped_; + /// The corresponding stamped message type with covariance. + using StampedTypeWithCovariance = geometry_msgs::msg::TwistWithCovarianceStamped_; }; -template -struct unstampedMessageTraits> +/** \brief Traits for geometry_msgs::msg::PoseWithCovariance. + * \tparam Alloc Message Allocator + */ +template +struct UnstampedMessageTraits> { - using stampedType = geometry_msgs::msg::PoseWithCovarianceStamped_; + /// The corresponding stamped message type with covariance. + using StampedType = geometry_msgs::msg::PoseWithCovarianceStamped_; }; -template -struct unstampedMessageTraits> +/** \brief Traits for geometry_msgs::msg::Wrench. + * \tparam Alloc Message Allocator + */ +template +struct UnstampedMessageTraits> { - using stampedType = geometry_msgs::msg::WrenchStamped_; + /// The corresponding stamped message type. + using StampedType = geometry_msgs::msg::WrenchStamped_; }; -template -struct unstampedMessageTraits> +/** \brief Traits for geometry_msgs::msg::Transform. + * \tparam Alloc Message Allocator + */ +template +struct UnstampedMessageTraits> { - using stampedType = geometry_msgs::msg::TransformStamped_; + /// The corresponding stamped message type. + using StampedType = geometry_msgs::msg::TransformStamped_; }; -template -struct unstampedMessageTraits> +/** \brief Traits for geometry_msgs::msg::Vector3. + * \tparam Alloc Message Allocator + */ +template +struct UnstampedMessageTraits> { - using stampedType = geometry_msgs::msg::Vector3Stamped_; + /// The corresponding stamped message type. + using StampedType = geometry_msgs::msg::Vector3Stamped_; }; -template -struct defaultStampedMessageTraits +/** \brief Template for fast implementation of StampedMessageTraits. + * \tparam StampedMessage Type of the stamped message. + * \tparam UnstampedMessage Type of the underlying message. + * \tparam member Pointer-to-member of the underlying message, e.g. \c &PoseStamped::pose . + */ +template +struct DefaultStampedMessageTraits { - using unstampedType = UnstampedMessage; - static unstampedType & accessMessage(StampedMessage & stamped) { return stamped.*member; } - static unstampedType getMessage(StampedMessage const & stamped) { return stamped.*member; } + /// The underlying message type. + using UntampedType = UnstampedMessage; + + /** \brief Read-Write access to the underlying message. + * \param[in] stamped Reference to the stamped message. + * \return Reference to the unstamped message. + */ + static UntampedType & accessMessage(StampedMessage & stamped) {return stamped.*member;} + + /** \brief Read-only access to the underlying message. + * \param[in] stamped Reference to the stamped message. + * \return Copy of the unstamped message. + */ + static UntampedType getMessage(StampedMessage const & stamped) {return stamped.*member;} }; // we use partial specializations (with the allocator as template parameter) // to avoid including all the message definitons -template -struct stampedMessageTraits> -: defaultStampedMessageTraits< +/** \brief Traits for geometry_msgs::msg::PointStamped. + * \tparam Alloc Message Allocator + */ +template +struct StampedMessageTraits> + : DefaultStampedMessageTraits< geometry_msgs::msg::PointStamped_, geometry_msgs::msg::Point_, - &geometry_msgs::msg::PointStamped_::point> + & geometry_msgs::msg::PointStamped_::point> { }; -template -struct stampedMessageTraits> -: defaultStampedMessageTraits< +/** \brief Traits for geometry_msgs::msg::VectorStamped. + * \tparam Alloc Message Allocator + */ +template +struct StampedMessageTraits> + : DefaultStampedMessageTraits< geometry_msgs::msg::VectorStamped_, geometry_msgs::msg::Vector_, - &geometry_msgs::msg::VectorStamped_::vector> + & geometry_msgs::msg::VectorStamped_::vector> { }; -template -struct stampedMessageTraits> -: defaultStampedMessageTraits< +/** \brief Traits for geometry_msgs::msg::QuaternionStamped. + * \tparam Alloc Message Allocator + */ +template +struct StampedMessageTraits> + : DefaultStampedMessageTraits< geometry_msgs::msg::QuaternionStamped_, geometry_msgs::msg::Quaternion_, - &geometry_msgs::msg::QuaternionStamped_::quaternion> + & geometry_msgs::msg::QuaternionStamped_::quaternion> { }; -template -struct stampedMessageTraits> -: defaultStampedMessageTraits< +/** \brief Traits for geometry_msgs::msg::PoseStamped. + * \tparam Alloc Message Allocator + */ +template +struct StampedMessageTraits> + : DefaultStampedMessageTraits< geometry_msgs::msg::PoseStamped_, geometry_msgs::msg::Pose_, - &geometry_msgs::msg::PoseStamped_::pose> + & geometry_msgs::msg::PoseStamped_::pose> { }; -template -struct stampedMessageTraits> -: defaultStampedMessageTraits< +/** \brief Traits for geometry_msgs::msg::TwistStamped. + * \tparam Alloc Message Allocator + */ +template +struct StampedMessageTraits> + : DefaultStampedMessageTraits< geometry_msgs::msg::TwistStamped_, geometry_msgs::msg::Twist_, - &geometry_msgs::msg::TwistStamped_::twist> + & geometry_msgs::msg::TwistStamped_::twist> { }; -template -struct stampedMessageTraits> -: defaultStampedMessageTraits< +/** \brief Traits for geometry_msgs::msg::WrenchStamped. + * \tparam Alloc Message Allocator + */ +template +struct StampedMessageTraits> + : DefaultStampedMessageTraits< geometry_msgs::msg::WrenchStamped_, geometry_msgs::msg::Wrench_, - &geometry_msgs::msg::WrenchStamped_::wrench> + & geometry_msgs::msg::WrenchStamped_::wrench> { }; -template -struct stampedMessageTraits> -: defaultStampedMessageTraits< +/** \brief Traits for geometry_msgs::msg::TransformStamped. + * \tparam Alloc Message Allocator + */ +template +struct StampedMessageTraits> + : DefaultStampedMessageTraits< geometry_msgs::msg::TransformStamped_, geometry_msgs::msg::Transform_, - &geometry_msgs::msg::TransformStamped_::transform> + & geometry_msgs::msg::TransformStamped_::transform> { }; -template -struct stampedMessageTraits> -: defaultStampedMessageTraits< +/** \brief Traits for geometry_msgs::msg::Vector3Stamped. + * \tparam Alloc Message Allocator + */ +template +struct StampedMessageTraits> + : DefaultStampedMessageTraits< geometry_msgs::msg::Vector3Stamped_, geometry_msgs::msg::Vector3_, - &geometry_msgs::msg::Vector3Stamped_::vector> + & geometry_msgs::msg::Vector3Stamped_::vector> { }; -template -struct stampedMessageTraits> +/** \brief Traits for geometry_msgs::msg::PoseWithCovarianceStamped. + * \tparam Alloc Message Allocator + */ +template +struct StampedMessageTraits> { - using unstampedType = geometry_msgs::msg::Pose_; - using stampedTypeWithCovariance = geometry_msgs::msg::PoseWithCovarianceStamped_; - using covarianceType = std::array; - - static unstampedType & accessMessage(stampedTypeWithCovariance & stamped) { return stamped.pose.pose; } - static unstampedType getMessage(stampedTypeWithCovariance const & stamped) { return stamped.pose.pose; } - static covarianceType & accessCovariance(stampedTypeWithCovariance & stamped) { return stamped.pose.covariance; } - static covarianceType getCovariance(stampedTypeWithCovariance const & stamped) { return stamped.pose.covariance; } + /// The underlying message type. + using UntampedType = geometry_msgs::msg::Pose_; + /// The message type itself. + using StampedTypeWithCovariance = geometry_msgs::msg::PoseWithCovarianceStamped_; + /// The covariance type. + using CovarianceType = std::array; + + /** \brief Read-Write access to the underlying message. + * \param[in] stamped Reference to the stamped message. + * \return Reference to the unstamped message. + */ + static UntampedType & accessMessage(StampedTypeWithCovariance & stamped) + { + return stamped.pose.pose; + } + /** \brief Read-only access to the underlying message. + * \param[in] stamped Reference to the stamped message. + * \return Copy of the unstamped message. + */ + static UntampedType getMessage(StampedTypeWithCovariance const & stamped) + { + return stamped.pose.pose; + } + /** \brief Read-Write access to the covariance. + * \param[in] stamped Reference to the stamped message with covariance. + * \return Reference to the covariance. + */ + static CovarianceType & accessCovariance(StampedTypeWithCovariance & stamped) + { + return stamped.pose.covariance; + } + /** \brief Read-only access to the covariance. + * \param[in] stamped Reference to the stamped message with covariance. + * \return Copy of the covariance. + */ + static CovarianceType getCovariance(StampedTypeWithCovariance const & stamped) + { + return stamped.pose.covariance; + } }; -template -struct stampedMessageTraits> +/** \brief Traits for geometry_msgs::msg::TwistWithCovarianceStamped. + * \tparam Alloc Message Allocator + */ +template +struct StampedMessageTraits> { - using unstampedType = geometry_msgs::msg::Twist_; - using stampedTypeWithCovariance = geometry_msgs::msg::TwistWithCovarianceStamped_; - using covarianceType = std::array; - - static unstampedType & accessMessage(stampedTypeWithCovariance & stamped) { return stamped.twist.twist; } - static unstampedType getMessage(stampedTypeWithCovariance const & stamped) { return stamped.twist.twist; } - static covarianceType & accessCovariance(stampedTypeWithCovariance & stamped) { return stamped.twist.covariance; } - static covarianceType getCovariance(stampedTypeWithCovariance const & stamped) { return stamped.twist.covariance; } + /// The underlying message type. + using UntampedType = geometry_msgs::msg::Twist_; + /// The message type itself. + using StampedTypeWithCovariance = geometry_msgs::msg::TwistWithCovarianceStamped_; + /// The covariance type. + using CovarianceType = std::array; + + /** \brief Read-Write access to the underlying message. + * \param[in] stamped Reference to the stamped message. + * \return Reference to the unstamped message. + */ + static UntampedType & accessMessage(StampedTypeWithCovariance & stamped) + { + return stamped.twist.twist; + } + /** \brief Read-only access to the underlying message. + * \param[in] stamped Reference to the stamped message. + * \return Copy of the unstamped message. + */ + static UntampedType getMessage(StampedTypeWithCovariance const & stamped) + { + return stamped.twist.twist; + } + /** \brief Read-Write access to the covariance. + * \param[in] stamped Reference to the stamped message with covariance. + * \return Reference to the covariance. + */ + static CovarianceType & accessCovariance(StampedTypeWithCovariance & stamped) + { + return stamped.twist.covariance; + } + /** \brief Read-only access to the covariance. + * \param[in] stamped Reference to the stamped message with covariance. + * \return Copy of the covariance. + */ + static CovarianceType getCovariance(StampedTypeWithCovariance const & stamped) + { + return stamped.twist.covariance; + } }; } // namespace impl diff --git a/tf2/include/tf2/impl/time_convert.h b/tf2/include/tf2/impl/time_convert.h index da28f5349..85c669ff2 100644 --- a/tf2/include/tf2/impl/time_convert.h +++ b/tf2/include/tf2/impl/time_convert.h @@ -42,9 +42,14 @@ namespace tf2 { namespace impl { -template <> -struct ImplDetails +/// \brief Conversion implementation for builtin_interfaces::msg::Time and tf2::TimePoint. +template<> +struct ConversionImplementation { + /** \brief Convert a tf2::TimePoint to a Time message. + * \param[in] t The tf2::TimePoint to convert. + * \param[out] time_msg The converted tf2::TimePoint, as a Time message. + */ static void toMsg(const tf2::TimePoint & t, builtin_interfaces::msg::Time & time_msg) { std::chrono::nanoseconds ns = @@ -55,6 +60,10 @@ struct ImplDetails time_msg.nanosec = static_cast(ns.count() % 1000000000ull); } + /** \brief Convert a Time message to a tf2::TimePoint. + * \param[in] time_msg The Time message to convert. + * \param[out] t The converted message, as a tf2::TimePoint. + */ static void fromMsg(const builtin_interfaces::msg::Time & time_msg, tf2::TimePoint & t) { int64_t d = time_msg.sec * 1000000000ull + time_msg.nanosec; @@ -63,9 +72,14 @@ struct ImplDetails } }; -template <> -struct ImplDetails +/// \brief Conversion implementation for builtin_interfaces::msg::Duration and tf2::Duration. +template<> +struct ConversionImplementation { + /** \brief Convert a tf2::Duration to a Duration message. + * \param[in] t The tf2::Duration to convert. + * \param[out] duration_msg The converted tf2::Duration, as a Duration message. + */ static void toMsg(const tf2::Duration & t, builtin_interfaces::msg::Duration & duration_msg) { std::chrono::nanoseconds ns = std::chrono::duration_cast(t); @@ -74,6 +88,10 @@ struct ImplDetails duration_msg.nanosec = static_cast(ns.count() % 1000000000ull); } + /** \brief Convert a Duration message to a tf2::Duration. + * \param[in] duration_msg The Duration message to convert. + * \param[out] duration The converted message, as a tf2::Duration. + */ static void fromMsg( const builtin_interfaces::msg::Duration & duration_msg, tf2::Duration & duration) { @@ -83,19 +101,27 @@ struct ImplDetails } }; -template <> -struct defaultMessage +/// \brief Default return type of tf2::toMsg() for tf2::TimePoint. +template<> +struct DefaultMessageForDatatype { + /// \brief Default return type of tf2::toMsg() for tf2::TimePoint. using type = builtin_interfaces::msg::Time; }; -template <> -struct defaultMessage +/// \brief Default return type of tf2::toMsg() for tf2::Duration. +template<> +struct DefaultMessageForDatatype { + /// \brief Default return type of tf2::toMsg() for tf2::Duration. using type = builtin_interfaces::msg::Duration; }; } // namespace impl +/** \brief Convert a Time message to a tf2::TimePoint. + * \param[in] time_msg The Time message to convert. + * \return The converted message, as a tf2::TimePoint. + */ inline tf2::TimePoint TimePointFromMsg(const builtin_interfaces::msg::Time & time_msg) { TimePoint tp; @@ -103,6 +129,10 @@ inline tf2::TimePoint TimePointFromMsg(const builtin_interfaces::msg::Time & tim return tp; } +/** \brief Convert a Duration message to a tf2::Duration. + * \param[in] duration_msg The Duration message to convert. + * \return The converted message, as a tf2::Duration. + */ inline tf2::Duration DurationFromMsg(const builtin_interfaces::msg::Duration & duration_msg) { Duration d; diff --git a/tf2_bullet/include/tf2_bullet/tf2_bullet.h b/tf2_bullet/include/tf2_bullet/tf2_bullet.h index da02ab382..d4ac6931f 100644 --- a/tf2_bullet/include/tf2_bullet/tf2_bullet.h +++ b/tf2_bullet/include/tf2_bullet/tf2_bullet.h @@ -54,7 +54,7 @@ namespace impl { /// \brief Default return type of tf2::toMsg() for btVector3. template<> -struct defaultMessage +struct DefaultMessageForDatatype { /// \brief Default return type of tf2::toMsg() for btVector3. using type = geometry_msgs::msg::Point; @@ -62,7 +62,7 @@ struct defaultMessage /// \brief Default return type of tf2::toMsg() for btQuaternion. template<> -struct defaultMessage +struct DefaultMessageForDatatype { /// \brief Default return type of tf2::toMsg() for btQuaternion. using type = geometry_msgs::msg::Quaternion; @@ -70,7 +70,7 @@ struct defaultMessage /// \brief Default return type of tf2::toMsg() for btTransform. template<> -struct defaultMessage +struct DefaultMessageForDatatype { /// \brief Default return type of tf2::toMsg() for btTransform. using type = geometry_msgs::msg::Transform; @@ -78,26 +78,26 @@ struct defaultMessage /// \brief Conversion implementation for geometry_msgs::msg::Point and btVector3. template<> -struct ImplDetails - : DefaultVectorImpl +struct ConversionImplementation + : DefaultVectorConversionImplementation { }; /// \brief Conversion implementation for geometry_msgs::msg::Vector3 and btVector3. template<> -struct ImplDetails - : DefaultVectorImpl +struct ConversionImplementation + : DefaultVectorConversionImplementation { }; /// \brief Conversion implementation for geometry_msgs::msg::Quaternion and Eigen::Quaterniond. template<> -struct ImplDetails - : DefaultQuaternionImpl {}; +struct ConversionImplementation + : DefaultQuaternionConversionImplementation {}; /// \brief Conversion implementation for geometry_msgs::msg::Transform and btTransform. template<> -struct ImplDetails +struct ConversionImplementation { /** \brief Convert a Transform message to a btTransform type. * \param[in] in The message to convert. diff --git a/tf2_eigen/include/tf2_eigen/tf2_eigen.h b/tf2_eigen/include/tf2_eigen/tf2_eigen.h index f379fc7c2..6e5082893 100644 --- a/tf2_eigen/include/tf2_eigen/tf2_eigen.h +++ b/tf2_eigen/include/tf2_eigen/tf2_eigen.h @@ -125,29 +125,29 @@ struct EigenTransformImpl /// \brief Conversion implementation for geometry_msgs::msg::Transform and Eigen::Affine3d. template<> -struct ImplDetails +struct ConversionImplementation : EigenTransformImpl { }; /// \brief Conversion implementation for geometry_msgs::msg::Transform and Eigen::Isometry3d. template<> -struct ImplDetails +struct ConversionImplementation : EigenTransformImpl { }; /// \brief Conversion implementation for geometry_msgs::msg::Point and Eigen::Vector3d. template<> -struct ImplDetails - : DefaultVectorImpl +struct ConversionImplementation + : DefaultVectorConversionImplementation { }; /// \brief Conversion implementation for geometry_msgs::msg::Vector3d and Eigen::Vector3d. template<> -struct ImplDetails - : DefaultVectorImpl +struct ConversionImplementation + : DefaultVectorConversionImplementation { }; @@ -161,7 +161,7 @@ struct DefaultTransformType /// \brief Default return type of tf2::toMsg() for Eigen::Vector3d. template<> -struct defaultMessage +struct DefaultMessageForDatatype { /// \brief Default return type of tf2::toMsg() for Eigen::Vector3d. using type = geometry_msgs::msg::Point; @@ -169,7 +169,7 @@ struct defaultMessage /// \brief Conversion implementation for geometry_msgs::msg::Quaternion and Eigen::Quaterniond. template<> -struct ImplDetails +struct ConversionImplementation { /** \brief Convert a Eigen Quaterniond type to a Quaternion message. * \param[in] in The Eigen Quaterniond to convert. @@ -195,7 +195,7 @@ struct ImplDetails /// \brief Default return type of tf2::toMsg() for Eigen::Quaterniond. template<> -struct defaultMessage +struct DefaultMessageForDatatype { /// \brief Default return type of tf2::toMsg() for Eigen::Quaterniond. using type = geometry_msgs::msg::Quaternion; @@ -280,21 +280,21 @@ struct EigenPoseImpl /// \brief Conversion implementation for geometry_msgs::msg::Pose and Eigen::Affine3d. template<> -struct ImplDetails +struct ConversionImplementation : public EigenPoseImpl { }; /// \brief Conversion implementation for geometry_msgs::msg::Pose and Eigen::Isometry3d. template<> -struct ImplDetails +struct ConversionImplementation : public EigenPoseImpl { }; /// \brief Default return type of tf2::toMsg() for Eigen::Affine3d. template<> -struct defaultMessage +struct DefaultMessageForDatatype { /// \brief Default return type of tf2::toMsg() for Eigen::Affine3d. using type = geometry_msgs::msg::Pose; @@ -302,7 +302,7 @@ struct defaultMessage /// \brief Default return type of tf2::toMsg() for Eigen::Affine3d. template<> -struct defaultMessage +struct DefaultMessageForDatatype { /// \brief Default return type of tf2::toMsg() for Eigen::Affine3d. using type = geometry_msgs::msg::Pose; @@ -310,7 +310,7 @@ struct defaultMessage /// \brief Conversion implementation for geometry_msgs::msg::Twist and Eigen::Matrix. template<> -struct ImplDetails, geometry_msgs::msg::Twist> +struct ConversionImplementation, geometry_msgs::msg::Twist> { /** \brief Convert a Eigen 6x1 Matrix type to a Twist message. * \param[in] in The 6x1 Eigen Matrix to convert. @@ -343,7 +343,7 @@ struct ImplDetails, geometry_msgs::msg::Twist> /// \brief Default return type of tf2::toMsg() for Eigen::Matrix. template<> -struct defaultMessage> +struct DefaultMessageForDatatype> { /// \brief Default return type of tf2::toMsg() for Eigen::Matrix. using type = geometry_msgs::msg::Twist; @@ -351,7 +351,7 @@ struct defaultMessage> /// \brief Conversion implementation for geometry_msgs::msg::Wrench and Eigen::Matrix. template<> -struct ImplDetails, geometry_msgs::msg::Wrench> +struct ConversionImplementation, geometry_msgs::msg::Wrench> { /** \brief Convert a Eigen 6x1 Matrix type to a Wrench message. * \param[in] in The 6x1 Eigen Matrix to convert. 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 e22599ec1..9a3106815 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 @@ -80,21 +80,21 @@ namespace impl /// \brief Conversion implementation for geometry_msgs::msg::Vector3 and tf2::Vector3. template<> -struct ImplDetails - : DefaultVectorImpl +struct ConversionImplementation + : DefaultVectorConversionImplementation { }; /// \brief Conversion implementation for geometry_msgs::msg::Point and tf2::Vector3. template<> -struct ImplDetails - : DefaultVectorImpl +struct ConversionImplementation + : DefaultVectorConversionImplementation { }; /// \brief Default return type of tf2::toMsg() for tf2::Vector3. template<> -struct defaultMessage +struct DefaultMessageForDatatype { /// \brief Default return type of tf2::toMsg() for tf2::Vector3. using type = geometry_msgs::msg::Vector3; @@ -265,12 +265,12 @@ namespace impl { /// \brief Conversion implementation for geometry_msgs::msg::Quaternion and tf2::Quaternion. template<> -struct ImplDetails - : DefaultQuaternionImpl {}; +struct ConversionImplementation + : DefaultQuaternionConversionImplementation {}; /// \brief Default return type of tf2::toMsg() for tf2::Quaternion. template<> -struct defaultMessage +struct DefaultMessageForDatatype { /// \brief Default return type of tf2::toMsg() for tf2::Quaternion. using type = geometry_msgs::msg::Quaternion; @@ -327,7 +327,7 @@ namespace impl { /// \brief Conversion implementation for geometry_msgs::msg::Transform and tf2::Transform. template<> -struct ImplDetails +struct ConversionImplementation { /** \brief Convert a tf2 Transform type to its equivalent geometry_msgs representation. * \param[in] in in A tf2 Transform object. @@ -357,7 +357,7 @@ struct ImplDetails /// \brief Default return type of tf2::toMsg() for tf2::Transform. template<> -struct defaultMessage +struct DefaultMessageForDatatype { /// \brief Default return type of tf2::toMsg() for tf2::Transform. using type = geometry_msgs::msg::Transform; @@ -388,7 +388,7 @@ namespace impl /// \brief Conversion implementation for geometry_msgs::msg::Pose and tf2::Transform. template<> -struct ImplDetails +struct ConversionImplementation { /** \brief Convert a tf2 Transform type to an equivalent geometry_msgs Pose message. * \param[in] in A tf2 Transform object. @@ -427,7 +427,7 @@ struct DefaultTransformType /// \brief Conversion implementation for geometry_msgs::msg::Wrench and tf2::Transform. template<> -struct ImplDetails, geometry_msgs::msg::Wrench> +struct ConversionImplementation, geometry_msgs::msg::Wrench> { /** \brief Convert a tf2 Wrench to an equivalent geometry_msgs Wrench message. * \param[in] in A tf2 Wrench object. @@ -452,7 +452,7 @@ struct ImplDetails, geometry_msgs::msg::Wrench> /// \brief Default return type of tf2::toMsg() for std::array. template<> -struct defaultMessage> +struct DefaultMessageForDatatype> { /// \brief Default return type of tf2::toMsg() for std::array. using type = geometry_msgs::msg::Wrench; diff --git a/tf2_kdl/include/tf2_kdl/tf2_kdl.h b/tf2_kdl/include/tf2_kdl/tf2_kdl.h index 0fffb9102..a660dd3de 100644 --- a/tf2_kdl/include/tf2_kdl/tf2_kdl.h +++ b/tf2_kdl/include/tf2_kdl/tf2_kdl.h @@ -78,7 +78,7 @@ namespace impl { /// \brief Conversion implementation for geometry_msgs::msg::Transform and KDL::Frame. template<> -struct ImplDetails +struct ConversionImplementation { /** \brief Convert a Transform message type to a KDL Frame. * \param[in] in The Transform message to convert. @@ -106,21 +106,21 @@ struct ImplDetails /// \brief Conversion implementation for geometry_msgs::msg::Vector3 and KDL::Vector. template<> -struct ImplDetails - : DefaultVectorImpl +struct ConversionImplementation + : DefaultVectorConversionImplementation { }; /// \brief Conversion implementation for geometry_msgs::msg::Point and KDL::Vector. template<> -struct ImplDetails - : DefaultVectorImpl +struct ConversionImplementation + : DefaultVectorConversionImplementation { }; /// \brief Default return type of tf2::toMsg() for KDL::Vector. template<> -struct defaultMessage +struct DefaultMessageForDatatype { /// \brief Default return type of tf2::toMsg() for KDL::Vector. using type = geometry_msgs::msg::Point; @@ -139,7 +139,7 @@ struct DefaultTransformType /// \brief Conversion implementation for geometry_msgs::msg::Twist and KDL::Twist. template<> -struct ImplDetails +struct ConversionImplementation { /** \brief Convert a KDL Twist type to a Twist message. * \param[in] in The KDL Twist to convert. @@ -172,7 +172,7 @@ struct DefaultTransformType /// \brief Default return type of tf2::toMsg() for KDL::Twist. template<> -struct defaultMessage +struct DefaultMessageForDatatype { /// \brief Default return type of tf2::toMsg() for KDL::Twist. using type = geometry_msgs::msg::Twist; @@ -184,7 +184,7 @@ struct defaultMessage /// \brief Conversion implementation for geometry_msgs::msg::Wrench and KDL::Wrench. template<> -struct ImplDetails +struct ConversionImplementation { /** \brief Convert a KDL Wrench type to a Wrench message. * \param[in] in The KDL Wrench to convert. @@ -209,7 +209,7 @@ struct ImplDetails /// \brief Default return type of tf2::toMsg() for KDL::Wrench. template<> -struct defaultMessage +struct DefaultMessageForDatatype { /// \brief Default return type of tf2::toMsg() for KDL::Wrench. using type = geometry_msgs::msg::Wrench; @@ -250,7 +250,7 @@ namespace impl { /// \brief Conversion implementation for geometry_msgs::msg::Pose and KDL::Frame. template<> -struct ImplDetails +struct ConversionImplementation { /** \brief Convert a KDL Frame type to a Pose message. * \param[in] in The KDL Frame to convert. @@ -275,7 +275,7 @@ struct ImplDetails /// \brief Default return type of tf2::toMsg() for KDL::Frame. template<> -struct defaultMessage +struct DefaultMessageForDatatype { /// \brief Default return type of tf2::toMsg() for KDL::Frame. using type = geometry_msgs::msg::Pose; @@ -291,7 +291,7 @@ struct DefaultTransformType /// \brief Conversion implementation for geometry_msgs::msg::Quaternion and KDL::Rotation. template<> -struct ImplDetails +struct ConversionImplementation { /** \brief Convert a KDL Rotation type to a Quaternion message. * \param[in] r The KDL Rotation to convert. @@ -314,7 +314,7 @@ struct ImplDetails /// \brief Default return type of tf2::toMsg() for KDL::Rotation. template<> -struct defaultMessage +struct DefaultMessageForDatatype { /// \brief Default return type of tf2::toMsg() for KDL::Rotation. using type = geometry_msgs::msg::Quaternion; From 3693ac6d285424a97ce1eeb95c8689ed6cfd99b7 Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Fri, 29 Jan 2021 16:23:08 +0000 Subject: [PATCH 31/51] Verbose error messages in case of missing includes --- test_tf2/test/test_convert.cpp | 66 ++++++++++++++++++++++++++---- tf2/include/tf2/impl/convert.h | 73 +++++++++++++++++++++++++++------- 2 files changed, 118 insertions(+), 21 deletions(-) diff --git a/test_tf2/test/test_convert.cpp b/test_tf2/test/test_convert.cpp index cd79efee3..e12b9135d 100644 --- a/test_tf2/test/test_convert.cpp +++ b/test_tf2/test/test_convert.cpp @@ -41,13 +41,65 @@ #include #include +#include + + +// test of tf2 type traits +static_assert( + !tf2::impl::MessageHasStdHeader::value, + "MessageHasStdHeader traits error"); +static_assert( + tf2::impl::MessageHasStdHeader::value, + "MessageHasStdHeader traits error"); + +namespace +{ +struct MyPODMessage +{ + int header; +}; + +template +struct MyAllocMessage +{ + int header; +}; + +static_assert( + !tf2::impl::MessageHasStdHeader::value, + "MessageHasStdHeader traits error"); + +template +struct MyAllocator +{ + using value_type = T; + using size_type = unsigned int; + T * allocate(size_type); + void deallocate(T *, size_type); + template + struct rebind { typedef MyAllocator other; }; +}; +using MyVec = geometry_msgs::msg::Vector3_>; +using MyVecStamped = geometry_msgs::msg::Vector3Stamped_>; +using MyMessage = MyAllocMessage>; + +static_assert(!tf2::impl::MessageHasStdHeader::value, "MessageHasStdHeader traits error"); +static_assert( + tf2::impl::MessageHasStdHeader::value, + "MessageHasStdHeader traits error"); + +static_assert( + !tf2::impl::MessageHasStdHeader::value, + "MessageHasStdHeader traits error"); +} + using Vector6d = Eigen::Matrix; TEST(tf2Convert, kdlToBullet) { double epsilon = 1e-9; - tf2::Stamped b(btVector3(1,2,3), tf2::timeFromSec(0), "my_frame"); + tf2::Stamped b(btVector3(1, 2, 3), tf2::timeFromSec(0), "my_frame"); tf2::Stamped b1 = b; tf2::Stamped k1; @@ -74,7 +126,7 @@ TEST(tf2Convert, kdlBulletROSConversions) { double epsilon = 1e-9; - tf2::Stamped b1(btVector3(1,2,3), tf2::timeFromSec(0), "my_frame"), b2, b3, b4; + tf2::Stamped b1(btVector3(1, 2, 3), tf2::timeFromSec(0), "my_frame"), b2, b3, b4; geometry_msgs::msg::PointStamped r1, r2, r3; tf2::Stamped k1, k2, k3; @@ -101,7 +153,7 @@ TEST(tf2Convert, kdlBulletROSConversions) TEST(tf2Convert, ConvertTf2Quaternion) { - tf2::Quaternion tq(1,2,3,4); + tf2::Quaternion tq(1, 2, 3, 4); Eigen::Quaterniond eq; tf2::convert(tq, eq); @@ -117,7 +169,7 @@ TEST(tf2Convert, PointVectorDefaultMessagetype) // as it can return a Vector3 or a Point for certain datatypes { // Bullet - const tf2::Stamped b1{btVector3{1.0, 3.0, 4.0}, tf2::get_now(), "my_frame" }; + const tf2::Stamped b1{btVector3{1.0, 3.0, 4.0}, tf2::get_now(), "my_frame"}; const geometry_msgs::msg::PointStamped msg = tf2::toMsg(b1); EXPECT_EQ(msg.point.x, 1.0); @@ -162,7 +214,7 @@ TEST(tf2Convert, PointVectorOtherMessagetype) { const tf2::Vector3 t1{2.0, 4.0, 5.0}; geometry_msgs::msg::Point msg; - const geometry_msgs::msg::Point& msg2 = tf2::toMsg(t1, msg); + const geometry_msgs::msg::Point & msg2 = tf2::toMsg(t1, msg); // returned reference is second argument EXPECT_EQ(&msg2, &msg); @@ -174,7 +226,7 @@ TEST(tf2Convert, PointVectorOtherMessagetype) // Eigen const Eigen::Vector3d e1{2.0, 4.0, 5.0}; geometry_msgs::msg::Vector3 msg; - const geometry_msgs::msg::Vector3& msg2 = tf2::toMsg(e1, msg); + const geometry_msgs::msg::Vector3 & msg2 = tf2::toMsg(e1, msg); // returned reference is second argument EXPECT_EQ(&msg2, &msg); @@ -286,7 +338,7 @@ TEST(TfEigenKdl, TestVector3dVector) EXPECT_EQ(eigen_v, eigen_v1); } -int main(int argc, char** argv) +int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); diff --git a/tf2/include/tf2/impl/convert.h b/tf2/include/tf2/impl/convert.h index 1a5f0a48c..4a435f0a4 100644 --- a/tf2/include/tf2/impl/convert.h +++ b/tf2/include/tf2/impl/convert.h @@ -32,14 +32,49 @@ #define TF2__IMPL__CONVERT_H_ #include +#include #include "../time.h" #include "forward.h" +#include + + namespace tf2 { namespace impl { + +/// Helper to always trigger \c static_assert s +template +constexpr bool alwaysFalse = false; + +/** \brief Check whether a message is stamped and has a std_msgs::msg::Header member. + * + * It will be indicated with the static member valiable \c value . + * \tparam Message The message to check + */ +template +struct MessageHasStdHeader : std::false_type {}; + +/** \brief Check whether a message is stamped and has a std_msgs::msg::Header member. + * + * It will be indicated with the static member valiable \c value . + * \tparam Message The message to check + */ +template +class MessageHasStdHeader +{ + template + static std::true_type headerIsStdHeader(std_msgs::msg::Header_); + template + static std::false_type headerIsStdHeader(...); + +public: + /// true if Message has a member \c header of type std_msgs::msg::Header. + static constexpr bool value = decltype(headerIsStdHeader(std::declval().header))::value; +}; + /** * \brief Mapping between Datatypes (like \c Vector3d ) and their default ROS Message types. * @@ -52,6 +87,9 @@ namespace impl template struct DefaultMessageForDatatype { + static_assert( + alwaysFalse, "No default message type defined, " + "please check your header file includes!"); // using type = ...; }; @@ -73,6 +111,9 @@ struct DefaultMessageForDatatype template struct DefaultTransformType { + static_assert( + alwaysFalse, "No default transform type defined, " + "please check your header file includes!"); // using type = ...; }; @@ -100,6 +141,9 @@ struct DefaultTransformType template struct ConversionImplementation { + static_assert( + alwaysFalse, "No Conversion Implementation available, " + "please check your header file includes!"); // void toMsg(const Datatype&, Message&); // void fromMsg(const Message&, Datatype&); }; @@ -140,6 +184,9 @@ struct ConversionImplementation template struct StampedMessageTraits { + static_assert( + alwaysFalse, "No traits for this stamped message type available, " + "please check your header file includes!"); // using UntampedType = ...; // static UntampedType& accessMessage(StampedMsg &); // static UntampedType getMessage(StampedMsg const&); @@ -172,6 +219,9 @@ struct StampedMessageTraits template struct UnstampedMessageTraits { + static_assert( + alwaysFalse, "No traits for this message type available, " + "please check your header file includes!"); // using StampedType = ...; // using StampedTypeWithCovariance = ...; }; @@ -364,41 +414,36 @@ inline void Converter::convert(const A & a, B & b) fromMsg<>(toMsg<>(a), 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 StampedAttributesHelper { + static_assert( + MessageHasStdHeader::value, + "Trying to use default implementation for stamped message, " + "but the datatype does not have a Header member."); + /**\brief Get the timestamp from data - * \param t The data input. + * \param[in] t The data input. * \return The timestamp associated with the data. - * - * The second parameter is needed to hide the default implementation if T is not a stamped ROS message. */ - static tf2::TimePoint getTimestamp( - const T & t, void_t::UntampedType> * = nullptr) + static tf2::TimePoint getTimestamp(const T & t) { tf2::TimePoint timestamp; tf2::fromMsg<>(t.header.stamp, timestamp); return timestamp; } /**\brief Get the frame_id from data - * \param t The data input. + * \param[in] t The data input. * \return The frame_id associated with the data. - * - * The second parameter is needed to hide the default implementation if T is not a stamped ROS message. */ - static std::string getFrameId( - const T & t, void_t::UntampedType> * = nullptr) + static std::string getFrameId(const T & t) { return t.header.frame_id; } From 7fa19a0c74e2d2eba99458eedb78bb80b0f196d9 Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Fri, 12 Feb 2021 00:25:41 +0000 Subject: [PATCH 32/51] Reorder headers --- tf2/include/tf2/impl/convert.h | 2 +- tf2_eigen/include/tf2_eigen/tf2_eigen.h | 164 ++++---- .../tf2_geometry_msgs/tf2_geometry_msgs.h | 379 +++++++++--------- tf2_kdl/include/tf2_kdl/tf2_kdl.h | 102 ++--- 4 files changed, 329 insertions(+), 318 deletions(-) diff --git a/tf2/include/tf2/impl/convert.h b/tf2/include/tf2/impl/convert.h index 4a435f0a4..8b00a12b7 100644 --- a/tf2/include/tf2/impl/convert.h +++ b/tf2/include/tf2/impl/convert.h @@ -63,7 +63,7 @@ struct MessageHasStdHeader : std::false_type {}; * \tparam Message The message to check */ template -class MessageHasStdHeader +class MessageHasStdHeader(Message::header), 0)> { template static std::true_type headerIsStdHeader(std_msgs::msg::Header_); diff --git a/tf2_eigen/include/tf2_eigen/tf2_eigen.h b/tf2_eigen/include/tf2_eigen/tf2_eigen.h index 6e5082893..82e7f1e5a 100644 --- a/tf2_eigen/include/tf2_eigen/tf2_eigen.h +++ b/tf2_eigen/include/tf2_eigen/tf2_eigen.h @@ -91,51 +91,9 @@ geometry_msgs::msg::TransformStamped eigenToTransform(const Eigen::Isometry3d & namespace impl { -/** \brief Template for Eigen::Isometry3d and Eigen::Affine3d conversion. - * \tparam mode Mode argument for Eigen::Transform template - */ -template -struct EigenTransformImpl -{ - /** \brief Convert a Transform message to an Eigen type. - * \param[in] msg The message to convert. - * \param[out] out The converted message, as an Eigen type. - */ - static void fromMsg( - const geometry_msgs::msg::Transform & msg, Eigen::Transform & out) - { - Eigen::Quaterniond q; - Eigen::Vector3d v; - tf2::fromMsg<>(msg.rotation, q); - tf2::fromMsg<>(msg.translation, v); - out = Eigen::Translation3d(v) * q; - } - - /** \brief Convert an Eigen Transform to a Transform message. - * \param[in] in The Eigen Transform to convert. - * \param[out] msg The converted Transform, as a message. - */ - static void toMsg( - const Eigen::Transform & in, geometry_msgs::msg::Transform & msg) - { - tf2::toMsg<>(Eigen::Quaterniond(in.linear()), msg.rotation); - tf2::toMsg<>(Eigen::Vector3d(in.translation()), msg.translation); - } -}; - -/// \brief Conversion implementation for geometry_msgs::msg::Transform and Eigen::Affine3d. -template<> -struct ConversionImplementation - : EigenTransformImpl -{ -}; - -/// \brief Conversion implementation for geometry_msgs::msg::Transform and Eigen::Isometry3d. -template<> -struct ConversionImplementation - : EigenTransformImpl -{ -}; +// --------------------- +// Vector +// --------------------- /// \brief Conversion implementation for geometry_msgs::msg::Point and Eigen::Vector3d. template<> @@ -167,6 +125,11 @@ struct DefaultMessageForDatatype using type = geometry_msgs::msg::Point; }; + +// --------------------- +// Quaternion +// --------------------- + /// \brief Conversion implementation for geometry_msgs::msg::Quaternion and Eigen::Quaterniond. template<> struct ConversionImplementation @@ -200,51 +163,57 @@ struct DefaultMessageForDatatype /// \brief Default return type of tf2::toMsg() for Eigen::Quaterniond. using type = geometry_msgs::msg::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, - * although it can not be used in tf2_ros::BufferInterface::transform because this - * functions rely on the existence of a time stamp and a frame id in the type which should - * get transformed. - * \param t_in The vector to transform, as a Eigen Quaterniond data type. - * \param t_out The transformed vector, as a Eigen Quaterniond data type. - * \param transform The timestamped transform to apply, as a TransformStamped message. + +// --------------------- +// Transform +// --------------------- + +/** \brief Template for Eigen::Isometry3d and Eigen::Affine3d conversion. + * \tparam mode Mode argument for Eigen::Transform template */ -template<> -inline -void doTransform( - const Eigen::Quaterniond & t_in, - Eigen::Quaterniond & t_out, - const geometry_msgs::msg::TransformStamped & transform) +template +struct EigenTransformImpl { - Eigen::Quaterniond t; - fromMsg(transform.transform.rotation, t); - t_out = t * t_in; -} + /** \brief Convert a Transform message to an Eigen type. + * \param[in] msg The message to convert. + * \param[out] out The converted message, as an Eigen type. + */ + static void fromMsg( + const geometry_msgs::msg::Transform & msg, Eigen::Transform & out) + { + Eigen::Quaterniond q; + Eigen::Vector3d v; + tf2::fromMsg<>(msg.rotation, q); + tf2::fromMsg<>(msg.translation, v); + out = Eigen::Translation3d(v) * q; + } -/** \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. - * \param t_out The transformed vector, as a timestamped Eigen Quaterniond data type. - * \param transform The timestamped transform to apply, as a TransformStamped message. - */ + /** \brief Convert an Eigen Transform to a Transform message. + * \param[in] in The Eigen Transform to convert. + * \param[out] msg The converted Transform, as a message. + */ + static void toMsg( + const Eigen::Transform & in, geometry_msgs::msg::Transform & msg) + { + tf2::toMsg<>(Eigen::Quaterniond(in.linear()), msg.rotation); + tf2::toMsg<>(Eigen::Vector3d(in.translation()), msg.translation); + } +}; + +/// \brief Conversion implementation for geometry_msgs::msg::Transform and Eigen::Affine3d. template<> -inline -void doTransform( - const tf2::Stamped & t_in, - tf2::Stamped & t_out, - const geometry_msgs::msg::TransformStamped & transform) +struct ConversionImplementation + : EigenTransformImpl { - t_out.frame_id_ = transform.header.frame_id; - tf2::fromMsg(transform.header.stamp, t_out.stamp_); - doTransform( - static_cast(t_in), - static_cast(t_out), transform); -} +}; -namespace impl +/// \brief Conversion implementation for geometry_msgs::msg::Transform and Eigen::Isometry3d. +template<> +struct ConversionImplementation + : EigenTransformImpl { +}; /** \brief Pose conversion template for Eigen types. * \tparam T Eigen transform type. @@ -308,6 +277,11 @@ struct DefaultMessageForDatatype using type = geometry_msgs::msg::Pose; }; + +// --------------------- +// Twist +// --------------------- + /// \brief Conversion implementation for geometry_msgs::msg::Twist and Eigen::Matrix. template<> struct ConversionImplementation, geometry_msgs::msg::Twist> @@ -349,6 +323,11 @@ struct DefaultMessageForDatatype> using type = geometry_msgs::msg::Twist; }; + +// --------------------- +// Wrench +// --------------------- + /// \brief Conversion implementation for geometry_msgs::msg::Wrench and Eigen::Matrix. template<> struct ConversionImplementation, geometry_msgs::msg::Wrench> @@ -417,6 +396,27 @@ geometry_msgs::msg::Vector3 toMsg2(const Eigen::Vector3d & in) msg.z = in[2]; return msg; } + +/** \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, + * although it can not be used in tf2_ros::BufferInterface::transform because this + * functions rely on the existence of a time stamp and a frame id in the type which should + * get transformed. + * \param[in] t_in The vector to transform, as a Eigen Quaterniond data type. + * \param[in,out] t_out The transformed vector, as a Eigen Quaterniond data type. + * \param[in] transform The timestamped transform to apply, as a TransformStamped message. + */ +template<> +inline +void doTransform( + const Eigen::Quaterniond & t_in, + Eigen::Quaterniond & t_out, + const geometry_msgs::msg::TransformStamped & transform) +{ + Eigen::Quaterniond t; + tf2::fromMsg<>(transform.transform.rotation, t); + t_out = Eigen::Quaterniond(t.toRotationMatrix() * t_in.toRotationMatrix()); +} } // namespace tf2 #endif // TF2_EIGEN__TF2_EIGEN_H_ 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 9a3106815..6342286e4 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 @@ -108,6 +108,153 @@ struct DefaultTransformType using type = tf2::Transform; }; + +/****************/ +/** Quaternion **/ +/****************/ + +/// \brief Conversion implementation for geometry_msgs::msg::Quaternion and tf2::Quaternion. +template<> +struct ConversionImplementation + : DefaultQuaternionConversionImplementation {}; + +/// \brief Default return type of tf2::toMsg() for tf2::Quaternion. +template<> +struct DefaultMessageForDatatype +{ + /// \brief Default return type of tf2::toMsg() for tf2::Quaternion. + using type = geometry_msgs::msg::Quaternion; +}; + +/// \brief Default Type for automatic tf2::doTransform() implementation for tf2::Quaternion. +template<> +struct DefaultTransformType +{ + /// \brief Default Type for automatic tf2::doTransform() implementation for tf2::Quaternion. + using type = tf2::Transform; +}; + + +/***************/ +/** Transform **/ +/***************/ + +/// \brief Conversion implementation for geometry_msgs::msg::Transform and tf2::Transform. +template<> +struct ConversionImplementation +{ + /** \brief Convert a tf2 Transform type to its equivalent geometry_msgs representation. + * \param[in] in in A tf2 Transform object. + * \param[out] out The Transform converted to a geometry_msgs message type. + */ + static void toMsg(const tf2::Transform & in, geometry_msgs::msg::Transform & out) + { + tf2::toMsg<>(in.getOrigin(), out.translation); + tf2::toMsg<>(in.getRotation(), out.rotation); + } + + /** \brief Convert a Transform message to its equivalent tf2 representation. + * \param[in] in A Transform message type. + * \param[out] out The Transform converted to a tf2 type. + */ + static void fromMsg(const geometry_msgs::msg::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 Default return type of tf2::toMsg() for tf2::Transform. +template<> +struct DefaultMessageForDatatype +{ + /// \brief Default return type of tf2::toMsg() for tf2::Transform. + using type = geometry_msgs::msg::Transform; +}; + + +/**********/ +/** Pose **/ +/**********/ + +/// \brief Conversion implementation for geometry_msgs::msg::Pose and tf2::Transform. +template<> +struct ConversionImplementation +{ + /** \brief Convert a tf2 Transform type to an equivalent geometry_msgs Pose message. + * \param[in] in A tf2 Transform object. + * \param[out] out The Transform converted to a geometry_msgs Pose message type. + */ + static void toMsg(const tf2::Transform & in, geometry_msgs::msg::Pose & out) + { + tf2::toMsg<>(in.getOrigin(), out.position); + tf2::toMsg<>(in.getRotation(), out.orientation); + } + + /** \brief Convert a geometry_msgs Pose message to an equivalent tf2 Transform type. + * \param[in] in A Pose message. + * \param[out] out The Pose converted to a tf2 Transform type. + */ + static void fromMsg(const geometry_msgs::msg::Pose & in, tf2::Transform & out) + { + out.setOrigin(tf2::Vector3(in.position.x, in.position.y, in.position.z)); + // w at the end in the constructor + out.setRotation( + tf2::Quaternion(in.orientation.x, in.orientation.y, in.orientation.z, in.orientation.w)); + } +}; + +/// \brief Default Type for automatic tf2::doTransform() implementation for tf2::Transform. +template<> +struct DefaultTransformType +{ + /// \brief Default Type for automatic tf2::doTransform() implementation for tf2::Transform. + using type = tf2::Transform; +}; + + +/************/ +/** Wrench **/ +/************/ + +/// \brief Conversion implementation for geometry_msgs::msg::Wrench and tf2::Transform. +template<> +struct ConversionImplementation, geometry_msgs::msg::Wrench> +{ + /** \brief Convert a tf2 Wrench to an equivalent geometry_msgs Wrench message. + * \param[in] in A tf2 Wrench object. + * \param[out] out The Wrench converted to a geometry_msgs Wrench message type. + */ + static void toMsg(const std::array & in, geometry_msgs::msg::Wrench & out) + { + tf2::toMsg<>(std::get<0>(in), out.force); + tf2::toMsg<>(std::get<1>(in), out.torque); + } + + /** \brief Convert a geometry_msgs Wrench message to an equivalent tf2 Wrench type. + * \param[in] msg A Wrench message. + * \param[out] out The Wrench converted to a tf2 Wrench type. + */ + static void fromMsg(const geometry_msgs::msg::Wrench & msg, std::array & out) + { + tf2::fromMsg<>(msg.force, std::get<0>(out)); + tf2::fromMsg<>(msg.torque, std::get<1>(out)); + } +}; + +/// \brief Default return type of tf2::toMsg() for std::array. +template<> +struct DefaultMessageForDatatype> +{ + /// \brief Default return type of tf2::toMsg() for std::array. + using type = geometry_msgs::msg::Wrench; +}; + /** \brief Template for geometry_msgs transformation with corresponding tf2 types. * This function is used to implement various tf2::doTransform() specializations for * geometry_msgs Messages. @@ -128,9 +275,29 @@ inline void doTransformWithTf( } } // namespace impl -/********************/ -/** Vector3Stamped **/ -/********************/ +/** \brief Specialization of tf2::convert for Eigen::Matrix and std::array. + * + * This specialization of the template defined in tf2/convert.h + * is for a Wrench represented in an Eigen matrix. + * To avoid a dependency on Eigen, the Eigen::Matrix template is forward declared. + * \param[in] in Wrench, as Eigen matrix + * \param[out] out The Wrench as tf2 type + * \tparam options Eigen::Matrix template parameter + * \tparam row Eigen::Matrix template parameter + * \tparam cols Eigen::Matrix template parameter + */ +template +void convert( + const Eigen::Matrix & in, std::array & out) +{ + out[0] = tf2::Vector3{in[0], in[1], in[2]}; + out[1] = tf2::Vector3{in[3], in[4], in[5]}; +} + + +/*************/ +/** Vector3 **/ +/*************/ /** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Vector type. * This function is a specialization of the doTransform template defined in tf2/convert.h. @@ -167,9 +334,9 @@ inline void doTransform( t_out.header.frame_id = transform.header.frame_id; } -/******************/ -/** PointStamped **/ -/******************/ +/***********/ +/** Point **/ +/***********/ /** \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. @@ -179,12 +346,10 @@ inline void doTransform( */ template<> inline void doTransform( - const geometry_msgs::msg::PointStamped & t_in, geometry_msgs::msg::PointStamped & t_out, + const geometry_msgs::msg::Point & t_in, geometry_msgs::msg::Point & t_out, const geometry_msgs::msg::TransformStamped & transform) { - impl::doTransformWithTf(t_in.point, t_out.point, transform); - t_out.header.stamp = transform.header.stamp; - t_out.header.frame_id = transform.header.frame_id; + impl::doTransformWithTf(t_in, t_out, transform); } /** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Point type. @@ -195,15 +360,17 @@ inline void doTransform( */ template<> inline void doTransform( - const geometry_msgs::msg::Point & t_in, geometry_msgs::msg::Point & t_out, + const geometry_msgs::msg::PointStamped & t_in, geometry_msgs::msg::PointStamped & t_out, const geometry_msgs::msg::TransformStamped & transform) { - impl::doTransformWithTf(t_in, t_out, transform); + impl::doTransformWithTf(t_in.point, t_out.point, transform); + t_out.header.stamp = transform.header.stamp; + t_out.header.frame_id = transform.header.frame_id; } -/*****************/ -/** PoseStamped **/ -/*****************/ +/**********/ +/** Pose **/ +/**********/ /** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Pose type. * This function is a specialization of the doTransform template defined in tf2/convert.h. @@ -213,12 +380,10 @@ inline void doTransform( */ template<> inline void doTransform( - const geometry_msgs::msg::PoseStamped & t_in, geometry_msgs::msg::PoseStamped & t_out, + const geometry_msgs::msg::Pose & t_in, geometry_msgs::msg::Pose & t_out, const geometry_msgs::msg::TransformStamped & transform) { - impl::doTransformWithTf(t_in.pose, t_out.pose, transform); - t_out.header.stamp = transform.header.stamp; - t_out.header.frame_id = transform.header.frame_id; + impl::doTransformWithTf(t_in, t_out, transform); } /** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Pose type. @@ -229,10 +394,12 @@ inline void doTransform( */ template<> inline void doTransform( - const geometry_msgs::msg::Pose & t_in, geometry_msgs::msg::Pose & t_out, + const geometry_msgs::msg::PoseStamped & t_in, geometry_msgs::msg::PoseStamped & t_out, const geometry_msgs::msg::TransformStamped & transform) { - impl::doTransformWithTf(t_in, t_out, transform); + impl::doTransformWithTf(t_in.pose, t_out.pose, transform); + t_out.header.stamp = transform.header.stamp; + t_out.header.frame_id = transform.header.frame_id; } /*******************************/ @@ -257,38 +424,11 @@ inline void doTransform( t_out.pose.covariance = t_in.pose.covariance; } + /****************/ /** Quaternion **/ /****************/ -namespace impl -{ -/// \brief Conversion implementation for geometry_msgs::msg::Quaternion and tf2::Quaternion. -template<> -struct ConversionImplementation - : DefaultQuaternionConversionImplementation {}; - -/// \brief Default return type of tf2::toMsg() for tf2::Quaternion. -template<> -struct DefaultMessageForDatatype -{ - /// \brief Default return type of tf2::toMsg() for tf2::Quaternion. - using type = geometry_msgs::msg::Quaternion; -}; - -/// \brief Default Type for automatic tf2::doTransform() implementation for tf2::Quaternion. -template<> -struct DefaultTransformType -{ - /// \brief Default Type for automatic tf2::doTransform() implementation for tf2::Quaternion. - using type = tf2::Transform; -}; - -} // namespace impl - -/***********************/ -/** QuaternionStamped **/ -/***********************/ /** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Quaternion type. * This function is a specialization of the doTransform template defined in tf2/convert.h. * \param[in] t_in The quaternion to transform, as a timestamped Quaternion3 message. @@ -319,51 +459,11 @@ inline void doTransform( t_out.header.frame_id = transform.header.frame_id; } + /***************/ /** Transform **/ /***************/ -namespace impl -{ -/// \brief Conversion implementation for geometry_msgs::msg::Transform and tf2::Transform. -template<> -struct ConversionImplementation -{ - /** \brief Convert a tf2 Transform type to its equivalent geometry_msgs representation. - * \param[in] in in A tf2 Transform object. - * \param[out] out The Transform converted to a geometry_msgs message type. - */ - static void toMsg(const tf2::Transform & in, geometry_msgs::msg::Transform & out) - { - tf2::toMsg<>(in.getOrigin(), out.translation); - tf2::toMsg<>(in.getRotation(), out.rotation); - } - - /** \brief Convert a Transform message to its equivalent tf2 representation. - * \param[in] in A Transform message type. - * \param[out] out The Transform converted to a tf2 type. - */ - static void fromMsg(const geometry_msgs::msg::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 Default return type of tf2::toMsg() for tf2::Transform. -template<> -struct DefaultMessageForDatatype -{ - /// \brief Default return type of tf2::toMsg() for tf2::Transform. - using type = geometry_msgs::msg::Transform; -}; -} // namespace impl - /** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Transform type. * This function is a specialization of the doTransform template defined in tf2/convert.h. * \param[in] t_in The frame to transform, as a timestamped Transform3 message. @@ -379,105 +479,6 @@ inline void doTransform( t_out.header.stamp = transform.header.stamp; t_out.header.frame_id = transform.header.frame_id; } - -namespace impl -{ -/**********/ -/** Pose **/ -/**********/ - -/// \brief Conversion implementation for geometry_msgs::msg::Pose and tf2::Transform. -template<> -struct ConversionImplementation -{ - /** \brief Convert a tf2 Transform type to an equivalent geometry_msgs Pose message. - * \param[in] in A tf2 Transform object. - * \param[out] out The Transform converted to a geometry_msgs Pose message type. - */ - static void toMsg(const tf2::Transform & in, geometry_msgs::msg::Pose & out) - { - tf2::toMsg<>(in.getOrigin(), out.position); - tf2::toMsg<>(in.getRotation(), out.orientation); - } - - /** \brief Convert a geometry_msgs Pose message to an equivalent tf2 Transform type. - * \param[in] in A Pose message. - * \param[out] out The Pose converted to a tf2 Transform type. - */ - static void fromMsg(const geometry_msgs::msg::Pose & in, tf2::Transform & out) - { - out.setOrigin(tf2::Vector3(in.position.x, in.position.y, in.position.z)); - // w at the end in the constructor - out.setRotation( - tf2::Quaternion(in.orientation.x, in.orientation.y, in.orientation.z, in.orientation.w)); - } -}; - -/// \brief Default Type for automatic tf2::doTransform() implementation for tf2::Transform. -template<> -struct DefaultTransformType -{ - /// \brief Default Type for automatic tf2::doTransform() implementation for tf2::Transform. - using type = tf2::Transform; -}; - -/************/ -/** Wrench **/ -/************/ - -/// \brief Conversion implementation for geometry_msgs::msg::Wrench and tf2::Transform. -template<> -struct ConversionImplementation, geometry_msgs::msg::Wrench> -{ - /** \brief Convert a tf2 Wrench to an equivalent geometry_msgs Wrench message. - * \param[in] in A tf2 Wrench object. - * \param[out] out The Wrench converted to a geometry_msgs Wrench message type. - */ - static void toMsg(const std::array & in, geometry_msgs::msg::Wrench & out) - { - tf2::toMsg<>(std::get<0>(in), out.force); - tf2::toMsg<>(std::get<1>(in), out.torque); - } - - /** \brief Convert a geometry_msgs Wrench message to an equivalent tf2 Wrench type. - * \param[in] msg A Wrench message. - * \param[out] out The Wrench converted to a tf2 Wrench type. - */ - static void fromMsg(const geometry_msgs::msg::Wrench & msg, std::array & out) - { - tf2::fromMsg<>(msg.force, std::get<0>(out)); - tf2::fromMsg<>(msg.torque, std::get<1>(out)); - } -}; - -/// \brief Default return type of tf2::toMsg() for std::array. -template<> -struct DefaultMessageForDatatype> -{ - /// \brief Default return type of tf2::toMsg() for std::array. - using type = geometry_msgs::msg::Wrench; -}; -} // namespace impl - -/** \brief Specialization of tf2::convert for Eigen::Matrix and std::array. - * - * This specialization of the template defined in tf2/convert.h - * is for a Wrench represented in an Eigen matrix. - * To avoid a dependency on Eigen, the Eigen::Matrix template is forward declared. - * \param[in] in Wrench, as Eigen matrix - * \param[out] out The Wrench as tf2 type - * \tparam options Eigen::Matrix template parameter - * \tparam row Eigen::Matrix template parameter - * \tparam cols Eigen::Matrix template parameter - */ -template -void convert( - const Eigen::Matrix & in, std::array & out) -{ - out[0] = tf2::Vector3{in[0], in[1], in[2]}; - out[1] = tf2::Vector3{in[3], in[4], in[5]}; -} - } // namespace tf2 diff --git a/tf2_kdl/include/tf2_kdl/tf2_kdl.h b/tf2_kdl/include/tf2_kdl/tf2_kdl.h index a660dd3de..61f3e1a58 100644 --- a/tf2_kdl/include/tf2_kdl/tf2_kdl.h +++ b/tf2_kdl/include/tf2_kdl/tf2_kdl.h @@ -76,33 +76,10 @@ inline geometry_msgs::msg::TransformStamped kdlToTransform(const KDL::Frame & k) namespace impl { -/// \brief Conversion implementation for geometry_msgs::msg::Transform and KDL::Frame. -template<> -struct ConversionImplementation -{ - /** \brief Convert a Transform message type to a KDL Frame. - * \param[in] in The Transform message to convert. - * \param[out] out The transform converted to a KDL Frame. - */ - static void fromMsg(geometry_msgs::msg::Transform const & in, KDL::Frame & out) - { - KDL::Rotation r; - KDL::Vector v; - tf2::fromMsg<>(in.translation, v); - tf2::fromMsg<>(in.rotation, r); - out = KDL::Frame(r, v); - } +// --------------------- +// Vector +// --------------------- - /** \brief Convert a KDL Frame type to a Transform message. - * \param[in] in The KDL Frame to convert. - * \param[out] out The KDL Frame converted to a Transform message. - */ - static void toMsg(KDL::Frame const & in, geometry_msgs::msg::Transform & out) - { - tf2::toMsg<>(in.p, out.translation); - tf2::toMsg<>(in.M, out.rotation); - } -}; /// \brief Conversion implementation for geometry_msgs::msg::Vector3 and KDL::Vector. template<> @@ -133,6 +110,7 @@ struct DefaultTransformType using type = KDL::Frame; }; + // --------------------- // Twist // --------------------- @@ -178,6 +156,7 @@ struct DefaultMessageForDatatype using type = geometry_msgs::msg::Twist; }; + // --------------------- // Wrench // --------------------- @@ -222,32 +201,40 @@ struct DefaultTransformType /// \brief Default Type for automatic tf2::doTransform() implementation for KDL::Wrench. using type = KDL::Frame; }; -} // namespace impl -/** \brief Specialization of tf2::convert for Eigen::Matrix and KDL::Wrench. - * - * This specialization of the template defined in tf2/convert.h - * is for a Wrench represented in an Eigen matrix. - * To avoid a dependency on Eigen, the Eigen::Matrix template is forward declared. - * \param[in] in Wrench, as Eigen matrix - * \param[out] out The Wrench as KDL::Wrench type - * \tparam options Eigen::Matrix template parameter - * \tparam row Eigen::Matrix template parameter - * \tparam cols Eigen::Matrix template parameter - */ -template -void convert(Eigen::Matrix const & in, KDL::Wrench & out) -{ - const auto msg = - toMsg, geometry_msgs::msg::Wrench>(in); - fromMsg<>(msg, out); -} // --------------------- // Frame // --------------------- -namespace impl + +/// \brief Conversion implementation for geometry_msgs::msg::Transform and KDL::Frame. +template<> +struct ConversionImplementation { + /** \brief Convert a Transform message type to a KDL Frame. + * \param[in] in The Transform message to convert. + * \param[out] out The transform converted to a KDL Frame. + */ + static void fromMsg(geometry_msgs::msg::Transform const & in, KDL::Frame & out) + { + KDL::Rotation r; + KDL::Vector v; + tf2::fromMsg<>(in.translation, v); + tf2::fromMsg<>(in.rotation, r); + out = KDL::Frame(r, v); + } + + /** \brief Convert a KDL Frame type to a Transform message. + * \param[in] in The KDL Frame to convert. + * \param[out] out The KDL Frame converted to a Transform message. + */ + static void toMsg(KDL::Frame const & in, geometry_msgs::msg::Transform & out) + { + tf2::toMsg<>(in.p, out.translation); + tf2::toMsg<>(in.M, out.rotation); + } +}; + /// \brief Conversion implementation for geometry_msgs::msg::Pose and KDL::Frame. template<> struct ConversionImplementation @@ -289,6 +276,11 @@ struct DefaultTransformType using type = KDL::Frame; }; + +// --------------------- +// Rotation +// --------------------- + /// \brief Conversion implementation for geometry_msgs::msg::Quaternion and KDL::Rotation. template<> struct ConversionImplementation @@ -340,6 +332,24 @@ void doTransform( out = t * in; } +/** \brief Specialization of tf2::convert for Eigen::Matrix and KDL::Wrench. + * + * This specialization of the template defined in tf2/convert.h + * is for a Wrench represented in an Eigen matrix. + * To avoid a dependency on Eigen, the Eigen::Matrix template is forward declared. + * \param[in] in Wrench, as Eigen matrix + * \param[out] out The Wrench as KDL::Wrench type + * \tparam options Eigen::Matrix template parameter + * \tparam row Eigen::Matrix template parameter + * \tparam cols Eigen::Matrix template parameter + */ +template +void convert(Eigen::Matrix const & in, KDL::Wrench & out) +{ + const auto msg = + toMsg, geometry_msgs::msg::Wrench>(in); + fromMsg<>(msg, out); +} } // namespace tf2 #endif // TF2_KDL__TF2_KDL_H_ From 1ac23bc8523766fa1d412ea6da331b50b1ae5cc0 Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Fri, 29 Jan 2021 21:50:42 +0000 Subject: [PATCH 33/51] Deprecate ToTransform and TransformTo Functions --- tf2_bullet/include/tf2_bullet/tf2_bullet.h | 4 +++- tf2_eigen/include/tf2_eigen/tf2_eigen.h | 8 ++++++-- tf2_eigen/test/tf2_eigen-test.cpp | 6 ++++-- .../include/tf2_geometry_msgs/tf2_geometry_msgs.h | 4 +++- tf2_kdl/include/tf2_kdl/tf2_kdl.h | 7 +++++-- 5 files changed, 21 insertions(+), 8 deletions(-) diff --git a/tf2_bullet/include/tf2_bullet/tf2_bullet.h b/tf2_bullet/include/tf2_bullet/tf2_bullet.h index d4ac6931f..7f7e20548 100644 --- a/tf2_bullet/include/tf2_bullet/tf2_bullet.h +++ b/tf2_bullet/include/tf2_bullet/tf2_bullet.h @@ -152,7 +152,9 @@ struct DefaultTransformType * \param[in] t The transform to convert, as a geometry_msgs TransformedStamped message. * \return The transform message converted to a Bullet btTransform. */ -inline btTransform transformToBullet(const geometry_msgs::msg::TransformStamped & t) +[[deprecated("Please use tf2::fromMsg()")]] +inline btTransform transformToBullet( + const geometry_msgs::msg::TransformStamped & t) { btTransform ans; fromMsg<>(t.transform, ans); diff --git a/tf2_eigen/include/tf2_eigen/tf2_eigen.h b/tf2_eigen/include/tf2_eigen/tf2_eigen.h index 82e7f1e5a..6df43346b 100644 --- a/tf2_eigen/include/tf2_eigen/tf2_eigen.h +++ b/tf2_eigen/include/tf2_eigen/tf2_eigen.h @@ -59,10 +59,14 @@ Eigen::Isometry3d transformToEigen(const geometry_msgs::msg::Transform & t) * \param t The transform to convert, as a geometry_msgs TransformedStamped message. * \return The transform message converted to an Eigen Isometry3d transform. */ +[[deprecated("Please use tf2::fromMsg()")]] inline -Eigen::Isometry3d transformToEigen(const geometry_msgs::msg::TransformStamped & t) +Eigen::Isometry3d transformToEigen( + const geometry_msgs::msg::TransformStamped & t) { - return transformToEigen(t.transform); + Eigen::Isometry3d res; + tf2::fromMsg<>(t.transform, res); + return res; } /** \brief Convert an Eigen Affine3d transform to the equivalent geometry_msgs message type. diff --git a/tf2_eigen/test/tf2_eigen-test.cpp b/tf2_eigen/test/tf2_eigen-test.cpp index d24555269..5164fcb7d 100644 --- a/tf2_eigen/test/tf2_eigen-test.cpp +++ b/tf2_eigen/test/tf2_eigen-test.cpp @@ -124,7 +124,8 @@ TEST(TfEigen, ConvertTransform) geometry_msgs::msg::Transform msg; tf2::toMsg(T, msg); - Eigen::Affine3d Tback = tf2::transformToEigen(msg); + Eigen::Affine3d Tback; + tf2::fromMsg(msg, Tback); EXPECT_TRUE(T.isApprox(Tback)); EXPECT_TRUE(tm.isApprox(Tback.matrix())); @@ -133,7 +134,8 @@ TEST(TfEigen, ConvertTransform) Eigen::Isometry3d I(tm); tf2::toMsg(T, msg); - Eigen::Isometry3d Iback = tf2::transformToEigen(msg); + Eigen::Isometry3d Iback; + tf2::fromMsg(msg, Iback); EXPECT_TRUE(I.isApprox(Iback)); EXPECT_TRUE(tm.isApprox(Iback.matrix())); 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 6342286e4..6232c7e08 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 @@ -62,8 +62,10 @@ namespace tf2 * \param t TransformStamped message to convert. * \return The converted KDL Frame. */ +[[deprecated("Please use tf2::fromMsg()")]] inline -KDL::Frame gmTransformToKDL(const geometry_msgs::msg::TransformStamped & t) +KDL::Frame +gmTransformToKDL(const geometry_msgs::msg::TransformStamped & t) { return KDL::Frame( KDL::Rotation::Quaternion( diff --git a/tf2_kdl/include/tf2_kdl/tf2_kdl.h b/tf2_kdl/include/tf2_kdl/tf2_kdl.h index 61f3e1a58..09ffd1af2 100644 --- a/tf2_kdl/include/tf2_kdl/tf2_kdl.h +++ b/tf2_kdl/include/tf2_kdl/tf2_kdl.h @@ -56,7 +56,9 @@ namespace tf2 * \param[in] t The transform to convert, as a geometry_msgs TransformedStamped message. * \return The transform message converted to an KDL Frame. */ -inline KDL::Frame transformToKDL(const geometry_msgs::msg::TransformStamped & t) +[[deprecated("Please use tf2::fromMsg()")]] inline KDL::Frame +transformToKDL( + const geometry_msgs::msg::TransformStamped & t) { KDL::Frame ans; tf2::fromMsg<>(t.transform, ans); @@ -67,7 +69,8 @@ inline KDL::Frame transformToKDL(const geometry_msgs::msg::TransformStamped & t) * \param[in] k The transform to convert, as an KDL Frame. * \return The transform converted to a TransformStamped message. */ -inline geometry_msgs::msg::TransformStamped kdlToTransform(const KDL::Frame & k) +[[deprecated("Please use tf2::toMsg()")]] inline geometry_msgs::msg::TransformStamped +kdlToTransform(const KDL::Frame & k) { geometry_msgs::msg::TransformStamped ans; tf2::toMsg<>(k, ans.transform); From 60abdead9abd1abf621d27806d0ccf03e7262bc2 Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Fri, 12 Feb 2021 11:42:57 +0000 Subject: [PATCH 34/51] Remove all overloads of tf2::convert Implementation is now only in impl::Converter --- tf2/include/tf2/convert.h | 15 +--- tf2/include/tf2/impl/convert.h | 79 ++++++++++++------- tf2/include/tf2/impl/forward.h | 2 +- .../tf2_geometry_msgs/tf2_geometry_msgs.h | 40 ++++++---- tf2_kdl/include/tf2_kdl/tf2_kdl.h | 41 +++++----- 5 files changed, 97 insertions(+), 80 deletions(-) diff --git a/tf2/include/tf2/convert.h b/tf2/include/tf2/convert.h index 2e5ada088..01c832333 100644 --- a/tf2/include/tf2/convert.h +++ b/tf2/include/tf2/convert.h @@ -198,20 +198,7 @@ inline void convert(const A & a, B & b) { impl::Converter< rosidl_generator_traits::is_message::value, - rosidl_generator_traits::is_message::value>::convert(a, b); -} - -/** \brief Overload of tf2::convert() for the same types. - * \param[in] a1 an object to convert from - * \param[in,out] a2 the object to convert to - * \tparam A Type of the object to convert - */ -template -void convert(const A & a1, A & a2) -{ - if (&a1 != &a2) { - a2 = a1; - } + rosidl_generator_traits::is_message::value, A, B>::convert(a, b); } /** \brief Function that converts from a row-major representation of a 6x6 diff --git a/tf2/include/tf2/impl/convert.h b/tf2/include/tf2/impl/convert.h index 8b00a12b7..17bac01cd 100644 --- a/tf2/include/tf2/impl/convert.h +++ b/tf2/include/tf2/impl/convert.h @@ -346,20 +346,13 @@ struct ConversionImplementation, Covariance /** \brief Helper for tf2::convert(). * \tparam IS_MESSAGE_A True if first argument is a message type. * \tparam IS_MESSAGE_B True if second argument is a message type. + * \tparam A Type of first argument. + * \tparam B Type of second argument. */ -template +template class Converter { public: - /** \brief Implementation of tf2::convert() depending of the argument types. - * \param[in] a Source of conversion. - * \param[out] b Target of conversion. - * \tparam A Type of first argument. - * \tparam B Type of second argument. - */ - template - static void convert(const A & a, B & b); - // The case where both A and B are messages should not happen: if you have two // messages that are interchangeable, well, that's against the ROS purpose: // only use one type. Worst comes to worst, specialize the original convert @@ -368,51 +361,77 @@ class Converter // used. // static_assert( - !(IS_MESSAGE_A && IS_MESSAGE_B), - "Conversion between two Message types is not supported!"); + !(IS_MESSAGE_A && IS_MESSAGE_B), "Conversion between two Message types is not supported!"); }; /** \brief Implementation of tf2::convert() for message-to-datatype conversion. - * \param[in] a Message to convert. - * \param[out] b Datatype to convert to. * \tparam A Message type. * \tparam B Datatype. */ -template<> template -inline void Converter::convert(const A & a, B & b) +struct Converter { - fromMsg<>(a, b); -} + + /** \brief Implementation of tf2::convert() for message-to-datatype conversion. + * \param[in] a Message to convert. + * \param[out] b Datatype to convert to. + */ + static void convert(const A & a, B & b) {fromMsg<>(a, b);} +}; /** \brief Implementation of tf2::convert() for datatype-to-message converiosn. - * \param[in] a Datatype to convert. - * \param[out] b Message to convert to. * \tparam A Datatype. * \tparam B Message. */ -template<> template -inline void Converter::convert(const A & a, B & b) +struct Converter { - b = toMsg(a); -} + /** \brief Implementation of tf2::convert() for datatype-to-message converiosn. + * \param[in] a Datatype to convert. + * \param[out] b Message to convert to. + */ + static void convert(const A & a, B & b) {b = toMsg(a);} +}; /** \brief Implementation of tf2::convert() for datatypes. * Converts the first argument to a message * (usually \c impl::DefaultMessageForDatatype::type ) * and then converts the message to the second argument. - * \param[in] a Source of conversion. - * \param[out] b Target of conversion. * \tparam A Datatype of first argument. * \tparam B Datatype of second argument. */ -template<> template -inline void Converter::convert(const A & a, B & b) +struct Converter::value>::type> { - fromMsg<>(toMsg<>(a), b); -} + /** \brief Implementation of tf2::convert() for datatypes. + * Converts the first argument to a message + * (usually \c impl::DefaultMessageForDatatype::type ) + * and then converts the message to the second argument. + * \param[in] a Source of conversion. + * \param[out] b Target of conversion. + */ + static void convert(const A & a, B & b) {fromMsg<>(toMsg<>(a), b);} +}; + +/** \brief Implementation of tf2::convert() for identical types. + * \tparam b Type is a Message + * \tparam A Type of the arguments + */ + +template +struct Converter +{ + /** \brief Overload of tf2::convert() for the same types. + * \param[in] a1 an object to convert from + * \param[in,out] a2 the object to convert to + */ + static void convert(const A & a1, A & a2) + { + if (&a1 != &a2) { + a2 = a1; + } + } +}; /** * \brief Default implementation for extracting timestamps and frame IDs. diff --git a/tf2/include/tf2/impl/forward.h b/tf2/include/tf2/impl/forward.h index 89c410095..01e1339c4 100644 --- a/tf2/include/tf2/impl/forward.h +++ b/tf2/include/tf2/impl/forward.h @@ -77,7 +77,7 @@ struct UnstampedMessageTraits; template struct StampedAttributesHelper; -template +template class Converter; } // namespace impl 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 6232c7e08..2bce6d554 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 @@ -275,26 +275,32 @@ inline void doTransformWithTf( tf2::doTransform(in_tf, out_tf, transform); tf2::convert(out_tf, out); } -} // namespace impl -/** \brief Specialization of tf2::convert for Eigen::Matrix and std::array. - * - * This specialization of the template defined in tf2/convert.h - * is for a Wrench represented in an Eigen matrix. - * To avoid a dependency on Eigen, the Eigen::Matrix template is forward declared. - * \param[in] in Wrench, as Eigen matrix - * \param[out] out The Wrench as tf2 type - * \tparam options Eigen::Matrix template parameter - * \tparam row Eigen::Matrix template parameter - * \tparam cols Eigen::Matrix template parameter +/** \brief Specialization of tf2::convert for Eigen::Matrix + * and std::array */ -template -void convert( - const Eigen::Matrix & in, std::array & out) +template<> +struct Converter, + std::array> { - out[0] = tf2::Vector3{in[0], in[1], in[2]}; - out[1] = tf2::Vector3{in[3], in[4], in[5]}; -} + /** \brief Specialization of tf2::convert for Eigen::Matrix and std::array. + * + * This specialization of the template defined in tf2/convert.h + * is for a Wrench represented in an Eigen matrix. + * To avoid a dependency on Eigen, the Eigen::Matrix template is forward declared. + * \param[in] in Wrench, as Eigen matrix + * \param[out] out The Wrench as tf2 type + * \tparam options Eigen::Matrix template parameter, used for indirection + */ + template + static void convert( + const Eigen::Matrix & in, std::array & out) + { + out[0] = tf2::Vector3{in[0], in[1], in[2]}; + out[1] = tf2::Vector3{in[3], in[4], in[5]}; + } +}; +} // namespace impl /*************/ diff --git a/tf2_kdl/include/tf2_kdl/tf2_kdl.h b/tf2_kdl/include/tf2_kdl/tf2_kdl.h index 09ffd1af2..d9de560cb 100644 --- a/tf2_kdl/include/tf2_kdl/tf2_kdl.h +++ b/tf2_kdl/include/tf2_kdl/tf2_kdl.h @@ -315,6 +315,29 @@ struct DefaultMessageForDatatype using type = geometry_msgs::msg::Quaternion; }; +/// \brief Specialization of tf2::convert for Eigen::Matrix and KDL::Wrench. +template<> +struct Converter, KDL::Wrench> +{ + /** \brief Specialization of tf2::convert for Eigen::Matrix and KDL::Wrench. + * + * This specialization of the template defined in tf2/convert.h + * is for a Wrench represented in an Eigen matrix. + * To avoid a dependency on Eigen, the Eigen::Matrix template is forward declared. + * \param[in] in Wrench, as Eigen matrix + * \param[out] out The Wrench as KDL::Wrench type + * \tparam options Eigen::Matrix template parameter, used for indirection + */ + template + static void convert( + Eigen::Matrix const & in, + KDL::Wrench & out) + { + const auto msg = + toMsg, geometry_msgs::msg::Wrench>(in); + fromMsg<>(msg, out); + } +}; } // namespace impl /** @@ -335,24 +358,6 @@ void doTransform( out = t * in; } -/** \brief Specialization of tf2::convert for Eigen::Matrix and KDL::Wrench. - * - * This specialization of the template defined in tf2/convert.h - * is for a Wrench represented in an Eigen matrix. - * To avoid a dependency on Eigen, the Eigen::Matrix template is forward declared. - * \param[in] in Wrench, as Eigen matrix - * \param[out] out The Wrench as KDL::Wrench type - * \tparam options Eigen::Matrix template parameter - * \tparam row Eigen::Matrix template parameter - * \tparam cols Eigen::Matrix template parameter - */ -template -void convert(Eigen::Matrix const & in, KDL::Wrench & out) -{ - const auto msg = - toMsg, geometry_msgs::msg::Wrench>(in); - fromMsg<>(msg, out); -} } // namespace tf2 #endif // TF2_KDL__TF2_KDL_H_ From 528401773fdd827bb04878264fd6cdeb9c90fca0 Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Fri, 12 Feb 2021 11:44:53 +0000 Subject: [PATCH 35/51] Apply uncrustify --- tf2/include/tf2/impl/convert.h | 34 +++++++++++++-------------- tf2/include/tf2/impl/forward.h | 22 ++++++++--------- tf2/include/tf2/transform_datatypes.h | 4 +++- 3 files changed, 31 insertions(+), 29 deletions(-) diff --git a/tf2/include/tf2/impl/convert.h b/tf2/include/tf2/impl/convert.h index 17bac01cd..bf11b2875 100644 --- a/tf2/include/tf2/impl/convert.h +++ b/tf2/include/tf2/impl/convert.h @@ -33,18 +33,15 @@ #include #include +#include #include "../time.h" #include "forward.h" -#include - - namespace tf2 { namespace impl { - /// Helper to always trigger \c static_assert s template constexpr bool alwaysFalse = false; @@ -55,7 +52,9 @@ constexpr bool alwaysFalse = false; * \tparam Message The message to check */ template -struct MessageHasStdHeader : std::false_type {}; +struct MessageHasStdHeader : std::false_type +{ +}; /** \brief Check whether a message is stamped and has a std_msgs::msg::Header member. * @@ -88,7 +87,8 @@ template struct DefaultMessageForDatatype { static_assert( - alwaysFalse, "No default message type defined, " + alwaysFalse, + "No default message type defined, " "please check your header file includes!"); // using type = ...; }; @@ -112,7 +112,8 @@ template struct DefaultTransformType { static_assert( - alwaysFalse, "No default transform type defined, " + alwaysFalse, + "No default transform type defined, " "please check your header file includes!"); // using type = ...; }; @@ -142,7 +143,8 @@ template struct ConversionImplementation { static_assert( - alwaysFalse, "No Conversion Implementation available, " + alwaysFalse, + "No Conversion Implementation available, " "please check your header file includes!"); // void toMsg(const Datatype&, Message&); // void fromMsg(const Message&, Datatype&); @@ -185,7 +187,8 @@ template struct StampedMessageTraits { static_assert( - alwaysFalse, "No traits for this stamped message type available, " + alwaysFalse, + "No traits for this stamped message type available, " "please check your header file includes!"); // using UntampedType = ...; // static UntampedType& accessMessage(StampedMsg &); @@ -220,7 +223,8 @@ template struct UnstampedMessageTraits { static_assert( - alwaysFalse, "No traits for this message type available, " + alwaysFalse, + "No traits for this message type available, " "please check your header file includes!"); // using StampedType = ...; // using StampedTypeWithCovariance = ...; @@ -250,9 +254,8 @@ struct DefaultMessageForDatatype> template struct DefaultMessageForDatatype> { - using type = - typename UnstampedMessageTraits::type>:: - StampedTypeWithCovariance; + using type = typename UnstampedMessageTraits< + typename DefaultMessageForDatatype::type>::StampedTypeWithCovariance; }; /** @@ -462,10 +465,7 @@ struct StampedAttributesHelper * \param[in] t The data input. * \return The frame_id associated with the data. */ - static std::string getFrameId(const T & t) - { - return t.header.frame_id; - } + static std::string getFrameId(const T & t) {return t.header.frame_id;} }; /** diff --git a/tf2/include/tf2/impl/forward.h b/tf2/include/tf2/impl/forward.h index 01e1339c4..9a3d58bb7 100644 --- a/tf2/include/tf2/impl/forward.h +++ b/tf2/include/tf2/impl/forward.h @@ -37,25 +37,25 @@ namespace tf2 { namespace impl { -template +template struct DefaultMessageForDatatype; -template +template struct DefaultTransformType; } // namespace impl -template +template class Stamped; -template +template class WithCovarianceStamped; -template +template B & toMsg(const A & a, B & b); -template ::type> +template::type> B toMsg(const A & a); -template +template void fromMsg(const A & a, B & b); std::array, 6> covarianceRowMajorToNested( @@ -65,16 +65,16 @@ std::array covarianceNestedToRowMajor( namespace impl { -template +template struct ConversionImplementation; -template +template struct StampedMessageTraits; -template +template struct UnstampedMessageTraits; -template +template struct StampedAttributesHelper; template diff --git a/tf2/include/tf2/transform_datatypes.h b/tf2/include/tf2/transform_datatypes.h index 3c1b2dbf1..ed9f5cd0e 100644 --- a/tf2/include/tf2/transform_datatypes.h +++ b/tf2/include/tf2/transform_datatypes.h @@ -64,7 +64,9 @@ class Stamped : public T } /** Full constructor */ - Stamped(const T & input, const builtin_interfaces::msg::Time & timestamp, const std::string & frame_id) + Stamped( + const T & input, const builtin_interfaces::msg::Time & timestamp, + const std::string & frame_id) : T(input), stamp_(TimePointFromMsg(timestamp)), frame_id_(frame_id) { } From 5838568c5f83424ce2303aee2984e9a8fdcd5bab Mon Sep 17 00:00:00 2001 From: Bjar Ne <43565432+gleichdick@users.noreply.github.com> Date: Fri, 19 Mar 2021 10:01:25 +0100 Subject: [PATCH 36/51] Add sensor_msgs test dependency --- tf2_sensor_msgs/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/tf2_sensor_msgs/CMakeLists.txt b/tf2_sensor_msgs/CMakeLists.txt index f05148f04..0caab16a3 100644 --- a/tf2_sensor_msgs/CMakeLists.txt +++ b/tf2_sensor_msgs/CMakeLists.txt @@ -38,6 +38,7 @@ if(BUILD_TESTING) target_link_libraries(test_tf2_sensor_msgs_cpp Eigen3::Eigen) ament_target_dependencies(test_tf2_sensor_msgs_cpp "rclcpp" + "sensor_msgs" "tf2" "tf2_ros" ) From 66eabfbff8c4a7352420ca3fc4923e4679977d9d Mon Sep 17 00:00:00 2001 From: Bjar Ne <43565432+gleichdick@users.noreply.github.com> Date: Fri, 19 Mar 2021 10:02:13 +0100 Subject: [PATCH 37/51] remove newlines in test_tf2/test/test_convert.cpp MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero --- test_tf2/test/test_convert.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/test_tf2/test/test_convert.cpp b/test_tf2/test/test_convert.cpp index e12b9135d..bd228ab66 100644 --- a/test_tf2/test/test_convert.cpp +++ b/test_tf2/test/test_convert.cpp @@ -298,7 +298,6 @@ TEST(TfEigenKdl, TestTwistMatrix) EXPECT_EQ(kdl_v, kdl_v1); } - TEST(TfEigenKdl, TestMatrixWrench) { Vector6d eigen_v; @@ -316,7 +315,6 @@ TEST(TfEigenKdl, TestMatrixWrench) EXPECT_EQ(eigen_v, eigen_v2); } - TEST(TfEigenKdl, TestVectorVector3d) { const auto kdl_v = KDL::Vector(1, 2, 3); From 4cc83f6c3de05942f39d3fe7d37d16eab7717fa1 Mon Sep 17 00:00:00 2001 From: Bjar Ne <43565432+gleichdick@users.noreply.github.com> Date: Fri, 19 Mar 2021 13:02:48 +0100 Subject: [PATCH 38/51] [tf2_sensor_msgs] Fix Eigen3 dependency --- tf2_sensor_msgs/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tf2_sensor_msgs/CMakeLists.txt b/tf2_sensor_msgs/CMakeLists.txt index 0caab16a3..cc5c49405 100644 --- a/tf2_sensor_msgs/CMakeLists.txt +++ b/tf2_sensor_msgs/CMakeLists.txt @@ -35,8 +35,8 @@ if(BUILD_TESTING) ament_add_gtest(test_tf2_sensor_msgs_cpp test/test_tf2_sensor_msgs.cpp) if(TARGET test_tf2_sensor_msgs_cpp) target_include_directories(test_tf2_sensor_msgs_cpp PUBLIC include) - target_link_libraries(test_tf2_sensor_msgs_cpp Eigen3::Eigen) ament_target_dependencies(test_tf2_sensor_msgs_cpp + "Eigen3" "rclcpp" "sensor_msgs" "tf2" From 7704c910f58f226cf49331973b36382fdc9144fa Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Fri, 19 Mar 2021 21:12:54 +0000 Subject: [PATCH 39/51] Fix -Wextra-qualification --- .../include/tf2_eigen_kdl/tf2_eigen_kdl.hpp | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/tf2_eigen_kdl/include/tf2_eigen_kdl/tf2_eigen_kdl.hpp b/tf2_eigen_kdl/include/tf2_eigen_kdl/tf2_eigen_kdl.hpp index 718cb7eeb..dd8050db4 100644 --- a/tf2_eigen_kdl/include/tf2_eigen_kdl/tf2_eigen_kdl.hpp +++ b/tf2_eigen_kdl/include/tf2_eigen_kdl/tf2_eigen_kdl.hpp @@ -98,73 +98,73 @@ TF2_EIGEN_KDL_PUBLIC void wrenchEigenToKDL(const Eigen::Matrix & e, KDL::Wrench & k); template<> -void tf2::convert(const KDL::Rotation & a, Eigen::Quaterniond & b) +void convert(const KDL::Rotation & a, Eigen::Quaterniond & b) { quaternionKDLToEigen(a, b); } template<> -void tf2::convert(const Eigen::Quaterniond & a, KDL::Rotation & b) +void convert(const Eigen::Quaterniond & a, KDL::Rotation & b) { quaternionEigenToKDL(a, b); } template<> -void tf2::convert(const KDL::Frame & a, Eigen::Affine3d & b) +void convert(const KDL::Frame & a, Eigen::Affine3d & b) { transformKDLToEigen(a, b); } template<> -void tf2::convert(const KDL::Frame & a, Eigen::Isometry3d & b) +void convert(const KDL::Frame & a, Eigen::Isometry3d & b) { transformKDLToEigen(a, b); } template<> -void tf2::convert(const Eigen::Affine3d & a, KDL::Frame & b) +void convert(const Eigen::Affine3d & a, KDL::Frame & b) { transformEigenToKDL(a, b); } template<> -void tf2::convert(const Eigen::Isometry3d & a, KDL::Frame & b) +void convert(const Eigen::Isometry3d & a, KDL::Frame & b) { transformEigenToKDL(a, b); } template<> -void tf2::convert(const KDL::Twist & a, Eigen::Matrix & b) +void convert(const KDL::Twist & a, Eigen::Matrix & b) { twistKDLToEigen(a, b); } template<> -void tf2::convert(const Eigen::Matrix & a, KDL::Twist & b) +void convert(const Eigen::Matrix & a, KDL::Twist & b) { twistEigenToKDL(a, b); } template<> -void tf2::convert(const KDL::Vector & a, Eigen::Matrix & b) +void convert(const KDL::Vector & a, Eigen::Matrix & b) { vectorKDLToEigen(a, b); } template<> -void tf2::convert(const Eigen::Matrix & a, KDL::Vector & b) +void convert(const Eigen::Matrix & a, KDL::Vector & b) { vectorEigenToKDL(a, b); } template<> -void tf2::convert(const KDL::Wrench & a, Eigen::Matrix & b) +void convert(const KDL::Wrench & a, Eigen::Matrix & b) { wrenchKDLToEigen(a, b); } template<> -void tf2::convert(const Eigen::Matrix & a, KDL::Wrench & b) +void convert(const Eigen::Matrix & a, KDL::Wrench & b) { wrenchEigenToKDL(a, b); } From 988635e757b0dee2431b74aa0176045709808529 Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Fri, 19 Mar 2021 21:35:48 +0000 Subject: [PATCH 40/51] Fix Bullet cmake --- test_tf2/CMakeLists.txt | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/test_tf2/CMakeLists.txt b/test_tf2/CMakeLists.txt index a29ddbe59..2271b3118 100644 --- a/test_tf2/CMakeLists.txt +++ b/test_tf2/CMakeLists.txt @@ -32,9 +32,11 @@ find_package(tf2_ros REQUIRED) ament_find_gtest() -find_package(PkgConfig REQUIRED) -pkg_check_modules(bullet REQUIRED bullet) -include_directories(include ${bullet_INCLUDE_DIRS}) +if(WIN32) + set(BULLET_ROOT $ENV{ChocolateyInstall}/lib/bullet) +endif() +find_package(Bullet REQUIRED) +include_directories(include ${BULLET_INCLUDE_DIRS}) ament_add_gtest(buffer_core_test test/buffer_core_test.cpp) if(TARGET buffer_core_test) From a336830a747469f203e3220bb5c4d1654ab4523e Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Fri, 19 Mar 2021 22:41:40 +0000 Subject: [PATCH 41/51] Proper forwarding of messages (Code C4099) --- tf2/include/tf2/impl/stamped_traits.h | 38 +++++++++++++-------------- 1 file changed, 19 insertions(+), 19 deletions(-) diff --git a/tf2/include/tf2/impl/stamped_traits.h b/tf2/include/tf2/impl/stamped_traits.h index a8e35ffc0..4aea11108 100644 --- a/tf2/include/tf2/impl/stamped_traits.h +++ b/tf2/include/tf2/impl/stamped_traits.h @@ -41,43 +41,43 @@ namespace geometry_msgs namespace msg { template -class Point_; +struct Point_; template -class Vector_; +struct Vector_; template -class Quaternion_; +struct Quaternion_; template -class Pose_; +struct Pose_; template -class Twist_; +struct Twist_; template -class PoseWithCovariance_; +struct PoseWithCovariance_; template -class Wrench_; +struct Wrench_; template -class PointStamped_; +struct PointStamped_; template -class VectorStamped_; +struct VectorStamped_; template -class QuaternionStamped_; +struct QuaternionStamped_; template -class PoseStamped_; +struct PoseStamped_; template -class TwistStamped_; +struct TwistStamped_; template -class PoseWithCovarianceStamped_; +struct PoseWithCovarianceStamped_; template -class WrenchStamped_; +struct WrenchStamped_; template -class TransformStamped_; +struct TransformStamped_; template -class Transform_; +struct Transform_; template -class Vector3_; +struct Vector3_; template -class Vector3Stamped_; +struct Vector3Stamped_; template -class TwistWithCovarianceStamped_; +struct TwistWithCovarianceStamped_; } // namespace msg } // namespace geometry_msgs From 3278ca2e17b08fba8c5122ca4dbfdc920aa5b5db Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Tue, 6 Apr 2021 17:27:52 +0000 Subject: [PATCH 42/51] Add static_cast to tf2_bullet bullet uses floats, which causes narrowing warnings --- tf2/include/tf2/impl/convert.h | 16 ++++++++++++---- tf2_bullet/include/tf2_bullet/tf2_bullet.h | 6 +++--- 2 files changed, 15 insertions(+), 7 deletions(-) diff --git a/tf2/include/tf2/impl/convert.h b/tf2/include/tf2/impl/convert.h index bf11b2875..1fb9d401e 100644 --- a/tf2/include/tf2/impl/convert.h +++ b/tf2/include/tf2/impl/convert.h @@ -508,8 +508,9 @@ struct StampedAttributesHelper> * * \tparam VectorType Datatype of the Vector. * \tparam Message Message type, like geometry_msgs::msg::Vector3. + * \tparam VectorMemberType Type of the x, y and \c z variables of the vector */ -template +template struct DefaultVectorConversionImplementation { /** \brief Convert a vector type to a vector-like message. @@ -529,7 +530,10 @@ struct DefaultVectorConversionImplementation */ static void fromMsg(const Message & msg, VectorType & out) { - out = VectorType(msg.x, msg.y, msg.z); + // cast to suppress narrowing warning + out = VectorType( + static_cast(msg.x), static_cast(msg.y), + static_cast(msg.z)); } }; @@ -537,8 +541,9 @@ struct DefaultVectorConversionImplementation * a geometry_msgs::msg::Quaternion message. * * \tparam QuaternionType Datatype of the Vector. + * \tparam QuaternionMemberType Type of the x, y, z and \c w variables of the quaternion */ -template +template struct DefaultQuaternionConversionImplementation { /** \brief Convert a quaternion type to a Quaternion message. @@ -559,7 +564,10 @@ struct DefaultQuaternionConversionImplementation */ static void fromMsg(const geometry_msgs::msg::Quaternion & msg, QuaternionType & out) { - out = QuaternionType(msg.x, msg.y, msg.z, msg.w); + // cast to suppress narrowing warning + out = QuaternionType( + static_cast(msg.x), static_cast(msg.y), + static_cast(msg.z), static_cast(msg.w)); } }; diff --git a/tf2_bullet/include/tf2_bullet/tf2_bullet.h b/tf2_bullet/include/tf2_bullet/tf2_bullet.h index 7f7e20548..c0489d8f6 100644 --- a/tf2_bullet/include/tf2_bullet/tf2_bullet.h +++ b/tf2_bullet/include/tf2_bullet/tf2_bullet.h @@ -79,21 +79,21 @@ struct DefaultMessageForDatatype /// \brief Conversion implementation for geometry_msgs::msg::Point and btVector3. template<> struct ConversionImplementation - : DefaultVectorConversionImplementation + : DefaultVectorConversionImplementation { }; /// \brief Conversion implementation for geometry_msgs::msg::Vector3 and btVector3. template<> struct ConversionImplementation - : DefaultVectorConversionImplementation + : DefaultVectorConversionImplementation { }; /// \brief Conversion implementation for geometry_msgs::msg::Quaternion and Eigen::Quaterniond. template<> struct ConversionImplementation - : DefaultQuaternionConversionImplementation {}; + : DefaultQuaternionConversionImplementation {}; /// \brief Conversion implementation for geometry_msgs::msg::Transform and btTransform. template<> From 7795dedb6ff1bd7bbae32bee1358ca4aa4f380b6 Mon Sep 17 00:00:00 2001 From: Bjar Ne <43565432+gleichdick@users.noreply.github.com> Date: Thu, 8 Apr 2021 20:17:35 +0200 Subject: [PATCH 43/51] Fix formatting nitpicks MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero --- tf2/include/tf2/impl/convert.h | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/tf2/include/tf2/impl/convert.h b/tf2/include/tf2/impl/convert.h index 1fb9d401e..28ead75e4 100644 --- a/tf2/include/tf2/impl/convert.h +++ b/tf2/include/tf2/impl/convert.h @@ -302,7 +302,7 @@ struct ConversionImplementation, StampedMessage> * \brief Partial specialization of impl::ConversionImplementation for stamped types with covariance. * * This partial specialization provides the conversion implementation ( \c toMsg() and \c fromMsg() ) - * between stamped types with covariance ( non-message types of tf2::WithCovarianceStamped\ + * between stamped types with covariance (non-message types of tf2::WithCovarianceStamped\ * and ROS message datatypes with a \c header member). * The covariance, the timestamp and the frame ID are preserved during the conversion. * The implementation of tf2::toMsg() and tf2::fromMsg() for the unstamped types without covariance @@ -398,7 +398,7 @@ struct Converter /** \brief Implementation of tf2::convert() for datatypes. * Converts the first argument to a message - * (usually \c impl::DefaultMessageForDatatype::type ) + * (usually \c impl::DefaultMessageForDatatype::type) * and then converts the message to the second argument. * \tparam A Datatype of first argument. * \tparam B Datatype of second argument. @@ -425,9 +425,9 @@ template struct Converter { /** \brief Overload of tf2::convert() for the same types. - * \param[in] a1 an object to convert from - * \param[in,out] a2 the object to convert to - */ + * \param[in] a1 an object to convert from + * \param[in,out] a2 the object to convert to + */ static void convert(const A & a1, A & a2) { if (&a1 != &a2) { @@ -461,6 +461,7 @@ struct StampedAttributesHelper tf2::fromMsg<>(t.header.stamp, timestamp); return timestamp; } + /**\brief Get the frame_id from data * \param[in] t The data input. * \return The frame_id associated with the data. @@ -479,6 +480,7 @@ struct StampedAttributesHelper> * \return The timestamp associated with the data. */ static tf2::TimePoint 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. @@ -497,6 +499,7 @@ struct StampedAttributesHelper> * \return The timestamp associated with the data. */ static tf2::TimePoint getTimestamp(const tf2::WithCovarianceStamped & t) {return t.stamp_;} + /** \brief Get the frame_id from data * \param t The data input. * \return The frame_id associated with the data. From d3fb3bdc7dd0e5cc7a24884bbd3fddf5e5c2f716 Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Thu, 8 Apr 2021 18:37:36 +0000 Subject: [PATCH 44/51] Add TODOs and comments --- tf2/include/tf2/impl/convert.h | 10 ++++++++++ tf2_geometry_msgs/CMakeLists.txt | 2 ++ tf2_kdl/CMakeLists.txt | 2 ++ tf2_sensor_msgs/CMakeLists.txt | 1 + 4 files changed, 15 insertions(+) diff --git a/tf2/include/tf2/impl/convert.h b/tf2/include/tf2/impl/convert.h index 28ead75e4..b8d8de493 100644 --- a/tf2/include/tf2/impl/convert.h +++ b/tf2/include/tf2/impl/convert.h @@ -90,6 +90,8 @@ struct DefaultMessageForDatatype alwaysFalse, "No default message type defined, " "please check your header file includes!"); + + // implement the following in the specializations for your own types // using type = ...; }; @@ -115,6 +117,8 @@ struct DefaultTransformType alwaysFalse, "No default transform type defined, " "please check your header file includes!"); + + // implement the following in the specializations for your own types // using type = ...; }; @@ -146,6 +150,8 @@ struct ConversionImplementation alwaysFalse, "No Conversion Implementation available, " "please check your header file includes!"); + + // implement the following in the specializations for your own types // void toMsg(const Datatype&, Message&); // void fromMsg(const Message&, Datatype&); }; @@ -190,6 +196,8 @@ struct StampedMessageTraits alwaysFalse, "No traits for this stamped message type available, " "please check your header file includes!"); + + // implement the following in the specializations for your own types // using UntampedType = ...; // static UntampedType& accessMessage(StampedMsg &); // static UntampedType getMessage(StampedMsg const&); @@ -226,6 +234,8 @@ struct UnstampedMessageTraits alwaysFalse, "No traits for this message type available, " "please check your header file includes!"); + + // implement the following in the specializations for your own types // using StampedType = ...; // using StampedTypeWithCovariance = ...; }; diff --git a/tf2_geometry_msgs/CMakeLists.txt b/tf2_geometry_msgs/CMakeLists.txt index b6d1429f7..286cc3b40 100644 --- a/tf2_geometry_msgs/CMakeLists.txt +++ b/tf2_geometry_msgs/CMakeLists.txt @@ -31,6 +31,8 @@ if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) find_package(rclcpp REQUIRED) + # TODO(ros2/geometry2#259) Switch to ament_lint_auto once headers + # are renamed to .hpp find_package(ament_cmake_cppcheck REQUIRED) find_package(ament_cmake_cpplint REQUIRED) find_package(ament_cmake_lint_cmake REQUIRED) diff --git a/tf2_kdl/CMakeLists.txt b/tf2_kdl/CMakeLists.txt index 3a058600f..844607ba0 100644 --- a/tf2_kdl/CMakeLists.txt +++ b/tf2_kdl/CMakeLists.txt @@ -34,6 +34,8 @@ if(BUILD_TESTING) find_package(rclcpp REQUIRED) find_package(tf2_msgs REQUIRED) + # TODO(ros2/geometry2#259) Switch to ament_lint_auto once headers + # are renamed to .hpp find_package(ament_cmake_cppcheck REQUIRED) find_package(ament_cmake_cpplint REQUIRED) find_package(ament_cmake_lint_cmake REQUIRED) diff --git a/tf2_sensor_msgs/CMakeLists.txt b/tf2_sensor_msgs/CMakeLists.txt index cc5c49405..713e82630 100644 --- a/tf2_sensor_msgs/CMakeLists.txt +++ b/tf2_sensor_msgs/CMakeLists.txt @@ -30,6 +30,7 @@ if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) find_package(ament_cmake_pytest REQUIRED) + # TODO(ros2/orocos_kinematics_dynamics): reenable when PyKDL is ready to use #ament_add_pytest_test(test_tf2_sensor_msgs_py test/test_tf2_sensor_msgs.py) ament_add_gtest(test_tf2_sensor_msgs_cpp test/test_tf2_sensor_msgs.cpp) From c565ee6935d210822b473e5e0cc032f9b5878e36 Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Mon, 12 Apr 2021 16:09:33 +0000 Subject: [PATCH 45/51] Fix ament_lint_auto settings for cppcheck and uc --- tf2_geometry_msgs/CMakeLists.txt | 15 +++++---------- tf2_kdl/CMakeLists.txt | 13 +++++-------- 2 files changed, 10 insertions(+), 18 deletions(-) diff --git a/tf2_geometry_msgs/CMakeLists.txt b/tf2_geometry_msgs/CMakeLists.txt index 286cc3b40..03271ed52 100644 --- a/tf2_geometry_msgs/CMakeLists.txt +++ b/tf2_geometry_msgs/CMakeLists.txt @@ -29,19 +29,14 @@ ament_auto_find_build_dependencies(REQUIRED ${required_dependencies}) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) + find_package(ament_lint_auto REQUIRED) find_package(rclcpp REQUIRED) - # TODO(ros2/geometry2#259) Switch to ament_lint_auto once headers + # TODO(ros2/geometry2#259) Remove once headers # are renamed to .hpp - find_package(ament_cmake_cppcheck REQUIRED) - find_package(ament_cmake_cpplint REQUIRED) - find_package(ament_cmake_lint_cmake REQUIRED) - find_package(ament_cmake_uncrustify REQUIRED) - - ament_cppcheck(LANGUAGE "c++") - ament_cpplint() - ament_lint_cmake() - ament_uncrustify(LANGUAGE "c++") + set(ament_cmake_uncrustify_ADDITIONAL_ARGS --language CPP) + set(ament_cmake_cppcheck_LANGUAGE c++) + ament_lint_auto_find_test_dependencies() ament_add_gtest(test_tf2_geometry_msgs test/test_tf2_geometry_msgs.cpp) if(TARGET test_tf2_geometry_msgs) diff --git a/tf2_kdl/CMakeLists.txt b/tf2_kdl/CMakeLists.txt index 844607ba0..1a727d5e7 100644 --- a/tf2_kdl/CMakeLists.txt +++ b/tf2_kdl/CMakeLists.txt @@ -31,18 +31,15 @@ install(DIRECTORY include/${PROJECT_NAME}/ if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) + find_package(ament_lint_auto REQUIRED) find_package(rclcpp REQUIRED) find_package(tf2_msgs REQUIRED) - # TODO(ros2/geometry2#259) Switch to ament_lint_auto once headers + # TODO(ros2/geometry2#259) Remove once headers # are renamed to .hpp - find_package(ament_cmake_cppcheck REQUIRED) - find_package(ament_cmake_cpplint REQUIRED) - find_package(ament_cmake_lint_cmake REQUIRED) - find_package(ament_cmake_uncrustify REQUIRED) - - ament_cppcheck(LANGUAGE "c++") - ament_uncrustify(LANGUAGE "c++") + set(ament_cmake_uncrustify_ADDITIONAL_ARGS --language CPP) + set(ament_cmake_cppcheck_LANGUAGE c++) + ament_lint_auto_find_test_dependencies() ament_add_gtest(test_kdl test/test_tf2_kdl.cpp) target_include_directories(test_kdl PUBLIC From 918978950f9c3a6aa34d11b7109c48ebb2ab6453 Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Mon, 12 Apr 2021 16:38:27 +0000 Subject: [PATCH 46/51] [tf2_geometry_msgs] pep8 formatting --- tf2_geometry_msgs/scripts/test.py | 13 ++-- .../src/tf2_geometry_msgs/__init__.py | 2 +- .../tf2_geometry_msgs/tf2_geometry_msgs.py | 72 ++++++++++++++----- 3 files changed, 65 insertions(+), 22 deletions(-) diff --git a/tf2_geometry_msgs/scripts/test.py b/tf2_geometry_msgs/scripts/test.py index 87eb3352f..56adf69cb 100755 --- a/tf2_geometry_msgs/scripts/test.py +++ b/tf2_geometry_msgs/scripts/test.py @@ -2,10 +2,12 @@ import unittest import rclpy -import PyKDL import tf2_ros import tf2_geometry_msgs -from geometry_msgs.msg import TransformStamped, PointStamped, Vector3Stamped, PoseStamped, PoseWithCovarianceStamped, Quaternion +from geometry_msgs.msg import (TransformStamped, PointStamped, + Vector3Stamped, PoseStamped, + PoseWithCovarianceStamped, Quaternion) + class GeometryMsgs(unittest.TestCase): def test_transform(self): @@ -17,7 +19,9 @@ def test_transform(self): t.header.frame_id = 'a' t.child_frame_id = 'b' b.set_transform(t, 'eitan_rocks') - out = b.lookup_transform('a','b', rclpy.time.Time(seconds=2.0).to_msg(), rclpy.time.Duration(seconds=2)) + out = b.lookup_transform('a', 'b', + rclpy.time.Time(seconds=2.0).to_msg(), + rclpy.time.Duration(seconds=2)) self.assertEqual(out.transform.translation.x, 1) self.assertEqual(out.transform.rotation.x, 1) self.assertEqual(out.header.frame_id, 'a') @@ -30,7 +34,7 @@ def test_transform(self): v.point.y = 2.0 v.point.z = 3.0 # b.registration.add(PointStamped) - out = b.transform(v, 'b', new_type = PointStamped) + out = b.transform(v, 'b', new_type=PointStamped) self.assertEqual(out.point.x, 0) self.assertEqual(out.point.y, -2) self.assertEqual(out.point.z, -3) @@ -100,6 +104,7 @@ def test_transform(self): self.assertEqual(out.vector.y, 0) self.assertEqual(out.vector.z, 0) + if __name__ == '__main__': rclpy.init(args=None) unittest.main() diff --git a/tf2_geometry_msgs/src/tf2_geometry_msgs/__init__.py b/tf2_geometry_msgs/src/tf2_geometry_msgs/__init__.py index e072bacb8..e895a1fc2 100644 --- a/tf2_geometry_msgs/src/tf2_geometry_msgs/__init__.py +++ b/tf2_geometry_msgs/src/tf2_geometry_msgs/__init__.py @@ -1 +1 @@ -from .tf2_geometry_msgs import * +from .tf2_geometry_msgs import * # noqa diff --git a/tf2_geometry_msgs/src/tf2_geometry_msgs/tf2_geometry_msgs.py b/tf2_geometry_msgs/src/tf2_geometry_msgs/tf2_geometry_msgs.py index e157bf4e7..070392dd1 100644 --- a/tf2_geometry_msgs/src/tf2_geometry_msgs/tf2_geometry_msgs.py +++ b/tf2_geometry_msgs/src/tf2_geometry_msgs/tf2_geometry_msgs.py @@ -27,27 +27,35 @@ # author: Wim Meeussen -from geometry_msgs.msg import PoseStamped, Vector3Stamped, PointStamped, PoseWithCovarianceStamped +from geometry_msgs.msg import (PoseStamped, Vector3Stamped, PointStamped, + PoseWithCovarianceStamped) import PyKDL import tf2_ros + def to_msg_msg(msg): return msg + tf2_ros.ConvertRegistration().add_to_msg(Vector3Stamped, to_msg_msg) tf2_ros.ConvertRegistration().add_to_msg(PoseStamped, to_msg_msg) tf2_ros.ConvertRegistration().add_to_msg(PointStamped, to_msg_msg) + def from_msg_msg(msg): return msg + tf2_ros.ConvertRegistration().add_from_msg(Vector3Stamped, from_msg_msg) tf2_ros.ConvertRegistration().add_from_msg(PoseStamped, from_msg_msg) tf2_ros.ConvertRegistration().add_from_msg(PointStamped, from_msg_msg) + def transform_to_kdl(t): - return PyKDL.Frame(PyKDL.Rotation.Quaternion(t.transform.rotation.x, t.transform.rotation.y, - t.transform.rotation.z, t.transform.rotation.w), + return PyKDL.Frame(PyKDL.Rotation.Quaternion(t.transform.rotation.x, + t.transform.rotation.y, + t.transform.rotation.z, + t.transform.rotation.w), PyKDL.Vector(t.transform.translation.x, t.transform.translation.y, t.transform.translation.z)) @@ -55,55 +63,85 @@ def transform_to_kdl(t): # PointStamped def do_transform_point(point, transform): - p = transform_to_kdl(transform) * PyKDL.Vector(point.point.x, point.point.y, point.point.z) + p = transform_to_kdl(transform) * PyKDL.Vector(point.point.x, + point.point.y, + point.point.z) res = PointStamped() res.point.x = p[0] res.point.y = p[1] res.point.z = p[2] res.header = transform.header return res + + tf2_ros.TransformRegistration().add(PointStamped, do_transform_point) # Vector3Stamped def do_transform_vector3(vector3, transform): - transform.transform.translation.x = 0.0; - transform.transform.translation.y = 0.0; - transform.transform.translation.z = 0.0; - p = transform_to_kdl(transform) * PyKDL.Vector(vector3.vector.x, vector3.vector.y, vector3.vector.z) + transform.transform.translation.x = 0.0 + transform.transform.translation.y = 0.0 + transform.transform.translation.z = 0.0 + p = transform_to_kdl(transform) * PyKDL.Vector(vector3.vector.x, + vector3.vector.y, + vector3.vector.z) res = Vector3Stamped() res.vector.x = p[0] res.vector.y = p[1] res.vector.z = p[2] res.header = transform.header return res + + tf2_ros.TransformRegistration().add(Vector3Stamped, do_transform_vector3) + # PoseStamped def do_transform_pose(pose, transform): - f = transform_to_kdl(transform) * PyKDL.Frame(PyKDL.Rotation.Quaternion(pose.pose.orientation.x, pose.pose.orientation.y, - pose.pose.orientation.z, pose.pose.orientation.w), - PyKDL.Vector(pose.pose.position.x, pose.pose.position.y, pose.pose.position.z)) + f = transform_to_kdl(transform) * PyKDL.Frame( + PyKDL.Rotation.Quaternion( + pose.pose.orientation.x, pose.pose.orientation.y, + pose.pose.orientation.z, pose.pose.orientation.w), + PyKDL.Vector(pose.pose.position.x, + pose.pose.position.y, + pose.pose.position.z)) res = PoseStamped() res.pose.position.x = f.p[0] res.pose.position.y = f.p[1] res.pose.position.z = f.p[2] - (res.pose.orientation.x, res.pose.orientation.y, res.pose.orientation.z, res.pose.orientation.w) = f.M.GetQuaternion() + (res.pose.orientation.x, + res.pose.orientation.y, + res.pose.orientation.z, + res.pose.orientation.w) = f.M.GetQuaternion() res.header = transform.header return res + + tf2_ros.TransformRegistration().add(PoseStamped, do_transform_pose) + # PoseWithCovarianceStamped def do_transform_pose_with_covariance_stamped(pose, transform): - f = transform_to_kdl(transform) * PyKDL.Frame(PyKDL.Rotation.Quaternion(pose.pose.pose.orientation.x, pose.pose.pose.orientation.y, - pose.pose.pose.orientation.z, pose.pose.pose.orientation.w), - PyKDL.Vector(pose.pose.pose.position.x, pose.pose.pose.position.y, pose.pose.pose.position.z)) + f = transform_to_kdl(transform) * PyKDL.Frame( + PyKDL.Rotation.Quaternion(pose.pose.pose.orientation.x, + pose.pose.pose.orientation.y, + pose.pose.pose.orientation.z, + pose.pose.pose.orientation.w), + PyKDL.Vector(pose.pose.pose.position.x, + pose.pose.pose.position.y, + pose.pose.pose.position.z)) res = PoseWithCovarianceStamped() res.pose.pose.position.x = f.p[0] res.pose.pose.position.y = f.p[1] res.pose.pose.position.z = f.p[2] - (res.pose.pose.orientation.x, res.pose.pose.orientation.y, res.pose.pose.orientation.z, res.pose.pose.orientation.w) = f.M.GetQuaternion() + (res.pose.pose.orientation.x, + res.pose.pose.orientation.y, + res.pose.pose.orientation.z, + res.pose.pose.orientation.w) = f.M.GetQuaternion() res.pose.covariance = pose.pose.covariance res.header = transform.header return res -tf2_ros.TransformRegistration().add(PoseWithCovarianceStamped, do_transform_pose_with_covariance_stamped) + + +tf2_ros.TransformRegistration().add(PoseWithCovarianceStamped, + do_transform_pose_with_covariance_stamped) From 0dbe0309f56ac556bc8b6efc3484d2459848b85d Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Mon, 12 Apr 2021 16:48:16 +0000 Subject: [PATCH 47/51] [tf2_kdl] pep8 formatting --- tf2_kdl/docs/source/conf.py | 93 +++++++++++++++++---------------- tf2_kdl/scripts/test.py | 30 ++++++----- tf2_kdl/src/tf2_kdl/__init__.py | 2 +- tf2_kdl/src/tf2_kdl/tf2_kdl.py | 36 ++++++++++--- 4 files changed, 95 insertions(+), 66 deletions(-) diff --git a/tf2_kdl/docs/source/conf.py b/tf2_kdl/docs/source/conf.py index 8fd31a968..a84d5729b 100644 --- a/tf2_kdl/docs/source/conf.py +++ b/tf2_kdl/docs/source/conf.py @@ -3,7 +3,8 @@ # tf2 documentation build configuration file, created by # sphinx-quickstart on Mon Jun 1 14:21:53 2009. # -# This file is execfile()d with the current directory set to its containing dir. +# This file is execfile()d with the current directory set to its +# containing dir. # # Note that not all possible configuration values are present in this # autogenerated file. @@ -11,18 +12,18 @@ # All configuration values have a default; values that are commented out # serve to show the default. -import sys, os - # If extensions (or modules to document with autodoc) are in another directory, # add these directories to sys.path here. If the directory is relative to the # documentation root, use os.path.abspath to make it absolute, like shown here. # sys.path.append(os.path.abspath('./src/tf2_kdl')) -# -- General configuration ----------------------------------------------------- +# -- General configuration ---------------------------------------------------- -# Add any Sphinx extension module names here, as strings. They can be extensions +# Add any Sphinx extension module names here, as strings. +# They can be extensions # coming with Sphinx (named 'sphinx.ext.*') or your custom ones. -extensions = ['sphinx.ext.autodoc', 'sphinx.ext.doctest', 'sphinx.ext.intersphinx', 'sphinx.ext.imgmath'] +extensions = ['sphinx.ext.autodoc', 'sphinx.ext.doctest', + 'sphinx.ext.intersphinx', 'sphinx.ext.imgmath'] # Add any paths that contain templates here, relative to this directory. templates_path = ['_templates'] @@ -31,7 +32,7 @@ source_suffix = '.rst' # The encoding of source files. -#source_encoding = 'utf-8' +# source_encoding = 'utf-8' # The master toctree document. master_doc = 'index' @@ -51,16 +52,16 @@ # The language for content autogenerated by Sphinx. Refer to documentation # for a list of supported languages. -#language = None +# language = None # There are two options for replacing |today|: either, you set today to some # non-false value, then it is used: -#today = '' +# today = '' # Else, today_fmt is used as the format for a strftime call. -#today_fmt = '%B %d, %Y' +# today_fmt = '%B %d, %Y' # List of documents that shouldn't be included in the build. -#unused_docs = [] +# unused_docs = [] # List of directories, relative to source directory, that shouldn't be searched # for source files. @@ -68,28 +69,29 @@ exclude_patterns = ['_CHANGELOG.rst'] -# The reST default role (used for this markup: `text`) to use for all documents. -#default_role = None +# The reST default role (used for this markup: `text`) to use +# for all documents. +# default_role = None # If true, '()' will be appended to :func: etc. cross-reference text. -#add_function_parentheses = True +# add_function_parentheses = True # If true, the current module name will be prepended to all description # unit titles (such as .. function::). -#add_module_names = True +# add_module_names = True # If true, sectionauthor and moduleauthor directives will be shown in the # output. They are ignored by default. -#show_authors = False +# show_authors = False # The name of the Pygments (syntax highlighting) style to use. pygments_style = 'sphinx' # A list of ignored prefixes for module index sorting. -#modindex_common_prefix = [] +# modindex_common_prefix = [] -# -- Options for HTML output --------------------------------------------------- +# -- Options for HTML output ------------------------------------------------ # The theme to use for HTML and HTML Help pages. Major themes that come with # Sphinx are currently 'default' and 'sphinxdoc'. @@ -98,81 +100,82 @@ # Theme options are theme-specific and customize the look and feel of a theme # further. For a list of options available for each theme, see the # documentation. -#html_theme_options = {} +# html_theme_options = {} # Add any paths that contain custom themes here, relative to this directory. -#html_theme_path = [] +# html_theme_path = [] # The name for this set of Sphinx documents. If None, it defaults to # " v documentation". -#html_title = None +# html_title = None # A shorter title for the navigation bar. Default is the same as html_title. -#html_short_title = None +# html_short_title = None # The name of an image file (relative to this directory) to place at the top # of the sidebar. -#html_logo = None +# html_logo = None # The name of an image file (within the static path) to use as favicon of the # docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32 # pixels large. -#html_favicon = None +# html_favicon = None # Add any paths that contain custom static files (such as style sheets) here, # relative to this directory. They are copied after the builtin static files, # so a file named "default.css" will overwrite the builtin "default.css". -#html_static_path = ['_static'] +# html_static_path = ['_static'] # If not '', a 'Last updated on:' timestamp is inserted at every page bottom, # using the given strftime format. -#html_last_updated_fmt = '%b %d, %Y' +# html_last_updated_fmt = '%b %d, %Y' # If true, SmartyPants will be used to convert quotes and dashes to # typographically correct entities. -#html_use_smartypants = True +# html_use_smartypants = True # Custom sidebar templates, maps document names to template names. -#html_sidebars = {} +# html_sidebars = {} # Additional templates that should be rendered to pages, maps page names to # template names. -#html_additional_pages = {} +# html_additional_pages = {} # If false, no module index is generated. -#html_use_modindex = True +# html_use_modindex = True # If false, no index is generated. -#html_use_index = True +# html_use_index = True # If true, the index is split into individual pages for each letter. -#html_split_index = False +# html_split_index = False # If true, links to the reST sources are added to the pages. -#html_show_sourcelink = True +# html_show_sourcelink = True # If true, an OpenSearch description file will be output, and all pages will # contain a tag referring to it. The value of this option must be the # base URL from which the finished HTML is served. -#html_use_opensearch = '' +# html_use_opensearch = '' # If nonempty, this is the file name suffix for HTML files (e.g. ".xhtml"). -#html_file_suffix = '' +# html_file_suffix = '' # Output file base name for HTML help builder. htmlhelp_basename = 'tfdoc' -# -- Options for LaTeX output -------------------------------------------------- +# -- Options for LaTeX output ------------------------------------------------- # The paper size ('letter' or 'a4'). -#latex_paper_size = 'letter' +# latex_paper_size = 'letter' # The font size ('10pt', '11pt' or '12pt'). -#latex_font_size = '10pt' +# latex_font_size = '10pt' # Grouping the document tree into LaTeX files. List of tuples -# (source start file, target name, title, author, documentclass [howto/manual]). +# (source start file, target name, title, author, +# documentclass [howto/manual]). latex_documents = [ ('index', 'tf.tex', u'stereo\\_utils Documentation', u'Tully Foote and Eitan Marder-Eppstein', 'manual'), @@ -180,25 +183,25 @@ # The name of an image file (relative to this directory) to place at the top of # the title page. -#latex_logo = None +# latex_logo = None # For "manual" documents, if this is true, then toplevel headings are parts, # not chapters. -#latex_use_parts = False +# latex_use_parts = False # Additional stuff for the LaTeX preamble. -#latex_preamble = '' +# latex_preamble = '' # Documents to append as an appendix to all manuals. -#latex_appendices = [] +# latex_appendices = [] # If false, no module index is generated. -#latex_use_modindex = True +# latex_use_modindex = True # Example configuration for intersphinx: refer to the Python standard library. intersphinx_mapping = { 'http://docs.python.org/': None, 'http://docs.opencv.org/3.0-last-rst/': None, - 'http://docs.scipy.org/doc/numpy' : None + 'http://docs.scipy.org/doc/numpy': None } diff --git a/tf2_kdl/scripts/test.py b/tf2_kdl/scripts/test.py index 8ecc490cb..e0318ea5a 100755 --- a/tf2_kdl/scripts/test.py +++ b/tf2_kdl/scripts/test.py @@ -4,11 +4,11 @@ import rclpy import PyKDL import tf2_ros -import tf2_kdl +import tf2_kdl # noqa(F401) from geometry_msgs.msg import TransformStamped, Quaternion -from copy import deepcopy import builtin_interfaces + class KDLConversions(unittest.TestCase): def test_transform(self): b = tf2_ros.Buffer() @@ -19,27 +19,32 @@ def test_transform(self): t.header.frame_id = 'a' t.child_frame_id = 'b' b.set_transform(t, 'eitan_rocks') - out = b.lookup_transform('a','b', rclpy.time.Time(seconds=2), rclpy.time.Duration(seconds=2)) + out = b.lookup_transform('a', 'b', + rclpy.time.Time(seconds=2), + rclpy.time.Duration(seconds=2)) self.assertEqual(out.transform.translation.x, 1) self.assertEqual(out.transform.rotation.x, 1) self.assertEqual(out.header.frame_id, 'a') self.assertEqual(out.child_frame_id, 'b') - v = PyKDL.Vector(1,2,3) - out = b.transform(tf2_ros.Stamped(v, builtin_interfaces.msg.Time(sec=2), 'a'), 'b') + v = PyKDL.Vector(1, 2, 3) + out = b.transform( + tf2_ros.Stamped(v, builtin_interfaces.msg.Time(sec=2), 'a'), 'b') self.assertEqual(out.x(), 0) self.assertEqual(out.y(), -2) self.assertEqual(out.z(), -3) - f = PyKDL.Frame(PyKDL.Rotation.RPY(1,2,3), PyKDL.Vector(1,2,3)) - out = b.transform(tf2_ros.Stamped(f, builtin_interfaces.msg.Time(sec=2), 'a'), 'b') + f = PyKDL.Frame(PyKDL.Rotation.RPY(1, 2, 3), PyKDL.Vector(1, 2, 3)) + out = b.transform( + tf2_ros.Stamped(f, builtin_interfaces.msg.Time(sec=2), 'a'), 'b') self.assertEqual(out.p.x(), 0) self.assertEqual(out.p.y(), -2) self.assertEqual(out.p.z(), -3) # TODO(tfoote) check values of rotation - t = PyKDL.Twist(PyKDL.Vector(1,2,3), PyKDL.Vector(4,5,6)) - out = b.transform(tf2_ros.Stamped(t, builtin_interfaces.msg.Time(sec=2), 'a'), 'b') + t = PyKDL.Twist(PyKDL.Vector(1, 2, 3), PyKDL.Vector(4, 5, 6)) + out = b.transform( + tf2_ros.Stamped(t, builtin_interfaces.msg.Time(sec=2), 'a'), 'b') self.assertEqual(out.vel.x(), 1) self.assertEqual(out.vel.y(), -8) self.assertEqual(out.vel.z(), 2) @@ -47,8 +52,9 @@ def test_transform(self): self.assertEqual(out.rot.y(), -5) self.assertEqual(out.rot.z(), -6) - w = PyKDL.Wrench(PyKDL.Vector(1,2,3), PyKDL.Vector(4,5,6)) - out = b.transform(tf2_ros.Stamped(w, builtin_interfaces.msg.Time(sec=2), 'a'), 'b') + w = PyKDL.Wrench(PyKDL.Vector(1, 2, 3), PyKDL.Vector(4, 5, 6)) + out = b.transform( + tf2_ros.Stamped(w, builtin_interfaces.msg.Time(sec=2), 'a'), 'b') self.assertEqual(out.force.x(), 1) self.assertEqual(out.force.y(), -2) self.assertEqual(out.force.z(), -3) @@ -57,7 +63,7 @@ def test_transform(self): self.assertEqual(out.torque.z(), -4) def test_convert(self): - v = PyKDL.Vector(1,2,3) + v = PyKDL.Vector(1, 2, 3) vs = tf2_ros.Stamped(v, builtin_interfaces.msg.Time(sec=2), 'a') vs2 = tf2_ros.convert(vs, PyKDL.Vector) self.assertEqual(vs.x(), 1) diff --git a/tf2_kdl/src/tf2_kdl/__init__.py b/tf2_kdl/src/tf2_kdl/__init__.py index 97eef01b2..49e4cd5a8 100644 --- a/tf2_kdl/src/tf2_kdl/__init__.py +++ b/tf2_kdl/src/tf2_kdl/__init__.py @@ -1 +1 @@ -from .tf2_kdl import * +from .tf2_kdl import * # noqa diff --git a/tf2_kdl/src/tf2_kdl/tf2_kdl.py b/tf2_kdl/src/tf2_kdl/tf2_kdl.py index 477e2b916..d281bdd4f 100644 --- a/tf2_kdl/src/tf2_kdl/tf2_kdl.py +++ b/tf2_kdl/src/tf2_kdl/tf2_kdl.py @@ -1,9 +1,9 @@ # Copyright (c) 2008, Willow Garage, Inc. # 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 @@ -12,7 +12,7 @@ # * 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 @@ -28,10 +28,10 @@ # author: Wim Meeussen import PyKDL -import rclpy import tf2_ros from geometry_msgs.msg import PointStamped + def transform_to_kdl(t): """Convert a geometry_msgs Transform message to a PyKDL Frame. @@ -41,8 +41,10 @@ def transform_to_kdl(t): :rtype: PyKDL.Frame """ - return PyKDL.Frame(PyKDL.Rotation.Quaternion(t.transform.rotation.x, t.transform.rotation.y, - t.transform.rotation.z, t.transform.rotation.w), + return PyKDL.Frame(PyKDL.Rotation.Quaternion(t.transform.rotation.x, + t.transform.rotation.y, + t.transform.rotation.z, + t.transform.rotation.w), PyKDL.Vector(t.transform.translation.x, t.transform.translation.y, t.transform.translation.z)) @@ -62,8 +64,10 @@ def do_transform_vector(vector, transform): res.header = transform.header return res + tf2_ros.TransformRegistration().add(PyKDL.Vector, do_transform_vector) + def to_msg_vector(vector): """Convert a PyKDL Vector to a geometry_msgs PointStamped message. @@ -79,8 +83,10 @@ def to_msg_vector(vector): msg.point.z = vector[2] return msg + tf2_ros.ConvertRegistration().add_to_msg(PyKDL.Vector, to_msg_vector) + def from_msg_vector(msg): """Convert a PointStamped message to a stamped PyKDL Vector. @@ -92,8 +98,10 @@ def from_msg_vector(msg): vector = PyKDL.Vector(msg.point.x, msg.point.y, msg.point.z) return tf2_ros.Stamped(vector, msg.header.stamp, msg.header.frame_id) + tf2_ros.ConvertRegistration().add_from_msg(PyKDL.Vector, from_msg_vector) + def convert_vector(vector): """Convert a generic stamped triplet message to a stamped PyKDL Vector. @@ -101,9 +109,14 @@ def convert_vector(vector): :return: The timestamped converted PyKDL vector. :rtype: PyKDL.Vector """ - return tf2_ros.Stamped(PyKDL.Vector(vector), vector.header.stamp, vector.header.frame_id) + return tf2_ros.Stamped(PyKDL.Vector(vector), + vector.header.stamp, + vector.header.frame_id) + + +tf2_ros.ConvertRegistration().add_convert((PyKDL.Vector, PyKDL.Vector), + convert_vector) -tf2_ros.ConvertRegistration().add_convert((PyKDL.Vector, PyKDL.Vector), convert_vector) def do_transform_frame(frame, transform): """Apply a transform in the form of a geometry_msgs message to a PyKDL Frame. @@ -118,8 +131,11 @@ def do_transform_frame(frame, transform): res = transform_to_kdl(transform) * frame res.header = transform.header return res + + tf2_ros.TransformRegistration().add(PyKDL.Frame, do_transform_frame) + def do_transform_twist(twist, transform): """Apply a transform in the form of a geometry_msgs message to a PyKDL Twist. @@ -133,6 +149,8 @@ def do_transform_twist(twist, transform): res = transform_to_kdl(transform) * twist res.header = transform.header return res + + tf2_ros.TransformRegistration().add(PyKDL.Twist, do_transform_twist) @@ -150,4 +168,6 @@ def do_transform_wrench(wrench, transform): res = transform_to_kdl(transform) * wrench res.header = transform.header return res + + tf2_ros.TransformRegistration().add(PyKDL.Wrench, do_transform_wrench) From 59c66bdb01a832dd796bda0418cc3632e07d98ab Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Mon, 12 Apr 2021 17:26:28 +0000 Subject: [PATCH 48/51] [tf2_kdl] fix pep257 --- tf2_kdl/src/tf2_kdl/tf2_kdl.py | 25 ++++++++++++++++--------- 1 file changed, 16 insertions(+), 9 deletions(-) diff --git a/tf2_kdl/src/tf2_kdl/tf2_kdl.py b/tf2_kdl/src/tf2_kdl/tf2_kdl.py index d281bdd4f..4b7d5b782 100644 --- a/tf2_kdl/src/tf2_kdl/tf2_kdl.py +++ b/tf2_kdl/src/tf2_kdl/tf2_kdl.py @@ -33,14 +33,14 @@ def transform_to_kdl(t): - """Convert a geometry_msgs Transform message to a PyKDL Frame. + """ + Convert a geometry_msgs Transform message to a PyKDL Frame. :param t: The Transform message to convert. :type t: geometry_msgs.msg.TransformStamped :return: The converted PyKDL frame. :rtype: PyKDL.Frame """ - return PyKDL.Frame(PyKDL.Rotation.Quaternion(t.transform.rotation.x, t.transform.rotation.y, t.transform.rotation.z, @@ -51,7 +51,8 @@ def transform_to_kdl(t): def do_transform_vector(vector, transform): - """Apply a transform in the form of a geometry_msgs message to a PyKDL vector. + """ + Apply a transform in the form of a geometry_msgs message to a PyKDL vector. :param vector: The PyKDL vector to transform. :type vector: PyKDL.Vector @@ -69,7 +70,8 @@ def do_transform_vector(vector, transform): def to_msg_vector(vector): - """Convert a PyKDL Vector to a geometry_msgs PointStamped message. + """ + Convert a PyKDL Vector to a geometry_msgs PointStamped message. :param vector: The vector to convert. :type vector: PyKDL.Vector @@ -88,7 +90,8 @@ def to_msg_vector(vector): def from_msg_vector(msg): - """Convert a PointStamped message to a stamped PyKDL Vector. + """ + Convert a PointStamped message to a stamped PyKDL Vector. :param msg: The PointStamped message to convert. :type msg: geometry_msgs.msg.PointStamped @@ -103,7 +106,8 @@ def from_msg_vector(msg): def convert_vector(vector): - """Convert a generic stamped triplet message to a stamped PyKDL Vector. + """ + Convert a generic stamped triplet message to a stamped PyKDL Vector. :param vector: The message to convert. :return: The timestamped converted PyKDL vector. @@ -119,7 +123,8 @@ def convert_vector(vector): def do_transform_frame(frame, transform): - """Apply a transform in the form of a geometry_msgs message to a PyKDL Frame. + """ + Apply a transform in the form of a geometry_msgs message to a PyKDL Frame. :param frame: The PyKDL frame to transform. :type frame: PyKDL.Frame @@ -137,7 +142,8 @@ def do_transform_frame(frame, transform): def do_transform_twist(twist, transform): - """Apply a transform in the form of a geometry_msgs message to a PyKDL Twist. + """ + Apply a transform in the form of a geometry_msgs message to a PyKDL Twist. :param twist: The PyKDL twist to transform. :type twist: PyKDL.Twist @@ -156,7 +162,8 @@ def do_transform_twist(twist, transform): # Wrench def do_transform_wrench(wrench, transform): - """Apply a transform in the form of a geometry_msgs message to a PyKDL Wrench. + """ + Apply a transform in the form of a geometry_msgs message to a PyKDL Wrench. :param wrench: The PyKDL wrench to transform. :type wrench: PyKDL.Wrench From ef72679deae27f992c1d7570af44c96e0b754b70 Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Mon, 12 Apr 2021 19:00:56 +0000 Subject: [PATCH 49/51] [tf2_geometry_msgs] fix copyright --- .../tf2_geometry_msgs/tf2_geometry_msgs.h | 56 +++++++++---------- tf2_geometry_msgs/scripts/test.py | 29 ++++++++++ .../src/tf2_geometry_msgs/__init__.py | 29 ++++++++++ .../tf2_geometry_msgs/tf2_geometry_msgs.py | 24 ++++---- .../test/test_tf2_geometry_msgs.cpp | 56 +++++++++---------- 5 files changed, 127 insertions(+), 67 deletions(-) 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 2bce6d554..5462f8e18 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 @@ -1,31 +1,31 @@ -/* - * Copyright (c) 2008, Willow Garage, Inc. - * 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. - */ +// Copyright 2008 Willow Garage, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + /** \author Wim Meeussen, Bjarne von Horn */ diff --git a/tf2_geometry_msgs/scripts/test.py b/tf2_geometry_msgs/scripts/test.py index 56adf69cb..c646893d6 100755 --- a/tf2_geometry_msgs/scripts/test.py +++ b/tf2_geometry_msgs/scripts/test.py @@ -1,5 +1,34 @@ #!/usr/bin/env python3 +# Copyright 2008 Willow Garage, Inc. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + + import unittest import rclpy import tf2_ros diff --git a/tf2_geometry_msgs/src/tf2_geometry_msgs/__init__.py b/tf2_geometry_msgs/src/tf2_geometry_msgs/__init__.py index e895a1fc2..078526158 100644 --- a/tf2_geometry_msgs/src/tf2_geometry_msgs/__init__.py +++ b/tf2_geometry_msgs/src/tf2_geometry_msgs/__init__.py @@ -1 +1,30 @@ +# Copyright 2008 Willow Garage, Inc. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + + from .tf2_geometry_msgs import * # noqa diff --git a/tf2_geometry_msgs/src/tf2_geometry_msgs/tf2_geometry_msgs.py b/tf2_geometry_msgs/src/tf2_geometry_msgs/tf2_geometry_msgs.py index 070392dd1..8b78f3b3c 100644 --- a/tf2_geometry_msgs/src/tf2_geometry_msgs/tf2_geometry_msgs.py +++ b/tf2_geometry_msgs/src/tf2_geometry_msgs/tf2_geometry_msgs.py @@ -1,22 +1,23 @@ -# Copyright (c) 2008, Willow Garage, Inc. -# All rights reserved. +# Copyright 2008 Willow Garage, Inc. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in the -# documentation and/or other materials provided with the distribution. -# * Neither the name of the Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived from -# this software without specific prior written permission. +# * 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 +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS @@ -25,6 +26,7 @@ # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. + # author: Wim Meeussen from geometry_msgs.msg import (PoseStamped, Vector3Stamped, PointStamped, diff --git a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp index d009e9ebd..e8abb5bfd 100644 --- a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp +++ b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp @@ -1,31 +1,31 @@ -/* - * Copyright (c) 2008, Willow Garage, Inc. - * 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. - */ +// Copyright 2008 Willow Garage, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + /** \author Wim Meeussen */ From 3e5fa250a38536ad9621f4b85fd3e47e3178fa95 Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Mon, 12 Apr 2021 19:12:09 +0000 Subject: [PATCH 50/51] [tf2_kdl] fix copyright --- tf2_kdl/docs/source/conf.py | 30 +++++++++++++++++ tf2_kdl/include/tf2_kdl/tf2_kdl.h | 56 +++++++++++++++---------------- tf2_kdl/scripts/test.py | 29 ++++++++++++++++ tf2_kdl/src/tf2_kdl/__init__.py | 29 ++++++++++++++++ tf2_kdl/src/tf2_kdl/tf2_kdl.py | 24 +++++++------ tf2_kdl/test/test_tf2_kdl.cpp | 56 +++++++++++++++---------------- 6 files changed, 157 insertions(+), 67 deletions(-) diff --git a/tf2_kdl/docs/source/conf.py b/tf2_kdl/docs/source/conf.py index a84d5729b..a3298e5e1 100644 --- a/tf2_kdl/docs/source/conf.py +++ b/tf2_kdl/docs/source/conf.py @@ -1,4 +1,34 @@ # -*- coding: utf-8 -*- + +# Copyright 2020 Open Source Robotics Foundation, Inc. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the Open Source Robotics Foundation, Inc. nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + + # # tf2 documentation build configuration file, created by # sphinx-quickstart on Mon Jun 1 14:21:53 2009. diff --git a/tf2_kdl/include/tf2_kdl/tf2_kdl.h b/tf2_kdl/include/tf2_kdl/tf2_kdl.h index d9de560cb..c9835ef84 100644 --- a/tf2_kdl/include/tf2_kdl/tf2_kdl.h +++ b/tf2_kdl/include/tf2_kdl/tf2_kdl.h @@ -1,31 +1,31 @@ -/* - * Copyright (c) 2008, Willow Garage, Inc. - * 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. - */ +// Copyright 2008 Willow Garage, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + /** \author Wim Meeussen, Bjarne von Horn */ diff --git a/tf2_kdl/scripts/test.py b/tf2_kdl/scripts/test.py index e0318ea5a..209e13184 100755 --- a/tf2_kdl/scripts/test.py +++ b/tf2_kdl/scripts/test.py @@ -1,5 +1,34 @@ #!/usr/bin/python +# Copyright 2008 Willow Garage, Inc. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + + import unittest import rclpy import PyKDL diff --git a/tf2_kdl/src/tf2_kdl/__init__.py b/tf2_kdl/src/tf2_kdl/__init__.py index 49e4cd5a8..e6b5dc616 100644 --- a/tf2_kdl/src/tf2_kdl/__init__.py +++ b/tf2_kdl/src/tf2_kdl/__init__.py @@ -1 +1,30 @@ +# Copyright 2008 Willow Garage, Inc. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + + from .tf2_kdl import * # noqa diff --git a/tf2_kdl/src/tf2_kdl/tf2_kdl.py b/tf2_kdl/src/tf2_kdl/tf2_kdl.py index 4b7d5b782..0471f865a 100644 --- a/tf2_kdl/src/tf2_kdl/tf2_kdl.py +++ b/tf2_kdl/src/tf2_kdl/tf2_kdl.py @@ -1,22 +1,23 @@ -# Copyright (c) 2008, Willow Garage, Inc. -# All rights reserved. +# Copyright 2008 Willow Garage, Inc. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in the -# documentation and/or other materials provided with the distribution. -# * Neither the name of the Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived from -# this software without specific prior written permission. +# * 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 +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS @@ -25,6 +26,7 @@ # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. + # author: Wim Meeussen import PyKDL diff --git a/tf2_kdl/test/test_tf2_kdl.cpp b/tf2_kdl/test/test_tf2_kdl.cpp index 78fbf25d8..d030ed949 100644 --- a/tf2_kdl/test/test_tf2_kdl.cpp +++ b/tf2_kdl/test/test_tf2_kdl.cpp @@ -1,31 +1,31 @@ -/* - * Copyright (c) 2008, Willow Garage, Inc. - * 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. - */ +// Copyright 2008 Willow Garage, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + /** \author Wim Meeussen */ From cc6d47a95a3c0f55447f7aed0d090a2c639d7b4a Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Thu, 13 May 2021 20:46:24 +0000 Subject: [PATCH 51/51] move headers to .cpp --- test_tf2/test/test_buffer_client.cpp | 4 ++-- test_tf2/test/test_convert.cpp | 6 +++--- test_tf2/test/test_tf2_bullet.cpp | 2 +- test_tf2/test/test_utils.cpp | 2 +- .../include/tf2_bullet/{tf2_bullet.h => tf2_bullet.hpp} | 6 +++--- tf2_bullet/mainpage.dox | 2 +- tf2_bullet/test/test_tf2_bullet.cpp | 2 +- tf2_eigen/include/tf2_eigen/{tf2_eigen.h => tf2_eigen.hpp} | 6 +++--- tf2_eigen/mainpage.dox | 2 +- tf2_eigen/test/tf2_eigen-test.cpp | 2 +- tf2_kdl/include/tf2_kdl/{tf2_kdl.h => tf2_kdl.hpp} | 6 +++--- tf2_kdl/test/test_tf2_kdl.cpp | 2 +- .../{tf2_sensor_msgs.h => tf2_sensor_msgs.hpp} | 6 +++--- tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp | 2 +- 14 files changed, 25 insertions(+), 25 deletions(-) rename tf2_bullet/include/tf2_bullet/{tf2_bullet.h => tf2_bullet.hpp} (98%) rename tf2_eigen/include/tf2_eigen/{tf2_eigen.h => tf2_eigen.hpp} (99%) rename tf2_kdl/include/tf2_kdl/{tf2_kdl.h => tf2_kdl.hpp} (99%) rename tf2_sensor_msgs/include/tf2_sensor_msgs/{tf2_sensor_msgs.h => tf2_sensor_msgs.hpp} (97%) diff --git a/test_tf2/test/test_buffer_client.cpp b/test_tf2/test/test_buffer_client.cpp index 1da762bcc..642545a03 100644 --- a/test_tf2/test/test_buffer_client.cpp +++ b/test_tf2/test/test_buffer_client.cpp @@ -37,9 +37,9 @@ #include #include -#include +#include #include -#include +#include #include #include diff --git a/test_tf2/test/test_convert.cpp b/test_tf2/test/test_convert.cpp index bd228ab66..2bb27766e 100644 --- a/test_tf2/test/test_convert.cpp +++ b/test_tf2/test/test_convert.cpp @@ -36,9 +36,9 @@ *********************************************************************/ #include #include -#include -#include -#include +#include +#include +#include #include #include diff --git a/test_tf2/test/test_tf2_bullet.cpp b/test_tf2/test/test_tf2_bullet.cpp index 81016a081..1b71d13ba 100644 --- a/test_tf2/test/test_tf2_bullet.cpp +++ b/test_tf2/test/test_tf2_bullet.cpp @@ -29,7 +29,7 @@ /** \author Wim Meeussen */ -#include +#include #include #include #include diff --git a/test_tf2/test/test_utils.cpp b/test_tf2/test/test_utils.cpp index aebeb1f47..d52ecbc8b 100644 --- a/test_tf2/test/test_utils.cpp +++ b/test_tf2/test/test_utils.cpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include double epsilon = 1e-9; diff --git a/tf2_bullet/include/tf2_bullet/tf2_bullet.h b/tf2_bullet/include/tf2_bullet/tf2_bullet.hpp similarity index 98% rename from tf2_bullet/include/tf2_bullet/tf2_bullet.h rename to tf2_bullet/include/tf2_bullet/tf2_bullet.hpp index c0489d8f6..a30cb6eb9 100644 --- a/tf2_bullet/include/tf2_bullet/tf2_bullet.h +++ b/tf2_bullet/include/tf2_bullet/tf2_bullet.hpp @@ -28,8 +28,8 @@ /** \author Wim Meeussen, Bjarne von Horn */ -#ifndef TF2_BULLET__TF2_BULLET_H_ -#define TF2_BULLET__TF2_BULLET_H_ +#ifndef TF2_BULLET__TF2_BULLET_HPP_ +#define TF2_BULLET__TF2_BULLET_HPP_ #include #include @@ -162,4 +162,4 @@ inline btTransform transformToBullet( } } // namespace tf2 -#endif // TF2_BULLET__TF2_BULLET_H_ +#endif // TF2_BULLET__TF2_BULLET_HPP_ diff --git a/tf2_bullet/mainpage.dox b/tf2_bullet/mainpage.dox index a2ec58cb5..6a0a2a9e4 100644 --- a/tf2_bullet/mainpage.dox +++ b/tf2_bullet/mainpage.dox @@ -13,7 +13,7 @@ wiki page for more information about datatype conversion in tf2. \section codeapi Code API -This library consists of one header only, tf2_bullet/tf2_bullet.h, which consists mostly of +This library consists of one header only, tf2_bullet/tf2_bullet.hpp, which consists mostly of specializations of template functions defined in tf2/convert.h. */ diff --git a/tf2_bullet/test/test_tf2_bullet.cpp b/tf2_bullet/test/test_tf2_bullet.cpp index 7ea274a8c..a6dd849f4 100644 --- a/tf2_bullet/test/test_tf2_bullet.cpp +++ b/tf2_bullet/test/test_tf2_bullet.cpp @@ -29,7 +29,7 @@ /** \author Wim Meeussen */ -#include +#include #include #include #include diff --git a/tf2_eigen/include/tf2_eigen/tf2_eigen.h b/tf2_eigen/include/tf2_eigen/tf2_eigen.hpp similarity index 99% rename from tf2_eigen/include/tf2_eigen/tf2_eigen.h rename to tf2_eigen/include/tf2_eigen/tf2_eigen.hpp index 6df43346b..df35ab5b6 100644 --- a/tf2_eigen/include/tf2_eigen/tf2_eigen.h +++ b/tf2_eigen/include/tf2_eigen/tf2_eigen.hpp @@ -26,8 +26,8 @@ /** \author Koji Terada, Bjarne von Horn */ -#ifndef TF2_EIGEN__TF2_EIGEN_H_ -#define TF2_EIGEN__TF2_EIGEN_H_ +#ifndef TF2_EIGEN__TF2_EIGEN_HPP_ +#define TF2_EIGEN__TF2_EIGEN_HPP_ #include #include @@ -423,4 +423,4 @@ void doTransform( } } // namespace tf2 -#endif // TF2_EIGEN__TF2_EIGEN_H_ +#endif // TF2_EIGEN__TF2_EIGEN_HPP_ diff --git a/tf2_eigen/mainpage.dox b/tf2_eigen/mainpage.dox index d12917516..1d6a02d90 100644 --- a/tf2_eigen/mainpage.dox +++ b/tf2_eigen/mainpage.dox @@ -13,7 +13,7 @@ wiki page for more information about datatype conversion in tf2. \section codeapi Code API -This library consists of one header only, tf2_eigen/tf2_eigen.h, which consists mostly of +This library consists of one header only, tf2_eigen/tf2_eigen.hpp, which consists mostly of specializations of template functions defined in tf2/convert.h. */ diff --git a/tf2_eigen/test/tf2_eigen-test.cpp b/tf2_eigen/test/tf2_eigen-test.cpp index 5164fcb7d..fb1b59a1a 100644 --- a/tf2_eigen/test/tf2_eigen-test.cpp +++ b/tf2_eigen/test/tf2_eigen-test.cpp @@ -36,7 +36,7 @@ #include #include #include -#include +#include #include #include diff --git a/tf2_kdl/include/tf2_kdl/tf2_kdl.h b/tf2_kdl/include/tf2_kdl/tf2_kdl.hpp similarity index 99% rename from tf2_kdl/include/tf2_kdl/tf2_kdl.h rename to tf2_kdl/include/tf2_kdl/tf2_kdl.hpp index c9835ef84..749d6a27d 100644 --- a/tf2_kdl/include/tf2_kdl/tf2_kdl.h +++ b/tf2_kdl/include/tf2_kdl/tf2_kdl.hpp @@ -29,8 +29,8 @@ /** \author Wim Meeussen, Bjarne von Horn */ -#ifndef TF2_KDL__TF2_KDL_H_ -#define TF2_KDL__TF2_KDL_H_ +#ifndef TF2_KDL__TF2_KDL_HPP_ +#define TF2_KDL__TF2_KDL_HPP_ #include #include @@ -360,4 +360,4 @@ void doTransform( } // namespace tf2 -#endif // TF2_KDL__TF2_KDL_H_ +#endif // TF2_KDL__TF2_KDL_HPP_ diff --git a/tf2_kdl/test/test_tf2_kdl.cpp b/tf2_kdl/test/test_tf2_kdl.cpp index d030ed949..3a42360cf 100644 --- a/tf2_kdl/test/test_tf2_kdl.cpp +++ b/tf2_kdl/test/test_tf2_kdl.cpp @@ -39,7 +39,7 @@ #include #include -#include +#include #include #include 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.hpp similarity index 97% rename from tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.h rename to tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.hpp index 5224899a3..c171116e5 100644 --- a/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.h +++ b/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.hpp @@ -27,8 +27,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef TF2_SENSOR_MSGS_H -#define TF2_SENSOR_MSGS_H +#ifndef TF2_SENSOR_MSGS_HPP +#define TF2_SENSOR_MSGS_HPP #include #include @@ -85,4 +85,4 @@ void doTransform(const sensor_msgs::msg::PointCloud2 &p_in, sensor_msgs::msg::Po } // namespace -#endif // TF2_SENSOR_MSGS_H +#endif // TF2_SENSOR_MSGS_HPP diff --git a/tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp b/tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp index cc87da4cf..4706c5701 100644 --- a/tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp +++ b/tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp @@ -30,7 +30,7 @@ #include #include #include -#include +#include #include #include