00001 #include <ros/ros.h>
00002 #include <sensor_msgs/PointCloud2.h>
00003 #include <pcl/io/pcd_io.h>
00004 #include <pcl/point_types.h>
00005 #include <pcl_ros/transforms.h>
00006
00007 #include <tf/transform_listener.h>
00008
00009 int main (int argc, char **argv)
00010 {
00011 ros::init(argc, argv, "write_transformed_pcd");
00012 sensor_msgs::PointCloud2 transformed_cloud;
00013
00014 if(argc < 3)
00015 {
00016 ROS_ERROR("Usage: write_transformed_pcd <point_cloud_topic> <frame_id>");
00017 exit(-1);
00018 }
00019 tf::TransformListener tfl(ros::Duration(20.0));
00020
00021 ros::Duration(1.0).sleep();
00022
00023 sensor_msgs::PointCloud2::ConstPtr cloud = ros::topic::waitForMessage<sensor_msgs::PointCloud2>(argv[1], ros::Duration(5.0));
00024 if(!cloud)
00025 {
00026 ROS_INFO("No message received on topic %s", argv[1]);
00027 exit(-2);
00028 }
00029 tfl.waitForTransform(argv[2], cloud->header.frame_id, cloud->header.stamp, ros::Duration(2.0));
00030 pcl_ros::transformPointCloud(argv[2], *cloud, transformed_cloud, tfl);
00031
00032 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZRGB>());
00033 pcl::fromROSMsg(transformed_cloud, *cloud_out);
00034
00035 pcl::PCDWriter writer;
00036 writer.write<pcl::PointXYZRGB> ("cloud.pcd", *cloud_out, false);
00037 return 0;
00038
00039 }
