From b54af825c95cf99b79116907f6cf2fd07fed4173 Mon Sep 17 00:00:00 2001
From: "weiwei.he" <weiwei.he@upower.com>
Date: Thu, 23 Mar 2023 13:28:41 +0800
Subject: [PATCH] merge "euclidean cluster node to read frame_id from parameter
 file"

merge back the commit that was done in autoware.auto but lack in
autoware.universe.

=== original commit information ===
commit 6fed7f4bafa888a0e82ddfffc15dc28e70f0a912
Author: Kazuki Fukazawa
<fukazawa@jsk.imi.i.u-tokyo.ac.jp>
Date:   Thu Mar 3 08:12:18 2022 +0000

[#1367] euclidean cluster node to read
  frame_id from parameter file
(remade)
---
 .../tf2_autoware_auto_msgs.hpp                | 30 +++++++++++++++++++
 .../test/test_tf2_autoware_auto_msgs.cpp      | 18 +++++++++++
 2 files changed, 48 insertions(+)

diff --git a/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp b/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp
index bcefe66d8bee2..1a912d3a1c6d7 100644
--- a/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp
+++ b/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp
@@ -23,6 +23,7 @@
 #include <autoware_auto_geometry_msgs/msg/quaternion32.hpp>
 #include <autoware_auto_perception_msgs/msg/bounding_box.hpp>
 #include <autoware_auto_perception_msgs/msg/bounding_box_array.hpp>
+#include <autoware_auto_perception_msgs/msg/point_clusters.hpp>
 #include <geometry_msgs/msg/point32.hpp>
 #include <geometry_msgs/msg/polygon.hpp>
 #include <geometry_msgs/msg/transform_stamped.hpp>
@@ -42,6 +43,7 @@ using autoware::common::types::float32_t;
 using autoware::common::types::float64_t;
 using BoundingBoxArray = autoware_auto_perception_msgs::msg::BoundingBoxArray;
 using BoundingBox = autoware_auto_perception_msgs::msg::BoundingBox;
+using Clusters = autoware_auto_perception_msgs::msg::PointClusters;
 
 namespace tf2
 {
@@ -67,6 +69,34 @@ inline void doTransform(
   t_out.z = static_cast<float>(v_out[2]);
 }
 
+/**************/
+/** Clusters **/
+/**************/
+
+/** \brief Apply a geometry_msgs TransformStamped to a Clusters type.
+ * This function is a specialization of the doTransform template defined in tf2/convert.h.
+ * \param t_in The Clusters to transform.
+ * \param t_out The transformed Clusters.
+ * \param transform The timestamped transform to apply, as a TransformStamped message.
+ */
+template<>
+inline void doTransform(
+  const Clusters & t_in, Clusters & t_out,
+  const geometry_msgs::msg::TransformStamped & transform)
+{
+  geometry_msgs::msg::Point32 tmp_in, tmp_out;
+  t_out.points.resize(t_in.points.size());
+  for (size_t i = 0; i < t_in.points.size(); ++i) {
+    tmp_in.x = t_in.points[i].x;
+    tmp_in.y = t_in.points[i].y;
+    tmp_in.z = t_in.points[i].z;
+    doTransform(tmp_in, tmp_out, transform);
+    t_out.points[i].x = tmp_out.x;
+    t_out.points[i].y = tmp_out.y;
+    t_out.points[i].z = tmp_out.z;
+  }
+}
+
 /*************/
 /** Polygon **/
 /*************/
diff --git a/common/autoware_auto_tf2/test/test_tf2_autoware_auto_msgs.cpp b/common/autoware_auto_tf2/test/test_tf2_autoware_auto_msgs.cpp
index ce81dd2276870..f4aa6982691c6 100644
--- a/common/autoware_auto_tf2/test/test_tf2_autoware_auto_msgs.cpp
+++ b/common/autoware_auto_tf2/test/test_tf2_autoware_auto_msgs.cpp
@@ -60,6 +60,24 @@ TEST(Tf2AutowareAuto, DoTransformPoint32)
   EXPECT_NEAR(p_out.z, 27, EPS);
 }
 
+TEST(Tf2AutowareAuto, DoTransformClusters)
+{
+  const auto trans = filled_transform();
+  autoware_auto_perception_msgs::msg::PointClusters p1;
+  p1.points.resize(1);
+  p1.points[0].x = 1;
+  p1.points[0].y = 2;
+  p1.points[0].z = 3;
+
+  // doTransform
+  autoware_auto_perception_msgs::msg::PointClusters p_out;
+  tf2::doTransform(p1, p_out, trans);
+
+  EXPECT_NEAR(p_out.points[0].x, 11, EPS);
+  EXPECT_NEAR(p_out.points[0].y, 18, EPS);
+  EXPECT_NEAR(p_out.points[0].z, 27, EPS);
+}
+
 TEST(Tf2AutowareAuto, DoTransformPolygon)
 {
   const auto trans = filled_transform();