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 #include <octomap/ColorOcTree.h>
00044 
00045 // new conversion functions  
00046 namespace octomap_msgs{
00047   // Note: fullMsgDataToMap() deleted, potentially causes confusion 
00048   // and (silent) errors in deserialization
00049   
00055   static inline octomap::AbstractOcTree* fullMsgToMap(const Octomap& msg){
00056     octomap::AbstractOcTree* tree = octomap::AbstractOcTree::createTree(msg.id, msg.resolution);    
00057     if (tree){
00058       std::stringstream datastream;
00059       if (msg.data.size() > 0){
00060         datastream.write((const char*) &msg.data[0], msg.data.size());
00061         tree->readData(datastream);
00062       }
00063     }
00064     
00065     return tree;      
00066   }
00067 
00068 
00069   template<class TreeType>
00070   void readTree(TreeType* octree, const Octomap& msg){
00071     std::stringstream datastream;
00072     if (msg.data.size() > 0){
00073       datastream.write((const char*) &msg.data[0], msg.data.size());
00074       octree->readBinaryData(datastream);
00075     }
00076   }
00077     
00078   
00085   static inline octomap::AbstractOcTree* binaryMsgToMap(const Octomap& msg){
00086     if (!msg.binary)
00087       return NULL;
00088 
00089     octomap::AbstractOcTree* tree;
00090     if (msg.id == "ColorOcTree"){
00091       octomap::ColorOcTree* octree = new octomap::ColorOcTree(msg.resolution);    
00092       readTree(octree, msg);
00093       tree = octree;
00094     } else {
00095       octomap::OcTree* octree = new octomap::OcTree(msg.resolution);    
00096       readTree(octree, msg);
00097       tree = octree;
00098     }
00099     return tree;      
00100   }
00101 
00102   // Note: binaryMsgDataToMap() deleted, potentially causes confusion 
00103   // and (silent) errors in deserialization
00104   
00105 
00110   static inline octomap::AbstractOcTree* msgToMap(const Octomap& msg){
00111     if (msg.binary)
00112       return binaryMsgToMap(msg);
00113     else
00114       return fullMsgToMap(msg);
00115   }
00116 
00117   // conversions via stringstream
00118   
00119   // TODO: read directly into buffer? see
00120   // http://stackoverflow.com/questions/132358/how-to-read-file-content-into-istringstream
00121   
00129   template <class OctomapT>
00130   static inline bool binaryMapToMsgData(const OctomapT& octomap, std::vector<int8_t>& mapData){
00131     std::stringstream datastream;
00132     if (!octomap.writeBinaryConst(datastream))
00133       return false;
00134     
00135     std::string datastring = datastream.str();
00136     mapData = std::vector<int8_t>(datastring.begin(), datastring.end());
00137     return true;
00138   }
00139   
00147   template <class OctomapT>
00148   static inline bool fullMapToMsgData(const OctomapT& octomap, std::vector<int8_t>& mapData){
00149     std::stringstream datastream;
00150     if (!octomap.write(datastream))
00151       return false;
00152     
00153     std::string datastring = datastream.str();
00154     mapData = std::vector<int8_t>(datastring.begin(), datastring.end());
00155     return true;
00156   }
00157   
00165   template <class OctomapT>
00166   static inline bool binaryMapToMsg(const OctomapT& octomap, Octomap& msg){
00167     msg.resolution = octomap.getResolution();
00168     msg.id = octomap.getTreeType();
00169     msg.binary = true;
00170     
00171     std::stringstream datastream;
00172     if (!octomap.writeBinaryData(datastream))
00173       return false;
00174     
00175     std::string datastring = datastream.str();
00176     msg.data = std::vector<int8_t>(datastring.begin(), datastring.end());
00177     return true;
00178   }
00179   
00187   template <class OctomapT>
00188   static inline bool fullMapToMsg(const OctomapT& octomap, Octomap& msg){
00189     msg.resolution = octomap.getResolution();
00190     msg.id = octomap.getTreeType();
00191     msg.binary = false;
00192     
00193     std::stringstream datastream;
00194     if (!octomap.writeData(datastream))
00195       return false;
00196     
00197     std::string datastring = datastream.str();
00198     msg.data = std::vector<int8_t>(datastring.begin(), datastring.end());
00199     return true;
00200   }
00201 
00202 }
00203 
00204 
00205 #endif
00206 


octomap_msgs
Author(s): Armin Hornung
autogenerated on Tue Jun 14 2016 01:00:26