Go to the documentation of this file.00001
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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
00045 namespace octomap_msgs{
00046
00047
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
00086
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
00101
00102
00103
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