Merge pull request #1524 from tier4/fix/pointcloud_preprocessor

fix(autoware_pointcloud_preprocessor): static TF listener as Filter option (autowarefoundation#8678)

common/autoware_universe_utils/include/autoware/universe_utils/ros/static_transform_buffer.hpp → common/autoware_universe_utils/include/autoware/universe_utils/ros/managed_transform_buffer.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef AUTOWARE__UNIVERSE_UTILS__ROS__STATIC_TRANSFORM_BUFFER_HPP_
-#define AUTOWARE__UNIVERSE_UTILS__ROS__STATIC_TRANSFORM_BUFFER_HPP_
+#ifndef AUTOWARE__UNIVERSE_UTILS__ROS__MANAGED_TRANSFORM_BUFFER_HPP_
+#define AUTOWARE__UNIVERSE_UTILS__ROS__MANAGED_TRANSFORM_BUFFER_HPP_
 
 #include "autoware/universe_utils/ros/transform_listener.hpp"
 
@@ -29,6 +29,7 @@
 #include <functional>
 #include <memory>
 #include <string>
+#include <string_view>
 #include <unordered_map>
 #include <utility>
 
@@ -58,12 +59,69 @@ struct PairEqual
   }
 };
 using TFMap = std::unordered_map<Key, Eigen::Matrix4f, std::hash<Key>, PairEqual>;
+constexpr std::array<std::string_view, 3> warn_frames = {"map", "odom", "world"};
 
-class StaticTransformBuffer
+class ManagedTransformBuffer
 {
 public:
-  StaticTransformBuffer() = default;
+  explicit ManagedTransformBuffer(rclcpp::Node * node, const bool & has_static_tf_only)
+  : node_(node)
+  {
+    if (has_static_tf_only) {
+      get_transform_ = [this](
+                         const std::string & target_frame, const std::string & source_frame,
+                         Eigen::Matrix4f & eigen_transform) {
+        return getStaticTransform(target_frame, source_frame, eigen_transform);
+      };
+    } else {
+      tf_listener_ = std::make_unique<autoware::universe_utils::TransformListener>(node);
+      get_transform_ = [this](
+                         const std::string & target_frame, const std::string & source_frame,
+                         Eigen::Matrix4f & eigen_transform) {
+        return getDynamicTransform(target_frame, source_frame, eigen_transform);
+      };
+    }
+  }
+
+  bool getTransform(
+    const std::string & target_frame, const std::string & source_frame,
+    Eigen::Matrix4f & eigen_transform)
+  {
+    return get_transform_(target_frame, source_frame, eigen_transform);
+  }
+
+  /**
+   * Transforms a point cloud from one frame to another.
+   *
+   * @param target_frame The target frame to transform the point cloud to.
+   * @param cloud_in The input point cloud to be transformed.
+   * @param cloud_out The transformed point cloud.
+   * @return True if the transformation is successful, false otherwise.
+   */
+  bool transformPointcloud(
+    const std::string & target_frame, const sensor_msgs::msg::PointCloud2 & cloud_in,
+    sensor_msgs::msg::PointCloud2 & cloud_out)
+  {
+    if (
+      pcl::getFieldIndex(cloud_in, "x") == -1 || pcl::getFieldIndex(cloud_in, "y") == -1 ||
+      pcl::getFieldIndex(cloud_in, "z") == -1) {
+      RCLCPP_ERROR(node_->get_logger(), "Input pointcloud does not have xyz fields");
+      return false;
+    }
+    if (target_frame == cloud_in.header.frame_id) {
+      cloud_out = cloud_in;
+      return true;
+    }
+    Eigen::Matrix4f eigen_transform;
+    if (!getTransform(target_frame, cloud_in.header.frame_id, eigen_transform)) {
+      return false;
+    }
+    pcl_ros::transformPointCloud(eigen_transform, cloud_in, cloud_out);
+    cloud_out.header.frame_id = target_frame;
+    return true;
+  }
 
+private:
   /**
    * @brief Retrieves a transform between two static frames.
    *
@@ -72,17 +130,24 @@ class StaticTransformBuffer
    * Otherwise, transform matrix is set to identity and the function returns false. Transform
    * Listener is destroyed after function call.
    *
-   * @param node A pointer to the ROS node.
    * @param target_frame The target frame.
    * @param source_frame The source frame.
-   * @param eigen_transform The output Eigen transform matrix. Is set to identity if the transform
-   * is not found.
+   * @param eigen_transform The output Eigen transform matrix. It is set to the identity if the
+   * transform is not found.
    * @return True if the transform was successfully retrieved, false otherwise.
    */
-  bool getTransform(
-    rclcpp::Node * node, const std::string & target_frame, const std::string & source_frame,
+  bool getStaticTransform(
+    const std::string & target_frame, const std::string & source_frame,
     Eigen::Matrix4f & eigen_transform)
   {
+    if (
+      std::find(warn_frames.begin(), warn_frames.end(), target_frame) != warn_frames.end() ||
+      std::find(warn_frames.begin(), warn_frames.end(), source_frame) != warn_frames.end()) {
+      RCLCPP_WARN(
+        node_->get_logger(), "Using %s -> %s transform. This may not be a static transform.",
+        target_frame.c_str(), source_frame.c_str());
+    }
+
     auto key = std::make_pair(target_frame, source_frame);
     auto key_inv = std::make_pair(source_frame, target_frame);
 
@@ -109,11 +174,12 @@ class StaticTransformBuffer
     }
 
     // Get the transform from the TF tree
-    auto tf_listener = std::make_unique<autoware::universe_utils::TransformListener>(node);
-    auto tf = tf_listener->getTransform(
+    tf_listener_ = std::make_unique<autoware::universe_utils::TransformListener>(node_);
+    auto tf = tf_listener_->getTransform(
       target_frame, source_frame, rclcpp::Time(0), rclcpp::Duration(1000ms));
+    tf_listener_.reset();
     RCLCPP_DEBUG(
-      node->get_logger(), "Trying to enqueue %s -> %s transform to static TFs buffer...",
+      node_->get_logger(), "Trying to enqueue %s -> %s transform to static TFs buffer...",
       target_frame.c_str(), source_frame.c_str());
     if (tf == nullptr) {
       eigen_transform = Eigen::Matrix4f::Identity();
@@ -124,42 +190,39 @@ class StaticTransformBuffer
     return true;
   }
 
-  /**
-   * Transforms a point cloud from one frame to another.
+  /** @brief Retrieves a transform between two dynamic frames.
    *
-   * @param node A pointer to the ROS node.
-   * @param target_frame The target frame to transform the point cloud to.
-   * @param cloud_in The input point cloud to be transformed.
-   * @param cloud_out The transformed point cloud.
-   * @return True if the transformation is successful, false otherwise.
+   * This function attempts to retrieve a transform between the target frame and the source frame.
+   * If successful, the transformation matrix is assigned to the output parameter, and the function
+   * returns true. Otherwise, the transformation matrix is set to the identity and the function
+   * returns false.
+   *
+   * @param target_frame The target frame.
+   * @param source_frame The source frame.
+   * @param eigen_transform The output Eigen transformation matrix. It is set to the identity if the
+   * transform is not found.
+   * @return True if the transform was successfully retrieved, false otherwise.
    */
-  bool transformPointcloud(
-    rclcpp::Node * node, const std::string & target_frame,
-    const sensor_msgs::msg::PointCloud2 & cloud_in, sensor_msgs::msg::PointCloud2 & cloud_out)
+  bool getDynamicTransform(
+    const std::string & target_frame, const std::string & source_frame,
+    Eigen::Matrix4f & eigen_transform)
   {
-    if (
-      pcl::getFieldIndex(cloud_in, "x") == -1 || pcl::getFieldIndex(cloud_in, "y") == -1 ||
-      pcl::getFieldIndex(cloud_in, "z") == -1) {
-      RCLCPP_ERROR(node->get_logger(), "Input pointcloud does not have xyz fields");
-      return false;
-    }
-    if (target_frame == cloud_in.header.frame_id) {
-      cloud_out = cloud_in;
-      return true;
-    }
-    Eigen::Matrix4f eigen_transform;
-    if (!getTransform(node, target_frame, cloud_in.header.frame_id, eigen_transform)) {
+    auto tf = tf_listener_->getTransform(
+      target_frame, source_frame, rclcpp::Time(0), rclcpp::Duration(1000ms));
+    if (tf == nullptr) {
+      eigen_transform = Eigen::Matrix4f::Identity();
       return false;
     }
-    pcl_ros::transformPointCloud(eigen_transform, cloud_in, cloud_out);
-    cloud_out.header.frame_id = target_frame;
+    pcl_ros::transformAsMatrix(*tf, eigen_transform);
     return true;
   }
 
-private:
   TFMap buffer_;
+  rclcpp::Node * const node_;
+  std::unique_ptr<autoware::universe_utils::TransformListener> tf_listener_;
+  std::function<bool(const std::string &, const std::string &, Eigen::Matrix4f &)> get_transform_;
 };
 
 }  // namespace autoware::universe_utils
 
-#endif  // AUTOWARE__UNIVERSE_UTILS__ROS__STATIC_TRANSFORM_BUFFER_HPP_
+#endif  // AUTOWARE__UNIVERSE_UTILS__ROS__MANAGED_TRANSFORM_BUFFER_HPP_

common/autoware_universe_utils/test/src/ros/test_static_transform_buffer.cpp → common/autoware_universe_utils/test/src/ros/test_managed_transform_buffer.cpp

@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "autoware/universe_utils/ros/static_transform_buffer.hpp"
+#include "autoware/universe_utils/ros/managed_transform_buffer.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 #include <tf2_eigen/tf2_eigen.hpp>
@@ -22,11 +22,11 @@
 
 #include <memory>
 
-class TestStaticTransformBuffer : public ::testing::Test
+class TestManagedTransformBuffer : public ::testing::Test
 {
 protected:
   std::shared_ptr<rclcpp::Node> node_{nullptr};
-  std::shared_ptr<autoware::universe_utils::StaticTransformBuffer> static_tf_buffer_{nullptr};
+  std::shared_ptr<autoware::universe_utils::ManagedTransformBuffer> managed_tf_buffer_{nullptr};
   std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tf_broadcaster_;
   geometry_msgs::msg::TransformStamped tf_base_to_lidar_;
   Eigen::Matrix4f eigen_base_to_lidar_;
@@ -54,8 +54,9 @@ class TestStaticTransformBuffer : public ::testing::Test
 
   void SetUp() override
   {
-    node_ = std::make_shared<rclcpp::Node>("test_static_transform_buffer");
-    static_tf_buffer_ = std::make_shared<autoware::universe_utils::StaticTransformBuffer>();
+    node_ = std::make_shared<rclcpp::Node>("test_managed_transform_buffer");
+    managed_tf_buffer_ =
+      std::make_shared<autoware::universe_utils::ManagedTransformBuffer>(node_.get(), true);
     tf_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(node_);
 
     tf_base_to_lidar_ = generateTransformMsg(
@@ -91,120 +92,99 @@ class TestStaticTransformBuffer : public ::testing::Test
     ASSERT_TRUE(rclcpp::ok());
   }
 
-  void TearDown() override { static_tf_buffer_.reset(); }
+  void TearDown() override { managed_tf_buffer_.reset(); }
 };
 
-TEST_F(TestStaticTransformBuffer, TestTransformNoExist)
+TEST_F(TestManagedTransformBuffer, TestTransformNoExist)
 {
   Eigen::Matrix4f transform;
-  auto success = static_tf_buffer_->getTransform(node_.get(), "base_link", "fake_link", transform);
+  auto success = managed_tf_buffer_->getTransform("base_link", "fake_link", transform);
   EXPECT_TRUE(transform.isIdentity());
   EXPECT_FALSE(success);
 }
 
-TEST_F(TestStaticTransformBuffer, TestTransformBase)
+TEST_F(TestManagedTransformBuffer, TestTransformBase)
 {
   Eigen::Matrix4f eigen_base_to_lidar;
-  auto success =
-    static_tf_buffer_->getTransform(node_.get(), "base_link", "lidar_top", eigen_base_to_lidar);
+  auto success = managed_tf_buffer_->getTransform("base_link", "lidar_top", eigen_base_to_lidar);
   EXPECT_TRUE(eigen_base_to_lidar.isApprox(eigen_base_to_lidar_, 0.001));
   EXPECT_TRUE(success);
 }
 
-TEST_F(TestStaticTransformBuffer, TestTransformSameFrame)
+TEST_F(TestManagedTransformBuffer, TestTransformSameFrame)
 {
   Eigen::Matrix4f eigen_base_to_base;
-  auto success =
-    static_tf_buffer_->getTransform(node_.get(), "base_link", "base_link", eigen_base_to_base);
+  auto success = managed_tf_buffer_->getTransform("base_link", "base_link", eigen_base_to_base);
   EXPECT_TRUE(eigen_base_to_base.isApprox(Eigen::Matrix4f::Identity(), 0.001));
   EXPECT_TRUE(success);
 }
 
-TEST_F(TestStaticTransformBuffer, TestTransformInverse)
+TEST_F(TestManagedTransformBuffer, TestTransformInverse)
 {
   Eigen::Matrix4f eigen_lidar_to_base;
-  auto success =
-    static_tf_buffer_->getTransform(node_.get(), "lidar_top", "base_link", eigen_lidar_to_base);
+  auto success = managed_tf_buffer_->getTransform("lidar_top", "base_link", eigen_lidar_to_base);
   EXPECT_TRUE(eigen_lidar_to_base.isApprox(eigen_base_to_lidar_.inverse(), 0.001));
   EXPECT_TRUE(success);
 }
 
-TEST_F(TestStaticTransformBuffer, TestTransformMultipleCall)
+TEST_F(TestManagedTransformBuffer, TestTransformMultipleCall)
 {
   Eigen::Matrix4f eigen_transform;
-  EXPECT_FALSE(
-    static_tf_buffer_->getTransform(node_.get(), "base_link", "fake_link", eigen_transform));
+  EXPECT_FALSE(managed_tf_buffer_->getTransform("base_link", "fake_link", eigen_transform));
   EXPECT_TRUE(eigen_transform.isApprox(Eigen::Matrix4f::Identity(), 0.001));
-  EXPECT_TRUE(
-    static_tf_buffer_->getTransform(node_.get(), "lidar_top", "base_link", eigen_transform));
+  EXPECT_TRUE(managed_tf_buffer_->getTransform("lidar_top", "base_link", eigen_transform));
   EXPECT_TRUE(eigen_transform.isApprox(eigen_base_to_lidar_.inverse(), 0.001));
-  EXPECT_TRUE(
-    static_tf_buffer_->getTransform(node_.get(), "fake_link", "fake_link", eigen_transform));
+  EXPECT_TRUE(managed_tf_buffer_->getTransform("fake_link", "fake_link", eigen_transform));
   EXPECT_TRUE(eigen_transform.isApprox(Eigen::Matrix4f::Identity(), 0.001));
-  EXPECT_TRUE(
-    static_tf_buffer_->getTransform(node_.get(), "base_link", "lidar_top", eigen_transform));
+  EXPECT_TRUE(managed_tf_buffer_->getTransform("base_link", "lidar_top", eigen_transform));
   EXPECT_TRUE(eigen_transform.isApprox(eigen_base_to_lidar_, 0.001));
-  EXPECT_FALSE(
-    static_tf_buffer_->getTransform(node_.get(), "fake_link", "lidar_top", eigen_transform));
+  EXPECT_FALSE(managed_tf_buffer_->getTransform("fake_link", "lidar_top", eigen_transform));
   EXPECT_TRUE(eigen_transform.isApprox(Eigen::Matrix4f::Identity(), 0.001));
-  EXPECT_TRUE(
-    static_tf_buffer_->getTransform(node_.get(), "base_link", "lidar_top", eigen_transform));
+  EXPECT_TRUE(managed_tf_buffer_->getTransform("base_link", "lidar_top", eigen_transform));
   EXPECT_TRUE(eigen_transform.isApprox(eigen_base_to_lidar_, 0.001));
 }
 
-TEST_F(TestStaticTransformBuffer, TestTransformEmptyPointCloud)
+TEST_F(TestManagedTransformBuffer, TestTransformEmptyPointCloud)
 {
   auto cloud_in = std::make_unique<sensor_msgs::msg::PointCloud2>();
   cloud_in->header.frame_id = "lidar_top";
   cloud_in->header.stamp = rclcpp::Time(10, 100'000'000);
   auto cloud_out = std::make_unique<sensor_msgs::msg::PointCloud2>();
 
-  EXPECT_FALSE(
-    static_tf_buffer_->transformPointcloud(node_.get(), "lidar_top", *cloud_in, *cloud_out));
-  EXPECT_FALSE(
-    static_tf_buffer_->transformPointcloud(node_.get(), "base_link", *cloud_in, *cloud_out));
-  EXPECT_FALSE(
-    static_tf_buffer_->transformPointcloud(node_.get(), "fake_link", *cloud_in, *cloud_out));
+  EXPECT_FALSE(managed_tf_buffer_->transformPointcloud("lidar_top", *cloud_in, *cloud_out));
+  EXPECT_FALSE(managed_tf_buffer_->transformPointcloud("base_link", *cloud_in, *cloud_out));
+  EXPECT_FALSE(managed_tf_buffer_->transformPointcloud("fake_link", *cloud_in, *cloud_out));
 }
 
-TEST_F(TestStaticTransformBuffer, TestTransformEmptyPointCloudNoHeader)
+TEST_F(TestManagedTransformBuffer, TestTransformEmptyPointCloudNoHeader)
 {
   auto cloud_in = std::make_unique<sensor_msgs::msg::PointCloud2>();
   auto cloud_out = std::make_unique<sensor_msgs::msg::PointCloud2>();
 
-  EXPECT_FALSE(
-    static_tf_buffer_->transformPointcloud(node_.get(), "lidar_top", *cloud_in, *cloud_out));
-  EXPECT_FALSE(
-    static_tf_buffer_->transformPointcloud(node_.get(), "base_link", *cloud_in, *cloud_out));
-  EXPECT_FALSE(
-    static_tf_buffer_->transformPointcloud(node_.get(), "fake_link", *cloud_in, *cloud_out));
+  EXPECT_FALSE(managed_tf_buffer_->transformPointcloud("lidar_top", *cloud_in, *cloud_out));
+  EXPECT_FALSE(managed_tf_buffer_->transformPointcloud("base_link", *cloud_in, *cloud_out));
+  EXPECT_FALSE(managed_tf_buffer_->transformPointcloud("fake_link", *cloud_in, *cloud_out));
 }
 
-TEST_F(TestStaticTransformBuffer, TestTransformPointCloud)
+TEST_F(TestManagedTransformBuffer, TestTransformPointCloud)
 {
   auto cloud_out = std::make_unique<sensor_msgs::msg::PointCloud2>();
 
   // Transform cloud with header
-  EXPECT_TRUE(
-    static_tf_buffer_->transformPointcloud(node_.get(), "lidar_top", *cloud_in_, *cloud_out));
-  EXPECT_TRUE(
-    static_tf_buffer_->transformPointcloud(node_.get(), "base_link", *cloud_in_, *cloud_out));
-  EXPECT_FALSE(
-    static_tf_buffer_->transformPointcloud(node_.get(), "fake_link", *cloud_in_, *cloud_out));
+  EXPECT_TRUE(managed_tf_buffer_->transformPointcloud("lidar_top", *cloud_in_, *cloud_out));
+  EXPECT_TRUE(managed_tf_buffer_->transformPointcloud("base_link", *cloud_in_, *cloud_out));
+  EXPECT_FALSE(managed_tf_buffer_->transformPointcloud("fake_link", *cloud_in_, *cloud_out));
 }
 
-TEST_F(TestStaticTransformBuffer, TestTransformPointCloudNoHeader)
+TEST_F(TestManagedTransformBuffer, TestTransformPointCloudNoHeader)
 {
   auto cloud_out = std::make_unique<sensor_msgs::msg::PointCloud2>();
 
   // Transform cloud without header
   auto cloud_in = std::make_unique<sensor_msgs::msg::PointCloud2>(*cloud_in_);
   cloud_in->header.frame_id = "";
   cloud_in->header.stamp = rclcpp::Time(0, 0);
-  EXPECT_FALSE(
-    static_tf_buffer_->transformPointcloud(node_.get(), "lidar_top", *cloud_in, *cloud_out));
-  EXPECT_FALSE(
-    static_tf_buffer_->transformPointcloud(node_.get(), "base_link", *cloud_in, *cloud_out));
-  EXPECT_FALSE(
-    static_tf_buffer_->transformPointcloud(node_.get(), "fake_link", *cloud_in, *cloud_out));
+  EXPECT_FALSE(managed_tf_buffer_->transformPointcloud("lidar_top", *cloud_in, *cloud_out));
+  EXPECT_FALSE(managed_tf_buffer_->transformPointcloud("base_link", *cloud_in, *cloud_out));
+  EXPECT_FALSE(managed_tf_buffer_->transformPointcloud("fake_link", *cloud_in, *cloud_out));
 }