conversions.h
Go to the documentation of this file.
1 
9 /*
10  * Copyright (c) 2011-2013, A. Hornung, University of Freiburg
11  * All rights reserved.
12  *
13  * Redistribution and use in source and binary forms, with or without
14  * modification, are permitted provided that the following conditions are met:
15  *
16  * * Redistributions of source code must retain the above copyright
17  * notice, this list of conditions and the following disclaimer.
18  * * Redistributions in binary form must reproduce the above copyright
19  * notice, this list of conditions and the following disclaimer in the
20  * documentation and/or other materials provided with the distribution.
21  * * Neither the name of the University of Freiburg nor the names of its
22  * contributors may be used to endorse or promote products derived from
23  * this software without specific prior written permission.
24  *
25  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
26  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
27  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
28  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
29  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
30  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
31  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
32  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
33  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
34  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  */
37 
38 #ifndef OCTOMAP_MSGS_CONVERT_MSGS_H
39 #define OCTOMAP_MSGS_CONVERT_MSGS_H
40 
41 #include <octomap/octomap.h>
42 #include <octomap_msgs/Octomap.h>
43 #include <octomap/ColorOcTree.h>
44 
45 // new conversion functions
46 namespace octomap_msgs{
47  // Note: fullMsgDataToMap() deleted, potentially causes confusion
48  // and (silent) errors in deserialization
49 
55  static inline octomap::AbstractOcTree* fullMsgToMap(const Octomap& msg){
56  octomap::AbstractOcTree* tree = octomap::AbstractOcTree::createTree(msg.id, msg.resolution);
57  if (tree){
58  std::stringstream datastream;
59  if (msg.data.size() > 0){
60  datastream.write((const char*) &msg.data[0], msg.data.size());
61  tree->readData(datastream);
62  }
63  }
64 
65  return tree;
66  }
67 
68 
69  template<class TreeType>
70  void readTree(TreeType* octree, const Octomap& msg){
71  std::stringstream datastream;
72  if (msg.data.size() > 0){
73  datastream.write((const char*) &msg.data[0], msg.data.size());
74  octree->readBinaryData(datastream);
75  }
76  }
77 
78 
85  static inline octomap::AbstractOcTree* binaryMsgToMap(const Octomap& msg){
86  if (!msg.binary)
87  return NULL;
88 
89  octomap::AbstractOcTree* tree;
90  if (msg.id == "ColorOcTree"){
91  octomap::ColorOcTree* octree = new octomap::ColorOcTree(msg.resolution);
92  readTree(octree, msg);
93  tree = octree;
94  } else {
95  octomap::OcTree* octree = new octomap::OcTree(msg.resolution);
96  readTree(octree, msg);
97  tree = octree;
98  }
99  return tree;
100  }
101 
102  // Note: binaryMsgDataToMap() deleted, potentially causes confusion
103  // and (silent) errors in deserialization
104 
105 
110  static inline octomap::AbstractOcTree* msgToMap(const Octomap& msg){
111  if (msg.binary)
112  return binaryMsgToMap(msg);
113  else
114  return fullMsgToMap(msg);
115  }
116 
117  // conversions via stringstream
118 
119  // TODO: read directly into buffer? see
120  // http://stackoverflow.com/questions/132358/how-to-read-file-content-into-istringstream
121 
129  template <class OctomapT>
130  static inline bool binaryMapToMsgData(const OctomapT& octomap, std::vector<int8_t>& mapData){
131  std::stringstream datastream;
132  if (!octomap.writeBinaryConst(datastream))
133  return false;
134 
135  std::string datastring = datastream.str();
136  mapData = std::vector<int8_t>(datastring.begin(), datastring.end());
137  return true;
138  }
139 
147  template <class OctomapT>
148  static inline bool fullMapToMsgData(const OctomapT& octomap, std::vector<int8_t>& mapData){
149  std::stringstream datastream;
150  if (!octomap.write(datastream))
151  return false;
152 
153  std::string datastring = datastream.str();
154  mapData = std::vector<int8_t>(datastring.begin(), datastring.end());
155  return true;
156  }
157 
165  template <class OctomapT>
166  static inline bool binaryMapToMsg(const OctomapT& octomap, Octomap& msg){
167  msg.resolution = octomap.getResolution();
168  msg.id = octomap.getTreeType();
169  msg.binary = true;
170 
171  std::stringstream datastream;
172  if (!octomap.writeBinaryData(datastream))
173  return false;
174 
175  std::string datastring = datastream.str();
176  msg.data = std::vector<int8_t>(datastring.begin(), datastring.end());
177  return true;
178  }
179 
187  template <class OctomapT>
188  static inline bool fullMapToMsg(const OctomapT& octomap, Octomap& msg){
189  msg.resolution = octomap.getResolution();
190  msg.id = octomap.getTreeType();
191  msg.binary = false;
192 
193  std::stringstream datastream;
194  if (!octomap.writeData(datastream))
195  return false;
196 
197  std::string datastring = datastream.str();
198  msg.data = std::vector<int8_t>(datastring.begin(), datastring.end());
199  return true;
200  }
201 
202 }
203 
204 
205 #endif
206 
static bool fullMapToMsg(const OctomapT &octomap, Octomap &msg)
Serialization of an octree into binary data e.g. for messages and services. Full probability version ...
Definition: conversions.h:188
static octomap::AbstractOcTree * msgToMap(const Octomap &msg)
Convert an octomap representation to a new octree (full probabilities or binary). You will need to fr...
Definition: conversions.h:110
static bool binaryMapToMsg(const OctomapT &octomap, Octomap &msg)
Serialization of an octree into binary data e.g. for messages and services. Compact binary version (s...
Definition: conversions.h:166
void readTree(TreeType *octree, const Octomap &msg)
Definition: conversions.h:70
static octomap::AbstractOcTree * binaryMsgToMap(const Octomap &msg)
Creates a new octree by deserializing from msg, e.g. from a message or service (binary: only free and...
Definition: conversions.h:85
static octomap::AbstractOcTree * fullMsgToMap(const Octomap &msg)
Creates a new octree by deserializing from a message that contains the full map information (i...
Definition: conversions.h:55
static bool fullMapToMsgData(const OctomapT &octomap, std::vector< int8_t > &mapData)
Serialization of an octree into binary data e.g. for messages and services. Full probability version ...
Definition: conversions.h:148
static bool binaryMapToMsgData(const OctomapT &octomap, std::vector< int8_t > &mapData)
Serialization of an octree into binary data e.g. for messages and services. Compact binary version (s...
Definition: conversions.h:130


octomap_msgs
Author(s): Armin Hornung
autogenerated on Mon Feb 28 2022 22:56:03