From 8b5440ac3b274ed57ddd364b5ed640cade4bdaa2 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 21 Aug 2023 13:00:49 +0200 Subject: [PATCH] Add doTransform support for Point32, Polygon and PolygonStamped (backport #616) (#619) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Add doTransform support for Point32, Polygon and PolygonStamped (#616) Co-authored-by: Guillaume Doisy (cherry picked from commit 70d8c95cf54ceeff7b5a3bc8de379b599a146beb) Co-authored-by: Alejandro Hernández Cordero --- .../tf2_geometry_msgs/tf2_geometry_msgs.hpp | 169 ++++++++++++++++++ .../test/test_tf2_geometry_msgs.cpp | 61 +++++++ 2 files changed, 230 insertions(+) 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 e630c4cc1..48c36b4a6 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 @@ -43,9 +43,11 @@ #include "geometry_msgs/msg/vector3_stamped.hpp" #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/point.hpp" +#include "geometry_msgs/msg/point32.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/pose_with_covariance.hpp" #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" +#include "geometry_msgs/msg/polygon_stamped.hpp" #include "geometry_msgs/msg/wrench.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" #include "kdl/frames.hpp" @@ -243,6 +245,54 @@ void fromMsg(const geometry_msgs::msg::Point & in, tf2::Vector3 & out) out = tf2::Vector3(in.x, in.y, in.z); } +/*************/ +/** Point32 **/ +/*************/ + +/** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Point32 type. + * This function is a specialization of the doTransform template defined in tf2/convert.h. + * \param t_in The point to transform, as a Point32 message. + * \param t_out The transformed point, as a Point32 message. + * \param transform The timestamped transform to apply, as a TransformStamped message. + */ +template<> +inline +void doTransform( + const geometry_msgs::msg::Point32 & t_in, + geometry_msgs::msg::Point32 & 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 = static_cast(v_out[0]); + t_out.y = static_cast(v_out[1]); + t_out.z = static_cast(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. + * \return The Vector3 converted to a geometry_msgs message type. + */ +inline +geometry_msgs::msg::Point32 & toMsg(const tf2::Vector3 & in, geometry_msgs::msg::Point32 & out) +{ + out.x = static_cast(in.getX()); + out.y = static_cast(in.getY()); + out.z = static_cast(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::Point32 & in, tf2::Vector3 & out) +{ + out = tf2::Vector3(in.x, in.y, in.z); +} + /******************/ /** PointStamped **/ /******************/ @@ -395,6 +445,125 @@ void fromMsg(const geometry_msgs::msg::PoseStamped & msg, geometry_msgs::msg::Po out = msg; } +/*************/ +/** Polygon **/ +/*************/ + +/** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs PolygonStamped type. + * This function is a specialization of the doTransform template defined in tf2/convert.h. + * \param poly_in The Polygon to transform. + * \param poly_out The transformed Polygon. + * \param transform The timestamped transform to apply, as a TransformStamped message. + */ +template<> +inline +void doTransform( + const geometry_msgs::msg::Polygon & poly_in, + geometry_msgs::msg::Polygon & poly_out, + const geometry_msgs::msg::TransformStamped & transform) +{ + poly_out.points.clear(); + for (auto & point : poly_in.points) { + geometry_msgs::msg::Point32 point_transformed; + doTransform(point, point_transformed, transform); + poly_out.points.push_back(point_transformed); + } +} + +/** \brief Trivial "conversion" function for Polygon message type. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param in A Polygon message. + * \return The input argument. + */ +inline +geometry_msgs::msg::Polygon toMsg(const geometry_msgs::msg::Polygon & in) +{ + return in; +} + +/** \brief Trivial "conversion" function for Polygon message type. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param msg A Polygon message. + * \param out The input argument. + */ +inline +void fromMsg( + const geometry_msgs::msg::Polygon & msg, + geometry_msgs::msg::Polygon & out) +{ + out = msg; +} + +/********************/ +/** PolygonStamped **/ +/********************/ + +/** \brief Extract a timestamp from the header of a PolygonStamped message. + * This function is a specialization of the getTimestamp template defined in tf2/convert.h. + * \param t PolygonStamped message to extract the timestamp from. + * \return The timestamp of the message. + */ +template<> +inline +tf2::TimePoint getTimestamp(const geometry_msgs::msg::PolygonStamped & t) +{ + return tf2_ros::fromMsg(t.header.stamp); +} + +/** \brief Extract a frame ID from the header of a PolygonStamped 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::PolygonStamped & t) +{ + return t.header.frame_id; +} + +/** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs PolygonStamped type. + * This function is a specialization of the doTransform template defined in tf2/convert.h. + * \param poly_in The PolygonStamped to transform. + * \param poly_out The transformed PolygonStamped. + * \param transform The timestamped transform to apply, as a TransformStamped message. + */ +template<> +inline +void doTransform( + const geometry_msgs::msg::PolygonStamped & poly_in, + geometry_msgs::msg::PolygonStamped & poly_out, + const geometry_msgs::msg::TransformStamped & transform) +{ + doTransform(poly_in.polygon, poly_out.polygon, transform); + + poly_out.header.stamp = transform.header.stamp; + poly_out.header.frame_id = transform.header.frame_id; +} + +/** \brief Trivial "conversion" function for Polygon 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::PolygonStamped toMsg(const geometry_msgs::msg::PolygonStamped & in) +{ + return in; +} + +/** \brief Trivial "conversion" function for Polygon 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::PolygonStamped & msg, + geometry_msgs::msg::PolygonStamped & out) +{ + out = msg; +} /************************/ /** PoseWithCovariance **/ diff --git a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp index 9d397f6f2..4d699b265 100644 --- a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp +++ b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp @@ -397,6 +397,21 @@ TEST(TfGeometry, Point) EXPECT_NEAR(res.z, 27, EPS); } + // non-stamped 32 + { + geometry_msgs::msg::Point32 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; @@ -422,6 +437,52 @@ TEST(TfGeometry, Point) } } +TEST(TfGeometry, Polygon) +{ + // non-stamped + { + geometry_msgs::msg::Polygon v1, res; + geometry_msgs::msg::Point32 p; + p.x = 1; + p.y = 2; + p.z = 3; + v1.points.push_back(p); + + geometry_msgs::msg::TransformStamped t = generate_stamped_transform(); + + tf2::doTransform(v1, res, t); + EXPECT_NEAR(res.points[0].x, 11, EPS); + EXPECT_NEAR(res.points[0].y, 18, EPS); + EXPECT_NEAR(res.points[0].z, 27, EPS); + } + + // stamped + { + geometry_msgs::msg::PolygonStamped v1, res; + geometry_msgs::msg::Point32 p; + p.x = 1; + p.y = 2; + p.z = 3; + v1.polygon.points.push_back(p); + v1.header.stamp = tf2_ros::toMsg(tf2::timeFromSec(2)); + v1.header.frame_id = "A"; + + // simple api + geometry_msgs::msg::PolygonStamped v_simple = + tf_buffer->transform(v1, "B", tf2::durationFromSec(2.0)); + EXPECT_NEAR(v_simple.polygon.points[0].x, -9, EPS); + EXPECT_NEAR(v_simple.polygon.points[0].y, 18, EPS); + EXPECT_NEAR(v_simple.polygon.points[0].z, 27, EPS); + + // advanced api + geometry_msgs::msg::PolygonStamped v_advanced = + tf_buffer->transform(v1, "B", tf2::timeFromSec(2.0), "A", tf2::durationFromSec(3.0)); + EXPECT_NEAR(v_simple.polygon.points[0].x, -9, EPS); + EXPECT_NEAR(v_simple.polygon.points[0].y, 18, EPS); + EXPECT_NEAR(v_simple.polygon.points[0].z, 27, EPS); + } +} + TEST(TfGeometry, Quaternion) { // rotated by -90° around y