Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
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 #include <octomap/AbstractOcTree.h>
00036 #include <octomap/OcTree.h>
00037 #include <octomap/CountingOcTree.h>
00038
00039
00040 namespace octomap {
00041 AbstractOcTree::AbstractOcTree(){
00042
00043 }
00044
00045 bool AbstractOcTree::write(const std::string& filename) const{
00046 std::ofstream file(filename.c_str(), std::ios_base::out | std::ios_base::binary);
00047
00048 if (!file.is_open()){
00049 OCTOMAP_ERROR_STR("Filestream to "<< filename << " not open, nothing written.");
00050 return false;
00051 } else {
00052
00053 write(file);
00054 file.close();
00055 }
00056
00057 return true;
00058 }
00059
00060
00061 bool AbstractOcTree::write(std::ostream &s) const{
00062 s << fileHeader <<"\n# (feel free to add / change comments, but leave the first line as it is!)\n#\n";
00063 s << "id " << getTreeType() << std::endl;
00064 s << "size "<< size() << std::endl;
00065 s << "res " << getResolution() << std::endl;
00066 s << "data" << std::endl;
00067
00068
00069 writeData(s);
00070
00071 return true;
00072 }
00073
00074 AbstractOcTree* AbstractOcTree::read(const std::string& filename){
00075 std::ifstream file(filename.c_str(), std::ios_base::in |std::ios_base::binary);
00076
00077 if (!file.is_open()){
00078 OCTOMAP_ERROR_STR("Filestream to "<< filename << " not open, nothing read.");
00079 return NULL;
00080 } else {
00081
00082 return read(file);
00083 }
00084 }
00085
00086
00087 AbstractOcTree* AbstractOcTree::read(std::istream &s){
00088
00089
00090 std::string line;
00091 std::getline(s, line);
00092 if (line.compare(0,fileHeader.length(), fileHeader) !=0){
00093 OCTOMAP_ERROR_STR("First line of OcTree file header does not start with \""<< fileHeader);
00094 return NULL;
00095 }
00096
00097 std::string id;
00098 unsigned size;
00099 double res;
00100 if (!AbstractOcTree::readHeader(s, id, size, res))
00101 return NULL;
00102
00103
00104
00105 OCTOMAP_DEBUG_STR("Reading octree type "<< id);
00106
00107 AbstractOcTree* tree = createTree(id, res);
00108
00109 if (tree){
00110 if (size > 0)
00111 tree->readData(s);
00112
00113 OCTOMAP_DEBUG_STR("Done ("<< tree->size() << " nodes)");
00114 }
00115
00116 return tree;
00117 }
00118
00119 bool AbstractOcTree::readHeader(std::istream& s, std::string& id, unsigned& size, double& res){
00120 id = "";
00121 size = 0;
00122 res = 0.0;
00123
00124 std::string token;
00125 bool headerRead = false;
00126 while(s.good() && !headerRead) {
00127 s >> token;
00128 if (token == "data"){
00129 headerRead = true;
00130
00131 char c;
00132 do {
00133 c = s.get();
00134 } while(s.good() && (c != '\n'));
00135 }
00136 else if (token.compare(0,1,"#") == 0){
00137
00138 char c;
00139 do {
00140 c = s.get();
00141 } while(s.good() && (c != '\n'));
00142 }
00143 else if (token == "id")
00144 s >> id;
00145 else if (token == "res")
00146 s >> res;
00147 else if (token == "size")
00148 s >> size;
00149 else{
00150 OCTOMAP_WARNING_STR("Unknown keyword in OcTree header, skipping: "<<token);
00151 char c;
00152 do {
00153 c = s.get();
00154 } while(s.good() && (c != '\n'));
00155
00156 }
00157
00158 }
00159
00160 if (!headerRead) {
00161 OCTOMAP_ERROR_STR("Error reading OcTree header");
00162 return false;
00163 }
00164
00165 if (id == "") {
00166 OCTOMAP_ERROR_STR("Error reading OcTree header, ID not set");
00167 return false;
00168 }
00169
00170 if (res <= 0.0) {
00171 OCTOMAP_ERROR_STR("Error reading OcTree header, res <= 0.0");
00172 return false;
00173 }
00174
00175 if (id == "1"){
00176 OCTOMAP_WARNING("You are using a deprecated id \"%s\", changing to \"OcTree\" (you should update your file header)\n", id.c_str());
00177 id = "OcTree";
00178 }
00179
00180 return true;
00181
00182 }
00183
00184 AbstractOcTree* AbstractOcTree::createTree(const std::string class_name, double res){
00185 std::map<std::string, AbstractOcTree*>::iterator it = classIDMapping().find(class_name);
00186 if (it == classIDMapping().end()){
00187 OCTOMAP_ERROR("Could not create octree of type %s, not in store in classIDMapping\n", class_name.c_str());
00188 return NULL;
00189 } else {
00190 AbstractOcTree* tree = it->second->create();
00191
00192 tree->setResolution(res);
00193 return tree;
00194 }
00195 }
00196
00197 std::map<std::string, AbstractOcTree*>& AbstractOcTree::classIDMapping(){
00198
00199
00200
00201 static std::map<std::string, AbstractOcTree*>* map = new std::map<std::string, AbstractOcTree*>();
00202 return *map;
00203 }
00204
00205 void AbstractOcTree::registerTreeType(AbstractOcTree* tree){
00206 classIDMapping()[tree->getTreeType()] = tree;
00207 }
00208
00209
00210 const std::string AbstractOcTree::fileHeader = "# Octomap OcTree file";
00211 }