00001 /* 00002 * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees 00003 * http://octomap.github.com/ 00004 * 00005 * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg 00006 * All rights reserved. 00007 * License: New BSD 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions are met: 00011 * 00012 * * Redistributions of source code must retain the above copyright 00013 * notice, this list of conditions and the following disclaimer. 00014 * * Redistributions in binary form must reproduce the above copyright 00015 * notice, this list of conditions and the following disclaimer in the 00016 * documentation and/or other materials provided with the distribution. 00017 * * Neither the name of the University of Freiburg nor the names of its 00018 * contributors may be used to endorse or promote products derived from 00019 * this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00022 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00023 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00024 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00025 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00026 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00027 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00028 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00029 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00030 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00031 * POSSIBILITY OF SUCH DAMAGE. 00032 */ 00033 00034 #ifndef OCTOMAP_OCTREE_STAMPED_H 00035 #define OCTOMAP_OCTREE_STAMPED_H 00036 00037 00038 #include <octomap/OcTreeNode.h> 00039 #include <octomap/OccupancyOcTreeBase.h> 00040 #include <ctime> 00041 00042 namespace octomap { 00043 00044 // node definition 00045 class OcTreeNodeStamped : public OcTreeNode { 00046 00047 public: 00048 OcTreeNodeStamped() : OcTreeNode(), timestamp(0) {} 00049 00050 OcTreeNodeStamped(const OcTreeNodeStamped& rhs) : OcTreeNode(rhs), timestamp(rhs.timestamp) {} 00051 00052 bool operator==(const OcTreeNodeStamped& rhs) const{ 00053 return (rhs.value == value && rhs.timestamp == timestamp); 00054 } 00055 00056 // children 00057 inline OcTreeNodeStamped* getChild(unsigned int i) { 00058 return static_cast<OcTreeNodeStamped*> (OcTreeNode::getChild(i)); 00059 } 00060 inline const OcTreeNodeStamped* getChild(unsigned int i) const { 00061 return static_cast<const OcTreeNodeStamped*> (OcTreeNode::getChild(i)); 00062 } 00063 00064 bool createChild(unsigned int i) { 00065 if (children == NULL) allocChildren(); 00066 children[i] = new OcTreeNodeStamped(); 00067 return true; 00068 } 00069 00070 // timestamp 00071 inline unsigned int getTimestamp() const { return timestamp; } 00072 inline void updateTimestamp() { timestamp = (unsigned int) time(NULL);} 00073 inline void setTimestamp(unsigned int timestamp) {this->timestamp = timestamp; } 00074 00075 // update occupancy and timesteps of inner nodes 00076 inline void updateOccupancyChildren() { 00077 this->setLogOdds(this->getMaxChildLogOdds()); // conservative 00078 updateTimestamp(); 00079 } 00080 00081 protected: 00082 unsigned int timestamp; 00083 }; 00084 00085 00086 // tree definition 00087 class OcTreeStamped : public OccupancyOcTreeBase <OcTreeNodeStamped> { 00088 00089 public: 00091 OcTreeStamped(double resolution) : OccupancyOcTreeBase<OcTreeNodeStamped>(resolution) {}; 00092 00095 OcTreeStamped* create() const {return new OcTreeStamped(resolution); } 00096 00097 std::string getTreeType() const {return "OcTreeStamped";} 00098 00100 unsigned int getLastUpdateTime(); 00101 00102 void degradeOutdatedNodes(unsigned int time_thres); 00103 00104 virtual void updateNodeLogOdds(OcTreeNodeStamped* node, const float& update) const; 00105 void integrateMissNoTime(OcTreeNodeStamped* node) const; 00106 00107 protected: 00112 class StaticMemberInitializer{ 00113 public: 00114 StaticMemberInitializer() { 00115 OcTreeStamped* tree = new OcTreeStamped(0.1); 00116 AbstractOcTree::registerTreeType(tree); 00117 } 00118 }; 00120 static StaticMemberInitializer ocTreeStampedMemberInit; 00121 00122 }; 00123 00124 } // end namespace 00125 00126 #endif