37 #include <sensor_msgs/PointCloud2.h>
38 #include <tf/transform_listener.h>
39 #include <pcl/conversions.h>
40 #include <pcl/point_cloud.h>
41 #include <pcl/point_types.h>
42 #include <pcl_ros/transforms.h>
43 #include <pcl_conversions/pcl_conversions.h>
45 #define PFLN printf("Line=%d\n", __LINE__);
58 static int counter=0; counter++;
59 ROS_INFO(
"Message received n= %d",counter);
62 pcl::PointCloud<pcl::PointXYZRGB> cloud_in;
63 pcl::PointCloud<pcl::PointXYZRGB> cloud_trans;
68 pcl::PCLPointCloud2 pcl_pc;
69 pcl_conversions::toPCL(*msg, pcl_pc);
70 pcl::fromPCLPointCloud2(pcl_pc, cloud_in);
73 tf::StampedTransform transform;
78 catch (tf::TransformException ex)
80 ROS_ERROR(
"%s",ex.what());
84 pcl_ros::transformPointCloud (cloud_in,cloud_trans,transform);
85 cloud_trans.header.frame_id=
frame_id;
86 sensor_msgs::PointCloud2 msg_pub;
88 pcl::toROSMsg(cloud_trans, msg_pub);
93 int main(
int argc,
char **argv)
95 ROS_INFO(
"Started transform cloud nodelet");
96 ros::init(argc,argv,
"transform_cloud_nodelet");
100 tf::TransformListener listener_center_bumper;
104 string pointcloud_subscribed;
105 if (!n.hasParam(
"pointcloud_to_transform"))
107 ROS_ERROR(
"Must set properly a input pointcloud");
111 n.getParam(
"pointcloud_to_transform", pointcloud_subscribed);
114 if (!n.hasParam(
"frame_id"))
116 ROS_ERROR(
"Must set properly a frame_id to transform");
123 string pointcloud_transformed;
124 if (!n.hasParam(
"pointcloud_transformed"))
126 ROS_ERROR(
"Must set properly a topic to publish the transformed cloud");
130 n.getParam(
"pointcloud_transformed", pointcloud_transformed);
132 ros::Subscriber sub = n.subscribe(pointcloud_subscribed, 1,
topic_callback);
133 pub=n.advertise<sensor_msgs::PointCloud2>(pointcloud_transformed,1000);