transformPointCloud.cpp

#include <pcl_ros/transforms.hpp>

bool transformCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
                    const std::string target_frame)
{
  const std::string source_frame = cloud->header.frame_id;
  geometry_msgs::msg::TransformStamped transform_stamped;
  bool success = getTransform(source_frame, target_frame, transform_stamped);
  if (not success) { return false; }
  pcl_ros::transformPointCloud(*cloud, *cloud, transform_stamped);
  cloud->header.frame_id = target_frame;
  return true;
}

bool getTransform(const std::string source_frame,
                  const std::string target_frame,
                  geometry_msgs::msg::TransformStamped& transform_stamped)
{
  try {
    transform_stamped = tf_buffer_->lookupTransform(
        target_frame,
        source_frame,
        tf2::TimePointZero);
  } catch (tf2::TransformException & ex) {
    RCLCPP_ERROR(this->get_logger(),
                 "Could not get transform from %s to target frame %s, Exception: %s",
                 source_frame.c_str(),
                 target_frame.c_str(),
                 ex.what());
    return false;
  }
  return true;
}