From 58d87dfc27881af63eb0e68fc3793a028f4428d0 Mon Sep 17 00:00:00 2001
From: Markus Weber <markus.weberino@gmail.com>
Date: Wed, 19 Jun 2024 07:57:42 +0200
Subject: [PATCH 1/2] do not clear poly_in before transforming points
---
.../tf2_geometry_msgs/tf2_geometry_msgs.hpp | 9 ++++-----
.../test/test_tf2_geometry_msgs.cpp | 17 +++++++++++++++++
2 files changed, 21 insertions(+), 5 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 2a60fa3ae..0fd1f76bf 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
@@ -463,12 +463,11 @@ void doTransform(
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);
+ std::vector<geometry_msgs::msg::Point32> points_transformed(poly_in.points.size(), {});
+ for (size_t i=0; i < poly_in.points.size(); ++i) {
+ doTransform(poly_in.points[i], points_transformed[i], transform);
}
+ poly_out.points = points_transformed;
}
/** \brief Trivial "conversion" function for Polygon 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 069965a0c..7fe56c2d3 100644
--- a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp
+++ b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp
@@ -458,6 +458,23 @@ TEST(TfGeometry, Polygon)
EXPECT_NEAR(res.points[0].z, 27, EPS);
}
+ // non-stamped by reference
+ {
+ geometry_msgs::msg::Polygon res;
+ geometry_msgs::msg::Point32 p;
+ res.x = 1;
+ res.y = 2;
+ res.z = 3;
+ v1.points.push_back(p);
+
+ geometry_msgs::msg::TransformStamped t = generate_stamped_transform();
+
+ tf2::doTransform(res, 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;
From 9652a514b7572164889f368e6d0b3a5271fcee5e Mon Sep 17 00:00:00 2001
From: Markus Weber <markus.weberino@gmail.com>
Date: Sat, 22 Jun 2024 14:05:31 +0200
Subject: [PATCH 2/2] fix test
---
tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp | 8 ++++----
1 file changed, 4 insertions(+), 4 deletions(-)
diff --git a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp
index 7fe56c2d3..00183b60e 100644
--- a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp
+++ b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp
@@ -462,10 +462,10 @@ TEST(TfGeometry, Polygon)
{
geometry_msgs::msg::Polygon res;
geometry_msgs::msg::Point32 p;
- res.x = 1;
- res.y = 2;
- res.z = 3;
- v1.points.push_back(p);
+ p.x = 1;
+ p.y = 2;
+ p.z = 3;
+ res.points.push_back(p);
geometry_msgs::msg::TransformStamped t = generate_stamped_transform();