Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039 #include <ros/ros.h>
00040
00041 #include <sensor_msgs/PointCloud2.h>
00042
00043
00044 #include <pcl/io/io.h>
00045 #include <pcl/io/pcd_io.h>
00046 #include <pcl/point_types.h>
00047
00048 #include <pcl_conversions/pcl_conversions.h>
00049
00050 using namespace std;
00051
00059 class PointCloudToPCD
00060 {
00061 protected:
00062 ros::NodeHandle nh_;
00063
00064 private:
00065 std::string prefix_;
00066
00067 public:
00068 string cloud_topic_;
00069
00070 ros::Subscriber sub_;
00071
00073
00074 void
00075 cloud_cb (const pcl::PCLPointCloud2::ConstPtr& cloud)
00076 {
00077 if ((cloud->width * cloud->height) == 0)
00078 return;
00079
00080 ROS_INFO ("Received %d data points in frame %s with the following fields: %s",
00081 (int)cloud->width * cloud->height,
00082 cloud->header.frame_id.c_str (),
00083 pcl::getFieldsList (*cloud).c_str ());
00084
00085 std::stringstream ss;
00086 ss << prefix_ << cloud->header.stamp << ".pcd";
00087 ROS_INFO ("Data saved to %s", ss.str ().c_str ());
00088
00089 pcl::io::savePCDFile (ss.str (), *cloud, Eigen::Vector4f::Zero (),
00090 Eigen::Quaternionf::Identity (), false);
00091 }
00092
00094 PointCloudToPCD ()
00095 {
00096
00097 ros::NodeHandle priv_nh("~");
00098 if (priv_nh.getParam ("prefix", prefix_))
00099 {
00100 ROS_INFO_STREAM ("PCD file prefix is: " << prefix_);
00101 }
00102 else if (nh_.getParam ("prefix", prefix_))
00103 {
00104 ROS_WARN_STREAM ("Non-private PCD prefix parameter is DEPRECATED: "
00105 << prefix_);
00106 }
00107
00108 cloud_topic_ = "input";
00109
00110 sub_ = nh_.subscribe (cloud_topic_, 1, &PointCloudToPCD::cloud_cb, this);
00111 ROS_INFO ("Listening for incoming data on topic %s",
00112 nh_.resolveName (cloud_topic_).c_str ());
00113 }
00114 };
00115
00116
00117 int
00118 main (int argc, char** argv)
00119 {
00120 ros::init (argc, argv, "pointcloud_to_pcd", ros::init_options::AnonymousName);
00121
00122 PointCloudToPCD b;
00123 ros::spin ();
00124
00125 return (0);
00126 }
00127