Add doTransform support for Point32, Polygon and PolygonStamped (backport #616)

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<float>(v_out[0]);
+  t_out.y = static_cast<float>(v_out[1]);
+  t_out.z = static_cast<float>(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<float>(in.getX());
+  out.y = static_cast<float>(in.getY());
+  out.z = static_cast<float>(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 **/

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