feat(autoware_utils_pcl): split package (#29)

* feat(autoware_utils_pcl): split package

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* add maintainer

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* fix build error

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* add compatibility

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* rename package

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* update readme

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

---------

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

Author: isamu-takagi

autoware_utils/README.md

@@ -47,7 +47,6 @@ The ROS module provides utilities for working with ROS messages and nodes:
 - **`debug_publisher.hpp`**: A helper class for publishing debug messages with timestamps.
 - **`diagnostics_interface.hpp`**: An interface for publishing diagnostic messages.
 - **`logger_level_configure.hpp`**: Utility for configuring logger levels dynamically.
-- **`managed_transform_buffer.hpp`**: A managed buffer for handling static and dynamic transforms.
 - **`marker_helper.hpp`**: Helper functions for creating and manipulating visualization markers.
 - **`msg_covariance.hpp`**: Indices for accessing covariance matrices in ROS messages.
 - **`msg_operation.hpp`**: Overloaded operators for quaternion messages.
@@ -56,12 +55,10 @@ The ROS module provides utilities for working with ROS messages and nodes:
 - **`processing_time_publisher.hpp`**: Publishes processing times as diagnostic messages.
 - **`published_time_publisher.hpp`**: Tracks and publishes the time when messages are published.
 - **`self_pose_listener.hpp`**: Listens to the self-pose of the vehicle.
-- **`transform_listener.hpp`**: Manages transformation listeners.
 - **`update_param.hpp`**: Updates parameters from remote nodes.
 - **`uuid_helper.hpp`**: Utilities for generating and managing UUIDs.
 - **`wait_for_param.hpp`**: Waits for parameters from remote nodes.
 - **`debug_traits.hpp`**: Traits for identifying debug message types.
-- **`pcl_conversion.hpp`**: Efficient conversion and transformation of PointCloud2 messages to PCL point clouds.
 
 #### System Module
 
@@ -72,10 +69,6 @@ The system module provides low-level utilities for performance monitoring and er
 - **`stop_watch.hpp`**: Measures elapsed time for profiling.
 - **`time_keeper.hpp`**: Tracks and reports the processing time of various functions.
 
-#### Transform Module
-
-Efficient methods for transforming and manipulating point clouds.
-
 ## Usage
 
 ### Including Headers
@@ -140,36 +133,6 @@ int main() {
 
 ### Detailed Usage Examples
 
-#### Transform Point Clouds with ManagedTransformBuffer
-
-```cpp
-#include "autoware_utils/ros/managed_transform_buffer.hpp"
-#include "sensor_msgs/msg/point_cloud2.hpp"
-#include <rclcpp/rclcpp.hpp>
-
-int main(int argc, char * argv[]) {
-  rclcpp::init(argc, argv);
-  auto node = rclcpp::Node::make_shared("transform_node");
-
-  // Initialize ManagedTransformBuffer
-  autoware_utils::ManagedTransformBuffer transform_buffer(node, false);
-
-  // Load point cloud data
-  sensor_msgs::msg::PointCloud2 cloud_in; // Assume this is populated with data
-  sensor_msgs::msg::PointCloud2 cloud_out;
-
-  // Transform point cloud from "base_link" to "map" frame
-  if (transform_buffer.transform_pointcloud("map", cloud_in, cloud_out)) {
-    RCLCPP_INFO(node->get_logger(), "Point cloud transformed successfully.");
-  } else {
-    RCLCPP_ERROR(node->get_logger(), "Failed to transform point cloud.");
-  }
-
-  rclcpp::shutdown();
-  return 0;
-}
-```
-
 #### Update Parameters Dynamically with update_param.hpp
 
 ```cpp
@@ -250,36 +213,6 @@ int main(int argc, char * argv[]) {
 }
 ```
 
-#### Efficient Point Cloud Conversion with pcl_conversion.hpp
-
-```cpp
-#include "autoware_utils/ros/pcl_conversion.hpp"
-#include "sensor_msgs/msg/point_cloud2.hpp"
-#include <pcl/point_types.h>
-#include <pcl/PCLPointCloud2.h>
-#include <Eigen/Core>
-#include <rclcpp/rclcpp.hpp>
-
-int main(int argc, char * argv[]) {
-  rclcpp::init(argc, argv);
-  auto node = rclcpp::Node::make_shared("pcl_conversion_node");
-
-  // Load point cloud data
-  sensor_msgs::msg::PointCloud2 cloud_in; // Assume this is populated with data
-  pcl::PointCloud<pcl::PointXYZ> pcl_cloud;
-
-  // Define transformation matrix
-  Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
-  // Populate transform matrix with actual values
-
-  // Convert and transform point cloud
-  autoware_utils::transform_point_cloud_from_ros_msg(cloud_in, pcl_cloud, transform);
-
-  rclcpp::shutdown();
-  return 0;
-}
-```
-
 #### Handling Debug Message Types with debug_traits.hpp
 
 ```cpp

autoware_utils/include/autoware_utils/ros/managed_transform_buffer.hpp

@@ -1,4 +1,4 @@
-// Copyright 2024 TIER IV, Inc.
+// Copyright 2025 The Autoware Contributors
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -15,213 +15,12 @@
 #ifndef AUTOWARE_UTILS__ROS__MANAGED_TRANSFORM_BUFFER_HPP_
 #define AUTOWARE_UTILS__ROS__MANAGED_TRANSFORM_BUFFER_HPP_
 
-#include "autoware_utils/ros/transform_listener.hpp"
-
-#include <eigen3/Eigen/Core>
-#include <pcl_ros/transforms.hpp>
-#include <rclcpp/rclcpp.hpp>
-
-#include <sensor_msgs/point_cloud2_iterator.hpp>
-
-#include <pcl_conversions/pcl_conversions.h>
-
-#include <chrono>
-#include <functional>
-#include <memory>
-#include <string>
-#include <string_view>
-#include <unordered_map>
-#include <utility>
-
-namespace std
-{
-template <>
-struct hash<std::pair<std::string, std::string>>
-{
-  size_t operator()(const std::pair<std::string, std::string> & p) const
-  {
-    size_t h1 = std::hash<std::string>{}(p.first);
-    size_t h2 = std::hash<std::string>{}(p.second);
-    return h1 ^ (h2 << 1u);
-  }
-};
-}  // namespace std
+#include <autoware_utils_pcl/managed_transform_buffer.hpp>
 
 namespace autoware_utils
 {
-using std::chrono_literals::operator""ms;
-using Key = std::pair<std::string, std::string>;
-struct PairEqual
-{
-  bool operator()(const Key & p1, const Key & p2) const
-  {
-    return p1.first == p2.first && p1.second == p2.second;
-  }
-};
-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 ManagedTransformBuffer
-{
-public:
-  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 get_static_transform(target_frame, source_frame, eigen_transform);
-      };
-    } else {
-      tf_listener_ = std::make_unique<autoware_utils::TransformListener>(node);
-      get_transform_ = [this](
-                         const std::string & target_frame, const std::string & source_frame,
-                         Eigen::Matrix4f & eigen_transform) {
-        return get_dynamic_transform(target_frame, source_frame, eigen_transform);
-      };
-    }
-  }
-
-  bool get_transform(
-    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 transform_pointcloud(
-    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 (!get_transform(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.
-   *
-   * This function attempts to retrieve a transform between the target frame and the source frame.
-   * If success, the transform matrix is set to the output parameter and the function returns true.
-   * Otherwise, transform matrix is set to identity and the function returns false. Transform
-   * Listener is destroyed after function call.
-   *
-   * @param target_frame The target frame.
-   * @param source_frame The source frame.
-   * @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 get_static_transform(
-    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);
-
-    // Check if the transform is already in the buffer
-    auto it = buffer_.find(key);
-    if (it != buffer_.end()) {
-      eigen_transform = it->second;
-      return true;
-    }
-
-    // Check if the inverse transform is already in the buffer
-    auto it_inv = buffer_.find(key_inv);
-    if (it_inv != buffer_.end()) {
-      eigen_transform = it_inv->second.inverse();
-      buffer_[key] = eigen_transform;
-      return true;
-    }
-
-    // Check if transform is needed
-    if (target_frame == source_frame) {
-      eigen_transform = Eigen::Matrix4f::Identity();
-      buffer_[key] = eigen_transform;
-      return true;
-    }
-
-    // Get the transform from the TF tree
-    tf_listener_ = std::make_unique<autoware_utils::TransformListener>(node_);
-    auto tf = tf_listener_->get_transform(
-      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...",
-      target_frame.c_str(), source_frame.c_str());
-    if (tf == nullptr) {
-      eigen_transform = Eigen::Matrix4f::Identity();
-      return false;
-    }
-    pcl_ros::transformAsMatrix(*tf, eigen_transform);
-    buffer_[key] = eigen_transform;
-    return true;
-  }
-
-  /** @brief Retrieves a transform between two dynamic frames.
-   *
-   * 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 get_dynamic_transform(
-    const std::string & target_frame, const std::string & source_frame,
-    Eigen::Matrix4f & eigen_transform)
-  {
-    auto tf = tf_listener_->get_transform(
-      target_frame, source_frame, rclcpp::Time(0), rclcpp::Duration(1000ms));
-    if (tf == nullptr) {
-      eigen_transform = Eigen::Matrix4f::Identity();
-      return false;
-    }
-    pcl_ros::transformAsMatrix(*tf, eigen_transform);
-    return true;
-  }
 
-  TFMap buffer_;
-  rclcpp::Node * const node_;
-  std::unique_ptr<autoware_utils::TransformListener> tf_listener_;
-  std::function<bool(const std::string &, const std::string &, Eigen::Matrix4f &)> get_transform_;
-};
+using namespace autoware_utils_pcl;  // NOLINT(build/namespaces)
 
 }  // namespace autoware_utils
 

autoware_utils/include/autoware_utils/ros/pcl_conversion.hpp

@@ -1,4 +1,4 @@
-// Copyright 2023 Tier IV, Inc.
+// Copyright 2025 The Autoware Contributors
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -15,57 +15,12 @@
 #ifndef AUTOWARE_UTILS__ROS__PCL_CONVERSION_HPP_
 #define AUTOWARE_UTILS__ROS__PCL_CONVERSION_HPP_
 
-#include <pcl/common/transforms.h>
-#include <pcl_conversions/pcl_conversions.h>
-
-#include <vector>
+#include <autoware_utils_pcl/pcl_conversion.hpp>
 
 namespace autoware_utils
 {
-/**
- * @brief a faster implementation of converting sensor_msgs::msg::PointCloud2 to
- * pcl::PointCloud<pcl::PointXYZ> and transform the cloud
- *
- * @tparam Scalar
- * @param cloud      input PointCloud2 message
- * @param pcl_cloud  output transformed pcl cloud
- * @param transform  eigen transformation matrix
- */
-template <typename Scalar>
-void transform_point_cloud_from_ros_msg(
-  const sensor_msgs::msg::PointCloud2 & cloud, pcl::PointCloud<pcl::PointXYZ> & pcl_cloud,
-  const Eigen::Matrix<Scalar, 4, 4> & transform)
-{
-  // Copy info fields
-  pcl_conversions::toPCL(cloud.header, pcl_cloud.header);
-  pcl_cloud.width = cloud.width;
-  pcl_cloud.height = cloud.height;
-  pcl_cloud.is_dense = cloud.is_dense == 1;
-
-  pcl::MsgFieldMap field_map;
-  std::vector<pcl::PCLPointField> msg_fields;
-  pcl_conversions::toPCL(cloud.fields, msg_fields);
-  pcl::createMapping<pcl::PointXYZ>(msg_fields, field_map);
-
-  // transform point data
-  std::uint32_t num_points = cloud.width * cloud.height;
-  pcl_cloud.points.resize(num_points);
-  std::uint8_t * cloud_data = reinterpret_cast<std::uint8_t *>(&pcl_cloud.points[0]);
-  pcl::detail::Transformer<Scalar> tf(transform);
 
-  for (std::uint32_t row = 0; row < cloud.height; ++row) {
-    const std::uint8_t * row_data = &cloud.data[row * cloud.row_step];
-    for (std::uint32_t col = 0; col < cloud.width; ++col) {
-      const std::uint8_t * msg_data = row_data + col * cloud.point_step;
-      for (const pcl::detail::FieldMapping & mapping : field_map) {
-        const float * msg_ptr = reinterpret_cast<const float *>(msg_data);
-        float * pcl_ptr = reinterpret_cast<float *>(cloud_data + mapping.struct_offset);
-        tf.se3(msg_ptr, pcl_ptr);
-      }
-      cloud_data += sizeof(pcl::PointXYZ);
-    }
-  }
-}
+using namespace autoware_utils_pcl;  // NOLINT(build/namespaces)
 
 }  // namespace autoware_utils