AbstractOcTree.cpp
Go to the documentation of this file.
1 /*
2  * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
3  * http://octomap.github.com/
4  *
5  * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg
6  * All rights reserved.
7  * License: New BSD
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above copyright
15  * notice, this list of conditions and the following disclaimer in the
16  * documentation and/or other materials provided with the distribution.
17  * * Neither the name of the University of Freiburg nor the names of its
18  * contributors may be used to endorse or promote products derived from
19  * this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
22  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
23  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
24  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
25  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
26  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
27  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
28  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
29  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
30  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31  * POSSIBILITY OF SUCH DAMAGE.
32  */
33 
34 
35 #include <octomap/AbstractOcTree.h>
36 #include <octomap/OcTree.h>
37 #include <octomap/CountingOcTree.h>
38 
39 
40 namespace octomap {
42 
43  }
44 
45  bool AbstractOcTree::write(const std::string& filename) const{
46  std::ofstream file(filename.c_str(), std::ios_base::out | std::ios_base::binary);
47 
48  if (!file.is_open()){
49  OCTOMAP_ERROR_STR("Filestream to "<< filename << " not open, nothing written.");
50  return false;
51  } else {
52  // TODO: check is_good of finished stream, return
53  write(file);
54  file.close();
55  }
56 
57  return true;
58  }
59 
60 
61  bool AbstractOcTree::write(std::ostream &s) const{
62  s << fileHeader <<"\n# (feel free to add / change comments, but leave the first line as it is!)\n#\n";
63  s << "id " << getTreeType() << std::endl;
64  s << "size "<< size() << std::endl;
65  s << "res " << getResolution() << std::endl;
66  s << "data" << std::endl;
67 
68  // write the actual data:
69  writeData(s);
70  // TODO: ret.val, checks stream?
71  return true;
72  }
73 
74  AbstractOcTree* AbstractOcTree::read(const std::string& filename){
75  std::ifstream file(filename.c_str(), std::ios_base::in |std::ios_base::binary);
76 
77  if (!file.is_open()){
78  OCTOMAP_ERROR_STR("Filestream to "<< filename << " not open, nothing read.");
79  return NULL;
80  } else {
81  // TODO: check is_good of finished stream, warn?
82  return read(file);
83  }
84  }
85 
86 
88 
89  // check if first line valid:
90  std::string line;
91  std::getline(s, line);
92  if (line.compare(0,fileHeader.length(), fileHeader) !=0){
93  OCTOMAP_ERROR_STR("First line of OcTree file header does not start with \""<< fileHeader);
94  return NULL;
95  }
96 
97  std::string id;
98  unsigned size;
99  double res;
100  if (!AbstractOcTree::readHeader(s, id, size, res))
101  return NULL;
102 
103 
104  // otherwise: values are valid, stream is now at binary data!
105  OCTOMAP_DEBUG_STR("Reading octree type "<< id);
106 
107  AbstractOcTree* tree = createTree(id, res);
108 
109  if (tree){
110  if (size > 0)
111  tree->readData(s);
112 
113  OCTOMAP_DEBUG_STR("Done ("<< tree->size() << " nodes)");
114  }
115 
116  return tree;
117  }
118 
119  bool AbstractOcTree::readHeader(std::istream& s, std::string& id, unsigned& size, double& res){
120  id = "";
121  size = 0;
122  res = 0.0;
123 
124  std::string token;
125  bool headerRead = false;
126  while(s.good() && !headerRead) {
127  s >> token;
128  if (token == "data"){
129  headerRead = true;
130  // skip forward until end of line:
131  char c;
132  do {
133  c = s.get();
134  } while(s.good() && (c != '\n'));
135  }
136  else if (token.compare(0,1,"#") == 0){
137  // comment line, skip forward until end of line:
138  char c;
139  do {
140  c = s.get();
141  } while(s.good() && (c != '\n'));
142  }
143  else if (token == "id")
144  s >> id;
145  else if (token == "res")
146  s >> res;
147  else if (token == "size")
148  s >> size;
149  else{
150  OCTOMAP_WARNING_STR("Unknown keyword in OcTree header, skipping: "<<token);
151  char c;
152  do {
153  c = s.get();
154  } while(s.good() && (c != '\n'));
155 
156  }
157 
158  }
159 
160  if (!headerRead) {
161  OCTOMAP_ERROR_STR("Error reading OcTree header");
162  return false;
163  }
164 
165  if (id == "") {
166  OCTOMAP_ERROR_STR("Error reading OcTree header, ID not set");
167  return false;
168  }
169 
170  if (res <= 0.0) {
171  OCTOMAP_ERROR_STR("Error reading OcTree header, res <= 0.0");
172  return false;
173  }
174  // fix deprecated id value:
175  if (id == "1"){
176  OCTOMAP_WARNING("You are using a deprecated id \"%s\", changing to \"OcTree\" (you should update your file header)\n", id.c_str());
177  id = "OcTree";
178  }
179 
180  return true;
181 
182  }
183 
184  AbstractOcTree* AbstractOcTree::createTree(const std::string class_name, double res){
185  std::map<std::string, AbstractOcTree*>::iterator it = classIDMapping().find(class_name);
186  if (it == classIDMapping().end()){
187  OCTOMAP_ERROR("Could not create octree of type %s, not in store in classIDMapping\n", class_name.c_str());
188  return NULL;
189  } else {
190  AbstractOcTree* tree = it->second->create();
191 
192  tree->setResolution(res);
193  return tree;
194  }
195  }
196 
197  std::map<std::string, AbstractOcTree*>& AbstractOcTree::classIDMapping(){
198  // we will "leak" the memory of the map and all trees until program exits,
199  // but this ensures all static objects are there as long as needed
200  // http://www.parashift.com/c++-faq-lite/ctors.html#faq-10.15
201  static std::map<std::string, AbstractOcTree*>* map = new std::map<std::string, AbstractOcTree*>();
202  return *map;
203  }
204 
206  classIDMapping()[tree->getTreeType()] = tree;
207  }
208 
209 
210  const std::string AbstractOcTree::fileHeader = "# Octomap OcTree file";
211 }
static void registerTreeType(AbstractOcTree *tree)
virtual std::string getTreeType() const =0
returns actual class name as string for identification
static AbstractOcTree * read(const std::string &filename)
virtual std::istream & readData(std::istream &s)=0
static bool readHeader(std::istream &s, std::string &id, unsigned &size, double &res)
virtual size_t size() const =0
virtual std::ostream & writeData(std::ostream &s) const =0
virtual double getResolution() const =0
static AbstractOcTree * createTree(const std::string id, double res)
bool write(const std::string &filename) const
Write file header and complete tree to file (serialization)
#define OCTOMAP_WARNING(...)
Definition: octomap_types.h:75
#define OCTOMAP_WARNING_STR(args)
Definition: octomap_types.h:76
#define OCTOMAP_ERROR_STR(args)
Definition: octomap_types.h:78
static std::map< std::string, AbstractOcTree * > & classIDMapping()
create private store, Construct on first use
#define OCTOMAP_DEBUG_STR(args)
Definition: octomap_types.h:72
virtual void setResolution(double res)=0
virtual AbstractOcTree * create() const =0
virtual constructor: creates a new object of same type
#define OCTOMAP_ERROR(...)
Definition: octomap_types.h:77
static const std::string fileHeader


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Wed Jun 5 2019 19:26:27