|
Wiselib
|
wiselib.testing/algorithms/topology/lmst/lmst_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 ** Author: Víctor López Ferrando, victorlopez90@gmail.com ** 00020 ***************************************************************************/ 00021 #ifndef __ALGORITHMS_TOPOLOGY_LMST_TOPOLOGY_H__ 00022 #define __ALGORITHMS_TOPOLOGY_LMST_TOPOLOGY_H__ 00023 00024 #include "algorithms/topology/lmst/lmst_topology_message.h" 00025 #include "algorithms/topology/topology_control_base.h" 00026 #include "internal_interface/position/position.h" 00027 #include "util/pstl/priority_queue.h" 00028 #include "util/pstl/vector_static.h" 00029 #include "util/pstl/pair.h" 00030 #include <limits> 00031 00032 namespace wiselib 00033 { 00034 00040 template<typename OsModel_P, 00041 typename Localization_P, 00042 typename Float=double, 00043 uint16_t MAX_NODES = 32, 00044 typename Radio_P = typename OsModel_P::Radio, 00045 typename Timer_P = typename OsModel_P::Timer> 00046 class LmstTopology : 00047 public TopologyBase<OsModel_P> 00048 { 00049 public: 00050 typedef OsModel_P OsModel; 00051 typedef Localization_P Localization; 00052 typedef Radio_P Radio; 00053 typedef Timer_P Timer; 00054 00055 #ifdef DEBUG 00056 typedef typename OsModel::Debug Debug; 00057 #endif 00058 00059 typedef LmstTopology<OsModel_P, Localization_P, Float, MAX_NODES, Radio_P, Timer_P> self_type; 00060 typedef LmstTopologyMessage<OsModel, Radio, Float> TopologyMessage; 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 Timer::millis_t millis_t; 00067 00068 typedef Float position_t; 00069 typedef pair<position_t, node_id_t> PPI; 00070 typedef PositionType<position_t> Position; 00071 typedef vector_static<OsModel, node_id_t, MAX_NODES> Neighbors; 00072 static const uint8_t POSITION_SIZE = sizeof( Position ); 00073 00076 LmstTopology(); 00077 ~LmstTopology(); 00079 00082 void enable( void ); 00083 void disable( void ); 00085 00088 void timer_elapsed( 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( millis_t work_period ) 00100 { work_period_ = work_period; }; 00101 00102 Neighbors &topology(){ 00103 return N; 00104 } 00105 00106 position_t range_assignment(){ 00107 return radius; 00108 } 00109 00110 #ifdef DEBUG 00111 void init( Localization &loc, Radio& radio, Timer& timer, Debug& debug ) { 00112 loc_ = &loc; 00113 radio_ = &radio; 00114 timer_ = &timer; 00115 debug_ = &debug; 00116 } 00117 #else 00118 void init( Localization &loc, Radio& radio, Timer& timer) { 00119 loc_ = &loc; 00120 radio_ = &radio; 00121 timer_ = &timer; 00122 } 00123 #endif 00124 00125 void destruct() { 00126 } 00127 00128 private: 00129 00130 Radio& radio() 00131 { return *radio_; } 00132 00133 Timer& timer() 00134 { return *timer_; } 00135 00136 #ifdef DEBUG 00137 Debug& debug() 00138 { return *debug_; } 00139 #endif 00140 00141 Localization * loc_; 00142 Radio * radio_; 00143 Timer * timer_; 00144 #ifdef DEBUG 00145 Debug * debug_; 00146 #endif 00147 00150 enum LmstTopologyMsgIds { 00151 LtMsgIdBroadcastPosition = 200, 00152 LtMsgIdNeighbourNotification = 201, 00153 }; 00154 00157 Neighbors N; // Topology 00158 position_t radius; 00160 00161 millis_t startup_time_; 00162 millis_t work_period_; 00163 00164 TopologyMessage positionMessage; 00165 uint8_t neighbourMessage; 00166 int callback_id; 00167 bool enabled; 00168 bool first; 00169 00170 void generate_topology(); 00171 00172 vector_static<OsModel, node_id_t, MAX_NODES> NV; 00173 vector_static<OsModel, Position, MAX_NODES> Pos; 00174 vector_static<OsModel, position_t, MAX_NODES> D; 00175 vector_static<OsModel, node_id_t, MAX_NODES> p; 00176 vector_static<OsModel, bool, MAX_NODES> is_in_PQ; 00177 priority_queue< OsModel, PPI, MAX_NODES*10 > PQ; 00178 00179 }; 00180 // ----------------------------------------------------------------------- 00181 // ----------------------------------------------------------------------- 00182 // ----------------------------------------------------------------------- 00183 template<typename OsModel_P, 00184 typename Localization_P, 00185 typename Float, 00186 uint16_t MAX_NODES, 00187 typename Radio_P, 00188 typename Timer_P> 00189 LmstTopology<OsModel_P, Localization_P, Float, MAX_NODES, Radio_P, Timer_P>:: 00190 LmstTopology() 00191 : startup_time_ ( 2000 ), 00192 work_period_ ( 5000 ), 00193 positionMessage( LtMsgIdBroadcastPosition ), 00194 neighbourMessage( LtMsgIdNeighbourNotification ) 00195 {} 00196 // ----------------------------------------------------------------------- 00197 template<typename OsModel_P, 00198 typename Localization_P, 00199 typename Float, 00200 uint16_t MAX_NODES, 00201 typename Radio_P, 00202 typename Timer_P> 00203 LmstTopology<OsModel_P, Localization_P, Float, MAX_NODES, Radio_P, Timer_P>:: 00204 ~LmstTopology() 00205 { 00206 #ifdef DEBUG 00207 //debug().debug( "LmstTopology Destroyed\n" ); 00208 #endif 00209 }; 00210 // ----------------------------------------------------------------------- 00211 template<typename OsModel_P, 00212 typename Localization_P, 00213 typename Float, 00214 uint16_t MAX_NODES, 00215 typename Radio_P, 00216 typename Timer_P> 00217 void 00218 LmstTopology<OsModel_P, Localization_P, Float, MAX_NODES, Radio_P, Timer_P>:: 00219 enable( void ) 00220 { 00221 radio().enable_radio(); 00222 #ifdef DEBUG 00223 debug().debug( "%i: LmstTopology Boots\n", radio().id() ); 00224 #endif 00225 callback_id=radio().template reg_recv_callback<self_type, &self_type::receive>( 00226 this ); 00227 timer().template set_timer<self_type, &self_type::timer_elapsed>( 00228 startup_time_, this, 0 ); 00229 enabled=true; 00230 first=true; 00231 } 00232 // ----------------------------------------------------------------------- 00233 template<typename OsModel_P, 00234 typename Localization_P, 00235 typename Float, 00236 uint16_t MAX_NODES, 00237 typename Radio_P, 00238 typename Timer_P> 00239 void 00240 LmstTopology<OsModel_P, Localization_P, Float, MAX_NODES, Radio_P, Timer_P>:: 00241 disable( void ) 00242 { 00243 enabled=false; 00244 #ifdef DEBUG 00245 debug().debug( "%i: Called LmstTopology::disable\n", radio().id() ); 00246 #endif 00247 radio().unreg_recv_callback( callback_id ); 00248 } 00249 // ----------------------------------------------------------------------- 00250 template<typename OsModel_P, 00251 typename Localization_P, 00252 typename Float, 00253 uint16_t MAX_NODES, 00254 typename Radio_P, 00255 typename Timer_P> 00256 void 00257 LmstTopology<OsModel_P, Localization_P, Float, MAX_NODES, Radio_P, Timer_P>:: 00258 timer_elapsed( void* userdata ) 00259 { 00260 if(!enabled) 00261 return; 00262 #ifdef DEBUG 00263 debug().debug( "%i: Executing TimerElapsed 'LmstTopology'\n", 00264 radio().id() ); 00265 #endif 00266 00267 #ifdef DEBUG 00268 debug().debug( "%i: Generating topology\n", 00269 radio().id() ); 00270 #endif 00271 generate_topology(); 00272 if(!first) 00273 TopologyBase<OsModel>::notify_listeners(); 00274 first=false; 00275 for ( size_t i = 0; i < N.size(); ++i ) 00276 radio().send( N[i], 1, (uint8_t*)&neighbourMessage ); 00277 Position pos = loc_->position(); 00278 #ifdef DEBUG 00279 debug().debug( "%i: Broadcasting position (%i, %i, %i)\n", 00280 radio().id(), (int)pos.x, (int)pos.y, (int)pos.z ); 00281 #endif 00282 positionMessage.set_position( pos ); 00283 radio().send( Radio::BROADCAST_ADDRESS, 1 + POSITION_SIZE, (uint8_t*)&positionMessage ); 00284 00285 timer().template set_timer<self_type, &self_type::timer_elapsed>( 00286 work_period_, this, 0 ); 00287 } 00288 // ----------------------------------------------------------------------- 00289 template<typename OsModel_P, 00290 typename Localization_P, 00291 typename Float, 00292 uint16_t MAX_NODES, 00293 typename Radio_P, 00294 typename Timer_P> 00295 void 00296 LmstTopology<OsModel_P, Localization_P, Float, MAX_NODES, Radio_P, Timer_P>:: 00297 receive( node_id_t from, size_t len, block_data_t *data ) 00298 { 00299 if ( !enabled||from == radio().id() ) 00300 return; 00301 00302 uint8_t msg_id = *data; 00303 if ( msg_id == LtMsgIdBroadcastPosition ) 00304 { 00305 TopologyMessage *msg = (TopologyMessage *)data; 00306 Position pos = msg->position(); 00307 #ifdef DEBUG 00308 debug().debug( "%i: Received position msg from %i: (%i, %i, %i)\n", 00309 radio().id(), from, (int)pos.x, (int)pos.y, (int)pos.z ); 00310 #endif 00311 NV.push_back( from ); 00312 Pos.push_back( pos ); 00313 } 00314 else if ( msg_id == LtMsgIdNeighbourNotification ) 00315 { 00316 #ifdef DEBUG 00317 debug().debug( "%i: Received neighbourhood message from %i\n", 00318 radio().id(), from ); 00319 #endif 00320 size_t i = 0; 00321 while ( i < N.size() && N[i] != from ) 00322 ++i; 00323 if ( i == N.size() ) 00324 { 00325 #ifdef DEBUG 00326 debug().debug( "%i: Added node %i in N because I didn't have it\n", 00327 radio().id(), from ); 00328 #endif 00329 N.push_back( from ); 00330 } 00331 } 00332 } 00333 // ----------------------------------------------------------------------- 00334 template<typename OsModel_P, 00335 typename Localization_P, 00336 typename Float, 00337 uint16_t MAX_NODES, 00338 typename Radio_P, 00339 typename Timer_P> 00340 void 00341 LmstTopology<OsModel_P, Localization_P, Float, MAX_NODES, Radio_P, Timer_P>:: 00342 generate_topology() 00343 { 00344 // Prim's algorithm to find the MST of the visible neighbourhood graph 00345 node_id_t me = NV.size(); 00346 NV.push_back( radio().id() ); 00347 Pos.push_back( loc_->position() ); 00348 D.clear(); 00349 for ( size_t i = 0; i < NV.size(); ++i ) 00350 D.push_back( std::numeric_limits<position_t>::infinity() ); 00351 p.clear(); 00352 for ( size_t i = 0; i < NV.size(); ++i ) 00353 p.push_back( -1 ); 00354 is_in_PQ.clear(); 00355 for ( size_t i = 0; i < NV.size(); ++i ) 00356 is_in_PQ.push_back( true ); 00357 PQ.clear(); 00358 PQ.push( PPI( 0.0, me ) ); 00359 D[me] = 0.0; 00360 N.clear(); // we'll keep the visible neighbours that have 'me' as parent 00361 while ( not PQ.empty() ) 00362 { 00363 node_id_t u = PQ.top().second; 00364 if ( p[u] == me ) 00365 N.push_back( NV[u] ); 00366 PQ.pop(); 00367 is_in_PQ[u] = false; 00368 for ( size_t i = 0; i < NV.size(); ++i ) 00369 { 00370 position_t w = dist(Pos[i], Pos[u]); 00371 if ( is_in_PQ[i] && w < D[i] ) 00372 { 00373 p[i] = u; 00374 D[i] = w; 00375 PQ.push( PPI(w, i) ); 00376 } 00377 } 00378 } 00379 radius = 0.0; 00380 for ( size_t i = 0; i < N.size(); ++i ) 00381 if ( dist(Pos[i], Pos[me]) > radius ) 00382 radius = dist( Pos[i], Pos[me] ); 00383 NV.clear(); 00384 Pos.clear(); 00385 } 00386 } 00387 #endif
