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
00053 static inline octomap::AbstractOcTree* fullMsgDataToMap(const std::vector<int8_t>& mapData){
00054 std::stringstream datastream;
00055 assert(mapData.size() > 0);
00056 datastream.write((const char*) &mapData[0], mapData.size());
00057 return octomap::AbstractOcTree::read(datastream);
00058 }
00059
00065 static inline octomap::AbstractOcTree* fullMsgToMap(const Octomap& msg){
00066 octomap::AbstractOcTree* tree = octomap::AbstractOcTree::createTree(msg.id, msg.resolution);
00067 if (tree){
00068 std::stringstream datastream;
00069 assert(msg.data.size() > 0);
00070 datastream.write((const char*) &msg.data[0], msg.data.size());
00071 tree->readData(datastream);
00072 }
00073
00074 return tree;
00075 }
00076
00083 static inline octomap::OcTree* binaryMsgToMap(const Octomap& msg){
00084 if (msg.id != "OcTree" || !msg.binary)
00085 return NULL;
00086
00087 octomap::OcTree* octree = new octomap::OcTree(msg.resolution);
00088 std::stringstream datastream;
00089 assert(msg.data.size() > 0);
00090 datastream.write((const char*) &msg.data[0], msg.data.size());
00091 octree->readBinaryData(datastream);
00092
00093 return octree;
00094 }
00095
00103 static inline octomap::OcTree* binaryMsgDataToMap(const std::vector<int8_t>& mapData){
00104 octomap::OcTree* octree = new octomap::OcTree(0.1);
00105 std::stringstream datastream;
00106 assert(mapData.size() > 0);
00107 datastream.write((const char*) &mapData[0], mapData.size());
00108 octree->readBinaryData(datastream);
00109 return octree;
00110 }
00111
00112
00117 static inline octomap::AbstractOcTree* msgToMap(const Octomap& msg){
00118 if (msg.binary)
00119 return binaryMsgToMap(msg);
00120 else
00121 return fullMsgToMap(msg);
00122 }
00123
00124
00125
00126
00127
00128
00136 template <class OctomapT>
00137 static inline bool binaryMapToMsgData(const OctomapT& octomap, std::vector<int8_t>& mapData){
00138 std::stringstream datastream;
00139 if (!octomap.writeBinaryConst(datastream))
00140 return false;
00141
00142 std::string datastring = datastream.str();
00143 mapData = std::vector<int8_t>(datastring.begin(), datastring.end());
00144 return true;
00145 }
00146
00154 template <class OctomapT>
00155 static inline bool fullMapToMsgData(const OctomapT& octomap, std::vector<int8_t>& mapData){
00156 std::stringstream datastream;
00157 if (!octomap.write(datastream))
00158 return false;
00159
00160 std::string datastring = datastream.str();
00161 mapData = std::vector<int8_t>(datastring.begin(), datastring.end());
00162 return true;
00163 }
00164
00172 template <class OctomapT>
00173 static inline bool binaryMapToMsg(const OctomapT& octomap, Octomap& msg){
00174 msg.resolution = octomap.getResolution();
00175 msg.id = octomap.getTreeType();
00176 msg.binary = true;
00177
00178 std::stringstream datastream;
00179 if (!octomap.writeBinaryData(datastream))
00180 return false;
00181
00182 std::string datastring = datastream.str();
00183 msg.data = std::vector<int8_t>(datastring.begin(), datastring.end());
00184 return true;
00185 }
00186
00194 template <class OctomapT>
00195 static inline bool fullMapToMsg(const OctomapT& octomap, Octomap& msg){
00196 msg.resolution = octomap.getResolution();
00197 msg.id = octomap.getTreeType();
00198 msg.binary = false;
00199
00200 std::stringstream datastream;
00201 if (!octomap.writeData(datastream))
00202 return false;
00203
00204 std::string datastring = datastream.str();
00205 msg.data = std::vector<int8_t>(datastring.begin(), datastring.end());
00206 return true;
00207 }
00208
00209 }
00210
00211
00212 namespace octomap {
00216 template <class OctomapT>
00217 static inline void octomapMapToMsg(const OctomapT& octomap, octomap_msgs::Octomap& mapMsg) __attribute__ ((deprecated));
00218
00219 template <class OctomapT>
00220 static inline void octomapMapToMsg(const OctomapT& octomap, octomap_msgs::Octomap& mapMsg){
00221 mapMsg.header.stamp = ros::Time::now();
00222
00223 octomapMapToMsgData(octomap, mapMsg.data);
00224 }
00225
00229 template <class OctomapT>
00230 static inline void octomapMapToMsgData(const OctomapT& octomap, std::vector<int8_t>& mapData) __attribute__ ((deprecated));
00231
00232 template <class OctomapT>
00233 static inline void octomapMapToMsgData(const OctomapT& octomap, std::vector<int8_t>& mapData){
00234
00235
00236
00237
00238 std::stringstream datastream;
00239 octomap.writeBinaryConst(datastream);
00240 std::string datastring = datastream.str();
00241 mapData = std::vector<int8_t>(datastring.begin(), datastring.end());
00242 }
00243
00244
00248 template <class OctomapT>
00249 static inline void octomapMsgToMap(const octomap_msgs::Octomap& mapMsg, OctomapT& octomap) __attribute__ ((deprecated));
00250
00251 template <class OctomapT>
00252 static inline void octomapMsgToMap(const octomap_msgs::Octomap& mapMsg, OctomapT& octomap) {
00253 octomapMsgDataToMap(mapMsg.data, octomap);
00254 }
00255
00259 template <class OctomapT>
00260 static inline void octomapMsgDataToMap(const std::vector<int8_t>& mapData, OctomapT& octomap) __attribute__ ((deprecated));
00261
00262
00263 template <class OctomapT>
00264 static inline void octomapMsgDataToMap(const std::vector<int8_t>& mapData, OctomapT& octomap){
00265 std::stringstream datastream;
00266 assert(mapData.size() > 0);
00267 datastream.write((const char*) &mapData[0], mapData.size());
00268 octomap.readBinary(datastream);
00269 }
00270 }
00271
00272
00273 #endif
00274