StateChecker.hpp

#include <string>

using namespace std;

//OCTOMAP//
#include <octomap/octomap.h>

using namespace octomap;

//OMPL//
#include <ompl/config.h>
#include <ompl/base/SpaceInformation.h>
#include <ompl/base/spaces/RealVectorStateSpace.h>

namespace ob = ompl::base;

struct Checking_Box
{
    double x;
    double y;
    double z;
};

class StateChecker : public ob::StateValidityChecker
{
    public:
    

        StateChecker(const ob::SpaceInformationPtr &si);

        void loadMap(const std::string mapFile);
        void loadMap(octomap::OcTree* octree);

        void setCollisionMask(double x, double y, double z = 0.2);


        void setDepthLimit(double depth_limit);


        virtual bool isValid(const ob::State *state) const;


        ~StateChecker();

    private :

        int planningDimension; 
        //Octomap
        octomap::OcTree* _map = nullptr; 
        double _map_res, _depth_limit;

        Checking_Box _check_box; 
};