-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)
+std::array, 6> getCovarianceMatrix(const T & t)
{
- return t.frame_id_;
+ using traits = impl::StampedMessageTraits;
+ return covarianceRowMajorToNested(traits::getCovariance(t));
}
/**\brief Get the covariance matrix from data
@@ -119,53 +130,86 @@ 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.
- * \param a an object of whatever type
+/**
+ * \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::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::DefaultMessageForDatatype\::type.
* \return the conversion as a ROS message
*/
template
-B toMsg(const A & a);
+inline B toMsg(const A & a)
+{
+ B b;
+ impl::ConversionImplementation::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.
- * \param a a ROS message to convert from
- * \param b the object to convert to
+/**
+ * \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::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
-void fromMsg(const A &, B & b);
+inline B & toMsg(const A & a, B & b)
+{
+ impl::ConversionImplementation::toMsg(a, b);
+ return 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::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
+inline void fromMsg(const A & a, B & b)
+{
+ impl::ConversionImplementation::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
+ * \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
-void convert(const A & a, B & b)
-{
- impl::Converter::value,
- rosidl_generator_traits::is_message::value>::convert(a, b);
-}
-
-template
-void convert(const A & a1, A & a2)
+inline void convert(const A & a, B & b)
{
- if (&a1 != &a2) {
- a2 = a1;
- }
+ impl::Converter<
+ rosidl_generator_traits::is_message::value,
+ rosidl_generator_traits::is_message::value, A, B>::convert(a, b);
}
-/**\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;
@@ -182,13 +226,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 3821564a0..b8d8de493 100644
--- a/tf2/include/tf2/impl/convert.h
+++ b/tf2/include/tf2/impl/convert.h
@@ -26,53 +26,563 @@
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
+/** \author Vincent Rabaud, Bjarne von Horn */
+
#ifndef TF2__IMPL__CONVERT_H_
#define TF2__IMPL__CONVERT_H_
+#include
+#include
+#include
+
+#include "../time.h"
+#include "forward.h"
+
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(Message::header), 0)>
+{
+ 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.
+ *
+ * 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 DefaultMessageForDatatype
+{
+ static_assert(
+ alwaysFalse,
+ "No default message type defined, "
+ "please check your header file includes!");
+
+ // implement the following in the specializations for your own types
+ // using type = ...;
+};
+
+/**
+ * \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
+{
+ static_assert(
+ alwaysFalse,
+ "No default transform type defined, "
+ "please check your header file includes!");
+
+ // implement the following in the specializations for your own types
+ // 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 ) and
+ * ( tf2::WithCovarianceStamped\ and geometry_msgs::...WithCovarianceStamped )
+ * is done automatically.
+ */
+template
+struct ConversionImplementation
+{
+ static_assert(
+ 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&);
+};
+
+/**
+ * \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 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,
+ * 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 UntampedType = 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.
+ *
+ * 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
+{
+ static_assert(
+ 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&);
+ // static CovarianceType & accessCovariance(MsgWithCovarianceStamped &);
+ // static CovarianceType getCovariance(MsgWithCovarianceStamped const&);
+};
+
+/**
+ * \brief Mapping of stamped Messages for unstamped Messages
+ *
+ * This struct is needed for the deduction of the return type of
+ * tf2::convert() for tf2::Stamped\<\> datatypes.
+ * Its specializations should contain an alias (named \c StampedType )
+ * of the stamped type.
+ * Example:
+ * \code
+ * template<>
+ * struct UnstampedMessageTraits
+ * {
+ * using StampedType = geometry_msgs::Vector3Stamped;
+ * };
+ * \endcode
+ *
+ * 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
+{
+ static_assert(
+ 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 = ...;
+};
+
+/**
+ * \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 DefaultMessageForDatatype>
+{
+ using type =
+ typename UnstampedMessageTraits::type>::StampedType;
+};
+
+/**
+ * \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<
+ typename DefaultMessageForDatatype::type>::StampedTypeWithCovariance;
+};
+
+/**
+ * \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.
+ * \tparam Datatype Unstamped non-message type
+ * \tparam StampedMessage Stamped ROS message type
+ */
+template
+struct ConversionImplementation, StampedMessage>
+{
+ /// Typedefs and utility functions for the given message
+ using traits = StampedMessageTraits;
-template
+ /** \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));
+ tf2::toMsg<>(s.stamp_, msg.header.stamp);
+ 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));
+ tf2::fromMsg<>(msg.header.stamp, s.stamp_);
+ s.frame_id_ = msg.header.frame_id;
+ }
+};
+
+/**
+ * \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>
+{
+ /// 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;
+ 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);
+ }
+
+ /** \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)
+ {
+ 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);
+ }
+};
+
+/** \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
class Converter
{
public:
- 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.
+ //
+ static_assert(
+ !(IS_MESSAGE_A && IS_MESSAGE_B), "Conversion between two Message types is not supported!");
};
-// 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< >
+/** \brief Implementation of tf2::convert() for message-to-datatype conversion.
+ * \tparam A Message type.
+ * \tparam B Datatype.
+ */
template
-inline void Converter::convert(const A & a, B & b);
+struct Converter
+{
-template< >
+ /** \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.
+ * \tparam A Datatype.
+ * \tparam B Message.
+ */
template
-inline void Converter::convert(const A & a, B & b)
+struct Converter
{
- 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.
+ */
+ static void convert(const A & a, B & b) {b = toMsg(a);}
+};
-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.
+ * \tparam A Datatype of first argument.
+ * \tparam B Datatype of second argument.
+ */
template
-inline void Converter::convert(const A & a, B & b)
+struct Converter::value>::type>
{
- 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.
+ */
+ static void convert(const A & a, B & b) {fromMsg<>(toMsg<>(a), b);}
+};
-template< >
-template
-inline void Converter::convert(const A & a, B & 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.
+ *
+ * Both static member functions are for stamped ROS messages.
+ *
+ * \tparam T Arbitrary datatype
+ */
+template
+struct StampedAttributesHelper
{
- fromMsg(toMsg(a), b);
-}
+ 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[in] t The data input.
+ * \return The timestamp associated with the data.
+ */
+ 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[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;}
+};
+
+/**
+ * \brief Partial specialization of StampedAttributesHelper for tf2::Stamped\<\> 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::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.
+ */
+ static std::string getFrameId(const tf2::Stamped & t) {return t.frame_id_;}
+};
+
+/**
+ * \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.
+ * \tparam VectorMemberType Type of the x, y and \c z variables of the vector
+ */
+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)
+ {
+ msg.x = in[0];
+ msg.y = in[1];
+ msg.z = in[2];
+ }
+
+ /** \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)
+ {
+ // cast to suppress narrowing warning
+ out = VectorType(
+ static_cast(msg.x), static_cast(msg.y),
+ static_cast(msg.z));
+ }
+};
+
+/** \brief Generic conversion of a quaternion and
+ * 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
+struct DefaultQuaternionConversionImplementation
+{
+ /** \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)
+ {
+ msg.x = in[0];
+ msg.y = in[1];
+ msg.z = in[2];
+ msg.w = in[3];
+ }
+
+ /** \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)
+ {
+ // cast to suppress narrowing warning
+ out = QuaternionType(
+ static_cast(msg.x), static_cast(msg.y),
+ static_cast(msg.z), static_cast(msg.w));
+ }
+};
} // namespace impl
} // namespace tf2
diff --git a/tf2/include/tf2/impl/forward.h b/tf2/include/tf2/impl/forward.h
new file mode 100644
index 000000000..9a3d58bb7
--- /dev/null
+++ b/tf2/include/tf2/impl/forward.h
@@ -0,0 +1,86 @@
+// 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_
+
+#include
+
+namespace tf2
+{
+namespace impl
+{
+template
+struct DefaultMessageForDatatype;
+template
+struct DefaultTransformType;
+} // namespace impl
+
+template
+class Stamped;
+
+template
+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);
+
+std::array, 6> covarianceRowMajorToNested(
+ const std::array & row_major);
+std::array covarianceNestedToRowMajor(
+ const std::array, 6> & nested_array);
+
+namespace impl
+{
+template
+struct ConversionImplementation;
+
+template
+struct StampedMessageTraits;
+
+template
+struct UnstampedMessageTraits;
+
+template
+struct StampedAttributesHelper;
+
+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
new file mode 100644
index 000000000..4aea11108
--- /dev/null
+++ b/tf2/include/tf2/impl/stamped_traits.h
@@ -0,0 +1,395 @@
+/*
+ * 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_
+
+#include "forward.h"
+
+// forward declarations
+namespace geometry_msgs
+{
+namespace msg
+{
+template
+struct Point_;
+template
+struct Vector_;
+template
+struct Quaternion_;
+template
+struct Pose_;
+template
+struct Twist_;
+template
+struct PoseWithCovariance_;
+template
+struct Wrench_;
+template
+struct PointStamped_;
+template
+struct VectorStamped_;
+template
+struct QuaternionStamped_;
+template
+struct PoseStamped_;
+template
+struct TwistStamped_;
+template
+struct PoseWithCovarianceStamped_;
+template
+struct WrenchStamped_;
+template
+struct TransformStamped_;
+template
+struct Transform_;
+template
+struct Vector3_;
+template
+struct Vector3Stamped_;
+template
+struct TwistWithCovarianceStamped_;
+} // namespace msg
+} // namespace geometry_msgs
+
+namespace tf2
+{
+namespace impl
+{
+
+/** \brief Traits for geometry_msgs::msg::Point.
+ * \tparam Alloc Message Allocator
+*/
+template
+struct UnstampedMessageTraits>
+{
+ /// The corresponding stamped message type.
+ using StampedType = geometry_msgs::msg::PointStamped_;
+};
+
+/** \brief Traits for geometry_msgs::msg::Vector.
+ * \tparam Alloc Message Allocator
+ */
+template
+struct UnstampedMessageTraits>
+{
+ /// The corresponding stamped message type.
+ using StampedType = geometry_msgs::msg::VectorStamped_;
+};
+
+/** \brief Traits for geometry_msgs::msg::Quaternion.
+ * \tparam Alloc Message Allocator
+ */
+template
+struct UnstampedMessageTraits>
+{
+ /// The corresponding stamped message type.
+ using StampedType = geometry_msgs::msg::QuaternionStamped_;
+};
+
+/** \brief Traits for geometry_msgs::msg::Pose.
+ * \tparam Alloc Message Allocator
+ */
+template
+struct UnstampedMessageTraits>
+{
+ /// The corresponding stamped message type.
+ using StampedType = geometry_msgs::msg::PoseStamped_;
+ /// The corresponding stamped message type with covariance.
+ using StampedTypeWithCovariance = geometry_msgs::msg::PoseWithCovarianceStamped_;
+};
+
+/** \brief Traits for geometry_msgs::msg::Twist.
+ * \tparam Alloc Message Allocator
+ */
+template
+struct UnstampedMessageTraits>
+{
+ /// The corresponding stamped message type.
+ using StampedType = geometry_msgs::msg::TwistStamped_;
+ /// The corresponding stamped message type with covariance.
+ using StampedTypeWithCovariance = geometry_msgs::msg::TwistWithCovarianceStamped_;
+};
+
+/** \brief Traits for geometry_msgs::msg::PoseWithCovariance.
+ * \tparam Alloc Message Allocator
+ */
+template
+struct UnstampedMessageTraits>
+{
+ /// The corresponding stamped message type with covariance.
+ using StampedType = geometry_msgs::msg::PoseWithCovarianceStamped_;
+};
+
+/** \brief Traits for geometry_msgs::msg::Wrench.
+ * \tparam Alloc Message Allocator
+ */
+template
+struct UnstampedMessageTraits>
+{
+ /// The corresponding stamped message type.
+ using StampedType = geometry_msgs::msg::WrenchStamped_;
+};
+
+/** \brief Traits for geometry_msgs::msg::Transform.
+ * \tparam Alloc Message Allocator
+ */
+template