diff --git a/test_tf2/test/test_convert.cpp b/test_tf2/test/test_convert.cpp index 432bbfe5d..a70f53fa6 100644 --- a/test_tf2/test/test_convert.cpp +++ b/test_tf2/test/test_convert.cpp @@ -34,16 +34,73 @@ * * Author: Eitan Marder-Eppstein *********************************************************************/ + #include -#include + #include +#include #include +#include + #include #include -#include + #include #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"); +} // namespace + +using Vector6d = Eigen::Matrix; #include @@ -108,8 +165,7 @@ TEST(tf2Convert, ConvertTf2Quaternion) { const tf2::Quaternion tq(1, 2, 3, 4); Eigen::Quaterniond eq; - // TODO(gleichdick): switch to tf2::convert() when it's working - tf2::fromMsg(tf2::toMsg(tq), eq); + tf2::convert(tq, eq); EXPECT_EQ(tq.w(), eq.w()); EXPECT_EQ(tq.x(), eq.x()); @@ -188,6 +244,106 @@ 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, 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) +{ + 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); 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 eadd899eb..ae2576f24 100644 --- a/tf2/include/tf2/convert.h +++ b/tf2/include/tf2/convert.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 Tully Foote */ +/** \author Tully Foote, Bjarne von Horn */ #ifndef TF2__CONVERT_H_ #define TF2__CONVERT_H_ @@ -35,79 +35,99 @@ #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 +#include +#include + + +#include "exceptions.h" +#include "impl/convert.h" +#include "impl/stamped_traits.hpp" +#include "impl/time_convert.hpp" +#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 +/** \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 void doTransform( - const T & data_in, T & data_out, - const geometry_msgs::msg::TransformStamped & transform); - -/**\brief Get the timestamp from data - * \param[in] t The data input. - * \return The timestamp associated with the data. - */ -template -tf2::TimePoint getTimestamp(const T & t); + 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; +} -/**\brief Get the frame_id from data - * \param[in] t The data input. - * \return The frame_id associated with the data. +/** \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 -std::string getFrameId(const T & t); +void doTransform( + 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 covariance matrix from data +/** + * \brief Get the timestamp from data * \param[in] t The data input. - * \return The covariance matrix associated with the data. + * \return The timestamp associated with the data. */ template -std::array, 6> getCovarianceMatrix(const T & t); +inline tf2::TimePoint getTimestamp(const T & t) +{ + return impl::StampedAttributesHelper::getTimestamp(t); +} -/**\brief Get the frame_id from data - * - * An implementation for Stamped

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

& t) +template +inline std::string getFrameId(const T & t) { - return t.stamp_; + return impl::StampedAttributesHelper::getFrameId(t); } -/**\brief Get the frame_id from data - * - * An implementation for Stamped

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

& t) +template +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 +/** + * \brief Get the covariance matrix from data * * An implementation for WithCovarianceStamped

datatypes. * @@ -120,53 +140,87 @@ 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 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 +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 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 -void fromMsg(const A &, B & b); +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; std::array::const_iterator ss = row_major.begin(); @@ -177,13 +231,16 @@ 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..b8cfedfbb 100644 --- a/tf2/include/tf2/impl/convert.h +++ b/tf2/include/tf2/impl/convert.h @@ -26,54 +26,594 @@ // 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.hpp" + 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; + + /** \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); + } +}; -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. + * \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 { - fromMsg(toMsg(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 + */ + 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 +{ + 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.hpp b/tf2/include/tf2/impl/forward.hpp new file mode 100644 index 000000000..1f3f0962d --- /dev/null +++ b/tf2/include/tf2/impl/forward.hpp @@ -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_HPP_ +#define TF2__IMPL__FORWARD_HPP_ + +#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.hpp b/tf2/include/tf2/impl/stamped_traits.hpp new file mode 100644 index 000000000..32a7b7822 --- /dev/null +++ b/tf2/include/tf2/impl/stamped_traits.hpp @@ -0,0 +1,425 @@ +/* + * 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_HPP_ +#define TF2_IMPL_STAMPED_TRAITS_HPP_ + +#include "forward.hpp" + +// 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 +struct UnstampedMessageTraits> +{ + /// The corresponding stamped message type. + using StampedType = geometry_msgs::msg::TransformStamped_; +}; + +/** + * \brief Traits for geometry_msgs::msg::Vector3. + * \tparam Alloc Message Allocator + */ +template +struct UnstampedMessageTraits> +{ + /// The corresponding stamped message type. + using StampedType = geometry_msgs::msg::Vector3Stamped_; +}; + +/** + * \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 +{ + /// 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 + +/** + * \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> +{ +}; + +/** + * \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> +{ +}; + +/** + * \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> +{ +}; + +/** + * \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> +{ +}; + +/** + * \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> +{ +}; + +/** + * \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> +{ +}; + +/** + * \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> +{ +}; + +/** + * \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> +{ +}; + +/** + * \brief Traits for geometry_msgs::msg::PoseWithCovarianceStamped. + * \tparam Alloc Message Allocator + */ +template +struct StampedMessageTraits> +{ + /// 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; + } +}; + +/** + * \brief Traits for geometry_msgs::msg::TwistWithCovarianceStamped. + * \tparam Alloc Message Allocator + */ +template +struct StampedMessageTraits> +{ + /// 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 +} // namespace tf2 + +#endif // TF2_IMPL_STAMPED_TRAITS_HPP_ diff --git a/tf2/include/tf2/impl/time_convert.hpp b/tf2/include/tf2/impl/time_convert.hpp new file mode 100644 index 000000000..48fd0a150 --- /dev/null +++ b/tf2/include/tf2/impl/time_convert.hpp @@ -0,0 +1,150 @@ +// 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_HPP_ +#define TF2__IMPL__TIME_CONVERT_HPP_ + +#include +#include +#include + +#include "../time.h" +#include "forward.hpp" + +namespace tf2 +{ +namespace impl +{ +/// \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 = + 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); + } + + /** + * \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; + std::chrono::nanoseconds ns(d); + t = tf2::TimePoint(std::chrono::duration_cast(ns)); + } +}; + +/// \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); + std::chrono::seconds s = std::chrono::duration_cast(t); + duration_msg.sec = static_cast(s.count()); + 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) + { + int64_t d = duration_msg.sec * 1000000000ull + duration_msg.nanosec; + std::chrono::nanoseconds ns(d); + duration = tf2::Duration(std::chrono::duration_cast(ns)); + } +}; + +/// \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; +}; + +/// \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; + tf2::fromMsg<>(time_msg, tp); + 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; + tf2::fromMsg<>(duration_msg, d); + return d; +} +} // namespace tf2 + +#endif // TF2__IMPL__TIME_CONVERT_HPP_ diff --git a/tf2/include/tf2/transform_datatypes.h b/tf2/include/tf2/transform_datatypes.h index 9e57e2258..06d99aaf1 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.hpp" +#include namespace tf2 { @@ -61,6 +63,14 @@ 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 0f22398cd..08cdc49bd 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 diff --git a/tf2_bullet/CMakeLists.txt b/tf2_bullet/CMakeLists.txt index 693275c52..ba381bf8c 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) diff --git a/tf2_bullet/include/tf2_bullet/tf2_bullet.hpp b/tf2_bullet/include/tf2_bullet/tf2_bullet.hpp index 0f1408d88..f27e96812 100644 --- a/tf2_bullet/include/tf2_bullet/tf2_bullet.hpp +++ b/tf2_bullet/include/tf2_bullet/tf2_bullet.hpp @@ -26,19 +26,19 @@ // 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_HPP_ #define TF2_BULLET__TF2_BULLET_HPP_ -#include #include #include #include -#include -#include +#include +#include -#include +#include +#include #if (BT_BULLET_VERSION <= 282) // Suppress compilation warning on older versions of Bullet. @@ -51,96 +51,119 @@ 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) +namespace impl { - return btTransform( - btQuaternion( - static_cast(t.transform.rotation.x), - static_cast(t.transform.rotation.y), - static_cast(t.transform.rotation.z), - static_cast(t.transform.rotation.w)), - btVector3( - static_cast(t.transform.translation.x), - static_cast(t.transform.translation.y), - static_cast(t.transform.translation.z))); -} +/// \brief Default return type of tf2::toMsg() for btVector3. +template<> +struct DefaultMessageForDatatype +{ + /// \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 DefaultMessageForDatatype +{ + /// \brief Default return type of tf2::toMsg() for btQuaternion. + using type = geometry_msgs::msg::Quaternion; +}; -/** \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) +/// \brief Default return type of tf2::toMsg() for btTransform. +template<> +struct DefaultMessageForDatatype { - t_out = - tf2::Stamped( - transformToBullet(transform) * t_in, - tf2_ros::fromMsg(transform.header.stamp), transform.header.frame_id); -} + /// \brief Default return type of tf2::toMsg() for btTransform. + using type = geometry_msgs::msg::Transform; +}; -/** \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 & in) +/// \brief Conversion implementation for geometry_msgs::msg::Point and btVector3. +template<> +struct ConversionImplementation + : DefaultVectorConversionImplementation { - 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; -} +}; -/** \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 & out) +/// \brief Conversion implementation for geometry_msgs::msg::Vector3 and btVector3. +template<> +struct ConversionImplementation + : DefaultVectorConversionImplementation { - out.stamp_ = tf2_ros::fromMsg(msg.header.stamp); - out.frame_id_ = msg.header.frame_id; - out[0] = static_cast(msg.point.x); - out[1] = static_cast(msg.point.y); - out[2] = static_cast(msg.point.z); -} +}; +/// \brief Conversion implementation for geometry_msgs::msg::Quaternion and Eigen::Quaterniond. +template<> +struct ConversionImplementation + : DefaultQuaternionConversionImplementation {}; -/** \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) +/// \brief Conversion implementation for geometry_msgs::msg::Transform and btTransform. +template<> +struct ConversionImplementation { - t_out = - tf2::Stamped( - transformToBullet(transform) * t_in, - tf2_ros::fromMsg(transform.header.stamp), transform.header.frame_id); -} + /** + * \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; + btQuaternion rot; + tf2::fromMsg<>(in.rotation, rot); + tf2::fromMsg<>(in.translation, trans); + 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); + tf2::toMsg<>(in.getOrigin(), out.translation); + } +}; +/// \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[in] t The transform to convert, as a geometry_msgs TransformedStamped message. + * \return The transform message converted to a Bullet btTransform. + */ +[[deprecated("Please use tf2::fromMsg()")]] +inline btTransform transformToBullet( + const geometry_msgs::msg::TransformStamped & t) +{ + btTransform ans; + fromMsg<>(t.transform, ans); + return ans; +} } // namespace tf2 #endif // TF2_BULLET__TF2_BULLET_HPP_ diff --git a/tf2_bullet/test/test_tf2_bullet.cpp b/tf2_bullet/test/test_tf2_bullet.cpp index 998f8f2fb..3721424ea 100644 --- a/tf2_bullet/test/test_tf2_bullet.cpp +++ b/tf2_bullet/test/test_tf2_bullet.cpp @@ -51,6 +51,24 @@ 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, 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); + EXPECT_EQ(transform2, transform); +} int main(int argc, char ** argv) { diff --git a/tf2_eigen/include/tf2_eigen/tf2_eigen.hpp b/tf2_eigen/include/tf2_eigen/tf2_eigen.hpp index 12866c042..9ce043e70 100644 --- a/tf2_eigen/include/tf2_eigen/tf2_eigen.hpp +++ b/tf2_eigen/include/tf2_eigen/tf2_eigen.hpp @@ -24,49 +24,55 @@ * POSSIBILITY OF SUCH DAMAGE. */ -/** \author Koji Terada */ +/** \author Koji Terada, Bjarne von Horn */ #ifndef TF2_EIGEN__TF2_EIGEN_HPP_ #define TF2_EIGEN__TF2_EIGEN_HPP_ #include -#include +#include + #include +#include #include #include +#include #include #include -#include -#include -#include - +#include namespace tf2 { - -/** \brief Convert a timestamped transform to the equivalent Eigen data type. +/** + * \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. */ 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. +/** + * \brief Convert a timestamped transform to the equivalent Eigen data type. * \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. +/** + * \brief Convert an Eigen Affine3d transform to the equivalent geometry_msgs message type. * \param T The transform to convert, as an Eigen Affine3d transform. * \return The transform converted to a TransformStamped message. */ @@ -74,20 +80,12 @@ 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; } -/** \brief Convert an Eigen Isometry3d transform to the equivalent geometry_msgs message type. +/** + * \brief Convert an Eigen Isometry3d transform to the equivalent geometry_msgs message type. * \param T The transform to convert, as an Eigen Isometry3d transform. * \return The transform converted to a TransformStamped message. */ @@ -95,306 +93,315 @@ 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; } -/** \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) +namespace impl { - t_out = Eigen::Vector3d(transformToEigen(transform) * t_in); -} +// --------------------- +// Vector +// --------------------- -/** \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) +/// \brief Conversion implementation for geometry_msgs::msg::Point and Eigen::Vector3d. +template<> +struct ConversionImplementation + : DefaultVectorConversionImplementation { - geometry_msgs::msg::Point msg; - msg.x = in.x(); - msg.y = in.y(); - msg.z = in.z(); - return msg; -} +}; -/** \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 Conversion implementation for geometry_msgs::msg::Vector3d and Eigen::Vector3d. +template<> +struct ConversionImplementation + : DefaultVectorConversionImplementation { - 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) +/// \brief Default Type for automatic tf2::doTransform() implementation for Eigen::Vector3d. +template<> +struct DefaultTransformType { - out.x = in.x(); - out.y = in.y(); - out.z = in.z(); - return out; -} + /// \brief Default Type for automatic tf2::doTransform() implementation for Eigen::Vector3d. + using type = Eigen::Isometry3d; +}; -/** \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) +/// \brief Default return type of tf2::toMsg() for Eigen::Vector3d. +template<> +struct DefaultMessageForDatatype { - out.x() = msg.x; - out.y() = msg.y; - out.z() = msg.z; -} + /// \brief Default return type of tf2::toMsg() for Eigen::Vector3d. + using type = geometry_msgs::msg::Point; +}; -/** \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. - */ + +// --------------------- +// Quaternion +// --------------------- + +/// \brief Conversion implementation for geometry_msgs::msg::Quaternion and Eigen::Quaterniond. template<> -inline -void doTransform( - const tf2::Stamped & t_in, - tf2::Stamped & t_out, - const geometry_msgs::msg::TransformStamped & transform) +struct ConversionImplementation +{ + /** + * \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. + */ + 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. + * \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) + { + out = Eigen::Quaterniond(msg.w, msg.x, msg.y, msg.z); + } +}; + +/// \brief Default return type of tf2::toMsg() for Eigen::Quaterniond. +template<> +struct DefaultMessageForDatatype { - t_out = tf2::Stamped( - transformToEigen(transform) * t_in, - tf2_ros::fromMsg(transform.header.stamp), - transform.header.frame_id); -} + /// \brief Default return type of tf2::toMsg() for Eigen::Quaterniond. + using type = geometry_msgs::msg::Quaternion; +}; -/** \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)); -} +// --------------------- +// Transform +// --------------------- -/** \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. +/** + * \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<> -inline -void doTransform( - const Eigen::Affine3d & t_in, - Eigen::Affine3d & t_out, - const geometry_msgs::msg::TransformStamped & transform) +struct ConversionImplementation + : EigenTransformImpl { - t_out = Eigen::Affine3d(transformToEigen(transform) * t_in); -} +}; +/// \brief Conversion implementation for geometry_msgs::msg::Transform and Eigen::Isometry3d. template<> -inline -void doTransform( - const Eigen::Isometry3d & t_in, - Eigen::Isometry3d & t_out, - const geometry_msgs::msg::TransformStamped & transform) +struct ConversionImplementation + : EigenTransformImpl { - t_out = Eigen::Isometry3d(transformToEigen(transform) * t_in); -} +}; -/** \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. +/** + * \brief Pose conversion template for Eigen types. + * \tparam T Eigen transform type. */ -inline -geometry_msgs::msg::Quaternion toMsg(const Eigen::Quaterniond & in) +template +struct EigenPoseImpl +{ + /** + * \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) + { + 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 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) + { + Eigen::Quaterniond q; + Eigen::Vector3d v; + tf2::fromMsg<>(msg.orientation, q); + tf2::fromMsg<>(msg.position, v); + out = Eigen::Translation3d(v) * q; + } +}; + +/// \brief Conversion implementation for geometry_msgs::msg::Pose and Eigen::Affine3d. +template<> +struct ConversionImplementation + : public EigenPoseImpl { - 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) +/// \brief Conversion implementation for geometry_msgs::msg::Pose and Eigen::Isometry3d. +template<> +struct ConversionImplementation + : public EigenPoseImpl { - out = Eigen::Quaterniond(msg.w, msg.x, msg.y, msg.z); -} +}; -/** \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. - */ +/// \brief Default return type of tf2::toMsg() for Eigen::Affine3d. template<> -inline -void doTransform( - const Eigen::Quaterniond & t_in, - Eigen::Quaterniond & t_out, - const geometry_msgs::msg::TransformStamped & transform) +struct DefaultMessageForDatatype { - Eigen::Quaterniond t; - fromMsg(transform.transform.rotation, t); - t_out = t * t_in; -} + /// \brief Default return type of tf2::toMsg() for Eigen::Affine3d. + using type = geometry_msgs::msg::Pose; +}; -/** \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) +/// \brief Default return type of tf2::toMsg() for Eigen::Affine3d. +template<> +struct DefaultMessageForDatatype { - 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 Default return type of tf2::toMsg() for Eigen::Affine3d. + using type = geometry_msgs::msg::Pose; +}; -/** \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. - * \param t_out The transformed vector, as a timestamped Eigen Quaterniond data type. - * \param transform The timestamped transform to apply, as a TransformStamped message. - */ +// --------------------- +// Twist +// --------------------- + +/// \brief Conversion implementation for geometry_msgs::msg::Twist and Eigen::Matrix. template<> -inline -void doTransform( - const tf2::Stamped & t_in, - tf2::Stamped & t_out, - const geometry_msgs::msg::TransformStamped & transform) +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. + * \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]; + 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. + * \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) + { + 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 Default return type of tf2::toMsg() for Eigen::Matrix. +template<> +struct DefaultMessageForDatatype> { - t_out.frame_id_ = transform.header.frame_id; - t_out.stamp_ = tf2_ros::fromMsg(transform.header.stamp); - doTransform( - static_cast(t_in), - static_cast(t_out), transform); -} + /// \brief Default return type of tf2::toMsg() for Eigen::Matrix. + using type = geometry_msgs::msg::Twist; +}; -/** \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) + +// --------------------- +// Wrench +// --------------------- + +/// \brief Conversion implementation for geometry_msgs::msg::Wrench and Eigen::Matrix. +template<> +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. + * \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]; + 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 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) + { + 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; + } +}; + +/// \brief Default Type for automatic tf2::doTransform() implementation for Eigen::Affine3d. +template<> +struct DefaultTransformType { - 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 Default Type for automatic tf2::doTransform() implementation for Eigen::Affine3d. + using type = Eigen::Isometry3d; +}; -/** \brief Convert a Eigen Isometry3d transform type to a Pose message. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param in The Eigen Isometry3d to convert. - * \return The Eigen transform converted to a Pose message. - */ -inline -geometry_msgs::msg::Pose toMsg(const Eigen::Isometry3d & in) + +/// \brief Default Type for automatic tf2::doTransform() implementation for Eigen::Isometry3d. +template<> +struct DefaultTransformType { - 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 Default Type for automatic tf2::doTransform() implementation for Eigen::Isometry3d. + using type = Eigen::Isometry3d; +}; + +} // namespace impl -/** \brief Convert a Eigen::Vector3d type to a geometry_msgs::msg::Vector3. +/** + * \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. * The function was renamed to toMsg2 to avoid overloading conflicts with definitions above. * @@ -405,235 +412,33 @@ 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); - 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]; + msg.x = in[0]; + msg.y = in[1]; + msg.z = in[2]; 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 - * 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, tf2_ros::fromMsg( - transform.header.stamp), transform.header.frame_id); -} - -/** \brief Apply a geometry_msgs TransformStamped to an Eigen Isometry transform. +/** + * \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 - * function relies on the existence of a time stamp and a frame id in the type which should + * functions rely 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. + * \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 tf2::Stamped & t_in, - tf2::Stamped & t_out, + const Eigen::Quaterniond & t_in, + Eigen::Quaterniond & 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); -} - -/** \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)); + Eigen::Quaterniond t; + tf2::fromMsg<>(transform.transform.rotation, t); + t_out = Eigen::Quaterniond(t.toRotationMatrix() * t_in.toRotationMatrix()); } - } // 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_HPP_ diff --git a/tf2_eigen/test/tf2_eigen-test.cpp b/tf2_eigen/test/tf2_eigen-test.cpp index 8a8a1112e..2d6321925 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: @@ -65,18 +64,17 @@ TEST(TfEigen, ConvertVector3dStamped) EXPECT_EQ(v, v1); } -// TODO(clalancette) Re-enable these tests once we have tf2/convert.h:convert(A, B) implemented -// TEST(TfEigen, ConvertVector3d) -// { -// const Eigen::Vector3d v(1,2,3); +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); + Eigen::Vector3d v1; + geometry_msgs::msg::Point p1; + tf2::convert(v, p1); + tf2::convert(p1, v1); -// EXPECT_EQ(v, v1); -// } + EXPECT_EQ(v, v1); +} TEST(TfEigen, ConvertAffine3dStamped) { @@ -96,20 +94,20 @@ TEST(TfEigen, ConvertAffine3dStamped) EXPECT_EQ(v.stamp_, v1.stamp_); } -// TODO(clalancette) Re-enable these tests once we have tf2/convert.h:convert(A, B) implemented -// TEST(TfEigen, ConvertAffine3d) -// { -// const Eigen::Affine3d v( -// Eigen::Translation3d(1,2,3) * Eigen::AngleAxis(1, Eigen::Vector3d::UnitX())); +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); + 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()); -// } + EXPECT_EQ(v.translation(), v1.translation()); + EXPECT_EQ(v.rotation(), v1.rotation()); +} TEST(TfEigen, ConvertTransform) { @@ -128,8 +126,10 @@ TEST(TfEigen, ConvertTransform) Eigen::Affine3d T(tm); - geometry_msgs::msg::TransformStamped msg = tf2::eigenToTransform(T); - Eigen::Affine3d Tback = tf2::transformToEigen(msg); + geometry_msgs::msg::Transform msg; + tf2::toMsg(T, msg); + Eigen::Affine3d Tback; + tf2::fromMsg(msg, Tback); EXPECT_TRUE(T.isApprox(Tback)); EXPECT_TRUE(tm.isApprox(Tback.matrix())); @@ -137,8 +137,9 @@ TEST(TfEigen, ConvertTransform) // same for Isometry Eigen::Isometry3d I(tm); - msg = tf2::eigenToTransform(T); - Eigen::Isometry3d Iback = tf2::transformToEigen(msg); + tf2::toMsg(T, msg); + Eigen::Isometry3d Iback; + tf2::fromMsg(msg, Iback); EXPECT_TRUE(I.isApprox(Iback)); EXPECT_TRUE(tm.isApprox(Iback.matrix())); 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..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 @@ -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 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 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 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 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 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 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 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 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 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 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 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 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/CMakeLists.txt b/tf2_geometry_msgs/CMakeLists.txt index 7d5fd6db7..03271ed52 100644 --- a/tf2_geometry_msgs/CMakeLists.txt +++ b/tf2_geometry_msgs/CMakeLists.txt @@ -29,7 +29,15 @@ 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) Remove once headers + # are renamed to .hpp + 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) target_include_directories(test_tf2_geometry_msgs PUBLIC include) diff --git a/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp b/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp index aa32b1ccd..26d8d7334 100644 --- a/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp +++ b/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp @@ -27,16 +27,15 @@ // POSSIBILITY OF SUCH DAMAGE. -/** \author Wim Meeussen */ +/** \author Wim Meeussen, Bjarne von Horn */ -#ifndef TF2_GEOMETRY_MSGS_HPP -#define TF2_GEOMETRY_MSGS_HPP +#ifndef TF2_GEOMETRY_MSGS__TF2_GEOMETRY_MSGS_HPP_ +#define TF2_GEOMETRY_MSGS__TF2_GEOMETRY_MSGS_HPP_ #include +#include #include #include -#include -#include #include #include #include @@ -48,789 +47,572 @@ #include #include #include +#include #include -namespace tf2 +#include + + +namespace Eigen { +/// \brief Forward declaration +template +class Matrix; +} -/** \brief Convert a TransformStamped message to a KDL frame. +namespace tf2 +{ +/** + * \brief Convert a TransformStamped message to a KDL frame. * \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) - { - 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 +{ /*************/ /** Vector3 **/ /*************/ -/** \brief Convert a tf2 Vector3 type to its equivalent geometry_msgs representation. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param in A tf2 Vector3 object. - * \return The Vector3 converted to a geometry_msgs message type. - */ -inline -geometry_msgs::msg::Vector3 toMsg(const tf2::Vector3 & in) +/// \brief Conversion implementation for geometry_msgs::msg::Vector3 and tf2::Vector3. +template<> +struct ConversionImplementation + : DefaultVectorConversionImplementation { - geometry_msgs::msg::Vector3 out; - out.x = in.getX(); - out.y = in.getY(); - out.z = in.getZ(); - return out; -} +}; -/** \brief Convert a Vector3 message to its equivalent tf2 representation. - * This function is a specialization of the fromMsg template defined in tf2/convert.h. - * \param in A Vector3 message type. - * \param out The Vector3 converted to a tf2 type. - */ -inline -void fromMsg(const geometry_msgs::msg::Vector3 & in, tf2::Vector3 & out) +/// \brief Conversion implementation for geometry_msgs::msg::Point and tf2::Vector3. +template<> +struct ConversionImplementation + : DefaultVectorConversionImplementation { - out = tf2::Vector3(in.x, in.y, in.z); -} - -/********************/ -/** Vector3Stamped **/ -/********************/ - -/** \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);} - -/** \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;} - -/** \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::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; - } +}; -/** \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) +/// \brief Default return type of tf2::toMsg() for tf2::Vector3. +template<> +struct DefaultMessageForDatatype { - return in; -} + /// \brief Default return type of tf2::toMsg() for tf2::Vector3. + using type = geometry_msgs::msg::Vector3; +}; -/** \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) +/// \brief Default Type for automatic tf2::doTransform() implementation for tf2::Vector3. +template<> +struct DefaultTransformType { - out = msg; -} + /// \brief Default Type for automatic tf2::doTransform() implementation for tf2::Vector3. + using type = tf2::Transform; +}; -/***********/ -/** Point **/ -/***********/ +/****************/ +/** Quaternion **/ +/****************/ -/** \brief Convert a tf2 Vector3 type to its equivalent geometry_msgs representation. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param in A tf2 Vector3 object. - * \return The Vector3 converted to a geometry_msgs message type. - */ -inline -geometry_msgs::msg::Point & toMsg(const tf2::Vector3 & in, geometry_msgs::msg::Point & out) -{ - out.x = in.getX(); - out.y = in.getY(); - out.z = in.getZ(); - return out; -} +/// \brief Conversion implementation for geometry_msgs::msg::Quaternion and tf2::Quaternion. +template<> +struct ConversionImplementation + : DefaultQuaternionConversionImplementation {}; -/** \brief Convert a Vector3 message to its equivalent tf2 representation. - * This function is a specialization of the fromMsg template defined in tf2/convert.h. - * \param in A Vector3 message type. - * \param out The Vector3 converted to a tf2 type. - */ -inline -void fromMsg(const geometry_msgs::msg::Point & in, tf2::Vector3 & out) +/// \brief Default return type of tf2::toMsg() for tf2::Quaternion. +template<> +struct DefaultMessageForDatatype { - out = tf2::Vector3(in.x, in.y, in.z); -} - + /// \brief Default return type of tf2::toMsg() for tf2::Quaternion. + using type = geometry_msgs::msg::Quaternion; +}; -/******************/ -/** PointStamped **/ -/******************/ +/// \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; +}; -/** \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;} +/***************/ +/** Transform **/ +/***************/ -/** \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::PointStamped& t_in, geometry_msgs::msg::PointStamped& t_out, const geometry_msgs::msg::TransformStamped& 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) { - 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; + tf2::toMsg<>(in.getOrigin(), out.translation); + tf2::toMsg<>(in.getRotation(), out.rotation); } -/** \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 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 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) +/// \brief Default return type of tf2::toMsg() for tf2::Transform. +template<> +struct DefaultMessageForDatatype { - out = msg; -} - + /// \brief Default return type of tf2::toMsg() for tf2::Transform. + using type = geometry_msgs::msg::Transform; +}; -/*****************/ -/** 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;} +/**********/ +/** 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. - * \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::PoseStamped& t_in, geometry_msgs::msg::PoseStamped& t_out, const geometry_msgs::msg::TransformStamped& transform) +/// \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) { - 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; + tf2::toMsg<>(in.getOrigin(), out.position); + tf2::toMsg<>(in.getRotation(), out.orientation); } -/** \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 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 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) +/// \brief Default Type for automatic tf2::doTransform() implementation for tf2::Transform. +template<> +struct DefaultTransformType { - 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 Default Type for automatic tf2::doTransform() implementation for tf2::Transform. + using type = tf2::Transform; +}; -/** \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. - * \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);} +/************/ +/** Wrench **/ +/************/ -/** \brief Transform the covariance matrix of a PoseWithCovariance message to a new frame. - * \param cov_in The covariance matrix to transform. - * \param transform The transform to apply, as a tf2::Transform structure. - * \return The transformed covariance matrix. - */ -inline -geometry_msgs::msg::PoseWithCovariance::_covariance_type transformCovariance( - const geometry_msgs::msg::PoseWithCovariance::_covariance_type & cov_in, - const tf2::Transform & transform) +/// \brief Conversion implementation for geometry_msgs::msg::Wrench and tf2::Transform. +template<> +struct ConversionImplementation, geometry_msgs::msg::Wrench> { - /** - * To transform a covariance matrix: - * - * \verbatim[R 0] COVARIANCE [R' 0 ] - [0 R] [0 R']\endverbatim - * - * Where: - * R is the rotation matrix (3x3). - * R' is the transpose of the rotation matrix. - * COVARIANCE is the 6x6 covariance matrix to be transformed. - * - * Reference: - * A. L. Garcia, “Linear Transformations of Random Vectors,” in Probability, - * Statistics, and Random Processes For Electrical Engineering, 3rd ed., - * Pearson Prentice Hall, 2008, pp. 320–322. - */ - - // get rotation matrix (and transpose) - const tf2::Matrix3x3 R = transform.getBasis(); - const tf2::Matrix3x3 R_transpose = R.transpose(); - - // convert covariance matrix into four 3x3 blocks - const tf2::Matrix3x3 cov_11(cov_in[0], cov_in[1], cov_in[2], - cov_in[6], cov_in[7], cov_in[8], - cov_in[12], cov_in[13], cov_in[14]); - const tf2::Matrix3x3 cov_12(cov_in[3], cov_in[4], cov_in[5], - cov_in[9], cov_in[10], cov_in[11], - cov_in[15], cov_in[16], cov_in[17]); - const tf2::Matrix3x3 cov_21(cov_in[18], cov_in[19], cov_in[20], - cov_in[24], cov_in[25], cov_in[26], - cov_in[30], cov_in[31], cov_in[32]); - const tf2::Matrix3x3 cov_22(cov_in[21], cov_in[22], cov_in[23], - cov_in[27], cov_in[28], cov_in[29], - cov_in[33], cov_in[34], cov_in[35]); - - // perform blockwise matrix multiplication - const tf2::Matrix3x3 result_11 = R * cov_11 * R_transpose; - const tf2::Matrix3x3 result_12 = R * cov_12 * R_transpose; - const tf2::Matrix3x3 result_21 = R * cov_21 * R_transpose; - const tf2::Matrix3x3 result_22 = R * cov_22 * R_transpose; - - // form the output - geometry_msgs::msg::PoseWithCovariance::_covariance_type cov_out; - cov_out[0] = result_11[0][0]; - cov_out[1] = result_11[0][1]; - cov_out[2] = result_11[0][2]; - cov_out[6] = result_11[1][0]; - cov_out[7] = result_11[1][1]; - cov_out[8] = result_11[1][2]; - cov_out[12] = result_11[2][0]; - cov_out[13] = result_11[2][1]; - cov_out[14] = result_11[2][2]; - - cov_out[3] = result_12[0][0]; - cov_out[4] = result_12[0][1]; - cov_out[5] = result_12[0][2]; - cov_out[9] = result_12[1][0]; - cov_out[10] = result_12[1][1]; - cov_out[11] = result_12[1][2]; - cov_out[15] = result_12[2][0]; - cov_out[16] = result_12[2][1]; - cov_out[17] = result_12[2][2]; - - cov_out[18] = result_21[0][0]; - cov_out[19] = result_21[0][1]; - cov_out[20] = result_21[0][2]; - cov_out[24] = result_21[1][0]; - cov_out[25] = result_21[1][1]; - cov_out[26] = result_21[1][2]; - cov_out[30] = result_21[2][0]; - cov_out[31] = result_21[2][1]; - cov_out[32] = result_21[2][2]; - - cov_out[21] = result_22[0][0]; - cov_out[22] = result_22[0][1]; - cov_out[23] = result_22[0][2]; - cov_out[27] = result_22[1][0]; - cov_out[28] = result_22[1][1]; - cov_out[29] = result_22[1][2]; - cov_out[33] = result_22[2][0]; - cov_out[34] = result_22[2][1]; - cov_out[35] = result_22[2][2]; - - return cov_out; -} - -// Forward declaration -void fromMsg(const geometry_msgs::msg::Transform& in, tf2::Transform& out); - -/** \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. - */ -template <> -inline - void doTransform(const geometry_msgs::msg::PoseWithCovarianceStamped& t_in, geometry_msgs::msg::PoseWithCovarianceStamped& t_out, const geometry_msgs::msg::TransformStamped& transform) + /** + * \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) { - KDL::Vector v(t_in.pose.pose.position.x, t_in.pose.pose.position.y, t_in.pose.pose.position.z); - KDL::Rotation r = KDL::Rotation::Quaternion(t_in.pose.pose.orientation.x, t_in.pose.pose.orientation.y, t_in.pose.pose.orientation.z, t_in.pose.pose.orientation.w); - - KDL::Frame v_out = gmTransformToKDL(transform) * KDL::Frame(r, v); - t_out.pose.pose.position.x = v_out.p[0]; - t_out.pose.pose.position.y = v_out.p[1]; - t_out.pose.pose.position.z = v_out.p[2]; - v_out.M.GetQuaternion(t_out.pose.pose.orientation.x, t_out.pose.pose.orientation.y, t_out.pose.pose.orientation.z, t_out.pose.pose.orientation.w); - t_out.header.stamp = transform.header.stamp; - t_out.header.frame_id = transform.header.frame_id; - - tf2::Transform tf_transform; - fromMsg(transform.transform, tf_transform); - t_out.pose.covariance = transformCovariance(t_in.pose.covariance, tf_transform); + tf2::toMsg<>(std::get<0>(in), out.force); + tf2::toMsg<>(std::get<1>(in), out.torque); } -/** \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 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 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. - */ +/// \brief Default return type of tf2::toMsg() for std::array. template<> -inline -geometry_msgs::msg::PoseWithCovarianceStamped toMsg(const tf2::WithCovarianceStamped& in) +struct DefaultMessageForDatatype> { - geometry_msgs::msg::PoseWithCovarianceStamped out; - out.header.stamp = tf2_ros::toMsg(in.stamp_); - out.header.frame_id = in.frame_id_; - out.pose.covariance = covarianceNestedToRowMajor(in.cov_mat_); - out.pose.pose.orientation.x = in.getRotation().getX(); - out.pose.pose.orientation.y = in.getRotation().getY(); - out.pose.pose.orientation.z = in.getRotation().getZ(); - out.pose.pose.orientation.w = in.getRotation().getW(); - out.pose.pose.position.x = in.getOrigin().getX(); - out.pose.pose.position.y = in.getOrigin().getY(); - out.pose.pose.position.z = in.getOrigin().getZ(); - return out; + /// \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. + * \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) +{ + TFDatatype in_tf, out_tf; + tf2::convert(in, in_tf); + tf2::doTransform(in_tf, out_tf, transform); + tf2::convert(out_tf, 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. +/** + * \brief Specialization of tf2::convert for Eigen::Matrix + * and std::array */ template<> -inline -void fromMsg(const geometry_msgs::msg::PoseWithCovarianceStamped& in, tf2::WithCovarianceStamped& out) +struct Converter, + std::array> { - out.stamp_ = tf2_ros::fromMsg(in.header.stamp); - out.frame_id_ = in.header.frame_id; - out.cov_mat_ = covarianceRowMajorToNested(in.pose.covariance); - tf2::Transform tmp; - fromMsg(in.pose.pose, tmp); - out.setData(tmp); -} + /** + * \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 -/****************/ -/** 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) -{ - geometry_msgs::msg::Quaternion out; - out.w = in.getW(); - out.x = in.getX(); - out.y = in.getY(); - out.z = in.getZ(); - return out; -} +/*************/ +/** Vector3 **/ +/*************/ -/** \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. +/** + * \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[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. */ -inline -void fromMsg(const geometry_msgs::msg::Quaternion& in, tf2::Quaternion& out) +template<> +inline void doTransform( + const geometry_msgs::msg::Vector3 & t_in, geometry_msgs::msg::Vector3 & t_out, + const geometry_msgs::msg::TransformStamped & transform) { - // w at the end in the constructor - out = tf2::Quaternion(in.x, in.y, in.z, in.w); + 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); } - -/***********************/ -/** 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. +/** + * \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 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 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 <> -inline -void doTransform(const geometry_msgs::msg::QuaternionStamped& t_in, geometry_msgs::msg::QuaternionStamped& t_out, const geometry_msgs::msg::TransformStamped& transform) +template<> +inline void doTransform( + const geometry_msgs::msg::Vector3Stamped & t_in, geometry_msgs::msg::Vector3Stamped & t_out, + const geometry_msgs::msg::TransformStamped & transform) { - tf2::Quaternion q_out = tf2::Quaternion(transform.transform.rotation.x, transform.transform.rotation.y, - transform.transform.rotation.z, transform.transform.rotation.w)* - tf2::Quaternion(t_in.quaternion.x, t_in.quaternion.y, t_in.quaternion.z, t_in.quaternion.w); - t_out.quaternion = toMsg(q_out); + doTransform<>(t_in.vector, t_out.vector, transform); 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; -} +/***********/ +/** Point **/ +/***********/ -/** \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. +/** + * \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[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. */ -inline -geometry_msgs::msg::QuaternionStamped toMsg(const tf2::Stamped& in) +template<> +inline void doTransform( + const geometry_msgs::msg::Point & t_in, geometry_msgs::msg::Point & t_out, + const geometry_msgs::msg::TransformStamped & transform) { - 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; + impl::doTransformWithTf(t_in, t_out, transform); } -/** \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. +/** + * \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[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. */ -inline -void fromMsg(const geometry_msgs::msg::QuaternionStamped& in, tf2::Stamped& out) +template<> +inline void doTransform( + const geometry_msgs::msg::PointStamped & t_in, geometry_msgs::msg::PointStamped & t_out, + const geometry_msgs::msg::TransformStamped & transform) { - 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); + 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; } +/**********/ +/** Pose **/ +/**********/ -/***************/ -/** 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. +/** + * \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[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. */ -inline -geometry_msgs::msg::Transform toMsg(const tf2::Transform& in) +template<> +inline void doTransform( + const geometry_msgs::msg::Pose & t_in, geometry_msgs::msg::Pose & t_out, + const geometry_msgs::msg::TransformStamped & transform) { - 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; + impl::doTransformWithTf(t_in, t_out, transform); } -/** \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. +/** + * \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[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. */ -inline -void fromMsg(const geometry_msgs::msg::Transform& in, tf2::Transform& out) +template<> +inline void doTransform( + const geometry_msgs::msg::PoseStamped & t_in, geometry_msgs::msg::PoseStamped & t_out, + const geometry_msgs::msg::TransformStamped & transform) { - 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)); + 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; } +/*******************************/ +/** PoseWithCovarianceStamped **/ +/*******************************/ -/**********************/ -/** TransformStamped **/ -/**********************/ - -/** \brief Extract a timestamp from the header of a Transform message. - * This function is a specialization of the getTimestamp template defined in tf2/convert.h. - * \param t TransformStamped message to extract the timestamp from. - * \return The timestamp of the message. +/** + * \brief Transform the covariance matrix of a PoseWithCovariance message to a new frame. + * \param cov_in The covariance matrix to transform. + * \param transform The transform to apply, as a tf2::Transform structure. + * \return The transformed covariance matrix. */ -template <> inline -tf2::TimePoint getTimestamp(const geometry_msgs::msg::TransformStamped& t) {return tf2_ros::fromMsg(t.header.stamp);} +geometry_msgs::msg::PoseWithCovariance::_covariance_type transformCovariance( + const geometry_msgs::msg::PoseWithCovariance::_covariance_type & cov_in, + const tf2::Transform & transform) +{ + /** + * To transform a covariance matrix: + * + * \verbatim[R 0] COVARIANCE [R' 0 ] + [0 R] [0 R']\endverbatim + * + * Where: + * R is the rotation matrix (3x3). + * R' is the transpose of the rotation matrix. + * COVARIANCE is the 6x6 covariance matrix to be transformed. + * + * Reference: + * A. L. Garcia, “Linear Transformations of Random Vectors,” in Probability, + * Statistics, and Random Processes For Electrical Engineering, 3rd ed., + * Pearson Prentice Hall, 2008, pp. 320–322. + */ + + // get rotation matrix (and transpose) + const tf2::Matrix3x3 R = transform.getBasis(); + const tf2::Matrix3x3 R_transpose = R.transpose(); + + // convert covariance matrix into four 3x3 blocks + const tf2::Matrix3x3 cov_11(cov_in[0], cov_in[1], cov_in[2], + cov_in[6], cov_in[7], cov_in[8], + cov_in[12], cov_in[13], cov_in[14]); + const tf2::Matrix3x3 cov_12(cov_in[3], cov_in[4], cov_in[5], + cov_in[9], cov_in[10], cov_in[11], + cov_in[15], cov_in[16], cov_in[17]); + const tf2::Matrix3x3 cov_21(cov_in[18], cov_in[19], cov_in[20], + cov_in[24], cov_in[25], cov_in[26], + cov_in[30], cov_in[31], cov_in[32]); + const tf2::Matrix3x3 cov_22(cov_in[21], cov_in[22], cov_in[23], + cov_in[27], cov_in[28], cov_in[29], + cov_in[33], cov_in[34], cov_in[35]); + + // perform blockwise matrix multiplication + const tf2::Matrix3x3 result_11 = R * cov_11 * R_transpose; + const tf2::Matrix3x3 result_12 = R * cov_12 * R_transpose; + const tf2::Matrix3x3 result_21 = R * cov_21 * R_transpose; + const tf2::Matrix3x3 result_22 = R * cov_22 * R_transpose; + + // form the output + geometry_msgs::msg::PoseWithCovariance::_covariance_type cov_out; + cov_out[0] = result_11[0][0]; + cov_out[1] = result_11[0][1]; + cov_out[2] = result_11[0][2]; + cov_out[6] = result_11[1][0]; + cov_out[7] = result_11[1][1]; + cov_out[8] = result_11[1][2]; + cov_out[12] = result_11[2][0]; + cov_out[13] = result_11[2][1]; + cov_out[14] = result_11[2][2]; + + cov_out[3] = result_12[0][0]; + cov_out[4] = result_12[0][1]; + cov_out[5] = result_12[0][2]; + cov_out[9] = result_12[1][0]; + cov_out[10] = result_12[1][1]; + cov_out[11] = result_12[1][2]; + cov_out[15] = result_12[2][0]; + cov_out[16] = result_12[2][1]; + cov_out[17] = result_12[2][2]; + + cov_out[18] = result_21[0][0]; + cov_out[19] = result_21[0][1]; + cov_out[20] = result_21[0][2]; + cov_out[24] = result_21[1][0]; + cov_out[25] = result_21[1][1]; + cov_out[26] = result_21[1][2]; + cov_out[30] = result_21[2][0]; + cov_out[31] = result_21[2][1]; + cov_out[32] = result_21[2][2]; + + cov_out[21] = result_22[0][0]; + cov_out[22] = result_22[0][1]; + cov_out[23] = result_22[0][2]; + cov_out[27] = result_22[1][0]; + cov_out[28] = result_22[1][1]; + cov_out[29] = result_22[1][2]; + cov_out[33] = result_22[2][0]; + cov_out[34] = result_22[2][1]; + cov_out[35] = result_22[2][2]; + + return cov_out; +} -/** \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;} -/** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Transform type. +/** + * \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 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 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 <> -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; - } - -/** \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) +template<> +inline void doTransform( + const geometry_msgs::msg::PoseWithCovarianceStamped & t_in, + geometry_msgs::msg::PoseWithCovarianceStamped & t_out, + const geometry_msgs::msg::TransformStamped & transform) { - return in; + 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; + tf2::Transform tf_transform; + tf2::fromMsg(transform.transform, tf_transform); + t_out.pose.covariance = transformCovariance(t_in.pose.covariance, tf_transform); } -/** \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. +/****************/ +/** Quaternion **/ +/****************/ + +/** + * \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. + * \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. */ -inline -void fromMsg(const geometry_msgs::msg::TransformStamped& in, tf2::Stamped & out) +template<> +inline void doTransform( + const geometry_msgs::msg::Quaternion & t_in, geometry_msgs::msg::Quaternion & t_out, + const geometry_msgs::msg::TransformStamped & transform) { - 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); + impl::doTransformWithTf(t_in, t_out, transform); } -/** \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. +/** + * \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. + * \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. */ -inline -geometry_msgs::msg::TransformStamped toMsg(const tf2::Stamped& in) +template<> +inline void doTransform( + const geometry_msgs::msg::QuaternionStamped & t_in, geometry_msgs::msg::QuaternionStamped & t_out, + const geometry_msgs::msg::TransformStamped & transform) { - 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; + 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; } -/**********/ -/** 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 ) -{ - out.position.x = in.getOrigin().getX(); - out.position.y = in.getOrigin().getY(); - out.position.z = in.getOrigin().getZ(); - out.orientation = toMsg(in.getRotation()); -} +/***************/ +/** Transform **/ +/***************/ -/** \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. +/** + * \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. + * \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. */ -inline -void fromMsg(const geometry_msgs::msg::Pose& in, tf2::Transform& out) +template<> +inline void doTransform( + const geometry_msgs::msg::TransformStamped & t_in, geometry_msgs::msg::TransformStamped & t_out, + const geometry_msgs::msg::TransformStamped & transform) { - 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)); + 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 tf2 -} // namespace -#endif // TF2_GEOMETRY_MSGS_HPP +#endif // TF2_GEOMETRY_MSGS__TF2_GEOMETRY_MSGS_HPP_ diff --git a/tf2_geometry_msgs/package.xml b/tf2_geometry_msgs/package.xml index b104189e9..34643272b 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_geometry_msgs/test/test_tf2_geometry_msgs.cpp b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp index 320560d2e..a3ae69ef6 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; @@ -105,6 +108,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) @@ -121,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); @@ -132,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); @@ -175,7 +226,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); @@ -187,8 +240,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); @@ -210,14 +264,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); @@ -234,14 +290,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); @@ -281,7 +340,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); diff --git a/tf2_kdl/CMakeLists.txt b/tf2_kdl/CMakeLists.txt index c9934979a..1a727d5e7 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 @@ -31,9 +31,16 @@ 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) Remove once headers + # are renamed to .hpp + 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 include diff --git a/tf2_kdl/include/tf2_kdl/tf2_kdl.hpp b/tf2_kdl/include/tf2_kdl/tf2_kdl.hpp index b183b564b..1419f4877 100644 --- a/tf2_kdl/include/tf2_kdl/tf2_kdl.hpp +++ b/tf2_kdl/include/tf2_kdl/tf2_kdl.hpp @@ -27,285 +27,350 @@ // POSSIBILITY OF SUCH DAMAGE. -/** \author Wim Meeussen */ +/** \author Wim Meeussen, Bjarne von Horn */ -#ifndef TF2_KDL_HPP -#define TF2_KDL_HPP +#ifndef TF2_KDL__TF2_KDL_HPP_ +#define TF2_KDL__TF2_KDL_HPP_ #include -#include -#include +#include + #include -#include -#include #include #include -#include #include +#include +#include +#include + +namespace Eigen +{ +/// 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. +/** + * \brief Convert a timestamped transform to the equivalent KDL data type. + * \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) - { - 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)); - } +[[deprecated("Please use tf2::fromMsg()")]] 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. +/** + * \brief Convert an KDL Frame to the equivalent geometry_msgs message type. + * \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 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; } +namespace impl +{ // --------------------- // 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, 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) + +/// \brief Conversion implementation for geometry_msgs::msg::Vector3 and KDL::Vector. +template<> +struct ConversionImplementation + : DefaultVectorConversionImplementation { - 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; -} +}; -/** \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 Conversion implementation for geometry_msgs::msg::Point and KDL::Vector. +template<> +struct ConversionImplementation + : DefaultVectorConversionImplementation { - 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; -} +}; + +/// \brief Default return type of tf2::toMsg() for KDL::Vector. +template<> +struct DefaultMessageForDatatype +{ + /// \brief Default return type of tf2::toMsg() for KDL::Vector. + using type = geometry_msgs::msg::Point; +}; + +/// \brief Default Type for automatic tf2::doTransform() implementation for KDL::Frame. +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) + +/// \brief Conversion implementation for geometry_msgs::msg::Twist and KDL::Twist. +template<> +struct ConversionImplementation +{ + /** + * \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) { - t_out = tf2::Stamped(transformToKDL(transform) * t_in, tf2_ros::fromMsg(transform.header.stamp), transform.header.frame_id); + tf2::toMsg<>(in.vel, msg.linear); + tf2::toMsg<>(in.rot, msg.angular); } -/** \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) + /** + * \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) + { + tf2::fromMsg<>(msg.linear, out.vel); + tf2::fromMsg<>(msg.angular, out.rot); + } +}; + +/// \brief Default Type for automatic tf2::doTransform() implementation for KDL::Twist. +template<> +struct DefaultTransformType { - 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 Default Type for automatic tf2::doTransform() implementation for KDL::Twist. + using type = KDL::Frame; +}; -/** \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) +/// \brief Default return type of tf2::toMsg() for KDL::Twist. +template<> +struct DefaultMessageForDatatype { - 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 Default return type of tf2::toMsg() for KDL::Twist. + using type = geometry_msgs::msg::Twist; +}; // --------------------- // 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) + +/// \brief Conversion implementation for geometry_msgs::msg::Wrench and KDL::Wrench. +template<> +struct ConversionImplementation +{ + /** + * \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) { - t_out = tf2::Stamped(transformToKDL(transform) * t_in, tf2_ros::fromMsg(transform.header.stamp), transform.header.frame_id); + tf2::toMsg<>(in.force, msg.force); + tf2::toMsg<>(in.torque, msg.torque); } -/** \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) -{ - 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 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) + { + tf2::fromMsg<>(msg.force, out.force); + tf2::fromMsg<>(msg.torque, out.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. - */ -inline -void fromMsg(const geometry_msgs::msg::WrenchStamped& msg, tf2::Stamped& out) +/// \brief Default return type of tf2::toMsg() for KDL::Wrench. +template<> +struct DefaultMessageForDatatype { - 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 Default return type of tf2::toMsg() for KDL::Wrench. + using type = geometry_msgs::msg::Wrench; +}; +/// \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; +}; // --------------------- // 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) + +/// \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) { - t_out = tf2::Stamped(transformToKDL(transform) * t_in, tf2_ros::fromMsg(transform.header.stamp), transform.header.frame_id); + KDL::Rotation r; + KDL::Vector v; + tf2::fromMsg<>(in.translation, v); + tf2::fromMsg<>(in.rotation, r); + out = KDL::Frame(r, v); } -/** \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) + /** + * \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 { - 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; -} + /** + * \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) + { + 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. + * \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) + { + tf2::fromMsg<>(msg.position, out.p); + tf2::fromMsg<>(msg.orientation, out.M); + } +}; + +/// \brief Default return type of tf2::toMsg() for KDL::Frame. +template<> +struct DefaultMessageForDatatype { - out.p[0] = msg.position.x; - out.p[1] = msg.position.y; - out.p[2] = msg.position.z; - out.M = KDL::Rotation::Quaternion(msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w); -} + /// \brief Default return type of tf2::toMsg() for KDL::Frame. + 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) +/// \brief Default Type for automatic tf2::doTransform() implementation for KDL::Frame. +template<> +struct DefaultTransformType { - 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 Default Type for automatic tf2::doTransform() implementation for KDL::Frame. + using type = KDL::Frame; +}; + + +// --------------------- +// Rotation +// --------------------- + +/// \brief Conversion implementation for geometry_msgs::msg::Quaternion and KDL::Rotation. +template<> +struct ConversionImplementation +{ + /** + * \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); + } +}; + +/// \brief Default return type of tf2::toMsg() for KDL::Rotation. +template<> +struct DefaultMessageForDatatype +{ + /// \brief Default return type of tf2::toMsg() for KDL::Rotation. + 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 -/** \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. +/** + * \brief Transform a KDL::Rotation + * + * \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 fromMsg(const geometry_msgs::msg::PoseStamped& msg, tf2::Stamped& out) +void doTransform( + const KDL::Rotation & in, KDL::Rotation & out, + const geometry_msgs::msg::TransformStamped & transform) { - out.stamp_ = tf2_ros::fromMsg(msg.header.stamp); - out.frame_id_ = msg.header.frame_id; - fromMsg(msg.pose, static_cast(out)); + KDL::Rotation t; + tf2::fromMsg<>(transform.transform.rotation, t); + out = t * in; } -} // namespace +} // namespace tf2 -#endif // TF2_KDL_HPP +#endif // TF2_KDL__TF2_KDL_HPP_ diff --git a/tf2_kdl/package.xml b/tf2_kdl/package.xml index 729ff6e7b..5aa6c4a66 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 diff --git a/tf2_kdl/test/test_tf2_kdl.cpp b/tf2_kdl/test/test_tf2_kdl.cpp index 33d083a8e..3a42360cf 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,19 +98,46 @@ 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); } +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( - 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; @@ -111,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); @@ -130,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 @@ -141,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); @@ -159,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 @@ -170,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); @@ -188,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 @@ -199,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); @@ -214,11 +250,13 @@ 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); tf_buffer = std::make_unique(clock); + tf_buffer->setUsingDedicatedThread(true); // populate buffer geometry_msgs::msg::TransformStamped t; 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.hpp b/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.hpp index de64c4008..ca06b558e 100644 --- a/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.hpp +++ b/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.hpp @@ -51,7 +51,7 @@ template<> inline tf2::TimePoint getTimestamp(const sensor_msgs::msg::PointCloud2 & p) { - return tf2_ros::fromMsg(p.header.stamp); + return tf2::TimePointFromMsg(p.header.stamp); } // method to extract frame id from object @@ -100,17 +100,6 @@ void doTransform( *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 tf2 #endif // TF2_SENSOR_MSGS__TF2_SENSOR_MSGS_HPP_