validity.cpp

Go to the documentation of this file.

/* Author: Zachary Kingston */

#include <ompl/base/spaces/WrapperStateSpace.h>

#include <se2ez/core/robot.h>
#include <se2ez/core/state.h>
#include <se2ez/core/collision.h>
#include <se2ez/core/box2d/collision.h>

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

using namespace se2ez;

///
/// ValidityChecker
///

plan::ValidityChecker::ValidityChecker(const ompl::base::SpaceInformationPtr &si, CollisionManagerPtr cm)
  : ompl::base::StateValidityChecker(si), cm_(cm)
{
    specs_.clearanceComputationType = ompl::base::StateValidityCheckerSpecs::BOUNDED_APPROXIMATE;
}

bool plan::ValidityChecker::isValid(const ompl::base::State *state) const
{
    auto *as = const_cast<StateSpace::StateType *>(state->as<StateSpace::StateType>());
    return !cm_->collide(as->state);
}

bool plan::ValidityChecker::isValid(const ompl::base::State *state, double &dist) const
{
    auto *as = const_cast<StateSpace::StateType *>(state->as<StateSpace::StateType>());
    dist = cm_->distance(as->state).distance;

    return dist > 0;
}

double plan::ValidityChecker::clearance(const ompl::base::State *state) const
{
    auto *as = const_cast<StateSpace::StateType *>(state->as<StateSpace::StateType>());
    return cm_->distance(as->state).distance;
}

CollisionManagerPtr plan::ValidityChecker::getCM()
{
    return cm_;
}

///
/// WrappedValidityChecker
///

plan::WrappedValidityChecker::WrappedValidityChecker(const ompl::base::SpaceInformationPtr &si,
                                                     CollisionManagerPtr cm)
  : plan::ValidityChecker(si, cm)
{
}

bool plan::WrappedValidityChecker::isValid(const ompl::base::State *state) const
{
    auto *cs = state->as<ompl::base::WrapperStateSpace::StateType>();
    return plan::ValidityChecker::isValid(cs->getState());
}

bool plan::WrappedValidityChecker::isValid(const ompl::base::State *state, double &dist) const
{
    auto *cs = state->as<ompl::base::WrapperStateSpace::StateType>();
    return plan::ValidityChecker::isValid(cs->getState(), dist);
}

double plan::WrappedValidityChecker::clearance(const ompl::base::State *state) const
{
    auto *cs = state->as<ompl::base::WrapperStateSpace::StateType>();
    return plan::ValidityChecker::clearance(cs->getState());
}