Program Listing for File conversions.h
↰ Return to documentation for file (/tmp/ws/src/octomap_msgs/include/octomap_msgs/conversions.h
)
/*
* Copyright (c) 2011-2013, A. Hornung, University of Freiburg
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the University of Freiburg nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef OCTOMAP_MSGS_CONVERT_MSGS_H
#define OCTOMAP_MSGS_CONVERT_MSGS_H
#include <octomap/octomap.h>
#include <octomap_msgs/msg/octomap.hpp>
#include <octomap/ColorOcTree.h>
// new conversion functions
namespace octomap_msgs{
// Note: fullMsgDataToMap() deleted, potentially causes confusion
// and (silent) errors in deserialization
static inline octomap::AbstractOcTree* fullMsgToMap(const octomap_msgs::msg::Octomap& msg){
octomap::AbstractOcTree* tree = octomap::AbstractOcTree::createTree(msg.id, msg.resolution);
if (tree){
std::stringstream datastream;
if (msg.data.size() > 0){
datastream.write((const char*) &msg.data[0], msg.data.size());
tree->readData(datastream);
}
}
return tree;
}
template<class TreeType>
void readTree(TreeType* octree, const octomap_msgs::msg::Octomap& msg){
std::stringstream datastream;
if (msg.data.size() > 0){
datastream.write((const char*) &msg.data[0], msg.data.size());
octree->readBinaryData(datastream);
}
}
static inline octomap::AbstractOcTree* binaryMsgToMap(const octomap_msgs::msg::Octomap& msg){
if (!msg.binary)
return NULL;
octomap::AbstractOcTree* tree;
if (msg.id == "ColorOcTree"){
octomap::ColorOcTree* octree = new octomap::ColorOcTree(msg.resolution);
readTree(octree, msg);
tree = octree;
} else {
octomap::OcTree* octree = new octomap::OcTree(msg.resolution);
readTree(octree, msg);
tree = octree;
}
return tree;
}
// Note: binaryMsgDataToMap() deleted, potentially causes confusion
// and (silent) errors in deserialization
static inline octomap::AbstractOcTree* msgToMap(const octomap_msgs::msg::Octomap& msg){
if (msg.binary)
return binaryMsgToMap(msg);
else
return fullMsgToMap(msg);
}
// conversions via stringstream
// TODO: read directly into buffer? see
// http://stackoverflow.com/questions/132358/how-to-read-file-content-into-istringstream
template <class OctomapT>
static inline bool binaryMapToMsgData(const OctomapT& octomap, std::vector<int8_t>& mapData){
std::stringstream datastream;
if (!octomap.writeBinaryConst(datastream))
return false;
std::string datastring = datastream.str();
mapData = std::vector<int8_t>(datastring.begin(), datastring.end());
return true;
}
template <class OctomapT>
static inline bool fullMapToMsgData(const OctomapT& octomap, std::vector<int8_t>& mapData){
std::stringstream datastream;
if (!octomap.write(datastream))
return false;
std::string datastring = datastream.str();
mapData = std::vector<int8_t>(datastring.begin(), datastring.end());
return true;
}
template <class OctomapT>
static inline bool binaryMapToMsg(const OctomapT& octomap, octomap_msgs::msg::Octomap& msg){
msg.resolution = octomap.getResolution();
msg.id = octomap.getTreeType();
msg.binary = true;
std::stringstream datastream;
if (!octomap.writeBinaryData(datastream))
return false;
std::string datastring = datastream.str();
msg.data = std::vector<int8_t>(datastring.begin(), datastring.end());
return true;
}
template <class OctomapT>
static inline bool fullMapToMsg(const OctomapT& octomap, octomap_msgs::msg::Octomap& msg){
msg.resolution = octomap.getResolution();
msg.id = octomap.getTreeType();
msg.binary = false;
std::stringstream datastream;
if (!octomap.writeData(datastream))
return false;
std::string datastring = datastream.str();
msg.data = std::vector<int8_t>(datastring.begin(), datastring.end());
return true;
}
}
#endif