octomap 1.5.0
include/octomap/OcTreeStamped.h
Go to the documentation of this file.
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