projection.cpp

Go to the documentation of this file.

/* Author: Zachary Kingston */

#include <se2ez/core/log.h>
#include <se2ez/core/math.h>
#include <se2ez/core/robot.h>
#include <se2ez/core/state.h>

#include <se2ez/plan/space.h>
#include <se2ez/plan/projection.h>

using namespace se2ez;

plan::Projection::Projection(const plan::StateSpace *ss, const std::string &frame)
  : ompl::base::ProjectionEvaluator(ss), robot_(ss->getRobot()), frame_(frame)
{
}

Eigen::Isometry2d plan::Projection::getPose(const ompl::base::State *state) const
{
    auto *as = const_cast<StateSpace::StateType *>(state->as<StateSpace::StateType>());
    return as->state->getPose(robot_, frame_);
}

void plan::Projection::defaultCellSizes()
{
    cellSizes_.resize(getDimension());
    for (unsigned int i = 0; i < getDimension(); ++i)
        cellSizes_[i] = 0.1;
}

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

unsigned int plan::R2Projection::getDimension() const
{
    return 2;
}

void plan::R2Projection::project(const ompl::base::State *state, Eigen::Ref<Eigen::VectorXd> projection) const
{
    projection = tf::flattenIsometry(getPose(state)).head(2);
}

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

unsigned int plan::SE2Projection::getDimension() const
{
    return 3;
}

void plan::SE2Projection::project(const ompl::base::State *state,
                                  Eigen::Ref<Eigen::VectorXd> projection) const
{
    projection = tf::flattenIsometry(getPose(state));
}