Go to the documentation of this file.00001
00002
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
00039
00040 #ifndef OCTOMAP_MSGS_CONVERT_MSGS_H
00041 #define OCTOMAP_MSGS_CONVERT_MSGS_H
00042
00043 #include <octomap/octomap.h>
00044 #include <octomap_msgs/OctomapBinary.h>
00045
00046
00047 namespace octomap_msgs{
00054 static inline octomap::AbstractOcTree* fullMsgDataToMap(const std::vector<int8_t>& mapData){
00055 std::stringstream datastream;
00056 assert(mapData.size() > 0);
00057 datastream.write((const char*) &mapData[0], mapData.size());
00058 return octomap::AbstractOcTree::read(datastream);
00059 }
00060
00067 static inline octomap::OcTree* binaryMsgDataToMap(const std::vector<int8_t>& mapData){
00068 octomap::OcTree* octree = new octomap::OcTree(0.1);
00069 std::stringstream datastream;
00070 assert(mapData.size() > 0);
00071 datastream.write((const char*) &mapData[0], mapData.size());
00072 octree->readBinary(datastream);
00073 return octree;
00074 }
00075
00076
00077
00078
00079
00080
00088 template <class OctomapT>
00089 static inline bool binaryMapToMsgData(const OctomapT& octomap, std::vector<int8_t>& mapData){
00090 std::stringstream datastream;
00091 if (!octomap.writeBinaryConst(datastream))
00092 return false;
00093
00094 std::string datastring = datastream.str();
00095 mapData = std::vector<int8_t>(datastring.begin(), datastring.end());
00096 return true;
00097 }
00098
00106 template <class OctomapT>
00107 static inline bool fullMapToMsgData(const OctomapT& octomap, std::vector<int8_t>& mapData){
00108 std::stringstream datastream;
00109 if (!octomap.write(datastream))
00110 return false;
00111
00112 std::string datastring = datastream.str();
00113 mapData = std::vector<int8_t>(datastring.begin(), datastring.end());
00114 return true;
00115 }
00116
00117 }
00118
00119
00120 namespace octomap {
00124 template <class OctomapT>
00125 static inline void octomapMapToMsg(const OctomapT& octomap, octomap_msgs::OctomapBinary& mapMsg) __attribute__ ((deprecated));
00126
00127 template <class OctomapT>
00128 static inline void octomapMapToMsg(const OctomapT& octomap, octomap_msgs::OctomapBinary& mapMsg){
00129 mapMsg.header.stamp = ros::Time::now();
00130
00131 octomapMapToMsgData(octomap, mapMsg.data);
00132 }
00133
00137 template <class OctomapT>
00138 static inline void octomapMapToMsgData(const OctomapT& octomap, std::vector<int8_t>& mapData) __attribute__ ((deprecated));
00139
00140 template <class OctomapT>
00141 static inline void octomapMapToMsgData(const OctomapT& octomap, std::vector<int8_t>& mapData){
00142
00143
00144
00145
00146 std::stringstream datastream;
00147 octomap.writeBinaryConst(datastream);
00148 std::string datastring = datastream.str();
00149 mapData = std::vector<int8_t>(datastring.begin(), datastring.end());
00150 }
00151
00152
00156 template <class OctomapT>
00157 static inline void octomapMsgToMap(const octomap_msgs::OctomapBinary& mapMsg, OctomapT& octomap) __attribute__ ((deprecated));
00158
00159 template <class OctomapT>
00160 static inline void octomapMsgToMap(const octomap_msgs::OctomapBinary& mapMsg, OctomapT& octomap) {
00161 octomapMsgDataToMap(mapMsg.data, octomap);
00162 }
00163
00167 template <class OctomapT>
00168 static inline void octomapMsgDataToMap(const std::vector<int8_t>& mapData, OctomapT& octomap) __attribute__ ((deprecated));
00169
00170
00171 template <class OctomapT>
00172 static inline void octomapMsgDataToMap(const std::vector<int8_t>& mapData, OctomapT& octomap){
00173 std::stringstream datastream;
00174 assert(mapData.size() > 0);
00175 datastream.write((const char*) &mapData[0], mapData.size());
00176 octomap.readBinary(datastream);
00177 }
00178 }
00179
00180
00181 #endif
00182