space.h

Go to the documentation of this file.

/* Author: Zachary Kingston */

#ifndef SE2EZ_PLAN_SPACE_
#define SE2EZ_PLAN_SPACE_

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

#include <se2ez/core/class_forward.h>

namespace se2ez
{
    /** \cond IGNORE */
    SE2EZ_CLASS_FORWARD(Robot)
    SE2EZ_CLASS_FORWARD(State)
    SE2EZ_CLASS_FORWARD(Frame)
    /** \endcond */

    namespace plan
    {
        /** \cond IGNORE */
        SE2EZ_CLASS_FORWARD(StateSpace)
        SE2EZ_CLASS_FORWARD(StateSampler)
        /** \endcond */

        class StateSpace : public ompl::base::RealVectorStateSpace
        {
        public:
            class StateType : public ompl::base::RealVectorStateSpace::StateType
            {
            public:
                StateType(const RobotPtr &robot);
                StatePtr state;
            };

            StateSpace(const RobotPtr &robot);

            void enforceBounds(ompl::base::State *state) const override;
            bool satisfiesBounds(const ompl::base::State *state) const override;
            double distance(const ompl::base::State *state1, const ompl::base::State *state2) const override;
            bool equalStates(const ompl::base::State *state1, const ompl::base::State *state2) const override;
            void interpolate(const ompl::base::State *from, const ompl::base::State *to, double t,
                             ompl::base::State *state) const override;

            void copyState(ompl::base::State *destination, const ompl::base::State *source) const override;
            ompl::base::State *allocState() const override;
            ompl::base::State *allocState(const StatePtr &state) const;
            void freeState(ompl::base::State *state) const override;

            void printState(const ompl::base::State *state, std::ostream &out) const override;

            double *getValueAddressAtIndex(ompl::base::State *state, unsigned int index) const override;

            ompl::base::StateSamplerPtr allocDefaultStateSampler() const override;
            void registerProjections() override;

            const RobotPtr getRobot() const;

        private:
            const RobotPtr robot_;
            std::vector<std::pair<std::string, Robot::FrameDataPtr>> frames_;
        };
    }  // namespace plan
}  // namespace se2ez

#endif