pointcloud_writer_node.cc
Go to the documentation of this file.
00001 #include<pcl/ros/conversions.h>
00002 #include<iostream>
00003 #include<fstream>
00004 #include"sensor_msgs/PointCloud2.h"
00005 #include"sensor_msgs/point_cloud_conversion.h"
00006 #include<cstring>
00007 #include<Eigen/Eigen>
00008 #include "ros/ros.h"
00009 #include "pcl/point_cloud.h"
00010 #include "sensor_msgs/PointCloud2.h"
00011 #include "nav_msgs/Odometry.h"
00012 #include "pcl/io/pcd_io.h"
00013 #include "message_filters/subscriber.h"
00014 #include "tf/message_filter.h"
00015 #include <tf/transform_broadcaster.h>
00016 #include <pcl_conversions/pcl_conversions.h>
00017 #include<pointcloud_vrml/pointcloud_utils.h>
00018 
00019 using namespace std;
00020 
00021 class WriterNode
00022 {
00023 protected:
00024     // Our NodeHandle
00025     ros::NodeHandle nh_;
00026     ros::Subscriber pc_;
00027     ros::Subscriber pose_;
00028     // Use the vector as a cyclic buffer (increment with std::rotate).
00029     pcl::PointCloud<pcl::PointXYZ> sensor_pc;
00030     int dumpNumber;
00031     const char* dumpPath;
00032 
00033     Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> Tinit, T;
00034     bool inited;
00035 
00036     boost::mutex data_mutex;
00037 
00038     FILE *fout;
00039 public:
00040     // Constructor
00041     WriterNode(const char* _dumpPath) : dumpNumber(0)
00042     {
00043         dumpPath = _dumpPath;
00044         pc_ = nh_.subscribe("/camera/rgb/points", 10, &WriterNode::log, this);
00045         pose_ = nh_.subscribe("/pose", 10, &WriterNode::updatePose, this);
00046         char fn[500];
00047 
00048         inited = false;
00049         snprintf (fn,499,"%s/pose.dat",dumpPath);
00050         fout = fopen(fn,"w");
00051     }
00052 
00053     ~WriterNode()
00054     {
00055         fclose(fout);
00056     }
00057 
00058     // Callback
00059     void log(const sensor_msgs::PointCloud2::ConstPtr& msg_in)
00060     {
00061         char fname [500];
00062         snprintf (fname,499,"%s/cloud%03d.wrl",dumpPath,dumpNumber);
00063         dumpNumber ++;
00064 
00065         pcl::fromROSMsg (*msg_in, sensor_pc);
00066         lslgeneric::writeToVRML<pcl::PointXYZ>(fname,sensor_pc);
00067         printf ("Dumped %s\n",fname);
00068 
00069         Eigen::Quaterniond qlocal;
00070         Eigen::Vector3d vlocal;
00071         Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> Tlocal;
00072 
00073         data_mutex.lock();
00074         if(!inited)
00075         {
00076             inited = true;
00077             Tinit = T;
00078             cout<<"Tinit:: "<<Tinit.translation().transpose()<<" r "<<Tinit.rotation().eulerAngles(0,1,2).transpose()<<endl;
00079         }
00080         Tlocal = Tinit.inverse()*T;
00081         data_mutex.unlock();
00082 
00083         vlocal = Tlocal.translation();
00084         qlocal = Tlocal.rotation();
00085         fprintf(fout,"0 %lf %lf %lf %lf %lf %lf %lf\n",
00086                 vlocal(0),vlocal(1),vlocal(2),qlocal.x(),qlocal.y(),qlocal.z(),qlocal.w());
00087 
00088     }
00089 
00090     void updatePose(const nav_msgs::Odometry::ConstPtr& msg_in)
00091     {
00092         data_mutex.lock();
00093         Eigen::Vector3d v;
00094         v<<msg_in->pose.pose.position.x,msg_in->pose.pose.position.y,msg_in->pose.pose.position.z;
00095         Eigen::Quaterniond q = Eigen::Quaterniond(msg_in->pose.pose.orientation.w,msg_in->pose.pose.orientation.x,
00096                                msg_in->pose.pose.orientation.y,msg_in->pose.pose.orientation.z);
00097         T = Eigen::Translation<double,3>(v)*q;
00098 
00099         data_mutex.unlock();
00100 
00101     }
00102 
00103 public:
00104     EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
00105 };
00106 
00107 
00108 
00109 int main(int argc, char**argv)
00110 {
00111 
00112     if(argc!=2)
00113     {
00114         cout<<"Usage: "<<argv[0]<<" PATH_TO_POINTCLOUDS \n";
00115         return -1;
00116     }
00117     ros::init(argc, argv, "pointcloud_writer");
00118 
00119     WriterNode nd(argv[1]);
00120     ros::spin();
00121 
00122 }


pointcloud_vrml
Author(s): Todor Stoyanov
autogenerated on Wed Oct 8 2014 23:30:29