OcTreeServerPCL.h
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 #ifndef OCTREE_SERVER_PCL_H_
00008 #define OCTREE_SERVER_PCL_H_
00009 
00010 #include <octomap/octomap.h>
00011 #include "OcTreePCL.h"
00012 #include <octomap_ros/OctomapBinary.h>
00013 #include <octomap_ros/GetOctomap.h>
00014 #include <iostream>
00015 
00016 
00017 namespace octomap_server{
00018 
00019         
00020         /*
00021         **
00022          * Converts an octree structure to a ROS octree msg as binary data
00023          *
00024          * @param octomap input OcTree
00025          * @param mapMsg output msg
00026          */
00027         static inline void octomapMapToMsg(const octomap::OcTreePCL& octree, octomap_ros::OctomapBinary& mapMsg){
00028                 // conversion via stringstream
00029 
00030                 // TODO: read directly into buffer? see
00031                 // http://stackoverflow.com/questions/132358/how-to-read-file-content-into-istringstream
00032                 std::stringstream datastream;
00033                 octree.writeBinaryConst(datastream);
00034                 std::string datastring = datastream.str();
00035                 mapMsg.header.stamp = ros::Time::now();
00036                 mapMsg.data = std::vector<int8_t>(datastring.begin(), datastring.end());
00037         }
00038         
00045         static inline void octomapMsgToMap(const octomap_ros::OctomapBinary& mapMsg, octomap::OcTreePCL& octree){
00046                 std::stringstream datastream;
00047                 assert(mapMsg.data.size() > 0);
00048                 datastream.write((const char*) &mapMsg.data[0], mapMsg.data.size());
00049                 octree.readBinary(datastream);
00050         }
00051 }
00052 
00053 #endif /* OCTREE_SERVER_PCL_H_ */
 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