Wiselib: wiselib.testing/algorithms/localization/distance_based/distance_based_localization

Wiselib
00001 /***************************************************************************
00002  ** This file is part of the generic algorithm library Wiselib.           **
00003  ** Copyright (C) 2008,2009 by the Wisebed (www.wisebed.eu) project.      **
00004  **                                                                       **
00005  ** The Wiselib is free software: you can redistribute it and/or modify   **
00006  ** it under the terms of the GNU Lesser General Public License as        **
00007  ** published by the Free Software Foundation, either version 3 of the    **
00008  ** License, or (at your option) any later version.                       **
00009  **                                                                       **
00010  ** The Wiselib is distributed in the hope that it will be useful,        **
00011  ** but WITHOUT ANY WARRANTY; without even the implied warranty of        **
00012  ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         **
00013  ** GNU Lesser General Public License for more details.                   **
00014  **                                                                       **
00015  ** You should have received a copy of the GNU Lesser General Public      **
00016  ** License along with the Wiselib.                                       **
00017  ** If not, see <http://www.gnu.org/licenses/>.                           **
00018  ***************************************************************************/
00019 #ifndef __ALGORITHMS_LOCALIZATION_DISTANCE_BASED_H
00020 #define __ALGORITHMS_LOCALIZATION_DISTANCE_BASED_H
00021 
00022 #include "algorithms/localization/distance_based/neighborhood/localization_neighbor_info.h"
00023 #include "algorithms/localization/distance_based/neighborhood/localization_neighborhood.h"
00024 #include "algorithms/localization/distance_based/neighborhood/localization_local_coordinate_system.h"
00025 #include "algorithms/localization/distance_based/util/localization_defutils.h"
00026 #include "algorithms/localization/distance_based/math/vec.h"
00027 
00028 namespace wiselib
00029 {
00030 
00032 
00065    template<typename OsModel_P,
00066             typename Radio_P,
00067             typename Timer_P,
00068             typename SharedData_P,
00069             typename DistanceModule_P,
00070             typename PositionModule_P,
00071             typename RefinementModule_P,
00072             typename Arithmatic_P ,
00073             typename Debug_P = typename OsModel_P::Debug>
00074    class DistanceBasedLocalization
00075    {
00076 
00077    public:
00078 
00079       typedef OsModel_P OsModel;
00080       typedef DistanceModule_P DistanceModule;
00081       typedef PositionModule_P PositionModule;
00082       typedef RefinementModule_P RefinementModule;
00083       typedef Radio_P Radio;
00084       typedef Timer_P Timer;
00085       typedef Debug_P Debug;
00086       typedef SharedData_P SharedData;
00087       typedef Arithmatic_P Arithmatic;
00088 
00089       typedef Vec<Arithmatic_P> position_t;
00090 
00091       typedef DistanceBasedLocalization<OsModel, Radio, Timer, SharedData, DistanceModule,
00092                   PositionModule, RefinementModule,Arithmatic ,Debug> self_type;
00093 
00094       typedef typename Radio::size_t size_t;
00095       typedef typename Radio::node_id_t node_id_t;
00096       typedef typename Radio::block_data_t block_data_t;
00097       // --------------------------------------------------------------------
00098       enum ErrorCodes
00099       {
00100          SUCCESS = OsModel::SUCCESS,
00101          ERR_UNSPEC = OsModel::ERR_UNSPEC,
00102          ERR_NOTIMPL = OsModel::ERR_NOTIMPL
00103       };
00104       // --------------------------------------------------------------------
00108       DistanceBasedLocalization();
00110 
00111       int init( Radio& radio, Timer& timer, Debug & debug,
00112                  SharedData& shared_data, DistanceModule& dist_module,
00113                  PositionModule& pos_module, RefinementModule& ref_module );
00114       int init( void );
00115       int destruct( void );
00116 
00123       void receive( node_id_t from, size_t len, block_data_t *data );
00131       void work( void *data );
00133       // --------------------------------------------------------------------
00134       SharedData& shared_data( void )
00135       { return shared_data_; }
00136       // --------------------------------------------------------------------
00137       void set_anchor( bool anchor )
00138       {
00139          shared_data_.set_anchor( anchor );
00140       }
00141       // --------------------------------------------------------------------
00142       void set_confidence( Arithmatic confidence )
00143       {
00144          shared_data_.set_confidence( confidence );
00145       }
00146       // --------------------------------------------------------------------
00147       void set_position( const Vec<Arithmatic_P>& position )
00148       {
00149          shared_data_.set_position( position );
00150       }
00151       // --------------------------------------------------------------------
00152       void set_active( bool active )
00153       {
00154          active_ = active;
00155       }
00156       // --------------------------------------------------------------------
00157       bool active( void )
00158       {
00159          return active_;
00160       }
00161 
00162       void inline roll_back(void){
00163         phase_  = distance;
00164         dist_module_->rollback();
00165         pos_module_->rollback();
00166         ref_module_->rollback();
00167         active_=true;
00168         //timer_->template set_timer<self_type, &self_type::work>( work_period_, this, 0 );
00169 
00170       }
00171 
00172 
00173 private:
00174       enum LocalizationPhase { distance, position, refinement };
00175       LocalizationPhase phase_;
00176 
00177       bool check_residue_;
00178       int idle_time_;
00179       int idle_shutdown_time_;
00180 
00181       int rollback_iteration_;
00182       int rollback_period_;
00183       int rollback_cnt_;
00184       int rollback_limit_;
00185       int work_period_;
00186 
00187       bool active_;
00188 
00189       typename Radio::self_pointer_t radio_;
00190       typename Timer::self_pointer_t timer_;
00191       typename Debug::self_pointer_t debug_;
00192 
00193       SharedData *shared_data_;
00194 
00195       DistanceModule *dist_module_;
00196       PositionModule *pos_module_;
00197       RefinementModule *ref_module_;
00198    };
00199    // -----------------------------------------------------------------------
00200    template<typename OsModel_P,
00201             typename Radio_P,
00202             typename Timer_P,
00203             typename SharedData_P,
00204             typename DistanceModule_P,
00205             typename PositionModule_P,
00206             typename RefinementModule_P,
00207             typename Arithmatic_P,
00208             typename Debug_P>
00209    DistanceBasedLocalization<OsModel_P, Radio_P, Timer_P, SharedData_P, DistanceModule_P, PositionModule_P, RefinementModule_P,Arithmatic_P ,Debug_P>::
00210    DistanceBasedLocalization()
00211       : phase_               ( distance ),
00212          check_residue_       ( true ),
00213          idle_time_           ( 5 ),
00214          idle_shutdown_time_  ( 0 ),
00215          rollback_iteration_  ( 0 ),
00216          rollback_period_     ( INT_MAX ),
00217          rollback_cnt_        ( 0 ),
00218          rollback_limit_      ( 0 ),
00219          //min_max
00220          work_period_         ( 1000 ),
00221          //work_period_         ( 1800 ),
00222          active_              ( true )
00223    {}
00224    // -----------------------------------------------------------------------
00225    template<typename OsModel_P,
00226             typename Radio_P,
00227             typename Timer_P,
00228             typename SharedData_P,
00229             typename DistanceModule_P,
00230             typename PositionModule_P,
00231             typename RefinementModule_P,
00232             typename Arithmatic_P,
00233             typename Debug_P>
00234    int
00235    DistanceBasedLocalization<OsModel_P, Radio_P, Timer_P, SharedData_P, DistanceModule_P, PositionModule_P, RefinementModule_P,Arithmatic_P ,Debug_P>::
00236    init( Radio& radio, Timer& timer, Debug & debug,
00237          SharedData& shared_data, DistanceModule& dist_module,
00238          PositionModule& pos_module, RefinementModule& ref_module )
00239    {
00240       radio_ = &radio;
00241       timer_ = &timer;
00242       debug_ = &debug;
00243       shared_data_ = &shared_data;
00244       dist_module_ = &dist_module;
00245       pos_module_ = &pos_module;
00246       ref_module_ = &ref_module;
00247 
00248       shared_data_->neighborhood().set_source( radio_->id() );
00249 
00250       dist_module_->set_shared_data( *shared_data_ );
00251       dist_module_->rollback();
00252       pos_module_->set_shared_data( *shared_data_ );
00253       pos_module_->rollback();
00254       ref_module_->set_shared_data( *shared_data_ );
00255       ref_module_->rollback();
00256 
00257       radio_->enable_radio();
00258 
00259 
00260 
00261 
00262       radio_->template reg_recv_callback<self_type, &self_type::receive>( this );
00263       timer_->template set_timer<self_type, &self_type::work>( work_period_, this, 0 );
00264 
00265       return SUCCESS;
00266    }
00267    // -----------------------------------------------------------------------
00268    template<typename OsModel_P,
00269             typename Radio_P,
00270             typename Timer_P,
00271             typename SharedData_P,
00272             typename DistanceModule_P,
00273             typename PositionModule_P,
00274             typename RefinementModule_P,
00275             typename Arithmatic_P,
00276             typename Debug_P>
00277    int
00278    DistanceBasedLocalization<OsModel_P, Radio_P, Timer_P, SharedData_P, DistanceModule_P, PositionModule_P, RefinementModule_P,Arithmatic_P, Debug_P>::
00279    init( void )
00280    {
00281       return ERR_NOTIMPL;
00282    }
00283    // -----------------------------------------------------------------------
00284    template<typename OsModel_P,
00285             typename Radio_P,
00286             typename Timer_P,
00287             typename SharedData_P,
00288             typename DistanceModule_P,
00289             typename PositionModule_P,
00290             typename RefinementModule_P,
00291             typename Arithmatic_P,
00292             typename Debug_P>
00293    int
00294    DistanceBasedLocalization<OsModel_P, Radio_P, Timer_P, SharedData_P, DistanceModule_P, PositionModule_P, RefinementModule_P,Arithmatic_P, Debug_P>::
00295    destruct( void )
00296    {
00297       return ERR_NOTIMPL;
00298    }
00299    // -----------------------------------------------------------------------
00300    template<typename OsModel_P,
00301             typename Radio_P,
00302             typename Timer_P,
00303             typename SharedData_P,
00304             typename DistanceModule_P,
00305             typename PositionModule_P,
00306             typename RefinementModule_P,
00307             typename Arithmatic_P,
00308             typename Debug_P>
00309    void
00310    DistanceBasedLocalization<OsModel_P, Radio_P, Timer_P, SharedData_P, DistanceModule_P, PositionModule_P, RefinementModule_P,Arithmatic_P, Debug_P>::
00311    receive( node_id_t from, size_t len, block_data_t *data )
00312    {
00313       dist_module_->receive( from, len, data );
00314       pos_module_->receive( from, len, data );
00315       ref_module_->receive( from, len, data );
00316    }
00317    // -----------------------------------------------------------------------
00318    template<typename OsModel_P,
00319             typename Radio_P,
00320             typename Timer_P,
00321             typename SharedData_P,
00322             typename DistanceModule_P,
00323             typename PositionModule_P,
00324             typename RefinementModule_P,
00325             typename Arithmatic_P,
00326             typename Debug_P>
00327    void
00328    DistanceBasedLocalization<OsModel_P, Radio_P, Timer_P, SharedData_P, DistanceModule_P, PositionModule_P, RefinementModule_P, Arithmatic_P, Debug_P>::
00329    work( void *data )
00330    {
00331       rollback_iteration_++;
00332 
00333       if ( phase_ == distance )
00334       {
00335          dist_module_->work();
00336 
00337          if ( dist_module_->finished() )
00338             phase_ = position;
00339       }
00340 
00341       if ( phase_ == position )
00342       {
00343          pos_module_->work();
00344 
00345          if ( pos_module_->finished() )
00346             phase_ = refinement;
00347       }
00348 
00349       if ( phase_ == refinement )
00350          ref_module_->work();
00351 
00352       if ( rollback_iteration_ == rollback_period_ )
00353       {
00354          rollback_iteration_ = 0;
00355          rollback_cnt_++;
00356 
00357          phase_ = distance;
00358          dist_module_->rollback();
00359          pos_module_->rollback();
00360          ref_module_->rollback();
00361       }
00362 
00363       if ( rollback_limit_ != 0 && rollback_cnt_ == rollback_limit_ )
00364          set_active( false );
00365 
00366       if ( rollback_limit_ == 0 && dist_module_->finished() && pos_module_->finished() && ref_module_->finished() )
00367          set_active( false );
00368 
00369       timer_->template set_timer<self_type, &self_type::work>( work_period_, this, 0 );
00370    }
00371 
00372 }// namespace wiselib
00373 #endif