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, Marlin Strub */
 
#ifndef OMPL_GEOMETRIC_PLANNERS_INFORMEDTREES_BITSTAR_
#define OMPL_GEOMETRIC_PLANNERS_INFORMEDTREES_BITSTAR_
 
#include <string>
#include <utility>
#include <vector>
 
#include "ompl/base/Planner.h"
#include "ompl/base/samplers/InformedStateSampler.h"
#include "ompl/datastructures/NearestNeighbors.h"
 
// 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.
            // ---
 
            class Vertex;
            class IdGenerator;
            class CostHelper;
            class ImplicitGraph;
            class SearchQueue;
 
            // ---
            // Aliases.
            // ---
 
            using VertexPtr = std::shared_ptr<Vertex>;
 
            using VertexConstPtr = std::shared_ptr<const Vertex>;
 
            using VertexWeakPtr = std::weak_ptr<Vertex>;
 
            using VertexPtrVector = std::vector<VertexPtr>;
 
            using VertexConstPtrVector = std::vector<VertexConstPtr>;
 
            using VertexId = unsigned int;
 
            using VertexPtrPair = std::pair<VertexPtr, VertexPtr>;
 
            using VertexConstPtrPair = std::pair<VertexConstPtr, VertexConstPtr>;
 
            using VertexPtrPairVector = std::vector<VertexPtrPair>;
 
            using VertexConstPtrPairVector = std::vector<VertexConstPtrPair>;
 
            using VertexPtrNNPtr = std::shared_ptr<NearestNeighbors<VertexPtr>>;
 
            using NameFunc = std::function<std::string()>;
 
            BITstar(const base::SpaceInformationPtr &spaceInfo, const std::string &name = "kBITstar");
 
            virtual ~BITstar() override = default;
 
            void setup() override;
 
            void clear() override;
 
            base::PlannerStatus solve(const base::PlannerTerminationCondition &terminationCondition) override;
 
            void getPlannerData(base::PlannerData &data) const override;
 
            // ---
            // Debugging info.
            // ---
 
            std::pair<const ompl::base::State *, const ompl::base::State *> getNextEdgeInQueue();
 
            ompl::base::Cost getNextEdgeValueInQueue();
 
            void getEdgeQueue(VertexConstPtrPairVector *edgesInQueue);
 
            unsigned int numIterations() const;
 
            ompl::base::Cost bestCost() const;
 
            unsigned int numBatches() const;
 
            // ---
            // Settings.
            // ---
 
            void setRewireFactor(double rewireFactor);
 
            double getRewireFactor() const;
 
            void setAverageNumOfAllowedFailedAttemptsWhenSampling(std::size_t number);
 
            std::size_t getAverageNumOfAllowedFailedAttemptsWhenSampling() 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();
 
        protected:
            // ---
            // The settings that turn BIT* into ABIT*.
            // ---
 
            void setInitialInflationFactor(double factor);
 
            void setInflationScalingParameter(double parameter);
 
            void setTruncationScalingParameter(double parameter);
 
            void enableCascadingRewirings(bool enable);
 
            // ---
            // Getters specific to ABIT*.
            // ---
 
            double getInitialInflationFactor() const;
 
            double getInflationScalingParameter() const;
 
            double getTruncationScalingParameter() const;
 
            double getCurrentInflationFactor() const;
 
            double getCurrentTruncationFactor() const;
 
        private:
            // ---
            // High level primitives.
            // ---
 
            void iterate();
 
            void newBatch();
 
            void prune();
 
            void publishSolution();
 
            // ---
            // Low level primitives.
            // ---
 
            std::vector<const ompl::base::State *> bestPathFromGoalToStart() const;
 
            bool checkEdge(const VertexConstPtrPair &edge);
 
            void blacklistEdge(const VertexPtrPair &edge) const;
 
            void whitelistEdge(const VertexPtrPair &edge) const;
 
            void addEdge(const VertexPtrPair &edge, const ompl::base::Cost &edgeCost);
 
            void replaceParent(const VertexPtrPair &edge, const ompl::base::Cost &edgeCost);
 
            void updateGoalVertex();
 
            // ---
            // Logging.
            // ---
 
            // Helper functions for logging
            void goalMessage() const;
 
            void endSuccessMessage() const;
 
            void endFailureMessage() const;
 
            void statusMessage(const ompl::msg::LogLevel &logLevel, const std::string &status) const;
 
            // ---
            // Progress properties.
            // ---
 
            std::string bestCostProgressProperty() const;
 
            std::string bestLengthProgressProperty() const;
 
            std::string currentFreeProgressProperty() const;
 
            std::string currentVertexProgressProperty() 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;
 
            // ---
            // Member variables (Make all are configured in setup() and reset in reset()).
            // ---
 
            std::shared_ptr<CostHelper> costHelpPtr_{nullptr};
 
            std::shared_ptr<ImplicitGraph> graphPtr_{nullptr};
 
            std::shared_ptr<SearchQueue> queuePtr_{nullptr};
 
            double initialInflationFactor_{1.0};
 
            double truncationFactor_{1.0};
 
            double truncationScalingParameter_{0.0};
 
            double inflationScalingParameter_{0.0};
 
            VertexConstPtr curGoalVertex_{nullptr};
 
            // Gets set in setup to the proper calls from OptimizationObjective
            ompl::base::Cost bestCost_;
 
            unsigned int bestLength_{0u};
 
            // Gets set in setup to the proper calls from OptimizationObjective.
            ompl::base::Cost prunedCost_{std::numeric_limits<double>::infinity()};
 
            // Gets set in setup with the proper call to Planner::si_->getSpaceMeasure()
            double prunedMeasure_{0.0};
 
            bool isFinalSearchOnBatch_{false};
 
            bool hasExactSolution_{false};
 
            bool isSearchDone_{false};
 
            bool stopLoop_{false};
 
            unsigned int numBatches_{0u};
 
            unsigned int numPrunings_{0u};
 
            unsigned int numIterations_{0u};
 
            unsigned int numRewirings_{0u};
 
            unsigned int numEdgeCollisionChecks_{0u};
 
            // ---
            // Parameters - Set defaults in construction/setup and do not reset in clear.
            // ---
 
            unsigned int samplesPerBatch_{100u};
 
            bool isPruningEnabled_{true};
 
            double pruneFraction_{0.05};
 
            bool stopOnSolutionChange_{false};
        };  // class BITstar
    }       // namespace geometric
}  // namespace ompl
 
#endif  // OMPL_GEOMETRIC_PLANNERS_INFORMEDTREES_BITSTAR_