Epsilon.h

Go to the documentation of this file.

 
#ifndef _GAMS_POSE_EPSILON_H_
#define _GAMS_POSE_EPSILON_H_
 
#include <iostream>
#include <string>
 
#include "gams/GamsExport.h"
#include "gams/pose/Position.h"
#include "gams/pose/Orientation.h"
 
namespace gams { namespace pose {
 
class GAMS_EXPORT PositionBounds {
public:
virtual bool check_position(
    const pose::Position &current,
    const pose::Position &target) const = 0;
 
virtual ~PositionBounds() = default;
};
 
class GAMS_EXPORT OrientationBounds {
public:
virtual bool check_orientation(
    const pose::Orientation &current,
    const pose::Orientation &target) const = 0;
 
virtual ~OrientationBounds() = default;
};
 
class GAMS_EXPORT PoseBounds :
public PositionBounds, public OrientationBounds {};
 
class GAMS_EXPORT Epsilon : public PoseBounds {
private:
double dist_ = 0.1, radians_ = M_PI/16;
public:
Epsilon() {}
 
Epsilon(double dist)
: dist_(dist) {}
 
Epsilon(double dist_tol, double radians_tol)
: dist_(dist_tol), radians_(radians_tol) {}
 
//
template<typename AngleUnits>
Epsilon(double dist, double angle, AngleUnits u)
: dist_(dist), radians_(u.to_radians(angle)) {}
 
bool check_position(
    const pose::Position &current,
    const pose::Position &target) const override {
return current.distance_to(target) <= dist_;
}
 
bool check_orientation(
    const pose::Orientation &current,
    const pose::Orientation &target) const override {
return fabs(current.angle_to(target)) <= radians_;
}
};
 
} }
 
#endif // _GAMS_POSE_EPSILON_H_