SearchQueue.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_SEARCHQUEUE_
#define OMPL_GEOMETRIC_PLANNERS_INFORMEDTREES_BITSTAR_SEARCHQUEUE_
 
#include <array>
#include <functional>
#include <map>
#include <unordered_map>
#include <utility>
#include <vector>
 
#include "ompl/base/Cost.h"
#include "ompl/base/OptimizationObjective.h"
#include "ompl/datastructures/BinaryHeap.h"
#include "ompl/datastructures/NearestNeighbors.h"
#include "ompl/geometric/planners/informedtrees/BITstar.h"
 
namespace ompl
{
    namespace geometric
    {

        class BITstar::SearchQueue
        {
        public:
            // ---
            // Aliases.
            // ---

            using SortKey = std::array<ompl::base::Cost, 3u>;

            using SortKeyAndVertexPtrPair = std::pair<SortKey, VertexPtrPair>;

            using EdgeComparisonFunction =
                std::function<bool(const SortKeyAndVertexPtrPair &, const SortKeyAndVertexPtrPair &)>;

            using EdgeQueue = ompl::BinaryHeap<SortKeyAndVertexPtrPair, EdgeComparisonFunction>;

            using EdgeQueueElemPtr = EdgeQueue::Element *;

            using EdgeQueueElemPtrVector = std::vector<EdgeQueueElemPtr>;
 
            // ---
            // Construction, initialization, and destruction.
            // ---

            SearchQueue(NameFunc nameFunc);

            virtual ~SearchQueue() = default;

            void setup(CostHelper *costHelpPtr, ImplicitGraph *graphPtr);

            void reset();

            void enableCascadingRewirings(bool enable);
 
            // ---
            // Insertion.
            // ---

            void insertOutgoingEdges(const VertexPtr &vertex);

            void insertOutgoingEdgesOfStartVertices();

            void insertOutgoingEdgesOfInconsistentVertices();

            void addToInconsistentSet(const VertexPtr &vertex);
 
            // ---
            // Access.
            // ---

            VertexPtrPair getFrontEdge();

            SortKey getFrontEdgeValue();

            VertexPtrPair popFrontEdge();
 
            // ---
            // Modification.
            // ---

            void clear();

            void clearInconsistentSet();

            void rebuildEdgeQueue();

            void update(const EdgeQueueElemPtr elementPtr);

            void setInflationFactor(double factor);

            void registerSolutionCost(const ompl::base::Cost &solutionCost);

            void removeInEdgesConnectedToVertexFromQueue(const VertexPtr &vertex);

            void removeOutEdgesConnectedToVertexFromQueue(const VertexPtr &vertex);

            void removeAllEdgesConnectedToVertexFromQueue(const VertexPtr &vertex);

            void removeFromInconsistentSet(const VertexPtr &vertex);
 
            // ---
            // Information access.
            // ---

            double getInflationFactor() const;

            std::shared_ptr<const unsigned int> getSearchId() const;

            bool canPossiblyImproveCurrentSolution(const VertexPtr &state) const;

            bool canPossiblyImproveCurrentSolution(const VertexPtrPair &edge) const;

            unsigned int numEdges();

            bool isEmpty();

            void getEdges(VertexConstPtrPairVector *edgeQueue);

            unsigned int numEdgesPopped() const;
 
        private:
            // ---
            // High level primitives.
            // ---

            void enqueueEdges(const VertexPtr &parent, const VertexPtrVector &possibleChildren);

            void enqueueEdgeConditionally(const VertexPtr &parent, const VertexPtr &child);

            void enqueueEdge(const VertexPtrPair &edge);
 
            // ---
            // Sorting.
            // ---

            SortKey createSortKey(const VertexPtrPair &edge) const;

            bool lexicographicalBetterThan(const std::array<ompl::base::Cost, 3> &lhs,
                                           const std::array<ompl::base::Cost, 3> &rhs) const;
 
            // ---
            // Debug helpers.
            // ---

            void assertSetup() const;
 
            // ---
            // Member variables (Make all are configured in setup() and reset in reset()).
            // ---

            NameFunc nameFunc_;

            bool isSetup_{false};

            bool isCascadingOfRewiringsEnabled_{false};

            CostHelper *costHelpPtr_{nullptr};

            ImplicitGraph *graphPtr_{nullptr};

            EdgeQueue edgeQueue_;

            VertexPtrVector inconsistentVertices_;

            double inflationFactor_{1.0};

            ompl::base::Cost solutionCost_{std::numeric_limits<double>::infinity()};

            bool hasExactSolution_{false};

            const std::shared_ptr<unsigned int> searchId_;

            unsigned int numEdgesPopped_{0u};
 
        };  // class SearchQueue
    }  // namespace geometric
}  // namespace ompl
#endif  // OMPL_GEOMETRIC_PLANNERS_INFORMEDTREES_BITSTAR_SEARCHQUEUE_