From d1112df57fb8eb4b4e200ef2ae9418114ac168e6 Mon Sep 17 00:00:00 2001
From: Luca Passarella <passarella.luca96@gmail.com>
Date: Wed, 17 Mar 2021 10:26:23 +0000
Subject: [PATCH] support for ROS Kinetic and newer; publishing cloud as
 pointer (no copy needed)

---
 src/transform_pointcloud.cpp | 44 ++++++++++++++++++++++++------------
 1 file changed, 30 insertions(+), 14 deletions(-)

diff --git a/src/transform_pointcloud.cpp b/src/transform_pointcloud.cpp
index 7dfced1..70c4c8f 100644
--- a/src/transform_pointcloud.cpp
+++ b/src/transform_pointcloud.cpp
@@ -2,7 +2,19 @@
 #include <ros/ros.h>
 #include <sensor_msgs/PointCloud2.h>
 #include <tf/transform_listener.h>
+
+#include <pcl/pcl_base.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include <pcl_conversions/pcl_conversions.h>
+
 #include <pcl_ros/transforms.h>
+#include <pcl_ros/point_cloud.h>
+
+
+
+
 #include <pluginlib/class_list_macros.h>
 
 namespace grull_transform_pointcloud {
@@ -12,30 +24,34 @@ namespace grull_transform_pointcloud {
     ros::Subscriber sub;
     std::string target_frame;
     tf::TransformListener listener;
+
     void callback(const sensor_msgs::PointCloud2::ConstPtr &msg) {
-      sensor_msgs::PointCloud2 output;
-      output.header.stamp = msg->header.stamp;//ros::Time::now();
+      sensor_msgs::PointCloud2::Ptr output = boost::make_shared<sensor_msgs::PointCloud2>();
       
       try {
-//           listener.waitForTransform(target_frame, msg->header.frame_id, msg->header.stamp, ros::Duration(0.5));
-          listener.waitForTransform(target_frame, msg->header.frame_id, ros::Time(0), ros::Duration(10.0), ros::Duration(0.01));
-          if (pcl_ros::transformPointCloud(target_frame, *msg, output, listener)) {
+          // listener.waitForTransform(target_frame, msg->header.frame_id, msg->header.stamp, ros::Duration(0.5));
+          // listener.waitForTransform(target_frame, msg->header.frame_id, ros::Time(0), ros::Duration(10.0), ros::Duration(0.01))
+          if (pcl_ros::transformPointCloud(target_frame, *msg, *output, listener)) {
+              output->header.stamp = msg->header.stamp;//ros::Time::now();
               pub.publish(output);
           }
-      } catch (tf::TransformException ex) {
-          ROS_ERROR("%s",ex.what());
+      } catch (const tf::TransformException & ex) {
+          ROS_ERROR_STREAM(ex.what());
       }
     }
+
   public:
-    virtual void onInit() {
-      getPrivateNodeHandle().getParam("target_frame", target_frame);
-      ros::NodeHandle n = getNodeHandle();
+    virtual void onInit() override {
+
+      ros::NodeHandle & n = getNodeHandle();
+      ros::NodeHandle & pn = getPrivateNodeHandle();
+
+
+      target_frame = pn.param<std::string>("target_frame", "");
       pub = n.advertise<sensor_msgs::PointCloud2>("output_topic", 1, true);
-      sub = n.subscribe("input_topic", 1,&transform_pointcloud::callback, this);
+      sub = n.subscribe("input_topic", 1, &transform_pointcloud::callback, this);
     }
   };
 }
 
-PLUGINLIB_DECLARE_CLASS(grull_transform_pointcloud, transform_pointcloud, 
-    grull_transform_pointcloud::transform_pointcloud, nodelet::Nodelet);
-
+PLUGINLIB_EXPORT_CLASS(grull_transform_pointcloud::transform_pointcloud, nodelet::Nodelet);