Program Listing for File VampStateValidityChecker.h

↰ Return to documentation for file (src/ompl/vamp/VampStateValidityChecker.h)

#pragma once

#include <ompl/base/SpaceInformation.h>
#include <ompl/base/State.h>
#include <ompl/base/StateValidityChecker.h>
#include <ompl/base/spaces/RealVectorStateSpace.h>

#include <ompl/vamp/Utils.h>

#include <vamp/planning/validate.hh>
#include <vamp/collision/environment.hh>

namespace ompl::vamp
{
    namespace ob = ompl::base;

    //==========================================================================
    // VAMP State Validity Checker for OMPL
    //==========================================================================

    template <typename Robot, std::size_t rake = ::vamp::FloatVectorWidth>
    class VampStateValidityChecker : public ob::StateValidityChecker
    {
    public:
        using Environment = ::vamp::collision::Environment<::vamp::FloatVector<rake>>;

        VampStateValidityChecker(ob::SpaceInformation *si, const Environment &env)
          : ob::StateValidityChecker(si), env_(env)
        {
        }

        VampStateValidityChecker(const ob::SpaceInformationPtr &si, const Environment &env)
          : ob::StateValidityChecker(si), env_(env)
        {
        }

        auto isValid(const ob::State *state) const -> bool override
        {
            auto configuration = ompl_to_vamp<Robot>(state);
            return ::vamp::planning::validate_motion<Robot, rake, 1>(configuration, configuration, env_);
        }

    private:
        const Environment &env_;
    };

}  // namespace ompl::vamp