From de72a4363dccad681abfef72d4e657a1df47173a Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Fri, 29 Jan 2021 13:05:51 +0000 Subject: [PATCH] more meaningful names --- tf2/include/tf2/convert.h | 91 +++-- tf2/include/tf2/impl/convert.h | 306 ++++++++++----- 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, 616 insertions(+), 302 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..de475d5b3 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,8 +375,8 @@ 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. @@ -288,7 +386,7 @@ struct DefaultStampedImpl * 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); @@ -302,39 +400,59 @@ struct DefaultStampedImpl * 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 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 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 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 +461,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 +471,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 +491,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 91e9c234a..25be1b8c3 100644 --- a/tf2_eigen/include/tf2_eigen/tf2_eigen.h +++ b/tf2_eigen/include/tf2_eigen/tf2_eigen.h @@ -122,29 +122,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 { }; @@ -158,7 +158,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; @@ -166,7 +166,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. @@ -192,7 +192,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; @@ -257,21 +257,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; @@ -279,7 +279,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; @@ -287,7 +287,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. @@ -320,7 +320,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; @@ -328,7 +328,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;