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