projection.h

Go to the documentation of this file.

/* Author: Zachary Kingston */

#ifndef SE2EZ_PLAN_PROJECTION_
#define SE2EZ_PLAN_PROJECTION_

#include <ompl/base/ProjectionEvaluator.h>

#include <se2ez/core/class_forward.h>

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

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

        /** \cond IGNORE */
        SE2EZ_CLASS_FORWARD(R2Projection)
        SE2EZ_CLASS_FORWARD(SE2Projection)
        /** \endcond */

        class Projection : public ompl::base::ProjectionEvaluator
        {
        protected:
            Projection(const plan::StateSpace *ss, const std::string &frame);
            Eigen::Isometry2d getPose(const ompl::base::State *state) const;

            void defaultCellSizes() override;

            const RobotPtr robot_;
            const std::string frame_;
        };

        class R2Projection : public Projection
        {
        public:
            R2Projection(const plan::StateSpace *ss, const std::string &frame);

            unsigned int getDimension() const override;
            void project(const ompl::base::State *state,
                         Eigen::Ref<Eigen::VectorXd> projection) const override;
        };

        class SE2Projection : public Projection
        {
        public:
            SE2Projection(const plan::StateSpace *ss, const std::string &frame);

            unsigned int getDimension() const override;
            void project(const ompl::base::State *state,
                         Eigen::Ref<Eigen::VectorXd> projection) const override;
        };
    }  // namespace plan
}  // namespace se2ez

#endif