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>

// 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, CostHelper.h, ImplicitGraph.h, and SearchQueue.h are forward declared and then
// included in the source file (.cpp) as they are member classes of BITstar.

// Debug setting. Defining BITSTAR_DEBUG enables (significant) debug output. Do not enable unless necessary.
//#define BITSTAR_DEBUG

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 CostHelper;
            class ImplicitGraph;
            class SearchQueue;
            // Helpful alias declarations:
            typedef std::shared_ptr<Vertex> VertexPtr;
            typedef std::shared_ptr<const Vertex> VertexConstPtr;
            typedef std::weak_ptr<Vertex> VertexWeakPtr;
            typedef std::vector<VertexPtr> VertexPtrVector;
            typedef std::vector<VertexConstPtr> VertexConstPtrVector;
            typedef unsigned int VertexId;
            typedef std::pair<VertexPtr, VertexPtr> VertexPtrPair;
            typedef std::pair<VertexConstPtr, VertexConstPtr> VertexConstPtrPair;
            typedef std::vector<VertexPtrPair> VertexPtrPairVector;
            typedef std::vector<VertexConstPtrPair> VertexConstPtrPairVector;
            typedef std::shared_ptr<NearestNeighbors<VertexPtr>> VertexPtrNNPtr;
            typedef std::shared_ptr<CostHelper> CostHelperPtr;
            typedef std::shared_ptr<ImplicitGraph> ImplicitGraphPtr;
            typedef std::shared_ptr<SearchQueue> SearchQueuePtr;

            typedef std::function<std::string()> NameFunc;

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

            ~BITstar() override = default;

            void setup() override;

            void clear() override;

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

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

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

            ompl::base::Cost getNextEdgeValueInQueue();

            void getEdgeQueue(VertexConstPtrPairVector *edgesInQueue);

            void getVertexQueue(VertexConstPtrVector *verticesInQueue);

            unsigned int numIterations() const;

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

            unsigned int numBatches() const;

            // Planner settings:
            void setRewireFactor(double rewireFactor);

            double getRewireFactor() const;

            void setSamplesPerBatch(unsigned int n);

            unsigned int getSamplesPerBatch() const;

            void setUseKNearest(bool useKNearest);

            bool getUseKNearest() 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;

            void setConsiderApproximateSolutions(bool findApproximate);

            bool getConsiderApproximateSolutions() const;

            template <template <typename T> class NN>
            void setNearestNeighbors();

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

            void newBatch();

            void prune();

            void publishSolution();

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

            bool checkEdge(const VertexConstPtrPair &edge);

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

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

            void updateGoalVertex();

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

            void endSuccessMessage() const;

            void endFailureMessage() const;

            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;

            std::string batchesProgressProperty() const;

            std::string pruningProgressProperty() const;

            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():
            CostHelperPtr costHelpPtr_;

            ImplicitGraphPtr graphPtr_;

            SearchQueuePtr queuePtr_;

            VertexConstPtr curGoalVertex_;

            ompl::base::Cost bestCost_;

            unsigned int bestLength_;

            ompl::base::Cost prunedCost_;

            double prunedMeasure_;

            bool hasExactSolution_;

            bool stopLoop_;

            // Informational variables - Make sure initialized in setup and reset in clear
            unsigned int numBatches_;

            unsigned int numPrunings_;

            unsigned int numIterations_;

            unsigned int numRewirings_;

            unsigned int numEdgeCollisionChecks_;

            // Parameters - Set defaults in construction/setup and DO NOT reset in clear.
            unsigned int samplesPerBatch_;

            bool usePruning_;

            double pruneFraction_;

            bool stopOnSolnChange_;
        };  // class: BITstar

        // Basic helpers
        template <typename T, typename U>
        std::pair<T, U> operator+(const std::pair<T, U> &lhs, const std::pair<T, U> &rhs)
        {
            return std::make_pair(lhs.first + rhs.first, lhs.second + rhs.second);
        }

    }  // geometric
}  // ompl
#endif  // OMPL_GEOMETRIC_PLANNERS_BITSTAR_BITSTAR_