BITstar.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 */

#ifndef OMPL_GEOMETRIC_PLANNERS_BITSTAR_BITSTAR_
#define OMPL_GEOMETRIC_PLANNERS_BITSTAR_BITSTAR_

//STL:
//std::string
#include <string>
//std::pair
#include <utility>
//std::vector
#include <vector>
//std::list
#include <list>

//OMPL:
//The base-class of planners:
#include "ompl/base/Planner.h"
//The nearest neighbours structure
#include "ompl/datastructures/NearestNeighbors.h"
//The informed sampler structure
#include "ompl/base/samplers/InformedStateSampler.h"
//Planner includes:
//#include "ompl/geometric/planners/PlannerIncludes.h"

//BIT*:
//The helper data classes, Vertex.h and IntegratedQueue.h are included *after* the declaration of the BITstar class as they are member classes of BITstar.



namespace ompl
{
    namespace geometric
    {
        class BITstar : public ompl::base::Planner
        {
        public:
            //Forward declarations so that the classes belong to BIT*:
            class Vertex;
            class IdGenerator;
            class IntegratedQueue;
            //Helpful typedefs:
            typedef std::shared_ptr<Vertex> VertexPtr;
            typedef std::shared_ptr<const Vertex> VertexConstPtr;
            typedef std::weak_ptr<Vertex> VertexWeakPtr;
            typedef std::shared_ptr<IntegratedQueue> IntegratedQueuePtr;
            typedef unsigned int VertexId;
            typedef std::pair<VertexPtr, VertexPtr> VertexPtrPair;
            typedef std::pair<VertexConstPtr, VertexConstPtr> VertexConstPtrPair;
            typedef std::shared_ptr< NearestNeighbors<VertexPtr> > VertexPtrNNPtr;


            BITstar(const base::SpaceInformationPtr& si, const std::string& name = "BITstar");

            virtual ~BITstar();

            virtual void setup();

            virtual void clear();

            base::PlannerStatus solve(const base::PlannerTerminationCondition& ptc);

            virtual void getPlannerData(base::PlannerData& data) const;

            // Planner info for debugging, etc:
            std::pair<const ompl::base::State*, const ompl::base::State*> getNextEdgeInQueue();

            ompl::base::Cost getNextEdgeValueInQueue();

            void getEdgeQueue(std::vector<std::pair<VertexConstPtr, VertexConstPtr> >* edgesInQueue);

            void getVertexQueue(std::vector<VertexConstPtr>* verticesInQueue);

            unsigned int numIterations() const;

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

            // Planner settings:
            template<template<typename T> class NN>
            void setNearestNeighbors();

            void setRewireFactor(double rewireFactor);

            double getRewireFactor() const;

            void setSamplesPerBatch(unsigned int n);

            unsigned int getSamplesPerBatch() const;

            void setKNearest(bool useKNearest);

            bool getKNearest() const;

            void setStrictQueueOrdering(bool beStrict);

            bool getStrictQueueOrdering() const;

            void setPruning(bool prune);

            bool getPruning() const;

            void setPruneThresholdFraction(double fractionalChange);

            double getPruneThresholdFraction() const;

            void setDelayRewiringUntilInitialSolution(bool delayRewiring);

            bool getDelayRewiringUntilInitialSolution() const;

            void setJustInTimeSampling(bool useJit);

            bool getJustInTimeSampling() const;

            void setDropSamplesOnPrune(bool dropSamples);

            bool getDropSamplesOnPrune() const;

            void setStopOnSolnImprovement(bool stopOnChange);

            bool getStopOnSolnImprovement() const;

        protected:
            //Everything is only protected so we can create modifications without duplicating code by deriving from the class:

            //Functions:
            void estimateMeasures();

            //BIT* primitives:
            virtual void iterate();

            void newBatch();

            void updateSamples(const VertexConstPtr& vertex);

            virtual bool prune();

            virtual bool resort();

            void publishSolution();

            //Helper functions for data manipulation and other low-level functions
            std::vector<const ompl::base::State*> bestPathFromGoalToStart() const;

            void updateStartAndGoalStates(const base::PlannerTerminationCondition& ptc);

            void pruneStartsGoals();

            void pruneSamples();

            bool checkEdge(const VertexConstPtrPair& edge);

            void dropSample(VertexPtr oldSample);

            void addEdge(const VertexPtrPair& newEdge, const ompl::base::Cost& edgeCost, const bool& removeFromFree, const bool& updateDescendants);

            void replaceParent(const VertexPtrPair& newEdge, const ompl::base::Cost& edgeCost, const bool& updateDescendants);

            void updateGoalVertex();

            void addSample(const VertexPtr& newSample);

            void addVertex(const VertexPtr& newVertex, const bool& removeFromFree);

            unsigned int nearestSamples(const VertexPtr& vertex, std::vector<VertexPtr>* neighbourSamples);

            unsigned int nearestVertices(const VertexPtr& vertex, std::vector<VertexPtr>* neighbourVertices);

            //Helper functions for sorting queues/nearest-neighbour structures and the related calculations.
            double nnDistance(const VertexConstPtr& a, const VertexConstPtr& b) const;

            //Helper functions for various heuristics.
            ompl::base::Cost lowerBoundHeuristicVertex(const VertexConstPtr& vertex) const;

            ompl::base::Cost currentHeuristicVertex(const VertexConstPtr& vertex) const;

            ompl::base::Cost lowerBoundHeuristicEdge(const VertexConstPtrPair& edgePair) const;

            ompl::base::Cost currentHeuristicEdge(const VertexConstPtrPair& edgePair) const;

            ompl::base::Cost currentHeuristicEdgeTarget(const VertexConstPtrPair& edgePair) const;

            ompl::base::Cost costToComeHeuristic(const VertexConstPtr& vertex) const;

            ompl::base::Cost edgeCostHeuristic(const VertexConstPtrPair& edgePair) const;

            ompl::base::Cost costToGoHeuristic(const VertexConstPtr& vertex) const;

            ompl::base::Cost trueEdgeCost(const VertexConstPtrPair& edgePair) const;

            ompl::base::Cost neighbourhoodCost(const VertexConstPtr& vertex) const;

            bool isCostWorseThan(const ompl::base::Cost& a, const ompl::base::Cost& b) const;

            bool isCostNotEquivalentTo(const ompl::base::Cost& a, const ompl::base::Cost& b) const;

            bool isCostBetterThanOrEquivalentTo(const ompl::base::Cost& a, const ompl::base::Cost& b) const;

            bool isCostWorseThanOrEquivalentTo(const ompl::base::Cost& a, const ompl::base::Cost& b) const;

            ompl::base::Cost combineCosts(const ompl::base::Cost& a, const ompl::base::Cost& b, const ompl::base::Cost& c) const;

            ompl::base::Cost combineCosts(const ompl::base::Cost& a, const ompl::base::Cost& b, const ompl::base::Cost& c, const ompl::base::Cost& d) const;

            double fractionalChange(const ompl::base::Cost& newCost, const ompl::base::Cost& oldCost) const;

            double fractionalChange(const ompl::base::Cost& newCost, const ompl::base::Cost& oldCost, const ompl::base::Cost& refCost) const;

            //Helper functions to calculate parameters:
            void initializeNearestTerms();

            virtual void updateNearestTerms();

            double calculateR(unsigned int N) const;

            unsigned int calculateK(unsigned int N) const;

            double minimumRggR() const;

            double minimumRggK() const;

            //Helper functions for logging
            virtual void goalMessage() const;

            virtual void endSuccessMessage() const;

            virtual void endFailureMessage() const;

            virtual void statusMessage(const ompl::msg::LogLevel& msgLevel, const std::string& status) const;

            // Planner progress property functions
            std::string bestCostProgressProperty() const;

            std::string bestLengthProgressProperty() const;

            std::string currentFreeProgressProperty() const;

            std::string currentVertexProgressProperty() const;

            std::string vertexQueueSizeProgressProperty() const;

            std::string edgeQueueSizeProgressProperty() const;

            std::string iterationProgressProperty() const;

            unsigned int numBatches() const;
            std::string batchesProgressProperty() const;

            std::string pruningProgressProperty() const;

            virtual std::string totalStatesCreatedProgressProperty() const;

            std::string verticesConstructedProgressProperty() const;

            std::string statesPrunedProgressProperty() const;

            std::string verticesDisconnectedProgressProperty() const;

            std::string rewiringProgressProperty() const;

            std::string stateCollisionCheckProgressProperty() const;

            std::string edgeCollisionCheckProgressProperty() const;

            std::string nearestNeighbourProgressProperty() const;

            std::string edgesProcessedProgressProperty() const;



            //Variables -- Make sure every one is configured in setup() and reset in clear():
            ompl::RNG                                                rng_;

            ompl::base::InformedSamplerPtr                           sampler_;

            ompl::base::OptimizationObjectivePtr                     opt_;

            std::list<VertexPtr>                                     startVertices_;

            std::list<VertexPtr>                                     goalVertices_;

            std::list<VertexPtr>                                     prunedStartVertices_;

            std::list<VertexPtr>                                     prunedGoalVertices_;

            VertexPtr                                                curGoalVertex_;

            VertexPtrNNPtr                                           freeStateNN_;

            VertexPtrNNPtr                                           vertexNN_;

            IntegratedQueuePtr                                       intQueue_;

            std::vector<VertexPtr>                                   newSamples_;

            std::vector<VertexPtr>                                   recycledSamples_;

            unsigned int                                             numUniformStates_;

            double                                                   r_;

            double                                                   k_rgg_;

            unsigned int                                            k_;

            ompl::base::Cost                                         bestCost_;

            unsigned int                                             bestLength_;

            ompl::base::Cost                                         prunedCost_;

            double                                                   prunedMeasure_;

            ompl::base::Cost                                         minCost_;

            ompl::base::Cost                                         costSampled_;

            bool                                                     hasSolution_;

            bool                                                     stopLoop_;


            //Informational variables - Make sure initialized in setup and reset in clear
            bool                                                     approximateSoln_;

            double                                                   approximateDiff_;

            unsigned int                                             numIterations_;

            unsigned int                                             numBatches_;

            unsigned int                                             numPrunings_;

            unsigned int                                             numSamples_;

            unsigned int                                             numVertices_;

            unsigned int                                             numFreeStatesPruned_;

            unsigned int                                             numVerticesDisconnected_;

            unsigned int                                             numRewirings_;

            unsigned int                                             numStateCollisionChecks_;

            unsigned int                                             numEdgeCollisionChecks_;

            unsigned int                                             numNearestNeighbours_;

            unsigned int                                             numEdgesProcessed_;

            //Parameters - Set defaults in construction/setup and DO NOT reset in clear.
            bool                                                     useStrictQueueOrdering_;

            double                                                   rewireFactor_;

            unsigned int                                             samplesPerBatch_;

            bool                                                     useKNearest_;

            bool                                                     usePruning_;

            double                                                   pruneFraction_;

            bool                                                     delayRewiring_;

            bool                                                     useJustInTimeSampling_;

            bool                                                     dropSamplesOnPrune_;

            bool                                                     stopOnSolnChange_;
        }; //class: BITstar
    } //geometric
} //ompl


//BIT* Includes:
//The Vertex ID generator class
#include "ompl/geometric/planners/bitstar/datastructures/IdGenerator.h"
//My vertex class:
#include "ompl/geometric/planners/bitstar/datastructures/Vertex.h"
//My queue class
#include "ompl/geometric/planners/bitstar/datastructures/IntegratedQueue.h"

#endif //OMPL_GEOMETRIC_PLANNERS_BITSTAR_BITSTAR_