vtkToPointCloud.cpp
Go to the documentation of this file.
00001 #include <fstream>
00002 #include <memory>
00003 
00004 #include "ros/ros.h"
00005 #include "ros/console.h"
00006 
00007 #include "pointmatcher/PointMatcher.h"
00008 #include "pointmatcher/IO.h"
00009 
00010 #include "pointmatcher_ros/point_cloud.h"
00011 #include "pointmatcher_ros/transform.h"
00012 #include "pointmatcher_ros/get_params_from_server.h"
00013 #include "pointmatcher_ros/ros_logger.h"
00014 
00015 
00016 using namespace std;
00017 using namespace PointMatcherSupport;
00018 
00019 class PublishVTK
00020 {
00021         typedef PointMatcher<float> PM;
00022         typedef PM::DataPoints DP;
00023         typedef PointMatcherIO<float> PMIO;
00024 
00025         ros::NodeHandle& n;
00026 
00027         // Parameters
00028         const string mapFrame;
00029         const string cloudTopic;
00030         const string csvListFiles;
00031         const string dataDirectory;
00032         const string singleFile;
00033         const double publishRate;
00034         const bool pauseEachMsg;
00035 
00036         ros::Publisher cloudPub;
00037         PMIO::FileInfoVector list;
00038         unsigned currentId;
00039 
00040 public:
00041         PublishVTK(ros::NodeHandle& n);
00042         void publish();
00043         void run();
00044 };
00045 
00046 PublishVTK::PublishVTK(ros::NodeHandle& n):
00047         n(n),
00048         mapFrame(getParam<string>("mapFrameId", "/map")),
00049         cloudTopic(getParam<string>("cloudTopic", "/point_cloud")),
00050         csvListFiles(getParam<string>("csvListFiles", "")),
00051         dataDirectory(getParam<string>("dataDirectory", "")),
00052         singleFile(getParam<string>("singleFile", "")),
00053         publishRate(getParam<double>("publishRate", 1.0)),
00054         pauseEachMsg(getParam<bool>("pauseEachMsg", false))
00055 {
00056         // ROS initialization
00057         cloudPub = n.advertise<sensor_msgs::PointCloud2>(cloudTopic, 1);
00058 
00059         if(csvListFiles != "")
00060         {
00061                 list = PMIO::FileInfoVector(csvListFiles, dataDirectory);
00062         }
00063         currentId = 0;
00064 }
00065 
00066 void PublishVTK::publish()
00067 {
00068         if (cloudPub.getNumSubscribers())
00069         {
00070 
00071                 DP cloud;
00072                 if(singleFile != "")
00073                         cloud = DP::load(singleFile);
00074                 else
00075                 {
00076                         if(csvListFiles != "")
00077                         {
00078                                 if(pauseEachMsg)
00079                                 {
00080                                         cout << endl << "Press <ENTER> to continue or <CTRL-c>  to exit" << endl;
00081                                         cin.clear();
00082                                         cin.ignore(INT_MAX, '\n');
00083                                 }
00084 
00085                                 ROS_INFO_STREAM("Publishing file [" << currentId << "/" << list.size() << "]: " << list[currentId].readingFileName);
00086                                 cloud = DP::load(list[currentId].readingFileName);
00087                                 currentId++;
00088                                 if(currentId >= list.size())
00089                                 {
00090                                         cout << endl << "Press <ENTER> to restart or <CTRL-c>  to exit" << endl;
00091                                         cin.clear();
00092                                         cin.ignore(INT_MAX, '\n');
00093                                         currentId = 0;
00094                                 }
00095                         }
00096                 }
00097 
00098                 if(singleFile != "" || csvListFiles != "")
00099                         cloudPub.publish(PointMatcher_ros::pointMatcherCloudToRosMsg<float>(cloud, mapFrame, ros::Time::now()));
00100                 else
00101                 {
00102                         ROS_ERROR_STREAM("No files found");
00103                         abort();
00104                 }
00105         }
00106 }
00107 
00108 void PublishVTK::run()
00109 {
00110         ros::Rate r(publishRate);
00111         while (ros::ok())
00112         {
00113                 publish();
00114                 r.sleep();
00115         }
00116 }
00117 
00118 // Main function supporting the ExportVtk class
00119 int main(int argc, char **argv)
00120 {
00121         ros::init(argc, argv, "VtkToPointCloud_node");
00122         ros::NodeHandle n;
00123         PublishVTK pub(n);
00124         pub.run();
00125         
00126         return 0;
00127 }


ethzasl_point_cloud_vtk_tools
Author(s): François Pomerleau and Stéphane Magnenat
autogenerated on Thu Jan 2 2014 11:16:38