Go to the documentation of this file.00001
00002
00003
00004
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
00023
00024
00025
00026
00027 static inline void octomapMapToMsg(const octomap::OcTreePCL& octree, octomap_ros::OctomapBinary& mapMsg){
00028
00029
00030
00031
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