Program Listing for File MapCollection.hxx
↰ Return to documentation for file (include/octomap/MapCollection.hxx)
/*
* OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
* https://octomap.github.io/
*
* Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg
* All rights reserved.
* License: New BSD
*
* 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.
*/
#include <stdio.h>
#include <sstream>
#include <fstream>
namespace octomap {
template <class MAPNODE>
MapCollection<MAPNODE>::MapCollection() {
}
template <class MAPNODE>
MapCollection<MAPNODE>::MapCollection(std::string filename) {
this->read(filename);
}
template <class MAPNODE>
MapCollection<MAPNODE>::~MapCollection() {
this->clear();
}
template <class MAPNODE>
void MapCollection<MAPNODE>::clear() {
// FIXME: memory leak, else we run into double frees in, e.g., the viewer...
// for(typename std::vector<MAPNODE*>::iterator it= nodes.begin(); it != nodes.end(); ++it)
// delete *it;
nodes.clear();
}
template <class MAPNODE>
bool MapCollection<MAPNODE>::read(std::string filenamefullpath) {
std::string path;
std::string filename;
splitPathAndFilename(filenamefullpath, &path, &filename);
std::ifstream infile;
infile.open(filenamefullpath.c_str(), std::ifstream::in);
if(!infile.is_open()){
OCTOMAP_ERROR_STR("Could not open "<< filenamefullpath << ". MapCollection not loaded.");
return false;
}
bool ok = true;
while(ok){
std::string nodeID;
ok = readTagValue("MAPNODEID", infile, &nodeID);
if(!ok){
//do not throw error, you could be at the end of the file
break;
}
std::string mapNodeFilename;
ok = readTagValue("MAPNODEFILENAME", infile, &mapNodeFilename);
if(!ok){
OCTOMAP_ERROR_STR("Could not read MAPNODEFILENAME.");
break;
}
std::string poseStr;
ok = readTagValue("MAPNODEPOSE", infile, &poseStr);
std::istringstream poseStream(poseStr);
float x,y,z;
poseStream >> x >> y >> z;
double roll,pitch,yaw;
poseStream >> roll >> pitch >> yaw;
ok = ok && !poseStream.fail();
if(!ok){
OCTOMAP_ERROR_STR("Could not read MAPNODEPOSE.");
break;
}
octomap::pose6d origin(x, y, z, roll, pitch, yaw);
MAPNODE* node = new MAPNODE(combinePathAndFilename(path,mapNodeFilename), origin);
node->setId(nodeID);
if(!ok){
for(unsigned int i=0; i<nodes.size(); i++){
delete nodes[i];
}
infile.close();
return false;
} else {
nodes.push_back(node);
}
}
infile.close();
return true;
}
template <class MAPNODE>
void MapCollection<MAPNODE>::addNode( MAPNODE* node){
nodes.push_back(node);
}
template <class MAPNODE>
MAPNODE* MapCollection<MAPNODE>::addNode(const Pointcloud& cloud, point3d sensor_origin) {
// TODO...
return 0;
}
template <class MAPNODE>
bool MapCollection<MAPNODE>::removeNode(const MAPNODE* n) {
// TODO...
return false;
}
template <class MAPNODE>
MAPNODE* MapCollection<MAPNODE>::queryNode(const point3d& p) {
for (const_iterator it = this->begin(); it != this->end(); ++it) {
point3d ptrans = (*it)->getOrigin().inv().transform(p);
typename MAPNODE::TreeType::NodeType* n = (*it)->getMap()->search(ptrans);
if (!n) continue;
if ((*it)->getMap()->isNodeOccupied(n)) return (*it);
}
return 0;
}
template <class MAPNODE>
bool MapCollection<MAPNODE>::isOccupied(const point3d& p) const {
for (const_iterator it = this->begin(); it != this->end(); ++it) {
point3d ptrans = (*it)->getOrigin().inv().transform(p);
typename MAPNODE::TreeType::NodeType* n = (*it)->getMap()->search(ptrans);
if (!n) continue;
if ((*it)->getMap()->isNodeOccupied(n)) return true;
}
return false;
}
template <class MAPNODE>
bool MapCollection<MAPNODE>::isOccupied(float x, float y, float z) const {
point3d q(x,y,z);
return this->isOccupied(q);
}
template <class MAPNODE>
double MapCollection<MAPNODE>::getOccupancy(const point3d& p) {
double max_occ_val = 0;
bool is_unknown = true;
for (const_iterator it = this->begin(); it != this->end(); ++it) {
point3d ptrans = (*it)->getOrigin().inv().transform(p);
typename MAPNODE::TreeType::NodeType* n = (*it)->getMap()->search(ptrans);
if (n) {
double occ = n->getOccupancy();
if (occ > max_occ_val) max_occ_val = occ;
is_unknown = false;
}
}
if (is_unknown) return 0.5;
return max_occ_val;
}
template <class MAPNODE>
bool MapCollection<MAPNODE>::castRay(const point3d& origin, const point3d& direction, point3d& end,
bool ignoreUnknownCells, double maxRange) const {
bool hit_obstacle = false;
double min_dist = 1e6;
// SPEEDUP: use openMP to do raycasting in parallel
// SPEEDUP: use bounding boxes to determine submaps
for (const_iterator it = this->begin(); it != this->end(); ++it) {
point3d origin_trans = (*it)->getOrigin().inv().transform(origin);
point3d direction_trans = (*it)->getOrigin().inv().rot().rotate(direction);
printf("ray from %.2f,%.2f,%.2f in dir %.2f,%.2f,%.2f in node %s\n",
origin_trans.x(), origin_trans.y(), origin_trans.z(),
direction_trans.x(), direction_trans.y(), direction_trans.z(),
(*it)->getId().c_str());
point3d temp_endpoint;
if ((*it)->getMap()->castRay(origin_trans, direction_trans, temp_endpoint, ignoreUnknownCells, maxRange)) {
printf("hit obstacle in node %s\n", (*it)->getId().c_str());
double current_dist = origin_trans.distance(temp_endpoint);
if (current_dist < min_dist) {
min_dist = current_dist;
end = (*it)->getOrigin().transform(temp_endpoint);
}
hit_obstacle = true;
} // end if hit obst
} // end for
return hit_obstacle;
}
template <class MAPNODE>
bool MapCollection<MAPNODE>::writePointcloud(std::string filename) {
Pointcloud pc;
for(typename std::vector<MAPNODE* >::iterator it = nodes.begin(); it != nodes.end(); ++it){
Pointcloud tmp = (*it)->generatePointcloud();
pc.push_back(tmp);
}
pc.writeVrml(filename);
return true;
}
template <class MAPNODE>
bool MapCollection<MAPNODE>::write(std::string filename) {
bool ok = true;
std::ofstream outfile(filename.c_str());
outfile << "#This file was generated by the write-method of MapCollection\n";
for(typename std::vector<MAPNODE* >::iterator it = nodes.begin(); it != nodes.end(); ++it){
std::string id = (*it)->getId();
pose6d origin = (*it)->getOrigin();
std::string nodemapFilename = "nodemap_";
nodemapFilename.append(id);
nodemapFilename.append(".bt");
outfile << "MAPNODEID " << id << "\n";
outfile << "MAPNODEFILENAME "<< nodemapFilename << "\n";
outfile << "MAPNODEPOSE " << origin.x() << " " << origin.y() << " " << origin.z() << " "
<< origin.roll() << " " << origin.pitch() << " " << origin.yaw() << std::endl;
ok = ok && (*it)->writeMap(nodemapFilename);
}
outfile.close();
return ok;
}
// TODO
template <class MAPNODE>
void MapCollection<MAPNODE>::insertScan(const Pointcloud& scan, const octomap::point3d& sensor_origin,
double maxrange, bool pruning, bool lazy_eval) {
fprintf(stderr, "ERROR: MapCollection::insertScan is not implemented yet.\n");
}
template <class MAPNODE>
MAPNODE* MapCollection<MAPNODE>::queryNode(std::string id) {
for (const_iterator it = this->begin(); it != this->end(); ++it) {
if ((*it)->getId() == id) return *(it);
}
return 0;
}
// TODO
template <class MAPNODE>
std::vector<Pointcloud*> MapCollection<MAPNODE>::segment(const Pointcloud& scan) const {
std::vector<Pointcloud*> result;
fprintf(stderr, "ERROR: MapCollection::segment is not implemented yet.\n");
return result;
}
// TODO
template <class MAPNODE>
MAPNODE* MapCollection<MAPNODE>::associate(const Pointcloud& scan) {
fprintf(stderr, "ERROR: MapCollection::associate is not implemented yet.\n");
return 0;
}
template <class MAPNODE>
void MapCollection<MAPNODE>::splitPathAndFilename(std::string &filenamefullpath,
std::string* path, std::string *filename) {
#ifdef WIN32
std::string::size_type lastSlash = filenamefullpath.find_last_of('\\');
#else
std::string::size_type lastSlash = filenamefullpath.find_last_of('/');
#endif
if (lastSlash != std::string::npos){
*filename = filenamefullpath.substr(lastSlash + 1);
*path = filenamefullpath.substr(0, lastSlash);
} else {
*filename = filenamefullpath;
*path = "";
}
}
template <class MAPNODE>
std::string MapCollection<MAPNODE>::combinePathAndFilename(std::string path, std::string filename) {
std::string result = path;
if(path != ""){
#ifdef WIN32
result.append("\\");
#else
result.append("/");
#endif
}
result.append(filename);
return result;
}
template <class MAPNODE>
bool MapCollection<MAPNODE>::readTagValue(std::string /*tag*/, std::ifstream& infile, std::string* value) {
std::string line;
while( getline(infile, line) ){
if(line.length() != 0 && line[0] != '#')
break;
}
*value = "";
std::string::size_type firstSpace = line.find(' ');
if(firstSpace != std::string::npos && firstSpace != line.size()-1){
*value = line.substr(firstSpace + 1);
return true;
}
else return false;
}
} // namespace