pcd_publisher.cpp
Go to the documentation of this file.
00001 #include <sys/types.h>
00002 #include <dirent.h>
00003 #include <errno.h>
00004 #include <vector>
00005 #include <string>
00006 #include <iostream>
00007 #include <ros/ros.h>
00008 #include <std_msgs/String.h>
00009 #include <pcl/point_types.h>
00010 #include <pcl_ros/point_cloud.h>
00011 #include <pcl/io/pcd_io.h>
00012 
00013 typedef pcl::PointXYZ             Point;
00014 typedef pcl::PointCloud<Point>    PointCloud;
00015 typedef pcl::PointXYZRGB          PointRGB;
00016 typedef pcl::PointCloud<PointRGB> PointCloudRGB;
00017 
00018 class PcdPublisher {
00019 
00020   // ROS properties
00021   ros::NodeHandle nh_;
00022   ros::NodeHandle nh_private_;
00023 
00024   // Publisher to send out the point cloud
00025   ros::Publisher point_cloud_pub_;
00026 
00027   // Timer for point cloud publication
00028   ros::WallTimer timer_;
00029 
00030   // PCD data
00031   std::string pcd_files_dir_;
00032   std::vector<std::string> pcd_files_;
00033   unsigned int pcd_counter_;
00034 
00035 public:
00036 
00040   PcdPublisher(std::string pcd_files_dir)
00041   {
00042     int len;
00043     double timer_period;
00044 
00045     // Node handlers
00046     ros::NodeHandle nh;
00047     ros::NodeHandle nh_private_("~");
00048 
00049     // Read parameters
00050     nh_private_.param("timer_period", timer_period, 0.1);
00051 
00052     // Init variables
00053     pcd_files_dir_ = pcd_files_dir;
00054     pcd_files_ = std::vector<std::string>();
00055     pcd_counter_ = 0;
00056 
00057     // Check if directory name finishes with "/"
00058     len = pcd_files_dir_.length();
00059     if (strcmp("/", &(pcd_files_dir_[len - 1])) != 0)
00060     {
00061       pcd_files_dir_ = pcd_files_dir_ + "/";
00062     }
00063 
00064     // List all pcd files into the directory
00065     listPcdFiles(pcd_files_dir_, pcd_files_);
00066 
00067     // Declare the point cloud topic
00068     point_cloud_pub_ = nh_private_.advertise<PointCloudRGB>("output", 1);
00069 
00070     // Init the timer
00071     timer_ = nh.createWallTimer(ros::WallDuration(timer_period), 
00072                             boost::bind(&PcdPublisher::readAndPublish,
00073                             this));
00074     ROS_INFO_STREAM("[PcdPublisher:] Timer started!");
00075   }
00076 
00077 protected:
00078 
00082   int listPcdFiles(std::string dir, std::vector<std::string> &files)
00083   {
00084     int len;
00085     DIR *dp;
00086     struct dirent *dirp;
00087 
00088     // Check if directory exists
00089     if((dp  = opendir(dir.c_str())) == NULL)
00090     {
00091       std::cout << "Error(" << errno << ") opening " << dir << std::endl;
00092       return errno;
00093     }
00094 
00095     // List the .pcd files
00096     while ((dirp = readdir(dp)) != NULL)
00097     {
00098       len = strlen(dirp->d_name);
00099       if (len >= 4)
00100       {
00101         if (strcmp(".pcd", &(dirp->d_name[len - 4])) == 0)
00102         {
00103           files.push_back(dirp->d_name);
00104         }
00105       }
00106     }
00107     closedir(dp);
00108 
00109     // Sort the files alphabetically
00110     std::sort(files.begin(), files.end());
00111     return 0;
00112   }
00113 
00117   void readAndPublish()
00118   {
00119     // Check if all pcd files have been published
00120     if (pcd_counter_ < pcd_files_.size())
00121     {   
00122       PointCloud::Ptr cloud(new PointCloud);
00123 
00124       if (pcl::io::loadPCDFile<Point>(pcd_files_dir_ + pcd_files_[pcd_counter_], *cloud) == -1)
00125       {
00126         ROS_ERROR_STREAM("Couldn't read file " << pcd_files_dir_ + pcd_files_[pcd_counter_]);
00127         return;
00128       }
00129 
00130       // Convert the point cloud to RGB
00131       PointCloudRGB::Ptr cloud_rgb(new PointCloudRGB);
00132       pcl::copyPointCloud(*cloud, *cloud_rgb);
00133 
00134       // Publish the point cloud
00135       point_cloud_pub_.publish(cloud_rgb);
00136       
00137       ROS_INFO_STREAM("[PcdPublisher:] Published " << cloud->width * cloud->height 
00138         << " data points from " << pcd_files_dir_ + pcd_files_[pcd_counter_]);
00139 
00140       pcd_counter_++;
00141     }
00142     else
00143     {
00144       // All files processed
00145       timer_.stop();
00146       ROS_INFO_STREAM("[PcdPublisher:] All PCD files processed. Timer stoped!");
00147     }
00148   }
00149 };
00150 
00154 int main(int argc, char **argv)
00155 {
00156   ros::init(argc, argv, "pcd_publisher");
00157 
00158   // First argument is the directory where pcd files are.
00159   std::string pcd_files_dir = argc > 1 ? argv[1] : "data";
00160 
00161   // Init the node
00162   PcdPublisher node(pcd_files_dir);
00163   ros::spin();
00164   return 0;
00165 }
00166 


pointcloud_tools
Author(s): Pep Lluis Negre
autogenerated on Mon Oct 6 2014 07:48:29