Go to the documentation of this file.00001 #include <ndt_map/ndt_map_hmt.h>
00002 #include <ndt_map/lazy_grid.h>
00003 #include <ndt_map/cell_vector.h>
00004
00005 #include <pcl_conversions/pcl_conversions.h>
00006 #include "pcl/point_cloud.h"
00007 #include "sensor_msgs/PointCloud2.h"
00008 #include "pcl/io/pcd_io.h"
00009
00010 #include "ros/ros.h"
00011 #include "pcl/point_cloud.h"
00012 #include "sensor_msgs/PointCloud2.h"
00013 #include "pcl/io/pcd_io.h"
00014 #include "pcl/features/feature.h"
00015 #include <cstdio>
00016
00017 #include <boost/shared_ptr.hpp>
00018 #include <boost/thread/mutex.hpp>
00019
00020 #include <Eigen/Eigen>
00021
00022 static int ctr = 0, ctr2 = 0;
00023 static boost::mutex mutex;
00024 static bool fresh = false;
00025
00026 void ndtCallback(const sensor_msgs::PointCloud2ConstPtr &msg)
00027 {
00028 pcl::PointCloud<pcl::PointXYZ> cloud;
00029 pcl::fromROSMsg(*msg,cloud);
00030
00031 ROS_INFO ("Received %d data points with the following fields: %s", (int)(msg->width * msg->height),
00032 pcl::getFieldsList (*msg).c_str ());
00033
00034 lslgeneric::NDTMap nd(new lslgeneric::LazyGrid(2));
00035
00036 Eigen::Affine3d T;
00037 T.setIdentity();
00038 nd.addPointCloud(T.translation(),cloud);
00039 nd.computeNDTCells();
00040
00041 ROS_INFO("Loaded point cloud");
00042 if(ctr%10 == 0) {
00043 nd.writeToJFF("maps02.jff");
00044 }
00045 ctr++;
00046
00047 }
00048
00049 int
00050 main (int argc, char** argv)
00051 {
00052
00053 ros::init(argc, argv, "ndt_builder");
00054 ros::NodeHandle n;
00055 ros::Subscriber sub1 = n.subscribe("points2_in", 10, ndtCallback);
00056 ros::spin();
00057
00058 return (0);
00059 }
00060
00061
00062