Vertex.h

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2014, University of Toronto
 *  All rights reserved.
 *
 *  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 Toronto 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.
 *********************************************************************/
 
/* Authors: Jonathan Gammell, Marlin Strub */
 
#ifndef OMPL_GEOMETRIC_PLANNERS_INFORMEDTREES_BITSTAR_VERTEX_
#define OMPL_GEOMETRIC_PLANNERS_INFORMEDTREES_BITSTAR_VERTEX_
 
#include <memory>
#include <vector>
 
#include "ompl/base/OptimizationObjective.h"
#include "ompl/base/SpaceInformation.h"
#include "ompl/geometric/planners/informedtrees/BITstar.h"
#include "ompl/geometric/planners/informedtrees/bitstar/SearchQueue.h"
 
namespace ompl
{
    namespace geometric
    {

        class BITstar::Vertex
        {
        public:
            // ---
            // Construction and destruction.
            // ---

            Vertex(ompl::base::SpaceInformationPtr spaceInformation, const CostHelper *const costHelpPtr,
                   SearchQueue *const queuePtr, const std::shared_ptr<const unsigned int> &approximationId,
                   bool root = false);

            virtual ~Vertex();
 
            // ---
            // State access.
            // ---

            BITstar::VertexId getId() const;

            ompl::base::State *state();

            ompl::base::State const *state() const;
 
            // ---
            // Graph information access.
            // ---

            bool isRoot() const;

            bool hasParent() const;

            bool isInTree() const;

            unsigned int getDepth() const;

            VertexConstPtr getParent() const;

            VertexPtr getParent();

            bool isConsistent() const;

            bool isPruned() const;

            bool isExpandedOnCurrentApproximation() const;

            bool isExpandedOnCurrentSearch() const;

            bool hasEverBeenExpandedAsRewiring() const;
 
            // ---
            // Graph modification.
            // ---

            void addParent(const VertexPtr &newParent, const ompl::base::Cost &edgeInCost);

            void removeParent(bool updateChildCosts);

            bool hasChildren() const;

            void getChildren(VertexConstPtrVector *children) const;

            void getChildren(VertexPtrVector *children);

            void addChild(const VertexPtr &newChild);

            void removeChild(const VertexPtr &oldChild);

            void blacklistChild(const VertexConstPtr &vertex);

            void whitelistChild(const VertexConstPtr &vertex);

            bool isBlacklistedAsChild(const VertexConstPtr &vertex) const;

            bool isWhitelistedAsChild(const VertexConstPtr &vertex) const;

            void clearBlacklist();

            void clearWhitelist();

            ompl::base::Cost getCost() const;

            ompl::base::Cost getEdgeInCost() const;

            void registerExpansion();

            void registerRewiringExpansion();

            void markPruned();

            void markUnpruned();
 
            // ---
            // Edge queue lookups.
            // ---

            void insertInEdgeQueueInLookup(const SearchQueue::EdgeQueueElemPtr &inEdge);

            void insertInEdgeQueueOutLookup(const SearchQueue::EdgeQueueElemPtr &outEdge);

            void removeFromEdgeQueueInLookup(const SearchQueue::EdgeQueueElemPtr &inEdge);

            void removeFromEdgeQueueOutLookup(const SearchQueue::EdgeQueueElemPtr &outEdge);

            void removeFromEdgeQueueInLookup(const SearchQueue::EdgeQueueElemPtrVector::const_iterator &inEdge);

            void removeFromEdgeQueueOutLookup(const SearchQueue::EdgeQueueElemPtrVector::const_iterator &outEdge);

            BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator edgeQueueInLookupConstBegin();

            BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator edgeQueueOutLookupConstBegin();

            BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator edgeQueueInLookupConstEnd();

            BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator edgeQueueOutLookupConstEnd();

            unsigned int edgeQueueInLookupSize();

            unsigned int edgeQueueOutLookupSize();

            void clearEdgeQueueInLookup();

            void clearEdgeQueueOutLookup();
 
        private:
            // ---
            // Internal bookkeeping.
            // ---

            void updateCostAndDepth(bool cascadeUpdates = true);
 
            // ---
            // Member variables.
            // ---

            BITstar::VertexId id_;

            ompl::base::SpaceInformationPtr si_;

            const CostHelper *const costHelpPtr_;

            SearchQueue *const queuePtr_;

            ompl::base::State *state_;

            bool isRoot_;

            bool isPruned_{false};

            unsigned int depth_{0u};

            VertexPtr parentPtr_;

            ompl::base::Cost edgeCost_;

            ompl::base::Cost cost_;

            ompl::base::Cost costAtExpansion_;

            std::vector<VertexWeakPtr> children_;

            SearchQueue::EdgeQueueElemPtrVector edgeQueueInLookup_;

            SearchQueue::EdgeQueueElemPtrVector edgeQueueOutLookup_;

            std::set<BITstar::VertexId> childIdBlacklist_;

            std::set<BITstar::VertexId> childIdWhitelist_;

            unsigned int lookupApproximationId_{0u};

            unsigned int expansionApproximationId_{0u};

            unsigned int expansionSearchId_{0u};

            bool hasEverBeenExpandedAsRewiring_{false};

            const std::shared_ptr<const unsigned int> currentSearchId_;

            const std::shared_ptr<const unsigned int> currentApproximationId_;

            void removeFromEdgeQueueInLookup(const SearchQueue::EdgeQueueElemPtrVector::iterator &iterToDelete);

            void removeFromEdgeQueueOutLookup(const SearchQueue::EdgeQueueElemPtrVector::iterator &iterToDelete);

            void clearLookupsIfOutdated();
        };  // class Vertex
    }  // namespace geometric
}  // namespace ompl
 
#endif  // OMPL_GEOMETRIC_PLANNERS_INFORMEDTREES_BITSTAR_VERTEX_