From b5ebd16d6dedef0bbd2936c39cd3581b91809b52 Mon Sep 17 00:00:00 2001 From: Khasreto Date: Wed, 15 Sep 2021 16:36:30 +0200 Subject: [PATCH] doTransform non stamped msgs (#452) * Add `doTransform` methods for non-stamped messages Co-authored-by: adamkrawczyk Co-authored-by: Abrar Rahman Protyasha Signed-off-by: Abrar Rahman Protyasha --- .../tf2_geometry_msgs/tf2_geometry_msgs.hpp | 227 ++++++- .../test/test_tf2_geometry_msgs.cpp | 557 +++++++++++++----- 2 files changed, 601 insertions(+), 183 deletions(-) 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..8c99b11a3 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 @@ -69,6 +69,22 @@ KDL::Frame gmTransformToKDL(const geometry_msgs::msg::TransformStamped& t) /** Vector3 **/ /*************/ +/** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Vector type. + * This function is a specialization of the doTransform template defined in tf2/convert.h. + * \param t_in The vector to transform, as a Vector3 message. + * \param t_out The transformed vector, as a Vector3 message. + * \param transform The timestamped transform to apply, as a TransformStamped message. + */ +template <> +inline + void doTransform(const geometry_msgs::msg::Vector3& t_in, geometry_msgs::msg::Vector3& t_out, const geometry_msgs::msg::TransformStamped& transform) + { + KDL::Vector v_out = gmTransformToKDL(transform).M * KDL::Vector(t_in.x, t_in.y, t_in.z); + t_out.x = v_out[0]; + t_out.y = v_out[1]; + t_out.z = v_out[2]; + } + /** \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. @@ -162,6 +178,22 @@ void fromMsg(const geometry_msgs::msg::Vector3Stamped& msg, geometry_msgs::msg:: /** Point **/ /***********/ +/** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Point type. + * This function is a specialization of the doTransform template defined in tf2/convert.h. + * \param t_in The point to transform, as a Point3 message. + * \param t_out The transformed point, as a Point3 message. + * \param transform The timestamped transform to apply, as a TransformStamped message. + */ +template <> +inline + void doTransform(const geometry_msgs::msg::Point& t_in, geometry_msgs::msg::Point& t_out, const geometry_msgs::msg::TransformStamped& transform) + { + KDL::Vector v_out = gmTransformToKDL(transform) * KDL::Vector(t_in.x, t_in.y, t_in.z); + t_out.x = v_out[0]; + t_out.y = v_out[1]; + t_out.z = v_out[2]; + } + /** \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. @@ -318,36 +350,18 @@ void fromMsg(const geometry_msgs::msg::PoseStamped& msg, geometry_msgs::msg::Pos } -/*******************************/ -/** PoseWithCovarianceStamped **/ -/*******************************/ +/************************/ +/** PoseWithCovariance **/ +/************************/ -/** \brief Extract a timestamp from the header of a Pose message. - * This function is a specialization of the getTimestamp template defined in tf2/convert.h. - * \param t PoseWithCovarianceStamped message to extract the timestamp from. - * \return The timestamp of the message. - */ -template <> -inline - tf2::TimePoint getTimestamp(const geometry_msgs::msg::PoseWithCovarianceStamped& t) {return tf2_ros::fromMsg(t.header.stamp);} - -/** \brief Extract a frame ID from the header of a Pose message. - * This function is a specialization of the getFrameId template defined in tf2/convert.h. - * \param t PoseWithCovarianceStamped message to extract the frame ID from. - * \return A string containing the frame ID of the message. - */ -template <> -inline - std::string getFrameId(const geometry_msgs::msg::PoseWithCovarianceStamped& t) {return t.header.frame_id;} - -/** \brief Extract a covariance matrix from a PoseWithCovarianceStamped message. +/** \brief Extract a covariance matrix from a PoseWithCovariance 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. + * \param t PoseWithCovariance 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);} + std::array, 6> getCovarianceMatrix(const geometry_msgs::msg::PoseWithCovariance& t) {return covarianceRowMajorToNested(t.covariance);} /** \brief Transform the covariance matrix of a PoseWithCovariance message to a new frame. * \param cov_in The covariance matrix to transform. @@ -441,13 +455,91 @@ geometry_msgs::msg::PoseWithCovariance::_covariance_type transformCovariance( 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 Pose3 message with covariance. + * \param t_out The transformed pose, as a Pose3 message with covariance. + * \param transform The timestamped transform to apply, as a TransformStamped message. + */ +template <> +inline + void doTransform(const geometry_msgs::msg::PoseWithCovariance& t_in, geometry_msgs::msg::PoseWithCovariance& t_out, const geometry_msgs::msg::TransformStamped& transform) + { + KDL::Vector v(t_in.pose.position.x, t_in.pose.position.y, t_in.pose.position.z); + KDL::Rotation r = KDL::Rotation::Quaternion(t_in.pose.orientation.x, t_in.pose.orientation.y, t_in.pose.orientation.z, t_in.pose.orientation.w); + + KDL::Frame v_out = gmTransformToKDL(transform) * KDL::Frame(r, v); + t_out.pose.position.x = v_out.p[0]; + t_out.pose.position.y = v_out.p[1]; + t_out.pose.position.z = v_out.p[2]; + v_out.M.GetQuaternion(t_out.pose.orientation.x, t_out.pose.orientation.y, t_out.pose.orientation.z, t_out.pose.orientation.w); + + tf2::Transform tf_transform; + fromMsg(transform.transform, tf_transform); + t_out.covariance = transformCovariance(t_in.covariance, tf_transform); + } + +/** \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 PoseWithCovariance message. + * \return The input argument. + */ +inline +geometry_msgs::msg::PoseWithCovariance toMsg(const geometry_msgs::msg::PoseWithCovariance& 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 PoseWithCovariance message. + * \param out The input argument. + */ +inline +void fromMsg(const geometry_msgs::msg::PoseWithCovariance& msg, geometry_msgs::msg::PoseWithCovariance& out) +{ + out = msg; +} + + +/*******************************/ +/** PoseWithCovarianceStamped **/ +/*******************************/ + +/** \brief Extract a timestamp from the header of a Pose message. + * This function is a specialization of the getTimestamp template defined in tf2/convert.h. + * \param t PoseWithCovarianceStamped message to extract the timestamp from. + * \return The timestamp of the message. + */ +template <> +inline + tf2::TimePoint getTimestamp(const geometry_msgs::msg::PoseWithCovarianceStamped& t) {return tf2_ros::fromMsg(t.header.stamp);} + +/** \brief Extract a frame ID from the header of a Pose message. + * This function is a specialization of the getFrameId template defined in tf2/convert.h. + * \param t PoseWithCovarianceStamped message to extract the frame ID from. + * \return A string containing the frame ID of the message. + */ +template <> +inline + std::string getFrameId(const geometry_msgs::msg::PoseWithCovarianceStamped& t) {return t.header.frame_id;} + +/** \brief Extract a covariance matrix from a PoseWithCovarianceStamped message. + * This function is a specialization of the getCovarianceMatrix template defined in tf2/convert.h. + * \param t PoseWithCovarianceStamped message to extract the covariance matrix from. + * \return A nested-array representation of the covariance matrix from the message. + */ +template <> +inline + std::array, 6> getCovarianceMatrix(const geometry_msgs::msg::PoseWithCovarianceStamped& t) {return covarianceRowMajorToNested(t.pose.covariance);} + /** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Pose type. * This function is a specialization of the doTransform template defined in tf2/convert.h. * \param t_in The pose to transform, as a timestamped Pose3 message with covariance. @@ -536,10 +628,30 @@ void fromMsg(const geometry_msgs::msg::PoseWithCovarianceStamped& in, tf2::WithC out.setData(tmp); } + /****************/ /** Quaternion **/ /****************/ +// Forward declaration +geometry_msgs::msg::Quaternion toMsg(const tf2::Quaternion& in); + +/** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Quaternion type. + * This function is a specialization of the doTransform template defined in tf2/convert.h. + * \param t_in The quaternion to transform, as a Quaternion3 message. + * \param t_out The transformed quaternion, as a Quaternion3 message. + * \param transform The timestamped transform to apply, as a TransformStamped message. + */ +template <> +inline +void doTransform(const geometry_msgs::msg::Quaternion& t_in, geometry_msgs::msg::Quaternion& t_out, const geometry_msgs::msg::TransformStamped& transform) +{ + 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.x, t_in.y, t_in.z, t_in.w); + t_out = toMsg(q_out); +} + /** \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. @@ -698,6 +810,29 @@ void fromMsg(const geometry_msgs::msg::Transform& in, tf2::Transform& out) out.setRotation(tf2::Quaternion(in.rotation.x, in.rotation.y, in.rotation.z, in.rotation.w)); } +/** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Transform type. + * This function is a specialization of the doTransform template defined in tf2/convert.h. + * \param t_in The frame to transform, as a Transform3 message. + * \param t_out The frame transform, as a Transform3 message. + * \param transform The timestamped transform to apply, as a TransformStamped message. + */ +template <> +inline +void doTransform(const geometry_msgs::msg::Transform& t_in, geometry_msgs::msg::Transform& t_out, const geometry_msgs::msg::TransformStamped& transform) + { + KDL::Vector v(t_in.translation.x, t_in.translation.y, + t_in.translation.z); + KDL::Rotation r = KDL::Rotation::Quaternion(t_in.rotation.x, + t_in.rotation.y, t_in.rotation.z, t_in.rotation.w); + + KDL::Frame v_out = gmTransformToKDL(transform) * KDL::Frame(r, v); + t_out.translation.x = v_out.p[0]; + t_out.translation.y = v_out.p[1]; + t_out.translation.z = v_out.p[2]; + v_out.M.GetQuaternion(t_out.rotation.x, t_out.rotation.y, + t_out.rotation.z, t_out.rotation.w); + } + /**********************/ /** TransformStamped **/ @@ -805,6 +940,48 @@ geometry_msgs::msg::TransformStamped toMsg(const tf2::Stamped& i /** 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 Pose3 message. + * \param t_out The transformed pose, as a Pose3 message. + * \param transform The timestamped transform to apply, as a TransformStamped message. + */ +template <> +inline + void doTransform(const geometry_msgs::msg::Pose& t_in, geometry_msgs::msg::Pose& t_out, const geometry_msgs::msg::TransformStamped& transform) + { + KDL::Vector v(t_in.position.x, t_in.position.y, t_in.position.z); + KDL::Rotation r = KDL::Rotation::Quaternion(t_in.orientation.x, t_in.orientation.y, t_in.orientation.z, t_in.orientation.w); + + KDL::Frame v_out = gmTransformToKDL(transform) * KDL::Frame(r, v); + t_out.position.x = v_out.p[0]; + t_out.position.y = v_out.p[1]; + t_out.position.z = v_out.p[2]; + v_out.M.GetQuaternion(t_out.orientation.x, t_out.orientation.y, t_out.orientation.z, t_out.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 in A Pose message. + * \return The input argument. + */ +inline +geometry_msgs::msg::Pose toMsg(const geometry_msgs::msg::Pose& 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 Pose message. + * \param out The input argument. + */ +inline +void fromMsg(const geometry_msgs::msg::Pose& msg, geometry_msgs::msg::Pose& out) +{ + out = msg; +} + /** \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. diff --git a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp index 320560d2e..a3b8dc6ae 100644 --- a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp +++ b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp @@ -47,8 +47,44 @@ std::unique_ptr tf_buffer = nullptr; static const double EPS = 1e-3; +geometry_msgs::msg::TransformStamped generate_stamped_transform() { + geometry_msgs::msg::TransformStamped t; + t.transform.translation.x = 10; + t.transform.translation.y = 20; + t.transform.translation.z = 30; + t.transform.rotation.w = 0; + t.transform.rotation.x = 1; + t.transform.rotation.y = 0; + t.transform.rotation.z = 0; + t.header.stamp = tf2_ros::toMsg(tf2::timeFromSec(2)); + t.header.frame_id = "A"; + t.child_frame_id = "B"; + return t; +} + TEST(TfGeometry, Conversions) { + // Quaternion + { + auto rotation = tf2::Quaternion(1.0, 2.0, 3.0, 4.0).normalized(); + tf2::Quaternion quat(rotation); + geometry_msgs::msg::Quaternion quat_msg; + tf2::convert(quat, quat_msg); + + EXPECT_NEAR(rotation.getX(), quat_msg.x, EPS); + EXPECT_NEAR(rotation.getY(), quat_msg.y, EPS); + EXPECT_NEAR(rotation.getZ(), quat_msg.z, EPS); + EXPECT_NEAR(rotation.getW(), quat_msg.w, EPS); + + tf2::Quaternion quat_from_msg; + tf2::convert(quat_msg, quat_from_msg); + + EXPECT_NEAR(quat_from_msg.getX(), quat_msg.x, EPS); + EXPECT_NEAR(quat_from_msg.getY(), quat_msg.y, EPS); + EXPECT_NEAR(quat_from_msg.getZ(), quat_msg.z, EPS); + EXPECT_NEAR(quat_from_msg.getW(), quat_msg.w, EPS); + } + // QuaternionStamped { auto rotation = tf2::Quaternion(1.0, 2.0, 3.0, 4.0).normalized(); @@ -74,6 +110,34 @@ TEST(TfGeometry, Conversions) EXPECT_EQ(quat_from_msg.frame_id_, quat_stamped_msg.header.frame_id); } + // Transform + { + auto rotation = tf2::Quaternion(1.0, 2.0, 3.0, 4.0).normalized(); + tf2::Vector3 translation(1.0, 2.0, 3.0); + tf2::Transform tf_(tf2::Transform(rotation, translation)); + geometry_msgs::msg::Transform tf_msg; + tf2::convert(tf_, tf_msg); + + EXPECT_NEAR(rotation.getX(), tf_msg.rotation.x, EPS); + EXPECT_NEAR(rotation.getY(), tf_msg.rotation.y, EPS); + EXPECT_NEAR(rotation.getZ(), tf_msg.rotation.z, EPS); + EXPECT_NEAR(rotation.getW(), tf_msg.rotation.w, EPS); + EXPECT_NEAR(translation.getX(), tf_msg.translation.x, EPS); + EXPECT_NEAR(translation.getY(), tf_msg.translation.y, EPS); + EXPECT_NEAR(translation.getZ(), tf_msg.translation.z, EPS); + + tf2::Transform tf_from_msg; + tf2::convert(tf_msg, tf_from_msg); + + EXPECT_NEAR(tf_from_msg.getRotation().getX(), tf_msg.rotation.x, EPS); + EXPECT_NEAR(tf_from_msg.getRotation().getY(), tf_msg.rotation.y, EPS); + EXPECT_NEAR(tf_from_msg.getRotation().getZ(), tf_msg.rotation.z, EPS); + EXPECT_NEAR(tf_from_msg.getRotation().getW(), tf_msg.rotation.w, EPS); + EXPECT_NEAR(tf_from_msg.getOrigin().getX(), tf_msg.translation.x, EPS); + EXPECT_NEAR(tf_from_msg.getOrigin().getY(), tf_msg.translation.y, EPS); + EXPECT_NEAR(tf_from_msg.getOrigin().getZ(), tf_msg.translation.z, EPS); + } + // TransformStamped { auto rotation = tf2::Quaternion(1.0, 2.0, 3.0, 4.0).normalized(); @@ -109,142 +173,247 @@ TEST(TfGeometry, Conversions) TEST(TfGeometry, Frame) { - geometry_msgs::msg::PoseStamped v1; - v1.pose.position.x = 1; - v1.pose.position.y = 2; - v1.pose.position.z = 3; - v1.pose.orientation.w = 0; - v1.pose.orientation.x = 1; - v1.pose.orientation.y = 0; - v1.pose.orientation.z = 0; - v1.header.stamp = tf2_ros::toMsg(tf2::timeFromSec(2)); - v1.header.frame_id = "A"; - - // simple api - 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); - EXPECT_NEAR(v_simple.pose.orientation.x, 0.0, EPS); - EXPECT_NEAR(v_simple.pose.orientation.y, 0.0, EPS); - EXPECT_NEAR(v_simple.pose.orientation.z, 0.0, EPS); - EXPECT_NEAR(v_simple.pose.orientation.w, 1.0, EPS); - - - // advanced api - 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); - EXPECT_NEAR(v_advanced.pose.orientation.x, 0.0, EPS); - EXPECT_NEAR(v_advanced.pose.orientation.y, 0.0, EPS); - EXPECT_NEAR(v_advanced.pose.orientation.z, 0.0, EPS); - EXPECT_NEAR(v_advanced.pose.orientation.w, 1.0, EPS); + // non-stamped + { + geometry_msgs::msg::Pose v1, res; + v1.position.x = 1; + v1.position.y = 2; + v1.position.z = 3; + v1.orientation.w = 0; + v1.orientation.x = 1; + v1.orientation.y = 0; + v1.orientation.z = 0; + + geometry_msgs::msg::TransformStamped t = generate_stamped_transform(); + + tf2::doTransform(v1, res, t); + EXPECT_NEAR(res.position.x, 11, EPS); + EXPECT_NEAR(res.position.y, 18, EPS); + EXPECT_NEAR(res.position.z, 27, EPS); + EXPECT_NEAR(res.orientation.x, 0.0, EPS); + EXPECT_NEAR(res.orientation.y, 0.0, EPS); + EXPECT_NEAR(res.orientation.z, 0.0, EPS); + EXPECT_NEAR(res.orientation.w, 1.0, EPS); + } + + // stamped + { + geometry_msgs::msg::PoseStamped v1; + v1.pose.position.x = 1; + v1.pose.position.y = 2; + v1.pose.position.z = 3; + v1.pose.orientation.w = 0; + v1.pose.orientation.x = 1; + v1.pose.orientation.y = 0; + v1.pose.orientation.z = 0; + v1.header.stamp = tf2_ros::toMsg(tf2::timeFromSec(2)); + v1.header.frame_id = "A"; + + // simple api + 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); + EXPECT_NEAR(v_simple.pose.orientation.x, 0.0, EPS); + EXPECT_NEAR(v_simple.pose.orientation.y, 0.0, EPS); + EXPECT_NEAR(v_simple.pose.orientation.z, 0.0, EPS); + EXPECT_NEAR(v_simple.pose.orientation.w, 1.0, EPS); + + + // advanced api + 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); + EXPECT_NEAR(v_advanced.pose.orientation.x, 0.0, EPS); + EXPECT_NEAR(v_advanced.pose.orientation.y, 0.0, EPS); + EXPECT_NEAR(v_advanced.pose.orientation.z, 0.0, EPS); + EXPECT_NEAR(v_advanced.pose.orientation.w, 1.0, EPS); + } } TEST(TfGeometry, FrameWithCovariance) { - 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 - }; - - geometry_msgs::msg::PoseWithCovariance::_covariance_type v1_expected_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 - }; - - // simple api - 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); - EXPECT_NEAR(v_simple.pose.pose.orientation.x, 0.0, EPS); - EXPECT_NEAR(v_simple.pose.pose.orientation.y, 0.0, EPS); - EXPECT_NEAR(v_simple.pose.pose.orientation.z, 0.0, EPS); - EXPECT_NEAR(v_simple.pose.pose.orientation.w, 1.0, EPS); - EXPECT_EQ(v_simple.pose.covariance, v1_expected_covariance); - - - // advanced api - 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); - EXPECT_NEAR(v_advanced.pose.pose.orientation.x, 0.0, EPS); - EXPECT_NEAR(v_advanced.pose.pose.orientation.y, 0.0, EPS); - EXPECT_NEAR(v_advanced.pose.pose.orientation.z, 0.0, EPS); - EXPECT_NEAR(v_advanced.pose.pose.orientation.w, 1.0, EPS); - EXPECT_EQ(v_advanced.pose.covariance, v1_expected_covariance); -} + // non-stamped + { + geometry_msgs::msg::PoseWithCovariance v1, res; + v1.pose.position.x = 1; + v1.pose.position.y = 2; + v1.pose.position.z = 3; + v1.pose.orientation.w = 0; + v1.pose.orientation.x = 1; + v1.pose.orientation.y = 0; + v1.pose.orientation.z = 0; + v1.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 + }; + + geometry_msgs::msg::PoseWithCovariance::_covariance_type v1_expected_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 + }; + + geometry_msgs::msg::TransformStamped t = generate_stamped_transform(); + + tf2::doTransform(v1, res, t); + EXPECT_NEAR(res.pose.position.x, 11, EPS); + EXPECT_NEAR(res.pose.position.y, 18, EPS); + EXPECT_NEAR(res.pose.position.z, 27, EPS); + EXPECT_NEAR(res.pose.orientation.x, 0.0, EPS); + EXPECT_NEAR(res.pose.orientation.y, 0.0, EPS); + EXPECT_NEAR(res.pose.orientation.z, 0.0, EPS); + EXPECT_NEAR(res.pose.orientation.w, 1.0, EPS); + EXPECT_EQ(res.covariance, v1_expected_covariance); + } + // stamped + { + 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 + }; + + geometry_msgs::msg::PoseWithCovariance::_covariance_type v1_expected_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 + }; + + // simple api + 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); + EXPECT_NEAR(v_simple.pose.pose.orientation.x, 0.0, EPS); + EXPECT_NEAR(v_simple.pose.pose.orientation.y, 0.0, EPS); + EXPECT_NEAR(v_simple.pose.pose.orientation.z, 0.0, EPS); + EXPECT_NEAR(v_simple.pose.pose.orientation.w, 1.0, EPS); + EXPECT_EQ(v_simple.pose.covariance, v1_expected_covariance); + + + // advanced api + 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); + EXPECT_NEAR(v_advanced.pose.pose.orientation.x, 0.0, EPS); + EXPECT_NEAR(v_advanced.pose.pose.orientation.y, 0.0, EPS); + EXPECT_NEAR(v_advanced.pose.pose.orientation.z, 0.0, EPS); + EXPECT_NEAR(v_advanced.pose.pose.orientation.w, 1.0, EPS); + EXPECT_EQ(v_advanced.pose.covariance, v1_expected_covariance); + } +} TEST(TfGeometry, Vector) { - geometry_msgs::msg::Vector3Stamped v1, res; - v1.vector.x = 1; - v1.vector.y = 2; - v1.vector.z = 3; - v1.header.stamp = tf2_ros::toMsg(tf2::timeFromSec(2)); - v1.header.frame_id = "A"; - - // simple api - 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)); - EXPECT_NEAR(v_advanced.vector.x, 1, EPS); - EXPECT_NEAR(v_advanced.vector.y, -2, EPS); - EXPECT_NEAR(v_advanced.vector.z, -3, EPS); + // non-stamped + { + geometry_msgs::msg::Vector3 v1, res; + v1.x = 1; + v1.y = 2; + v1.z = 3; + + geometry_msgs::msg::TransformStamped t = generate_stamped_transform(); + + tf2::doTransform(v1, res, t); + EXPECT_NEAR(res.x, 1, EPS); + EXPECT_NEAR(res.y, -2, EPS); + EXPECT_NEAR(res.z, -3, EPS); + } + + // stamped + { + geometry_msgs::msg::Vector3Stamped v1, res; + v1.vector.x = 1; + v1.vector.y = 2; + v1.vector.z = 3; + v1.header.stamp = tf2_ros::toMsg(tf2::timeFromSec(2)); + v1.header.frame_id = "A"; + + // simple api + 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)); + EXPECT_NEAR(v_advanced.vector.x, 1, EPS); + EXPECT_NEAR(v_advanced.vector.y, -2, EPS); + EXPECT_NEAR(v_advanced.vector.z, -3, EPS); + } } TEST(TfGeometry, Point) { - geometry_msgs::msg::PointStamped v1, res; - v1.point.x = 1; - v1.point.y = 2; - v1.point.z = 3; - v1.header.stamp = tf2_ros::toMsg(tf2::timeFromSec(2)); - v1.header.frame_id = "A"; - - // simple api - 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)); - EXPECT_NEAR(v_advanced.point.x, -9, EPS); - EXPECT_NEAR(v_advanced.point.y, 18, EPS); - EXPECT_NEAR(v_advanced.point.z, 27, EPS); + // non-stamped + { + geometry_msgs::msg::Point v1, res; + v1.x = 1; + v1.y = 2; + v1.z = 3; + + geometry_msgs::msg::TransformStamped t = generate_stamped_transform(); + + tf2::doTransform(v1, res, t); + EXPECT_NEAR(res.x, 11, EPS); + EXPECT_NEAR(res.y, 18, EPS); + EXPECT_NEAR(res.z, 27, EPS); + } + + // stamped + { + geometry_msgs::msg::PointStamped v1, res; + v1.point.x = 1; + v1.point.y = 2; + v1.point.z = 3; + v1.header.stamp = tf2_ros::toMsg(tf2::timeFromSec(2)); + v1.header.frame_id = "A"; + + // simple api + 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)); + EXPECT_NEAR(v_advanced.point.x, -9, EPS); + EXPECT_NEAR(v_advanced.point.y, 18, EPS); + EXPECT_NEAR(v_advanced.point.z, 27, EPS); + } } TEST(TfGeometry, Quaternion) @@ -253,31 +422,113 @@ TEST(TfGeometry, Quaternion) // 0, 0, -1 // 0, 1, 0, // 1, 0, 0 - geometry_msgs::msg::QuaternionStamped q1, res; - q1.quaternion.x = 0; - q1.quaternion.y = -1 * M_SQRT1_2; - q1.quaternion.z = 0; - q1.quaternion.w = M_SQRT1_2; - q1.header.stamp = tf2_ros::toMsg(tf2::timeFromSec(2)); - q1.header.frame_id = "A"; - - // simple api - const geometry_msgs::msg::QuaternionStamped q_simple = tf_buffer->transform( - q1, "B", tf2::durationFromSec( - 2.0)); - EXPECT_NEAR(q_simple.quaternion.x, M_SQRT1_2, EPS); - EXPECT_NEAR(q_simple.quaternion.y, 0, EPS); - EXPECT_NEAR(q_simple.quaternion.z, -1 * M_SQRT1_2, EPS); - EXPECT_NEAR(q_simple.quaternion.w, 0, EPS); - - // advanced api - const geometry_msgs::msg::QuaternionStamped q_advanced = tf_buffer->transform( - q1, "B", tf2::timeFromSec(2.0), - "A", tf2::durationFromSec(3.0)); - EXPECT_NEAR(q_advanced.quaternion.x, M_SQRT1_2, EPS); - EXPECT_NEAR(q_advanced.quaternion.y, 0, EPS); - EXPECT_NEAR(q_advanced.quaternion.z, -1 * M_SQRT1_2, EPS); - EXPECT_NEAR(q_advanced.quaternion.w, 0, EPS); + + // non-stamped + { + geometry_msgs::msg::Quaternion q1, res; + q1.x = 0; + q1.y = -1 * M_SQRT1_2; + q1.z = 0; + q1.w = M_SQRT1_2; + + geometry_msgs::msg::TransformStamped t = generate_stamped_transform(); + + tf2::doTransform(q1, res, t); + EXPECT_NEAR(res.x, M_SQRT1_2, EPS); + EXPECT_NEAR(res.y, 0, EPS); + EXPECT_NEAR(res.z, -1 * M_SQRT1_2, EPS); + EXPECT_NEAR(res.w, 0, EPS); + } + + // stamped + { + geometry_msgs::msg::QuaternionStamped q1, res; + q1.quaternion.x = 0; + q1.quaternion.y = -1 * M_SQRT1_2; + q1.quaternion.z = 0; + q1.quaternion.w = M_SQRT1_2; + q1.header.stamp = tf2_ros::toMsg(tf2::timeFromSec(2)); + q1.header.frame_id = "A"; + + // simple api + const geometry_msgs::msg::QuaternionStamped q_simple = tf_buffer->transform( + q1, "B", tf2::durationFromSec( + 2.0)); + EXPECT_NEAR(q_simple.quaternion.x, M_SQRT1_2, EPS); + EXPECT_NEAR(q_simple.quaternion.y, 0, EPS); + EXPECT_NEAR(q_simple.quaternion.z, -1 * M_SQRT1_2, EPS); + EXPECT_NEAR(q_simple.quaternion.w, 0, EPS); + + // advanced api + const geometry_msgs::msg::QuaternionStamped q_advanced = tf_buffer->transform( + q1, "B", tf2::timeFromSec(2.0), + "A", tf2::durationFromSec(3.0)); + EXPECT_NEAR(q_advanced.quaternion.x, M_SQRT1_2, EPS); + EXPECT_NEAR(q_advanced.quaternion.y, 0, EPS); + EXPECT_NEAR(q_advanced.quaternion.z, -1 * M_SQRT1_2, EPS); + EXPECT_NEAR(q_advanced.quaternion.w, 0, EPS); + } +} + +TEST(TfGeometry, Transform) +{ + // non-stamped + { + geometry_msgs::msg::Transform v1, res; + v1.translation.x = 1; + v1.translation.y = 2; + v1.translation.z = 3; + v1.rotation.w = 0; + v1.rotation.x = 1; + v1.rotation.y = 0; + v1.rotation.z = 0; + + geometry_msgs::msg::TransformStamped t = generate_stamped_transform(); + + tf2::doTransform(v1, res, t); + EXPECT_NEAR(res.translation.x, 11, EPS); + EXPECT_NEAR(res.translation.y, 18, EPS); + EXPECT_NEAR(res.translation.z, 27, EPS); + EXPECT_NEAR(res.rotation.x, 0.0, EPS); + EXPECT_NEAR(res.rotation.y, 0.0, EPS); + EXPECT_NEAR(res.rotation.z, 0.0, EPS); + EXPECT_NEAR(res.rotation.w, 1.0, EPS); + } + + // stamped + { + geometry_msgs::msg::TransformStamped v1; + v1.transform.translation.x = 1; + v1.transform.translation.y = 2; + v1.transform.translation.z = 3; + v1.transform.rotation.w = 0; + v1.transform.rotation.x = 1; + v1.transform.rotation.y = 0; + v1.transform.rotation.z = 0; + v1.header.stamp = tf2_ros::toMsg(tf2::timeFromSec(2)); + v1.header.frame_id = "A"; + + // simple api + geometry_msgs::msg::TransformStamped v_simple = tf_buffer->transform(v1, "B", tf2::durationFromSec(2.0)); + EXPECT_NEAR(v_simple.transform.translation.x, -9, EPS); + EXPECT_NEAR(v_simple.transform.translation.y, 18, EPS); + EXPECT_NEAR(v_simple.transform.translation.z, 27, EPS); + EXPECT_NEAR(v_simple.transform.rotation.x, 0.0, EPS); + EXPECT_NEAR(v_simple.transform.rotation.y, 0.0, EPS); + EXPECT_NEAR(v_simple.transform.rotation.z, 0.0, EPS); + EXPECT_NEAR(v_simple.transform.rotation.w, 1.0, EPS); + + // advanced api + geometry_msgs::msg::TransformStamped v_advanced = tf_buffer->transform(v1, "B", tf2::timeFromSec(2.0), + "A", tf2::durationFromSec(3.0)); + EXPECT_NEAR(v_advanced.transform.translation.x, -9, EPS); + EXPECT_NEAR(v_advanced.transform.translation.y, 18, EPS); + EXPECT_NEAR(v_advanced.transform.translation.z, 27, EPS); + EXPECT_NEAR(v_advanced.transform.rotation.x, 0.0, EPS); + EXPECT_NEAR(v_advanced.transform.rotation.y, 0.0, EPS); + EXPECT_NEAR(v_advanced.transform.rotation.z, 0.0, EPS); + EXPECT_NEAR(v_advanced.transform.rotation.w, 1.0, EPS); + } } @@ -289,17 +540,7 @@ int main(int argc, char **argv){ tf_buffer->setUsingDedicatedThread(true); // populate buffer - geometry_msgs::msg::TransformStamped t; - t.transform.translation.x = 10; - t.transform.translation.y = 20; - t.transform.translation.z = 30; - t.transform.rotation.w = 0; - t.transform.rotation.x = 1; - t.transform.rotation.y = 0; - t.transform.rotation.z = 0; - t.header.stamp = tf2_ros::toMsg(tf2::timeFromSec(2)); - t.header.frame_id = "A"; - t.child_frame_id = "B"; + geometry_msgs::msg::TransformStamped t = generate_stamped_transform(); tf_buffer->setTransform(t, "test"); int ret = RUN_ALL_TESTS();