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 bool binary_;
00067 bool compressed_;
00068
00069 public:
00070 string cloud_topic_;
00071
00072 ros::Subscriber sub_;
00073
00075
00076 void
00077 cloud_cb (const pcl::PCLPointCloud2::ConstPtr& cloud)
00078 {
00079 if ((cloud->width * cloud->height) == 0)
00080 return;
00081
00082 ROS_INFO ("Received %d data points in frame %s with the following fields: %s",
00083 (int)cloud->width * cloud->height,
00084 cloud->header.frame_id.c_str (),
00085 pcl::getFieldsList (*cloud).c_str ());
00086
00087 std::stringstream ss;
00088 ss << prefix_ << cloud->header.stamp << ".pcd";
00089 ROS_INFO ("Data saved to %s", ss.str ().c_str ());
00090
00091 pcl::PCDWriter writer;
00092 if(binary_)
00093 {
00094 if(compressed_)
00095 {
00096 writer.writeBinaryCompressed (ss.str (), *cloud, Eigen::Vector4f::Zero (),
00097 Eigen::Quaternionf::Identity ());
00098 }
00099 else
00100 {
00101 writer.writeBinary (ss.str (), *cloud, Eigen::Vector4f::Zero (),
00102 Eigen::Quaternionf::Identity ());
00103 }
00104 }
00105 else
00106 {
00107 writer.writeASCII (ss.str (), *cloud, Eigen::Vector4f::Zero (),
00108 Eigen::Quaternionf::Identity (), 8);
00109 }
00110
00111 }
00112
00114 PointCloudToPCD () : binary_(false), compressed_(false)
00115 {
00116
00117 ros::NodeHandle priv_nh("~");
00118 if (priv_nh.getParam ("prefix", prefix_))
00119 {
00120 ROS_INFO_STREAM ("PCD file prefix is: " << prefix_);
00121 }
00122 else if (nh_.getParam ("prefix", prefix_))
00123 {
00124 ROS_WARN_STREAM ("Non-private PCD prefix parameter is DEPRECATED: "
00125 << prefix_);
00126 }
00127
00128 priv_nh.getParam ("binary", binary_);
00129 priv_nh.getParam ("compressed", compressed_);
00130 if(binary_)
00131 {
00132 if(compressed_)
00133 {
00134 ROS_INFO_STREAM ("Saving as binary compressed PCD");
00135 }
00136 else
00137 {
00138 ROS_INFO_STREAM ("Saving as binary PCD");
00139 }
00140 }
00141 else
00142 {
00143 ROS_INFO_STREAM ("Saving as ASCII PCD");
00144 }
00145
00146 cloud_topic_ = "input";
00147
00148 sub_ = nh_.subscribe (cloud_topic_, 1, &PointCloudToPCD::cloud_cb, this);
00149 ROS_INFO ("Listening for incoming data on topic %s",
00150 nh_.resolveName (cloud_topic_).c_str ());
00151 }
00152 };
00153
00154
00155 int
00156 main (int argc, char** argv)
00157 {
00158 ros::init (argc, argv, "pointcloud_to_pcd", ros::init_options::AnonymousName);
00159
00160 PointCloudToPCD b;
00161 ros::spin ();
00162
00163 return (0);
00164 }
00165