pcl_to_octree_vanilla.cpp
Go to the documentation of this file.
00001 /*
00002  * Based on the original code by Kai M. Wurm and Armin Hornung
00003  * (http://octomap.sourceforge.net)
00004  * Author: Hozefa Indorewala
00005  */
00006 
00007 #include <ros/ros.h>
00008 #include <sensor_msgs/PointCloud.h>
00009 #include <sensor_msgs/PointCloud2.h>
00010 #include <sensor_msgs/point_cloud_conversion.h>
00011 #include "octomap/octomap.h"
00012 //#include "octree/OcTreeNode.h"
00013 //#include "octree/OcTree.h"
00014 //#include "octree/OcTreeServerPCL.h"
00015 #include <octomap_ros/conversions.h>
00016 #include <octomap/octomap.h>
00017 #include <pcl/point_types.h>
00018 #include <pcl/io/pcd_io.h>
00019 #include <pcl/ros/conversions.h>
00020 #include <visualization_msgs/MarkerArray.h>
00021 
00022 #include <vector>
00023 #include <iostream>
00024 
00025 class PclToOctree
00026 {
00027 public:
00028   PclToOctree();
00029   ~PclToOctree();
00030         void pclToOctreeCallback(const sensor_msgs::PointCloud& msg);
00031         void run();
00032     
00033 private:
00034   ros::NodeHandle nh_;
00035   double laser_offset_, octree_res_; 
00036   int octree_maxrange_, level_;
00037   std::string point_cloud_topic_, frame_id_;    
00038   bool visualize_octree_;
00039         // Publishes the octree in MarkerArray format so that it can be visualized in rviz
00040   ros::Publisher octree_marker_array_publisher_;
00041         
00042         /*
00043          * The following publisher, even though not required, is used because otherwise rviz 
00044          * cannot visualize the MarkerArray format without advertising the Marker format
00045          */
00046   ros::Publisher octree_marker_publisher_;
00047         
00048         ros::Publisher octree_binary_publisher_;
00049         
00050         // Subscribes to the PointCloud format to acquire point cloud data
00051         ros::Subscriber pointcloud_subscriber_;
00052         
00053         // Marker array to visualize the octree. It displays the occuplied cells of the octree
00054         visualization_msgs::MarkerArray octree_marker_array_msg_;
00055 };
00056 
00057 
00058 
00059 
00060 PclToOctree::PclToOctree() : nh_("~")
00061 {
00062   nh_.param("laser_offset", laser_offset_, 1.5);
00063   nh_.param("octree_resolution", octree_res_, 0.5);
00064   nh_.param("octree_maxrange", octree_maxrange_, -1);
00065   nh_.param("point_cloud_topic", point_cloud_topic_, std::string("/point_cloud")); 
00066   nh_.param("frame_id", frame_id_, std::string("/map"));
00067   nh_.param("level", level_, 0);
00068   nh_.param("visualize_octree", visualize_octree_, false); 
00069   ROS_INFO("pcl_to_octree node is up and running.");
00070   run();   
00071 }
00072 
00073 
00074 void PclToOctree::run()
00075 {
00076   octree_binary_publisher_ = nh_.advertise<octomap_ros::OctomapBinary>("octree_binary", 100);
00077     
00078   pointcloud_subscriber_ = nh_.subscribe(point_cloud_topic_, 100, &PclToOctree::pclToOctreeCallback, this);
00079     
00080   ros::spin();
00081 }
00082 
00083 PclToOctree::~PclToOctree()
00084 {
00085   ROS_INFO("Shutting down pcl_to_octree node!");
00086 }
00087 
00088 void PclToOctree::pclToOctreeCallback(const sensor_msgs::PointCloud& pointcloud_msg)
00089 {
00090   ROS_INFO("Received a point cloud.");
00091   sensor_msgs::PointCloud2 pointcloud2_msg;
00092   octomap_ros::OctomapBinary octree_msg;
00093       
00094   // Converting from PointCloud msg format to PointCloud2 msg format
00095   sensor_msgs::convertPointCloudToPointCloud2(pointcloud_msg, pointcloud2_msg);
00096   pcl::PointCloud<pcl::PointXYZ> pointcloud2_pcl;
00097     
00098   octomap::point3d octomap_3d_point;
00099   octomap::Pointcloud octomap_pointcloud;
00100       
00101   //Converting PointCloud2 msg format to pcl pointcloud format in order to read the 3d data
00102   pcl::fromROSMsg(pointcloud2_msg, pointcloud2_pcl);
00103       
00104   //Reading from pcl point cloud and saving it into octomap point cloud
00105   for(unsigned int i =0; i < pointcloud2_pcl.points.size(); i++)
00106   {
00107           octomap_3d_point(0) = pointcloud2_pcl.points[i].x;
00108           octomap_3d_point(1) = pointcloud2_pcl.points[i].y;
00109           octomap_3d_point(2) = pointcloud2_pcl.points[i].z;
00110           octomap_pointcloud.push_back(octomap_3d_point);
00111   }
00112     
00113   // Converting from octomap point cloud to octomap graph
00114   octomap::pose6d offset_trans(0,0,-laser_offset_,0,0,0);
00115   octomap::pose6d laser_pose(0,0,laser_offset_,0,0,0);
00116   octomap_pointcloud.transform(offset_trans);
00117     
00118     
00119   octomap::ScanGraph* octomap_graph = new octomap::ScanGraph();
00120   octomap_graph->addNode(&octomap_pointcloud, laser_pose);
00121     
00122   ROS_INFO("Number of points in graph: %d", octomap_graph->getNumPoints());
00123     
00124   // Converting from octomap graph to octomap tree (octree)
00125   octomap::OcTree* octree = new octomap::OcTree(octree_res_);
00126   for (octomap::ScanGraph::iterator scan_it = octomap_graph->begin(); scan_it != octomap_graph->end(); scan_it++)
00127   {
00128     octree->insertScan(**scan_it, octree_maxrange_, false);
00129   }
00130 
00131   
00132   //convert octree to OctreeBinary (serialization)
00133   octomap::octomapMapToMsg(*octree, octree_msg);
00134   octree_binary_publisher_.publish(octree_msg);
00135   ROS_INFO("OctreeBinary built and published");
00136 }
00137 
00138 int main(int argc, char** argv)
00139 {
00140   ros::init(argc, argv, "pcl_to_octree_vanilla");
00141   PclToOctree pcl_to_octree;
00142   return (0);
00143 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


pcl_to_octree
Author(s): Hozefa Indorewala, Dejan Pangercic
autogenerated on Sun Oct 6 2013 12:03:28