Wiselib: wiselib.testing/intermediate/robot/pose

Wiselib
00001 // vim: set noexpandtab ts=3 sw=3:
00002 
00003 #ifndef POSE_ESTIMATION_H
00004 #define POSE_ESTIMATION_H
00005 
00006 #include "util/standalone_math.h"
00007 #include "position.h"
00008 
00009 namespace wiselib {
00010    
00019    template<
00020       typename OsModel_P,
00021       typename Odometer_P,
00022       typename Position_P = Position2D<OsModel_P, double>,
00023       typename Math_P = StandaloneMath<OsModel_P>
00024    >
00025    class PoseEstimation {
00026       public:
00027          typedef OsModel_P OsModel;
00028          typedef Odometer_P Odometer;
00029          typedef Position_P position_t;
00030          typedef Math_P Math;
00031          typedef typename Odometer::angle_t angle_t;
00032          typedef typename Odometer::distance_t distance_t;
00033          typedef PoseEstimation<OsModel_P, Odometer_P, Position_P, Math_P> self_type;
00034          typedef self_type* self_pointer_t;
00035          
00036          enum ErrorCodes {
00037             SUCCESS = OsModel::SUCCESS, ERR_UNSPEC = OsModel::ERR_UNSPEC
00038          };
00039          
00040          PoseEstimation();
00041          
00042          int init();
00043          int init(Odometer&);
00044          int destruct();
00045          
00046          int reset_orientation(angle_t a = angle_t());
00047          int reset_position(position_t p = position_t());
00048          
00049          position_t operator()() { return position(); }
00050          position_t position() { return position_; }
00051          angle_t orientation() { return orientation_; }
00052          
00053          void on_state_change(int);
00054          
00055       private:
00056          position_t position_;
00057          angle_t orientation_;
00058          
00059          distance_t latest_distance_;
00060          angle_t latest_angle_;
00061          
00062          typename Odometer::self_pointer_t odometer_;
00063    };
00064    
00065    template<
00066       typename OsModel_P,
00067       typename Odometer_P,
00068       typename Position_P,
00069       typename Math_P
00070    >
00071    PoseEstimation<OsModel_P, Odometer_P, Position_P, Math_P>::
00072 	PoseEstimation() : odometer_(0) {
00073    }
00074    
00075    template<
00076       typename OsModel_P,
00077       typename Odometer_P,
00078       typename Position_P,
00079       typename Math_P
00080    >
00081    int
00082    PoseEstimation<OsModel_P, Odometer_P, Position_P, Math_P>::
00083 	init() {
00084       destruct();
00085       init(*odometer_);
00086       return SUCCESS;
00087    }
00088    
00089    template<
00090       typename OsModel_P,
00091       typename Odometer_P,
00092       typename Position_P,
00093       typename Math_P
00094    >
00095    int
00096    PoseEstimation<OsModel_P, Odometer_P, Position_P, Math_P>::
00097 	init(Odometer_P& odometer) {
00098       latest_distance_ = 0;
00099       latest_angle_ = 0;
00100       odometer_ = &odometer;
00101       odometer_->template register_state_callback<self_type, &self_type::on_state_change>(this);
00102       return SUCCESS;
00103    }
00104    
00105    template<
00106       typename OsModel_P,
00107       typename Odometer_P,
00108       typename Position_P,
00109       typename Math_P
00110    >
00111    int
00112    PoseEstimation<OsModel_P, Odometer_P, Position_P, Math_P>::
00113 	destruct() {
00114       // TODO: odometer->unregister_state_callback(idx);
00115       return SUCCESS;
00116    }
00117    
00118    template<
00119       typename OsModel_P,
00120       typename Odometer_P,
00121       typename Position_P,
00122       typename Math_P
00123    >
00124    int
00125    PoseEstimation<OsModel_P, Odometer_P, Position_P, Math_P>::
00126 	reset_orientation(PoseEstimation<OsModel_P, Odometer_P, Position_P, Math_P>::angle_t a) {
00127       orientation_ = a;
00128       return SUCCESS;
00129    }
00130    
00131    template<
00132       typename OsModel_P,
00133       typename Odometer_P,
00134       typename Position_P,
00135       typename Math_P
00136    >
00137    int
00138    PoseEstimation<OsModel_P, Odometer_P, Position_P, Math_P>::
00139 	reset_position(PoseEstimation<OsModel_P, Odometer_P, Position_P, Math_P>::position_t p) {
00140       position_ = p;
00141       return SUCCESS;
00142    }
00143    
00144    template<
00145       typename OsModel_P,
00146       typename Odometer_P,
00147       typename Position_P,
00148       typename Math_P
00149    >
00150    void
00151    PoseEstimation<OsModel_P, Odometer_P, Position_P, Math_P>::
00152 	on_state_change(int state) {
00153       if(state == Odometer::DATA_AVAILABLE) {
00154          distance_t l = odometer_->distance() - latest_distance_;
00155          angle_t a = odometer_->angle() - latest_angle_;
00156          
00157          orientation_ += a;
00158          position_ += position_t(
00159             -l * Math::sin(orientation_),
00160             l * Math::cos(orientation_)
00161          );
00162          
00163          latest_distance_ = odometer_->distance();
00164          latest_angle_ = odometer_->angle();
00165       } // DATA_AVAILABLE
00166    } // on_state_change()
00167    
00168 } // ns wiselib
00169 
00170 #endif // POSE_ESTIMATION_H
00171