pcd_exporter_node.cpp
Go to the documentation of this file.
00001 /******************************************************************************
00002  * \file
00003  *
00004  * $Id: exporter.cpp 619 2012-04-16 13:47:28Z ihulik $
00005  *
00006  * Copyright (C) Brno University of Technology
00007  *
00008  * This file is part of software developed by dcgm-robotics@FIT group.
00009  *
00010  * Author: Rostislav Hulik (ihulik@fit.vutbr.cz)
00011  * Supervised by: Michal Spanel (spanel@fit.vutbr.cz)
00012  * Date: 11.01.2012 (version 0.8)
00013  * 
00014  * This file is free software: you can redistribute it and/or modify
00015  * it under the terms of the GNU Lesser General Public License as published by
00016  * the Free Software Foundation, either version 3 of the License, or
00017  * (at your option) any later version.
00018  * 
00019  * This file is distributed in the hope that it will be useful,
00020  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00021  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00022  * GNU Lesser General Public License for more details.
00023  * 
00024  * You should have received a copy of the GNU Lesser General Public License
00025  * along with this file.  If not, see <http://www.gnu.org/licenses/>.
00026  */
00027 
00035 #include <srs_env_model_percp/but_seg_utils/pcd_exporter_node.h>
00036 
00037 // CV <-> ROS bridge
00038 #include <cv_bridge/cv_bridge.h>
00039 
00040 //PCL
00041 #include <pcl/point_types.h>
00042 #include <pcl/point_cloud.h>
00043 #include <pcl/filters/voxel_grid.h>
00044 
00045 // Message filters
00046 #include <message_filters/subscriber.h>
00047 #include <message_filters/synchronizer.h>
00048 #include <message_filters/sync_policies/approximate_time.h>
00049 
00050 #include <srs_env_model_percp/but_segmentation/normals.h>
00051 
00052 
00053 using namespace std;
00054 using namespace ros;
00055 using namespace cv;
00056 using namespace pcl;
00057 using namespace sensor_msgs;
00058 using namespace message_filters;
00059 using namespace but_plane_detector;
00060 
00061 namespace srs_env_model_percp
00062 {
00063 
00064         void callback( const sensor_msgs::ImageConstPtr& dep, const CameraInfoConstPtr& cam_info)
00065         {
00066                 Time begin = Time::now();
00067                 //  Debug info
00068                 cerr << "Recieved frame..." << endl;
00069                 cerr << "Cam info: fx:" << cam_info->K[0] << " fy:" << cam_info->K[4] << " cx:" << cam_info->K[2] <<" cy:" << cam_info->K[5] << endl;
00070                 cerr << "Depth image h:" << dep->height << " w:" << dep->width << " e:" << dep->encoding << " " << dep->step << endl;
00071 
00072                 //get image from message
00073                 cv_bridge::CvImagePtr cv_image = cv_bridge::toCvCopy(dep);
00074                 Mat depth = cv_image->image;
00075 
00076                 Normals normal(depth, cam_info);
00077 
00078                 PointCloud<pcl::PointXYZ>::Ptr cloud (new PointCloud<PointXYZ>);
00079 
00080                 for (int i = 0; i < normal.m_points.rows; ++i)
00081                 for (int j = 0; j < normal.m_points.cols; ++j)
00082                 {
00083                         Vec3f vector = normal.m_points.at<Vec3f>(i, j);
00084 
00085                         //pcl::Vec
00086                         cloud->push_back(pcl::PointXYZ(vector[0], vector[1], vector[2]));
00087                 }
00088 
00089                 VoxelGrid<PointXYZ> voxelgrid;
00090                 voxelgrid.setInputCloud(cloud);
00091                 voxelgrid.setLeafSize(0.05, 0.05, 0.05);
00092                 voxelgrid.filter(*cloud);
00093 
00094                 cloud->header.frame_id = OUTPUT_POINT_CLOUD_FRAMEID;
00095 
00096                 stringstream name;
00097                 name << "model_" << modelNo << ".pcd";
00098                 io::savePCDFile(name.str(), *cloud);
00099                 ++modelNo;
00100                 pub.publish(cloud);
00101 
00102 
00103                 Time end = ros::Time::now();
00104                 cerr << "Computation time: " << (end-begin).nsec/1000000.0 << " ms." << endl;
00105                 cerr << "=========================================================" << endl;
00106         }
00107 }
00108 
00109 
00110 int main( int argc, char** argv )
00111 {
00112         using namespace srs_env_model_percp;
00113 
00114         ros::init(argc, argv, NODE_NAME);
00115         ros::NodeHandle n;
00116 
00117         // subscribe depth info
00118         message_filters::Subscriber<Image> depth_sub(n, INPUT_IMAGE_TOPIC, 1);
00119         message_filters::Subscriber<CameraInfo> info_sub_depth(n, INPUT_CAM_INFO_TOPIC, 1);
00120 
00121         pub = n.advertise<pcl::PointCloud<pcl::PointXYZ> > (OUTPUT_POINT_CLOUD_TOPIC, 1);
00122         // sync images
00123         typedef sync_policies::ApproximateTime<Image, CameraInfo> MySyncPolicy;
00124         Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), depth_sub, info_sub_depth);
00125         sync.registerCallback(boost::bind(&callback, _1, _2));
00126 
00127 
00128 
00129         std::cerr << "PCD exporter node initialized and listening..." << std::endl;
00130         ros::spin();
00131 
00132         return 1;
00133 }


srs_env_model_percp
Author(s): Rostislav Hulik (ihulik@fit.vutbr.cz), Tomas Hodan, Michal Spanel (spanel@fit.vutbr.cz)
autogenerated on Sun Jan 5 2014 11:51:56