From 58d87dfc27881af63eb0e68fc3793a028f4428d0 Mon Sep 17 00:00:00 2001 From: Markus Weber 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 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 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();