Wiselib
wiselib.testing/algorithms/topology/cbtc/cbtc_topology.h
Go to the documentation of this file.
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_TOPOLOGY_CBTC_TOPOLOGY_H__
00020 #define __ALGORITHMS_TOPOLOGY_CBTC_TOPOLOGY_H__
00021 
00022 #include "algorithms/topology/cbtc/cbtc_topology_message.h"
00023 #include "algorithms/topology/cbtc/cbtc_topology_neighbours.h"
00024 #include "algorithms/topology/topology_control_base.h"
00025 #include "internal_interface/position/position.h"
00026 #include "util/pstl/vector_static.h"
00027 #include <math.h> // NOTE: required for atan
00028 
00029 #define PI 3.1415926535
00030 
00031 namespace wiselib
00032 {
00033 
00039    template<typename OsModel_P,
00040             typename Localization_P,
00041             typename Radio_P,
00042             uint16_t MAX_NODES = 32>
00043    class CbtcTopology: public TopologyBase<OsModel_P>
00044    {
00045    public:
00046       typedef OsModel_P OsModel;
00047       typedef Radio_P Radio;
00048 
00049 #ifdef DEBUG
00050       typedef typename OsModel::Debug Debug;
00051 #endif
00052       typedef Localization_P Localization;
00053 
00054       typedef typename OsModel_P::Timer Timer;
00055 
00056       typedef CbtcTopology<OsModel, Localization, Radio, MAX_NODES> self_type;
00057       typedef CbtcTopologyMessage<OsModel, Radio> TopologyMessage;
00058       typedef CbtcTopologyNeighbours<OsModel, Radio, MAX_NODES> Neighbours;
00059 
00060       typedef typename OsModel_P::size_t size_type;
00061 
00062       typedef typename Radio::node_id_t node_id_t;
00063       typedef typename Radio::size_t size_t;
00064       typedef typename Radio::block_data_t block_data_t;
00065 
00066       typedef typename Radio::TxPower Power;
00067 
00068       typedef typename Timer::millis_t millis_t;
00069 
00070       typedef typename Localization::position_t position_t;
00071       typedef vector_static<OsModel, node_id_t, MAX_NODES> Neighbors;
00072 
00075       CbtcTopology();
00076       ~CbtcTopology();
00078 
00081       void enable( void );
00082       void disable( void );
00084 
00087       void timer_elapsed_first_phase( void *userdata );
00088       void timer_elapsed_second_phase( void *userdata );
00090 
00093       void receive( node_id_t from, size_t len, block_data_t *data );
00095 
00096       inline void set_startup_time( millis_t startup_time )
00097       { startup_time_ = startup_time; };
00098 
00099       inline void set_work_period_1( millis_t work_period )
00100       { work_period_1 = work_period; };
00101       
00102       inline void set_work_period_2( millis_t work_period )
00103       { work_period_2 = work_period; };
00104 
00105       inline void set_alpha(double angle)
00106       { alpha = angle; };
00107 
00108       Neighbors &topology(){
00109          N.clear();
00110          size_type i;
00111          for ( i = 0; i < neighbours.size(); ++i ){
00112             N.push_back(neighbours[i].id);
00113          }
00114          return N;
00115       }
00116       
00117 #ifdef DEBUG
00118       void init( Localization &loc, Radio& radio, Timer& timer, Debug& debug ) {
00119          loc_=&loc;
00120          radio_ = &radio;
00121          timer_ = &timer;
00122          debug_ = &debug;
00123       }
00124 #else
00125       void init( Localization &loc, Radio& radio, Timer& timer) {
00126          loc_=&loc;
00127          radio_ = &radio;
00128          timer_ = &timer;
00129       }
00130 #endif
00131 
00132       void destruct() {
00133       }
00134 
00135    private:
00136 
00137       Radio& radio()
00138       { return *radio_; }
00139       
00140       Timer& timer()
00141       { return *timer_; }
00142       
00143 #ifdef DEBUG
00144       Debug& debug()
00145       { return *debug_; }
00146 #endif
00147       Localization * loc_;
00148       Radio * radio_;
00149       Timer * timer_;
00150 #ifdef DEBUG
00151       Debug * debug_;
00152 #endif
00153 
00156       enum CbtcTopologyMsgIds {
00157          CbtcMsgIdHello = 200, 
00158          CbtcMsgIdAck = 201, 
00159          CbtcMsgIdAsymmetric = 202, 
00160          CbtcMsgIdNDP = 203, 
00161       };
00162             
00163       millis_t startup_time_;
00164       millis_t work_period_1;
00165       millis_t work_period_2;
00166 
00167       double alpha;
00168 
00169       TopologyMessage helloMessage;
00170       TopologyMessage ackMessage;
00171       TopologyMessage asymmetricMessage;
00172       TopologyMessage NDPMessage;
00173 
00174       Neighbours neighbours;
00175 
00176       Power selfpower;
00177       position_t selfposition;
00178 
00179       Neighbors N; // Topology
00180 
00181       bool just_started;
00182       bool first_phase_done;
00183       bool boundary_node;
00184       bool enabled;
00185 
00186       void generate_topology();
00187       inline bool check_gap();
00188       inline void shrinkback();
00189       inline void pairwise();
00190       inline void pairwise_one_node(size_type index, int power);
00191    };
00192 
00193    // -----------------------------------------------------------------------
00194    // -----------------------------------------------------------------------
00195    // -----------------------------------------------------------------------
00196    template<typename OsModel_P,
00197             typename Localization_P,
00198             typename Radio_P,
00199             uint16_t MAX_NODES>
00200    CbtcTopology<OsModel_P, Localization_P, Radio_P, MAX_NODES>::
00201 	CbtcTopology()
00202       : startup_time_ ( 2000 ),
00203       work_period_1 ( 5000 ),
00204       work_period_2 (15000),
00205       alpha(2.617993),  // (5/6) * pi radians
00206       helloMessage( CbtcMsgIdHello ),
00207       ackMessage( CbtcMsgIdAck ),
00208       asymmetricMessage( CbtcMsgIdAsymmetric ),
00209       NDPMessage(CbtcMsgIdNDP),
00210       enabled(false)
00211    {
00212    }
00213 
00214    // -----------------------------------------------------------------------
00215    template<typename OsModel_P,
00216             typename Localization_P,
00217             typename Radio_P,
00218             uint16_t MAX_NODES>
00219    CbtcTopology<OsModel_P, Localization_P, Radio_P, MAX_NODES>::
00220 	~CbtcTopology()
00221    {
00222 #ifdef DEBUG
00223       debug().debug( "CbtcTopology Destroyed\n");
00224 #endif
00225    }
00226 
00227    // -----------------------------------------------------------------------
00228    template<typename OsModel_P,
00229             typename Localization_P,
00230             typename Radio_P,
00231             uint16_t MAX_NODES>
00232    void
00233    CbtcTopology<OsModel_P, Localization_P, Radio_P, MAX_NODES>::
00234 	enable( void )
00235    {
00236       enabled=true;
00237       just_started = true;
00238       selfpower = Power::MIN;
00239       first_phase_done = false;
00240       boundary_node = false;
00241 
00242       neighbours.set_id( radio().id() );
00243 
00244       radio().enable_radio();
00245 #ifdef DEBUG
00246       debug().debug( "%i: CbtcTopology Boots\n", radio().id() );
00247 #endif
00248       radio().template reg_recv_callback<self_type, &self_type::receive>(
00249                               this );
00250       timer().template set_timer<self_type,
00251       &self_type::timer_elapsed_first_phase>(startup_time_, this, 0 );
00252 
00253       selfposition = loc_->position();
00254    }
00255 
00256    // -----------------------------------------------------------------------
00257    template<typename OsModel_P,
00258             typename Localization_P,
00259             typename Radio_P,
00260             uint16_t MAX_NODES>
00261    void
00262    CbtcTopology<OsModel_P, Localization_P, Radio_P, MAX_NODES>::
00263    disable( void )
00264    {
00265          enabled=false;
00266 #ifdef DEBUG
00267       debug().debug( "%i: Called CbtcTopology::disable\n", radio().id() );
00268 #endif
00269    }
00270 
00271    // -----------------------------------------------------------------------
00272    template<typename OsModel_P,
00273             typename Localization_P,
00274             typename Radio_P,
00275             uint16_t MAX_NODES>
00276    void
00277    CbtcTopology<OsModel_P, Localization_P, Radio_P, MAX_NODES>::
00278 	timer_elapsed_first_phase( void* userdata )
00279    {
00280       if(!enabled)
00281          return;
00282       position_t pos;
00283 #ifdef DEBUG
00284       debug().debug( "%i: Executing TimerElapsed 'CbtcTopology' 1st Phase\n",
00285          radio().id() );
00286 #endif
00287 
00288       if(first_phase_done)
00289          return;
00290 
00291       if(selfpower == Power::MAX) {
00292          generate_topology();
00293          return;
00294       }
00295 
00296       if(just_started)
00297          just_started = false;
00298       else
00299          selfpower++;
00300 
00301       helloMessage.set_power(selfpower.to_ratio());
00302       pos = loc_->position();
00303       helloMessage.set_position(pos.x, pos.y);
00304 
00305       radio().set_power(selfpower);
00306       radio().send(
00307          radio().BROADCAST_ADDRESS,
00308          TopologyMessage::HELLO_SIZE,
00309          (uint8_t*)&helloMessage
00310       );
00311 
00312       timer().template set_timer<self_type, 
00313       &self_type::timer_elapsed_first_phase>( work_period_1, this, 0 );
00314    }
00315 
00316    // -----------------------------------------------------------------------
00317    template<typename OsModel_P,
00318             typename Localization_P,
00319             typename Radio_P,
00320             uint16_t MAX_NODES>
00321    void
00322    CbtcTopology<OsModel_P, Localization_P, Radio_P, MAX_NODES>::
00323 	timer_elapsed_second_phase( void* userdata )
00324    {
00325       if(!enabled)
00326          return;
00327       position_t pos;
00328 #ifdef DEBUG
00329       debug().debug( "%i: Executing TimerElapsed 'CbtcTopology' 2nd Phase\n",
00330          radio().id() );
00331 #endif
00332       if(!first_phase_done)
00333          return;
00334       
00335       
00336       if(neighbours.ndp_update() && check_gap()) {
00337          first_phase_done = false;
00338          timer().template set_timer<self_type, 
00339          &self_type::timer_elapsed_first_phase>( work_period_1, this, 0);
00340          return;
00341       }
00342 
00343       
00344       NDPMessage.set_power(selfpower.to_ratio());
00345       pos = loc_->position();
00346       NDPMessage.set_position(pos.x, pos.y);
00347 
00348       radio().send(
00349          radio().BROADCAST_ADDRESS,
00350          TopologyMessage::NDP_SIZE,
00351          (uint8_t*)&NDPMessage
00352       );
00353 
00354       timer().template set_timer<self_type,
00355        &self_type::timer_elapsed_second_phase>( work_period_2, this, 0 );
00356    }
00357 
00358    // -----------------------------------------------------------------------
00359    template<typename OsModel_P,
00360             typename Localization_P,
00361             typename Radio_P,
00362             uint16_t MAX_NODES>
00363    void
00364    CbtcTopology<OsModel_P, Localization_P, Radio_P, MAX_NODES>::
00365 	receive( node_id_t from, size_t len, block_data_t *data )
00366    {
00367       if(!enabled)
00368          return;
00369       TopologyMessage *msg = (TopologyMessage *)data;
00370       uint8_t msg_id = msg->msg_id();
00371 
00372       int p = msg->power();
00373       Power power;
00374       power.from_ratio(p);
00375 
00376       position_t pos;
00377       double angle;
00378 
00379       bool check_for_gap;
00380 
00381       if ( from == radio().id() )
00382          return;
00383 
00384       angle = atan2( msg->position_y() - selfposition.y, msg->position_x() - selfposition.x  );
00385 
00386       if ( msg_id == CbtcMsgIdHello ) {
00387 #ifdef DEBUG
00388          debug().debug( "%i: Received HELLO from %i with power: %i\n",
00389                   radio().id(), from, p );
00390 #endif
00391          neighbours.add_update_neighbour(from, p, angle, true);
00392          ackMessage.set_power(p);
00393          pos = loc_->position();
00394          ackMessage.set_position(pos.x, pos.y);
00395          radio().set_power( power );
00396          radio().send( from, TopologyMessage::ACK_SIZE, (uint8_t*)&ackMessage );
00397          radio().set_power( selfpower );
00398       }
00399       else if ( msg_id == CbtcMsgIdAck ) {
00400 #ifdef DEBUG
00401          debug().debug( "%i: Received ACK from %i x: %f y: %f\n",
00402                   radio().id(), from, msg->position_x(), msg->position_y() );
00403 
00404 #endif
00405          neighbours.add_update_neighbour(from, p, angle, false);
00406          if(!first_phase_done)
00407             generate_topology();
00408       }
00409       else if( msg_id == CbtcMsgIdAsymmetric ) {
00410 #ifdef DEBUG
00411          debug().debug( "%i: Received ASYMMETRIC from %i\n", radio().id(), from );
00412 #endif
00413          if(!first_phase_done)
00414             neighbours.add_asymmetric_to_remove(from);
00415       } else if( msg_id == CbtcMsgIdNDP) {
00416 #ifdef DEBUG
00417          debug().debug( "%i: Received NDP from %i\n", radio().id(), from );
00418 #endif
00419          if(!first_phase_done)
00420             return;
00421             
00422          check_for_gap = neighbours.add_update_ndp(from, p, angle);
00423          if(check_for_gap && check_gap()) {
00424             first_phase_done = false;
00425             timer().template set_timer<self_type, 
00426             &self_type::timer_elapsed_first_phase>( work_period_1, this, 0 );
00427             return;
00428          }
00429 
00430          shrinkback();
00431       }
00432    }
00433 
00434    // -----------------------------------------------------------------------
00435    template<typename OsModel_P,
00436             typename Localization_P,
00437             typename Radio_P,
00438             uint16_t MAX_NODES>
00439    void
00440    CbtcTopology<OsModel_P, Localization_P, Radio_P, MAX_NODES>::
00441 	generate_topology()
00442    {
00443       bool gap_exists;
00444       size_type i;
00445       int power;
00446 
00447       if(first_phase_done)
00448          return;
00449 
00450       gap_exists = check_gap();
00451 
00452       if(selfpower < Power::MAX && gap_exists)
00453          return;
00454 
00455       first_phase_done = true;
00456 
00457       if(gap_exists)
00458          boundary_node = true;
00459 
00460       neighbours.first_phase_done = true;
00461       neighbours.copy_to_NDP();
00462 
00463 #ifdef DEBUG
00464       debug().debug( "%i: First phase done. %soundary node\n",
00465          radio().id(), boundary_node ?"B":"Not b" );
00466       neighbours.print_basic();
00467 #endif
00468 
00469       //#Optimizations
00470 
00471       //Shrink-back operation
00472       if(boundary_node)
00473          shrinkback();
00474 
00475 #ifdef DEBUG
00476       neighbours.print_optimization("Nodes[Shrinkback]:");
00477 #endif
00478 
00479       //Asymmetric edge removal
00480       if(alpha <= 2 * PI / 3) {
00481          //Nodes told to be deleted
00482          for( i = 0; i < neighbours.ATR.size(); ++i){
00483             neighbours.delete_by_id(neighbours.ATR[i]);
00484          }
00485          neighbours.ATR.clear();
00486 
00487          //Tell nodes to delete self
00488          for( i = 0; i < neighbours.size(); ++i ){
00489             if(neighbours[i].asymmetric){
00490                radio().send(
00491                   neighbours[i].id,
00492                   TopologyMessage::ASYMMETRIC_SIZE,
00493                   (uint8_t*)&asymmetricMessage
00494                );
00495                neighbours.delete_by_index(i);
00496                i--;
00497             }
00498          }
00499       } else {
00500          for( i = 0; i < neighbours.size(); ++i ){
00501             neighbours[i].asymmetric = false;
00502          }
00503       }
00504 
00505 #ifdef DEBUG
00506       neighbours.print_optimization("Nodes[Asymmetric]:");
00507 #endif
00508 
00509       //Calculate the power to beacon
00510       if(!boundary_node) {
00511          power = (Power::MIN).to_ratio();
00512          for( i = 0; i < neighbours.size(); ++i ){
00513             if(neighbours[i].power > power)
00514                power = neighbours[i].power;
00515          }
00516          selfpower = Power::from_ratio(power);
00517       }
00518 
00519       //Pair-wise edge removal
00520       pairwise();
00521 
00522 #ifdef DEBUG
00523       neighbours.print_optimization("Nodes[Pairwise]:  ");
00524 #endif
00525 
00526       TopologyBase<OsModel>::notify_listeners();
00527 
00528       timer().template set_timer<self_type, 
00529       &self_type::timer_elapsed_second_phase>( work_period_2, this, 0 );
00530    }
00531 
00532    // -----------------------------------------------------------------------
00533    template<typename OsModel_P,
00534             typename Localization_P,
00535             typename Radio_P,
00536             uint16_t MAX_NODES>
00537    inline bool
00538    CbtcTopology<OsModel_P, Localization_P, Radio_P, MAX_NODES>::
00539    check_gap(){
00540       size_type first, i, j;
00541 
00542       if(neighbours.size() < 2)
00543          return true;
00544 
00545       for ( first = 0; first < neighbours.size(); ++first )
00546          if (!neighbours[first].asymmetric)
00547             break;
00548 
00549       if(first >= neighbours.size() - 1)
00550          return true;
00551 
00552       j = first;
00553       for ( i = first + 1; i < neighbours.size(); ++i ){
00554          if (!neighbours[i].asymmetric) {
00555             if(neighbours[i].angle - neighbours[j].angle > alpha)
00556                return true;
00557             j = i;
00558          }
00559       }
00560 
00561       return PI * 2.0 + neighbours[first].angle - neighbours[j].angle > alpha;
00562    }
00563 
00564 
00565    // -----------------------------------------------------------------------
00566    template<typename OsModel_P,
00567             typename Localization_P,
00568             typename Radio_P,
00569             uint16_t MAX_NODES>
00570    inline void
00571    CbtcTopology<OsModel_P, Localization_P, Radio_P, MAX_NODES>::
00572    shrinkback()
00573    {
00574       if(neighbours.size() < 3)
00575          return;
00576 
00577       int p_threshold;
00578       size_type i, j, k;
00579 
00580       for (p_threshold = (Power::MAX).to_ratio();
00581          p_threshold > (Power::MIN).to_ratio();
00582          p_threshold--) {
00583 
00584          for ( i = 0; i < neighbours.size(); ++i ){
00585             if (neighbours[i].power < p_threshold)
00586                break;
00587          }
00588 
00589          //There is no one or only one
00590          if (i >= neighbours.size() - 1)
00591             return;
00592 
00593          k = i;
00594          j = i + 1;
00595 
00596          for (; j < neighbours.size(); ++j ){
00597             if (neighbours[j].power < p_threshold) {
00598                if (j - k > 1 && neighbours[j].angle - neighbours[k].angle > alpha)
00599                   return;
00600                k = j;
00601             }
00602          }
00603 
00604          //There is only one
00605          if (k == i)
00606             return;
00607 
00608          if( !(i == 0 && k == neighbours.size() - 1) &&
00609          PI * 2 + neighbours[i].angle - neighbours[k].angle > alpha){
00610             return;
00611          }
00612 
00613          //delete same level
00614          neighbours.delete_by_power(p_threshold);
00615       }
00616 
00617    }
00618 
00619    // -----------------------------------------------------------------------
00620    template<typename OsModel_P,
00621             typename Localization_P,
00622             typename Radio_P,
00623             uint16_t MAX_NODES>
00624    inline void
00625    CbtcTopology<OsModel_P, Localization_P, Radio_P, MAX_NODES>::
00626    pairwise()
00627    {
00628       int i, j, fst_below, last_current, last_below;
00629       int p_threshold;
00630       double angle;
00631 
00632       for (p_threshold = (Power::MAX).to_ratio();
00633          p_threshold > (Power::MIN).to_ratio();
00634          p_threshold--) {
00635 
00636          for ( fst_below = 0; fst_below < (int)neighbours.size(); ++fst_below )
00637             if (neighbours[fst_below].power < p_threshold)
00638                break;
00639 
00640          if(fst_below == (int)neighbours.size())
00641             return;
00642 
00643          i = fst_below + 1;
00644          last_current = -1;
00645          last_below = fst_below;
00646 
00647          for (; i < (int)neighbours.size(); ++i ){
00648             //Node on the power level to be deleted
00649             if(neighbours[i].power == p_threshold) {
00650                if(last_current == -1) {
00651                   if(neighbours[i].angle - neighbours[last_below].angle > PI / 3)
00652                      last_current = i;
00653                }
00654             }
00655             //Otherwise node with a power below power to be deleted
00656             else {
00657                if(last_current != -1) {
00658                   for(j = last_current; j < i; j++) {
00659                      if(neighbours[i].angle - neighbours[j].angle > PI / 3)
00660                         break;
00661                   }
00662                   if (j < i)
00663                      return;
00664                   else
00665                      last_current = -1;
00666                }
00667                last_below = i;
00668             }
00669          }
00670 
00671          //There is just one nodes with power below power to be deleted
00672          if(last_below == fst_below){
00673             pairwise_one_node(fst_below, p_threshold);
00674             return;
00675          }
00676 
00677          //If all nodes at the end of the vector with power to be deleted
00678          //can be deleted
00679          if(last_current == -1) {
00680             angle = neighbours[last_below].angle - 2 * PI;
00681             for(i = 0; i < fst_below; i++){
00682                if(neighbours[i].angle - angle > PI / 3)
00683                   break;
00684             }
00685          }
00686          //Otherwise, if there are some nodes with power to be deleted
00687          // after the last node with power below
00688          else {
00689             angle = neighbours[fst_below].angle + 2 * PI;
00690             for(i = last_current; i < (int)neighbours.size(); i++){
00691                if(angle - neighbours[i].angle > PI / 3)
00692                   return;
00693             }
00694             i = 0;
00695          }
00696 
00697          for(; i < fst_below; i++){
00698             if(neighbours[fst_below].angle - neighbours[i].angle > PI / 3)
00699                return;
00700          }
00701 
00702          neighbours.delete_by_power(p_threshold);
00703       }
00704    }
00705 
00706    // -----------------------------------------------------------------------
00707    template<typename OsModel_P,
00708             typename Localization_P,
00709             typename Radio_P,
00710             uint16_t MAX_NODES>
00711    inline void
00712    CbtcTopology<OsModel_P, Localization_P, Radio_P, MAX_NODES>::
00713    pairwise_one_node(size_type index, int power)
00714    {
00715       size_type i;
00716 
00717       double angle1 = neighbours[index].angle;
00718       double angle2 = neighbours[index].angle - PI * 2;
00719       for ( i = 0; i < index; ++i ){
00720          if(angle1 - neighbours[i].angle < PI / 3)
00721             continue;
00722          else if (neighbours[i].angle - angle2 < PI / 3)
00723             continue;
00724          else
00725             return;
00726       }
00727 
00728       angle1 = neighbours[index].angle;
00729       angle2 = neighbours[index].angle + PI * 2;
00730       for (i = index + 1; i < neighbours.size(); ++i ){
00731          if(neighbours[i].angle - angle1 < PI / 3)
00732             continue;
00733          else if (angle2 - neighbours[i].angle  < PI / 3)
00734             continue;
00735          else
00736             return;
00737       }
00738 
00739       neighbours.delete_by_power(power);
00740    }
00741 }
00742 
00743 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines