conversions.h
Go to the documentation of this file.
00001 
00009 /*
00010  * Copyright (c) 2011-2013, A. Hornung, University of Freiburg
00011  * All rights reserved.
00012  *
00013  * Redistribution and use in source and binary forms, with or without
00014  * modification, are permitted provided that the following conditions are met:
00015  *
00016  *     * Redistributions of source code must retain the above copyright
00017  *       notice, this list of conditions and the following disclaimer.
00018  *     * Redistributions in binary form must reproduce the above copyright
00019  *       notice, this list of conditions and the following disclaimer in the
00020  *       documentation and/or other materials provided with the distribution.
00021  *     * Neither the name of the University of Freiburg nor the names of its
00022  *       contributors may be used to endorse or promote products derived from
00023  *       this software without specific prior written permission.
00024  *
00025  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00026  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00027  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00028  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00029  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00030  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00031  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00032  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00033  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00034  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00035  * POSSIBILITY OF SUCH DAMAGE.
00036  */
00037 
00038 #ifndef OCTOMAP_MSGS_CONVERT_MSGS_H
00039 #define OCTOMAP_MSGS_CONVERT_MSGS_H
00040 
00041 #include <octomap/octomap.h>
00042 #include <octomap_msgs/Octomap.h>
00043 
00044 // new conversion functions  
00045 namespace octomap_msgs{
00046   // Note: fullMsgDataToMap() deleted, potentially causes confusion 
00047   // and (silent) errors in deserialization
00048   
00054   static inline octomap::AbstractOcTree* fullMsgToMap(const Octomap& msg){
00055     octomap::AbstractOcTree* tree = octomap::AbstractOcTree::createTree(msg.id, msg.resolution);    
00056     if (tree){
00057       std::stringstream datastream;
00058       assert(msg.data.size() > 0);
00059       datastream.write((const char*) &msg.data[0], msg.data.size());
00060       tree->readData(datastream);          
00061     }
00062     
00063     return tree;      
00064   }
00065   
00072   static inline octomap::OcTree* binaryMsgToMap(const Octomap& msg){
00073     if (msg.id != "OcTree" || !msg.binary)
00074       return NULL;
00075     
00076     octomap::OcTree* octree = new octomap::OcTree(msg.resolution);    
00077     std::stringstream datastream;
00078     assert(msg.data.size() > 0);
00079     datastream.write((const char*) &msg.data[0], msg.data.size());
00080     octree->readBinaryData(datastream);          
00081     
00082     return octree;      
00083   }
00084 
00085   // Note: binaryMsgDataToMap() deleted, potentially causes confusion 
00086   // and (silent) errors in deserialization
00087   
00088 
00093   static inline octomap::AbstractOcTree* msgToMap(const Octomap& msg){
00094     if (msg.binary)
00095       return binaryMsgToMap(msg);
00096     else
00097       return fullMsgToMap(msg);
00098   }
00099 
00100   // conversions via stringstream
00101   
00102   // TODO: read directly into buffer? see
00103   // http://stackoverflow.com/questions/132358/how-to-read-file-content-into-istringstream
00104   
00112   template <class OctomapT>
00113   static inline bool binaryMapToMsgData(const OctomapT& octomap, std::vector<int8_t>& mapData){
00114     std::stringstream datastream;
00115     if (!octomap.writeBinaryConst(datastream))
00116       return false;
00117     
00118     std::string datastring = datastream.str();
00119     mapData = std::vector<int8_t>(datastring.begin(), datastring.end());
00120     return true;
00121   }
00122   
00130   template <class OctomapT>
00131   static inline bool fullMapToMsgData(const OctomapT& octomap, std::vector<int8_t>& mapData){
00132     std::stringstream datastream;
00133     if (!octomap.write(datastream))
00134       return false;
00135     
00136     std::string datastring = datastream.str();
00137     mapData = std::vector<int8_t>(datastring.begin(), datastring.end());
00138     return true;
00139   }
00140   
00148   template <class OctomapT>
00149   static inline bool binaryMapToMsg(const OctomapT& octomap, Octomap& msg){
00150     msg.resolution = octomap.getResolution();
00151     msg.id = octomap.getTreeType();
00152     msg.binary = true;
00153     
00154     std::stringstream datastream;
00155     if (!octomap.writeBinaryData(datastream))
00156       return false;
00157     
00158     std::string datastring = datastream.str();
00159     msg.data = std::vector<int8_t>(datastring.begin(), datastring.end());
00160     return true;
00161   }
00162   
00170   template <class OctomapT>
00171   static inline bool fullMapToMsg(const OctomapT& octomap, Octomap& msg){
00172     msg.resolution = octomap.getResolution();
00173     msg.id = octomap.getTreeType();
00174     msg.binary = false;
00175     
00176     std::stringstream datastream;
00177     if (!octomap.writeData(datastream))
00178       return false;
00179     
00180     std::string datastring = datastream.str();
00181     msg.data = std::vector<int8_t>(datastring.begin(), datastring.end());
00182     return true;
00183   }
00184 
00185 }
00186 
00187 
00188 #endif
00189 


octomap_msgs
Author(s): Armin Hornung
autogenerated on Fri Aug 28 2015 11:49:59