SpaceInformation.h

/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2010, Rice University
*  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 Rice University 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.
*********************************************************************/
 
/* Author: Ioan Sucan */
 
#ifndef OMPL_BASE_SPACE_INFORMATION_
#define OMPL_BASE_SPACE_INFORMATION_
 
#include "ompl/base/State.h"
#include "ompl/base/StateValidityChecker.h"
#include "ompl/base/MotionValidator.h"
#include "ompl/base/StateSpace.h"
#include "ompl/base/ValidStateSampler.h"
 
#include "ompl/util/ClassForward.h"
#include "ompl/util/Console.h"
#include "ompl/util/Exception.h"
 
#include <functional>
#include <utility>
#include <cstdlib>
#include <vector>
#include <iostream>
 
namespace ompl
{
    namespace base
    {
 
        OMPL_CLASS_FORWARD(SpaceInformation);
 
        using StateValidityCheckerFn = std::function<bool(const State *)>;
 
        class SpaceInformation
        {
        public:
            // non-copyable
            SpaceInformation(const SpaceInformation &) = delete;
            SpaceInformation &operator=(const SpaceInformation &) = delete;
 
            SpaceInformation(StateSpacePtr space);
 
            virtual ~SpaceInformation() = default;
 
            bool isValid(const State *state) const
            {
                return stateValidityChecker_->isValid(state);
            }
 
            const StateSpacePtr &getStateSpace() const
            {
                return stateSpace_;
            }
 
            bool equalStates(const State *state1, const State *state2) const
            {
                return stateSpace_->equalStates(state1, state2);
            }
 
            bool satisfiesBounds(const State *state) const
            {
                return stateSpace_->satisfiesBounds(state);
            }
 
            double distance(const State *state1, const State *state2) const
            {
                return stateSpace_->distance(state1, state2);
            }
 
            void enforceBounds(State *state) const
            {
                stateSpace_->enforceBounds(state);
            }
 
            void printState(const State *state, std::ostream &out = std::cout) const
            {
                stateSpace_->printState(state, out);
            }
 
            void setStateValidityChecker(const StateValidityCheckerPtr &svc)
            {
                stateValidityChecker_ = svc;
                setup_ = false;
            }
 
            void setStateValidityChecker(const StateValidityCheckerFn &svc);
 
            const StateValidityCheckerPtr &getStateValidityChecker() const
            {
                return stateValidityChecker_;
            }
 
            void setMotionValidator(const MotionValidatorPtr &mv)
            {
                motionValidator_ = mv;
                setup_ = false;
            }
 
            const MotionValidatorPtr &getMotionValidator() const
            {
                return motionValidator_;
            }
 
            MotionValidatorPtr& getMotionValidator()
            {
                return motionValidator_;
            }
 
            void setStateValidityCheckingResolution(double resolution)
            {
                stateSpace_->setLongestValidSegmentFraction(resolution);
                setup_ = false;
            }
 
            double getStateValidityCheckingResolution() const
            {
                return stateSpace_->getLongestValidSegmentFraction();
            }
 
            unsigned int getStateDimension() const
            {
                return stateSpace_->getDimension();
            }
 
            double getSpaceMeasure() const
            {
                return stateSpace_->getMeasure();
            }
 
            State *allocState() const
            {
                return stateSpace_->allocState();
            }
 
            void allocStates(std::vector<State *> &states) const
            {
                for (auto &state : states)
                    state = stateSpace_->allocState();
            }
 
            void freeState(State *state) const
            {
                stateSpace_->freeState(state);
            }
 
            void freeStates(std::vector<State *> &states) const
            {
                for (auto &state : states)
                    stateSpace_->freeState(state);
            }
 
            void copyState(State *destination, const State *source) const
            {
                stateSpace_->copyState(destination, source);
            }
 
            State *cloneState(const State *source) const
            {
                return stateSpace_->cloneState(source);
            }
 
            StateSamplerPtr allocStateSampler() const
            {
                return stateSpace_->allocStateSampler();
            }
 
            ValidStateSamplerPtr allocValidStateSampler() const;
 
            void setValidStateSamplerAllocator(const ValidStateSamplerAllocator &vssa);
 
            void clearValidStateSamplerAllocator();
 
            double getMaximumExtent() const
            {
                return stateSpace_->getMaximumExtent();
            }
 
            bool searchValidNearby(State *state, const State *near, double distance, unsigned int attempts) const;
 
            bool searchValidNearby(const ValidStateSamplerPtr &sampler, State *state, const State *near,
                                   double distance) const;
 
            unsigned int randomBounceMotion(const StateSamplerPtr &sss, const State *start, unsigned int steps,
                                            std::vector<State *> &states, bool alloc) const;
 
            virtual bool checkMotion(const State *s1, const State *s2, std::pair<State *, double> &lastValid) const
            {
                return motionValidator_->checkMotion(s1, s2, lastValid);
            }
 
            virtual bool checkMotion(const State *s1, const State *s2) const
            {
                return motionValidator_->checkMotion(s1, s2);
            }
 
            bool checkMotion(const std::vector<State *> &states, unsigned int count,
                             unsigned int &firstInvalidStateIndex) const;
 
            bool checkMotion(const std::vector<State *> &states, unsigned int count) const;
 
            virtual unsigned int getMotionStates(const State *s1, const State *s2, std::vector<State *> &states,
                                                 unsigned int count, bool endpoints, bool alloc) const;
 
            unsigned int getCheckedMotionCount() const
            {
                return motionValidator_->getCheckedMotionCount();
            }
 
            double probabilityOfValidState(unsigned int attempts) const;
 
            double averageValidMotionLength(unsigned int attempts) const;
 
            void samplesPerSecond(double &uniform, double &near, double &gaussian, unsigned int attempts) const;
 
            virtual void printSettings(std::ostream &out = std::cout) const;
 
            virtual void printProperties(std::ostream &out = std::cout) const;
 
            ParamSet &params()
            {
                return params_;
            }
 
            const ParamSet &params() const
            {
                return params_;
            }
 
            virtual void setup();
 
            bool isSetup() const;
 
        protected:
            void setDefaultMotionValidator();
 
            StateSpacePtr stateSpace_;
 
            StateValidityCheckerPtr stateValidityChecker_;
 
            MotionValidatorPtr motionValidator_;
 
            bool setup_;
 
            ValidStateSamplerAllocator vssa_;
 
            ParamSet params_;
        };
    }
}
 
#endif