|
Wiselib
|
wiselib.testing/algorithms/localization/distance_based/modules/distance/localization_gpsfree_lcs_module.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_LOCALIZATION_DISTANCE_BASED_LOCALIZATION_GPSFREE_LCS_MODULE_H 00020 #define __ALGORITHMS_LOCALIZATION_DISTANCE_BASED_LOCALIZATION_GPSFREE_LCS_MODULE_H 00021 00022 #include "algorithms/localization/distance_based/modules/localization_module.h" 00023 #include "algorithms/localization/distance_based/modules/distance/localization_gpsfree_lcs_messages.h" 00024 #include "algorithms/localization/distance_based/util/localization_defutils.h" 00025 00026 00027 namespace wiselib 00028 { 00029 00031 00068 template<typename OsModel_P, 00069 typename Radio_P, 00070 typename Clock_P, 00071 typename Distance_P, 00072 typename Debug_P, 00073 typename SharedData_P> 00074 class LocalizationGpsFreeLcsModule 00075 : public LocalizationModule<OsModel_P, Radio_P, SharedData_P> 00076 { 00077 00078 public: 00079 typedef OsModel_P OsModel; 00080 typedef Radio_P Radio; 00081 typedef Clock_P Clock; 00082 typedef Distance_P Distance; 00083 typedef Debug_P Debug; 00084 typedef SharedData_P SharedData; 00085 00086 typedef LocalizationGpsFreeLcsModule<OsModel, Radio, Clock, Distance, Debug, SharedData> self_type; 00087 typedef LocalizationModule<OsModel, Radio, SharedData> base_type; 00088 00089 typedef typename Radio::size_t size_t; 00090 typedef typename Radio::node_id_t node_id_t; 00091 typedef typename Radio::block_data_t block_data_t; 00092 00093 typedef typename Clock_P::time_t time_t; 00094 00095 typedef typename SharedData::DistanceMap DistanceMap; 00096 typedef typename SharedData::LocalCoordinateSystem LocalCoordinateSystem; 00097 typedef typename SharedData::Neighborhood::NeighborhoodIterator NeighborhoodIterator; 00098 00099 typedef LocalizationGpsFreeLcsInitMessage<OsModel, Radio> GpsFreeLcsInitMessage; 00100 typedef LocalizationGpsFreeLcsNeighborMessage<OsModel, Radio, DistanceMap> GpsFreeLcsNeighborMessage; 00101 00105 LocalizationGpsFreeLcsModule(); 00107 ~LocalizationGpsFreeLcsModule(); 00109 00110 00113 00117 void receive( node_id_t from, size_t len, block_data_t *data ); 00123 void work( void ); 00125 00126 00129 00132 bool finished( void ); 00134 void rollback( void ); 00135 00136 void init( Radio& radio, Clock& clock, Debug& debug, SharedData& shared_data, Distance& distance ) 00137 { 00138 radio_ = &radio; 00139 clock_ = &clock; 00140 debug_ = &debug; 00141 this->set_shared_data( shared_data ); 00142 distance_ = &distance; 00143 } 00144 00145 protected: 00146 00149 00154 bool process_gpsfree_lcs_init_message( node_id_t from, size_t len, block_data_t *data ); 00160 void broadcast_neighborhood( void ); 00166 bool process_gpsfree_lcs_neighbor_message( node_id_t from, size_t len, block_data_t *data ); 00168 00171 00173 void build_lcs( void ); 00175 00178 00181 void set_lcs_epsilon( double epsilon ) 00182 { this->local_coord_sys_w().set_epsilon( epsilon ); } 00184 00185 00186 private: 00187 00188 enum MessageIds 00189 { 00190 GPSFREE_LCS_INIT_MESSAGE = 206, 00191 GPSFREE_LCS_NEIGHBOR_MESSAGE = 207 00192 }; 00193 00194 enum GPSfreeLCSState 00195 { 00196 gflcs_init, 00197 gflcs_wait_bc, 00198 gflcs_broadcast, 00199 gflcs_wait_w, 00200 gflcs_work, 00201 gflcs_finished 00202 }; 00203 00204 00205 GPSfreeLCSState state_; 00206 time_t last_useful_msg_; 00207 00208 Radio* radio_; 00209 Clock* clock_; 00210 Debug* debug_; 00211 Distance* distance_; 00212 }; 00213 // ---------------------------------------------------------------------- 00214 // ---------------------------------------------------------------------- 00215 // ---------------------------------------------------------------------- 00216 template<typename OsModel_P, 00217 typename Radio_P, 00218 typename Clock_P, 00219 typename Distance_P, 00220 typename Debug_P, 00221 typename SharedData_P> 00222 LocalizationGpsFreeLcsModule<OsModel_P, Radio_P, Clock_P, Distance_P, Debug_P, SharedData_P>:: 00223 LocalizationGpsFreeLcsModule() 00224 : state_ ( gflcs_init ), 00225 last_useful_msg_ ( 0 ) 00226 {} 00227 // ---------------------------------------------------------------------- 00228 template<typename OsModel_P, 00229 typename Radio_P, 00230 typename Clock_P, 00231 typename Distance_P, 00232 typename Debug_P, 00233 typename SharedData_P> 00234 LocalizationGpsFreeLcsModule<OsModel_P, Radio_P, Clock_P, Distance_P, Debug_P, SharedData_P>:: 00235 ~LocalizationGpsFreeLcsModule() 00236 {} 00237 // ---------------------------------------------------------------------- 00238 template<typename OsModel_P, 00239 typename Radio_P, 00240 typename Clock_P, 00241 typename Distance_P, 00242 typename Debug_P, 00243 typename SharedData_P> 00244 void 00245 LocalizationGpsFreeLcsModule<OsModel_P, Radio_P, Clock_P, Distance_P, Debug_P, SharedData_P>:: 00246 receive( node_id_t from, size_t len, block_data_t *data ) 00247 { 00248 switch ( data[0] ) 00249 { 00250 case GPSFREE_LCS_INIT_MESSAGE: 00251 process_gpsfree_lcs_init_message( from, len, data ); 00252 break; 00253 case GPSFREE_LCS_NEIGHBOR_MESSAGE: 00254 process_gpsfree_lcs_neighbor_message( from, len, data ); 00255 break; 00256 } 00257 } 00258 // ---------------------------------------------------------------------- 00259 template<typename OsModel_P, 00260 typename Radio_P, 00261 typename Clock_P, 00262 typename Distance_P, 00263 typename Debug_P, 00264 typename SharedData_P> 00265 void 00266 LocalizationGpsFreeLcsModule<OsModel_P, Radio_P, Clock_P, Distance_P, Debug_P, SharedData_P>:: 00267 work( void ) 00268 { 00269 // send initial messages 00270 if ( state_ == gflcs_init ) 00271 { 00272 GpsFreeLcsInitMessage message; 00273 message.set_msg_id( GPSFREE_LCS_INIT_MESSAGE ); 00274 message.set_position( this->shared_data().position() ); 00275 radio_->send( Radio::BROADCAST_ADDRESS, message.buffer_size(), (block_data_t*)&message ); 00276 00277 state_ = gflcs_wait_bc; 00278 } 00279 00280 // after idle-time passed, initial messages of neighbors had already been 00281 // received and state is set to 'broadcast'. 00282 if ( state_ == gflcs_wait_bc && clock_->time() - last_useful_msg_ > this->shared_data().idle_time() ) 00283 { 00284 state_ = gflcs_broadcast; 00285 last_useful_msg_ = clock_->time(); 00286 } 00287 00288 // broadcast collected information 00289 if ( state_ == gflcs_broadcast ) 00290 { 00291 broadcast_neighborhood(); 00292 state_ = gflcs_wait_w; 00293 } 00294 00295 // after idle-time passed, neighborhood messages of neighbors had already been 00296 // received and state is set to 'work'. 00297 if ( state_ == gflcs_wait_w && clock_->time() - last_useful_msg_ > this->shared_data().idle_time() ) 00298 state_ = gflcs_work; 00299 00300 if ( state_ == gflcs_work ) 00301 { 00302 build_lcs(); 00303 state_ = gflcs_finished; 00304 } 00305 00306 // maybe shutdown processor 00307 if ( clock_->time() - last_useful_msg_ > this->shared_data().idle_time() ) 00308 { 00309 state_ = gflcs_finished; 00310 } 00311 } 00312 // ---------------------------------------------------------------------- 00313 template<typename OsModel_P, 00314 typename Radio_P, 00315 typename Clock_P, 00316 typename Distance_P, 00317 typename Debug_P, 00318 typename SharedData_P> 00319 bool 00320 LocalizationGpsFreeLcsModule<OsModel_P, Radio_P, Clock_P, Distance_P, Debug_P, SharedData_P>:: 00321 finished( void ) 00322 { 00323 return state_ == gflcs_finished; 00324 } 00325 // ---------------------------------------------------------------------- 00326 template<typename OsModel_P, 00327 typename Radio_P, 00328 typename Clock_P, 00329 typename Distance_P, 00330 typename Debug_P, 00331 typename SharedData_P> 00332 void 00333 LocalizationGpsFreeLcsModule<OsModel_P, Radio_P, Clock_P, Distance_P, Debug_P, SharedData_P>:: 00334 rollback( void ) 00335 { 00336 state_ = gflcs_init; 00337 last_useful_msg_ = clock_->time(); 00338 } 00339 00340 // ---------------------------------------------------------------------- 00341 template<typename OsModel_P, 00342 typename Radio_P, 00343 typename Clock_P, 00344 typename Distance_P, 00345 typename Debug_P, 00346 typename SharedData_P> 00347 bool 00348 LocalizationGpsFreeLcsModule<OsModel_P, Radio_P, Clock_P, Distance_P, Debug_P, SharedData_P>:: 00349 process_gpsfree_lcs_init_message( node_id_t from, size_t len, block_data_t* data ) 00350 { 00351 GpsFreeLcsInitMessage* msg = (GpsFreeLcsInitMessage*)data; 00352 Vec source_pos = msg->position(); 00353 double distance = distance_->distance( from ); 00354 00355 last_useful_msg_ = clock_->time(); 00356 00357 this->neighborhood().update_neighbor( from, distance ); 00358 00359 return true; 00360 } 00361 // ---------------------------------------------------------------------- 00362 template<typename OsModel_P, 00363 typename Radio_P, 00364 typename Clock_P, 00365 typename Distance_P, 00366 typename Debug_P, 00367 typename SharedData_P> 00368 void 00369 LocalizationGpsFreeLcsModule<OsModel_P, Radio_P, Clock_P, Distance_P, Debug_P, SharedData_P>:: 00370 broadcast_neighborhood( void ) 00371 { 00372 // send info about own neighborhood 00373 GpsFreeLcsNeighborMessage message; 00374 message.set_msg_id( GPSFREE_LCS_NEIGHBOR_MESSAGE ); 00375 DistanceMap dmap = this->neighborhood().neighbor_distance_map(); 00376 message.set_neighbors( dmap ); 00377 radio_->send( Radio::BROADCAST_ADDRESS, message.buffer_size(), (block_data_t*)&message ); 00378 } 00379 // ---------------------------------------------------------------------- 00380 template<typename OsModel_P, 00381 typename Radio_P, 00382 typename Clock_P, 00383 typename Distance_P, 00384 typename Debug_P, 00385 typename SharedData_P> 00386 bool 00387 LocalizationGpsFreeLcsModule<OsModel_P, Radio_P, Clock_P, Distance_P, Debug_P, SharedData_P>:: 00388 process_gpsfree_lcs_neighbor_message( node_id_t from, size_t len, block_data_t* data ) 00389 { 00390 GpsFreeLcsNeighborMessage* msg = (GpsFreeLcsNeighborMessage*)data; 00391 // set neighborhood of received node 00392 this->neighborhood().update_nneighbors( from, msg->neighbors() ); 00393 00394 return true; 00395 } 00396 // ---------------------------------------------------------------------- 00397 template<typename OsModel_P, 00398 typename Radio_P, 00399 typename Clock_P, 00400 typename Distance_P, 00401 typename Debug_P, 00402 typename SharedData_P> 00403 void 00404 LocalizationGpsFreeLcsModule<OsModel_P, Radio_P, Clock_P, Distance_P, Debug_P, SharedData_P>:: 00405 build_lcs( void ) 00406 { 00407 for ( NeighborhoodIterator 00408 it1 = this->neighborhood().begin_neighborhood(); 00409 it1 != this->neighborhood().end_neighborhood(); 00410 ++it1 ) 00411 for ( NeighborhoodIterator 00412 it2 = it1; 00413 it2 != this->neighborhood().end_neighborhood(); 00414 ++it2 ) 00415 { 00416 if ( it1 == it2 ) 00417 continue; 00418 00419 LocalCoordinateSystem lcs; 00420 lcs.update_basic_nodes( it1->first, it2->first, this->neighborhood() ); 00421 00422 for ( NeighborhoodIterator 00423 it3 = this->neighborhood().begin_neighborhood(); 00424 it3 != this->neighborhood().end_neighborhood(); 00425 ++it3 ) 00426 { 00427 if ( it3 == it1 || it3 == it2 ) 00428 continue; 00429 00430 lcs.update_node( it3->first ); 00431 } 00432 00433 if ( lcs.is_valid() && 00434 ( !this->local_coord_sys().is_valid() || 00435 this->local_coord_sys().size() < lcs.size() ) ) 00436 this->local_coord_sys() = lcs; 00437 } 00438 00439 this->local_coord_sys().set_src_node( radio_->id() ); 00440 } 00441 00442 }// namespace wiselib 00443 #endif
