Wiselib: wiselib.testing/intermediate/robot/conditional

Wiselib
00001 // vim: set noexpandtab ts=3 sw=3:
00002 
00003 #ifndef CONDITIONAL_MOTION_H
00004 #define CONDITIONAL_MOTION_H
00005 
00006 #include "util/delegates/delegate.hpp"
00007 #include "util/pstl/vector_static.h"
00008 
00009 namespace wiselib {
00018    template<
00019       typename OsModel_P,
00020       typename Robot_P,
00021       typename Value_P = int,
00022       typename OsModel_P::size_t MAX_RECEIVERS = 16
00023    >
00024    class ConditionalMotion {
00025       public:
00026          typedef OsModel_P OsModel;
00027          typedef Robot_P Robot;
00028          typedef Value_P value_t;
00029          typedef ConditionalMotion<OsModel_P, Robot_P, Value_P, MAX_RECEIVERS> self_type;
00030          typedef self_type* self_pointer_t;
00031          typedef delegate0<value_t> condition_delegate_t;
00032          typedef delegate0<void> stop_delegate_t;
00033          typedef vector_static<OsModel, stop_delegate_t, MAX_RECEIVERS> stop_delegates_t;
00034          
00035          enum ErrorCodes {
00036             SUCCESS = OsModel::SUCCESS, ERR_UNSPEC = OsModel::ERR_UNSPEC
00037          };
00038          
00039          enum Mode { VALUE, NONZERO };
00040          
00041          int init();
00042          int init(Robot&);
00043          int destruct();
00044          
00050          int move();
00051          
00056          int check_condition();
00057          
00063          void on_state_change(int);
00064          
00072          template<typename T, value_t (T::*TMethod)(void)>
00073          int reg_condition(T* obj, Mode mode, value_t stop_value = value_t());
00074          
00079          template<typename T, void (T::*TMethod)(void)>
00080          int reg_stop_callback(T* obj);
00081          int unreg_stop_callback(typename OsModel::size_t idx);
00082          
00083       private:
00084          condition_delegate_t condition_;
00085          value_t condition_value_;
00086          Mode condition_mode_;
00087          bool active_;
00088          
00089          stop_delegates_t stop_callbacks_;
00090          typename Robot::self_pointer_t robot_;
00091          
00092          int notify_stop_receivers();
00093          int state_callback_idx_;
00094    };
00095    
00096    template<
00097       typename OsModel_P,
00098       typename Robot_P,
00099       typename Value_P,
00100       typename OsModel_P::size_t MAX_RECEIVERS
00101    >
00102    int
00103    ConditionalMotion<OsModel_P, Robot_P, Value_P, MAX_RECEIVERS>::
00104 	init() {
00105       destruct();
00106       init(*robot_);
00107       return SUCCESS;
00108    }
00109    
00110    template<
00111       typename OsModel_P,
00112       typename Robot_P,
00113       typename Value_P,
00114       typename OsModel_P::size_t MAX_RECEIVERS
00115    >
00116    int
00117    ConditionalMotion<OsModel_P, Robot_P, Value_P, MAX_RECEIVERS>::
00118 	init(Robot_P& robot) {
00119       active_ = false;
00120       robot_ = &robot;
00121       state_callback_idx_ = robot_->template register_state_callback<self_type, &self_type::on_state_change>(this);
00122       return SUCCESS;
00123    }
00124    
00125    template<
00126       typename OsModel_P,
00127       typename Robot_P,
00128       typename Value_P,
00129       typename OsModel_P::size_t MAX_RECEIVERS
00130    >
00131    int
00132    ConditionalMotion<OsModel_P, Robot_P, Value_P, MAX_RECEIVERS>::
00133 	destruct() {
00134       robot_.unregister_state_callback(state_callback_idx_);
00135       return SUCCESS;
00136    }
00137    
00138    template<
00139       typename OsModel_P,
00140       typename Robot_P,
00141       typename Value_P,
00142       typename OsModel_P::size_t MAX_RECEIVERS
00143    >
00144    int
00145    ConditionalMotion<OsModel_P, Robot_P, Value_P, MAX_RECEIVERS>::
00146 	move() {
00147       active_ = true;
00148       if(!condition_) {
00149          return ERR_UNSPEC;
00150       }
00151       robot_->move();
00152       return SUCCESS;
00153    }
00154    
00155    template<
00156       typename OsModel_P,
00157       typename Robot_P,
00158       typename Value_P,
00159       typename OsModel_P::size_t MAX_RECEIVERS
00160    >
00161    void
00162    ConditionalMotion<OsModel_P, Robot_P, Value_P, MAX_RECEIVERS>::
00163 	on_state_change(int state) {
00164       if(state == Robot::DATA_AVAILABLE) {
00165          check_condition();
00166       }
00167    }
00168    
00169    template<
00170       typename OsModel_P,
00171       typename Robot_P,
00172       typename Value_P,
00173       typename OsModel_P::size_t MAX_RECEIVERS
00174    >
00175    int
00176    ConditionalMotion<OsModel_P, Robot_P, Value_P, MAX_RECEIVERS>::
00177 	check_condition() {
00178       if(!active_) {
00179          return ERR_UNSPEC;
00180       }
00181       
00182       bool stop = true;
00183       
00184       if(condition_)
00185       switch(condition_mode_) {
00186          case VALUE:
00187             stop = (condition_() == condition_value_);
00188             break;
00189          case NONZERO:
00190             stop = static_cast<bool>(condition_());
00191             break;
00192       }
00193       
00194       if(stop) {
00195          active_ = false;
00196          robot_->stop();
00197          notify_stop_receivers();
00198       }
00199       return SUCCESS;
00200    }
00201    
00202    template<
00203       typename OsModel_P,
00204       typename Robot_P,
00205       typename Value_P,
00206       typename OsModel_P::size_t MAX_RECEIVERS
00207    >
00208    template<typename T, Value_P (T::*TMethod)(void)>
00209    int
00210    ConditionalMotion<OsModel_P, Robot_P, Value_P, MAX_RECEIVERS>::
00211 	reg_condition(T* obj, Mode mode, value_t value) {
00212       condition_ = condition_delegate_t::template from_method<T, TMethod>(obj);
00213       condition_mode_ = mode;
00214       condition_value_ = value;
00215       return 0;
00216    }
00217    
00218    template<
00219       typename OsModel_P,
00220       typename Robot_P,
00221       typename Value_P,
00222       typename OsModel_P::size_t MAX_RECEIVERS
00223    >
00224    template<typename T, void (T::*TMethod)(void)>
00225    int
00226    ConditionalMotion<OsModel_P, Robot_P, Value_P, MAX_RECEIVERS>::
00227 	reg_stop_callback(T* obj) {
00228       if(stop_callbacks_.empty()) {
00229          stop_callbacks_.assign(MAX_RECEIVERS, stop_delegate_t());
00230       }
00231       
00232       for(typename OsModel::size_t i=0; i<stop_callbacks_.size(); i++) {
00233          if(stop_callbacks_[i] == stop_delegate_t()) {
00234             stop_callbacks_[i] = stop_delegate_t::template from_method<T, TMethod>(obj);
00235             return i;
00236          }
00237       }
00238       return -1;
00239    } // reg_stop_callback()
00240    
00241    template<
00242       typename OsModel_P,
00243       typename Robot_P,
00244       typename Value_P,
00245       typename OsModel_P::size_t MAX_RECEIVERS
00246    >
00247    int
00248    ConditionalMotion<OsModel_P, Robot_P, Value_P, MAX_RECEIVERS>::
00249 	unreg_stop_callback(typename OsModel_P::size_t idx) {
00250       stop_callbacks_[idx] = stop_delegate_t();
00251       return OsModel::SUCCESS;
00252    }
00253    
00254    template<
00255       typename OsModel_P,
00256       typename Robot_P,
00257       typename Value_P,
00258       typename OsModel_P::size_t MAX_RECEIVERS
00259    >
00260    int
00261    ConditionalMotion<OsModel_P, Robot_P, Value_P, MAX_RECEIVERS>::
00262 	notify_stop_receivers() {
00263       typedef typename stop_delegates_t::iterator iter_t;
00264       
00265       for(iter_t iter = stop_callbacks_.begin(); iter != stop_callbacks_.end(); ++iter) {
00266          if(*iter != stop_delegate_t()) {
00267             (*iter)();
00268          }
00269       }
00270       return SUCCESS;
00271    } // notify_stop_receivers()
00272 
00273 } // namespace wiselib
00274 
00275 #endif // CONDITIONAL_MOTION_H
00276