Wiselib: wiselib.testing/algorithms/tracking/PLTT

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 
00020 #ifndef __PLTT_TRACKER_H__
00021 #define __PLTT_TRACKER_H__
00022 
00023 #include "PLTT_config.h"
00024 #include "PLTT_message.h"
00025 
00026 namespace wiselib
00027 {
00028    template<   typename Os_P,
00029             typename PLTT_Agent_P,
00030             typename Node_P,
00031             typename Position_P,
00032             typename IntensityNumber_P,
00033             typename Timer_P,
00034             typename Radio_P,
00035             typename Rand_P,
00036             typename Clock_P,
00037             typename PLTT_TrackerTrackingMetric_P,
00038             typename Debug_P>
00039    class PLTT_TrackerType
00040    {
00041    public:
00042       typedef Os_P Os;
00043       typedef Radio_P Radio;
00044       typedef Debug_P Debug;
00045       typedef Node_P Node;
00046       typedef Position_P Position;
00047       typedef IntensityNumber_P IntensityNumber;
00048       typedef Rand_P Rand;
00049       typedef Clock_P Clock;
00050       typedef PLTT_Agent_P PLTT_Agent;
00051       typedef typename PLTT_Agent::AgentID AgentID;
00052       typedef Timer_P Timer;
00053       typedef PLTT_TrackerTrackingMetric_P PLTT_TrackerTrackingMetric;
00054       typedef PLTT_TrackerType<Os, PLTT_Agent, Node, Position, IntensityNumber, Timer, Radio, Rand, Clock, PLTT_TrackerTrackingMetric, Debug> self_type;
00055       typedef typename Radio::node_id_t node_id_t;
00056       typedef typename Radio::size_t size_t;
00057       typedef typename Radio::block_data_t block_data_t;
00058       typedef typename Radio::message_id_t message_id_t;
00059       typedef typename Timer::millis_t millis_t;
00060       typedef typename Radio::TxPower TxPower;
00061       typedef typename Radio::ExtendedData ExtendedData;
00062       typedef typename Clock::time_t time_t;
00063       typedef typename wiselib::vector_static<Os, AgentID, 100> AgentAckIDList;
00064       typedef typename AgentAckIDList::iterator AgentAckIDListIterator;
00065       typedef PLTT_MessageType<Os, Radio> Message;
00066       void init( Radio& radio, Timer& timer, Rand& rand, Clock& clock, Debug& debug )
00067       {
00068          radio_ = &radio;
00069          timer_ = &timer;
00070          rand_ = &rand;
00071          debug_ = &debug;
00072          clock_ = &clock;
00073       }
00074       Node* get_self()
00075       {
00076          return &self;
00077       }
00078       void set_self( Node _n )
00079       {
00080          self = _n;
00081       }
00082       // -----------------------------------------------------------------------
00083       PLTT_TrackerType()
00084       {}
00085       // -----------------------------------------------------------------------
00086       PLTT_TrackerType( node_id_t _tid, IntensityNumber _tar_max_inten, uint16_t _sm, int16_t _tp )
00087       {
00088          target_id = _tid;
00089          target_max_inten = _tar_max_inten;
00090          send_milis = _sm;
00091          trans_power.set_dB( _tp );
00092 #ifdef PLTT_TRACKER_TRACKING_METRICS
00093          messages_received_periodic = 0;
00094          messages_bytes_received_periodic = 0;
00095 #endif
00096       }
00097       // -----------------------------------------------------------------------
00098       ~PLTT_TrackerType()
00099       {}
00100       // -----------------------------------------------------------------------
00101       void enable( void )
00102       {
00103          radio().enable_radio();
00104          #ifdef ISENSE_PLTT_TRACKER_DEBUG_MISC
00105          debug().debug( "PLTT_Tracker %x: Boot - Tracking target of id %x \n", self.get_id(), target_id);
00106          #endif
00107          callback_id_ = radio().template reg_recv_callback<self_type, &self_type::receive>( this );
00108          timer().template set_timer<self_type, &self_type::send_echo>( 10000, this, 0);
00109          #ifdef PLTT_TRACKER_TRACKING_METRICS
00110          tracking_metrics_periodic.set_target_id( target_id );
00111          tracking_metrics.set_target_id( target_id );
00112          #ifndef MARIOS_DEMO
00113          timer().template set_timer<self_type, &self_type::print_tracking_metrics>( metrics_timer, this, 0 );
00114          #endif
00115          #endif
00116          #ifdef PLTT_TRACKER_TRACKING_METRICS
00117          timer().template set_timer<self_type, &self_type::cleanup_ack_agent_list>( metrics_timer+500, this, 0 );
00118          agent_sent_counter = 0;
00119          agent_received_counter = 0;
00120          #endif
00121       }
00122       // -----------------------------------------------------------------------
00123       void disable( void )
00124       {
00125          #ifdef ISENSE_PLTT_TRACKER_DEBUG_MISC
00126          debug().debug( "PLTT_Tracker %x: Disable \n", self.id() );
00127          #endif
00128          radio().disable();
00129       }
00130       // -----------------------------------------------------------------------
00131       void send( node_id_t destination, size_t len, block_data_t *data, message_id_t msg_id  )
00132       {
00133          radio().set_power( trans_power );
00134          Message message;
00135          message.set_msg_id( msg_id );
00136          message.set_payload( len, data );
00137          radio().send( destination, message.buffer_size(), (block_data_t*)&message );
00138       }
00139       // -----------------------------------------------------------------------
00140       void send_query( void* userdata )
00141       {
00142          #ifdef ISENSE_PLTT_TRACKER_DEBUG_QUERY
00143          debug().debug( "PLTT_Tracker %x: Send query\n", self.get_id() );
00144          #endif
00145          if ( current_link_metric != 0 )
00146          {
00147             agent.set_all( current_agent_id, Node( target_id, Position( 0, 0, 0 ) ), self, target_max_inten, 0 );
00148             #ifdef PLTT_TRACKER_TRACKING_METRICS
00149             agent.set_track_start( clock().time() );
00150             #ifdef ISENSE_PLTT_TRACKER_DEBUG_QUERY
00151             agent.print( debug(), clock() );
00152             #endif
00153             #endif
00154             size_t len = agent.get_buffer_size();
00155             block_data_t buf[Radio::MAX_MESSAGE_LENGTH];
00156             block_data_t* buff = buf;
00157             buff = agent.set_buffer_from( buff );
00158             #ifdef PLTT_TRACKER_TRACKING_METRICS
00159             agent.set_track_start( clock().time() );
00160             tracking_metrics_periodic.inc_query_messages_send();
00161             tracking_metrics_periodic.inc_query_messages_bytes_send( len + sizeof( message_id_t) + sizeof( size_t ) );
00162             tracking_metrics.inc_query_messages_send();
00163             tracking_metrics.inc_query_messages_bytes_send( len + sizeof( message_id_t) + sizeof( size_t ) );
00164             #endif
00165             debug().debug("sending agent %x", agent.get_agent_id());
00166             #ifdef PLTT_TRACKER_TRACKING_METRICS
00167             ++agent_sent_counter;
00168             #endif
00169             send( current_query_destination, len, buff, PLTT_QUERY_ID );
00170             current_link_metric = 0;
00171             #ifdef ISENSE_PLTT_TRACKER_DEBUG_QUERY
00172             debug().debug( "PLTT_Tracker %x: Send query - A track report was routed to %x\n", self.get_id(), current_query_destination );
00173             #endif
00174          }
00175          else
00176          {
00177             #ifdef ISENSE_PLTT_TRACKER_DEBUG_QUERY
00178             debug().debug( "PLTT_Tracker %x: Send query - No echo replies for %x report\n", self.get_id(), current_agent_id );
00179             #endif
00180          }
00181       }
00182       void send_echo( void* userdata )
00183       {
00184          #ifdef ISENSE_PLTT_TRACKER_DEBUG_QUERY
00185          debug().debug( "PLTT_Tracker %x: Send echo", self.get_id() );
00186          #endif
00187          current_agent_id = ( rand()() % ( 0xffffffff -1 ) + 1 );
00188          #ifdef ISENSE_PLTT_TRACKER_DEBUG_QUERY
00189          debug().debug( "PLTT_Tracker %x: Send echo - Generated a new rand id : %x \n", self.get_id(), current_agent_id );
00190          #endif
00191          size_t len = sizeof( AgentID );
00192          block_data_t buf[Radio::MAX_MESSAGE_LENGTH];
00193          block_data_t* buff = buf;
00194          write<Os, block_data_t, AgentID> ( buff, current_agent_id );
00195          #ifdef PLTT_TRACKER_TRACKING_METRICS
00196          tracking_metrics_periodic.inc_echo_messages_send();
00197          tracking_metrics_periodic.inc_echo_messages_bytes_send( sizeof( AgentID ) + sizeof( message_id_t) + sizeof( size_t ) );
00198          tracking_metrics.inc_echo_messages_send();
00199          tracking_metrics.inc_echo_messages_bytes_send( sizeof( AgentID ) + sizeof( message_id_t) + sizeof( size_t ) );
00200          #endif
00201          send( Radio::BROADCAST_ADDRESS, len, buff, PLTT_TRACK_ECHO_ID );
00202          #ifdef ISENSE_PLTT_TRACKER_DEBUG_QUERY
00203          AgentID tmp_id = read<Os,block_data_t, AgentID> ( buff );
00204          debug().debug( "PLTT_Tracker %x: Send echo - Send echo with tmp id: %x \n", self.get_id(), tmp_id );
00205          debug().debug( "PLTT_Tracker %x: Send echo - Planned another send echo in %i millis\n", self.get_id(), send_milis );
00206          debug().debug( "PLTT_Tracker %x: Send echo - Planned a send_query with the best of the replies in %i millis\n", self.get_id(), 500);
00207          #endif
00208          timer().template set_timer<self_type, &self_type::send_echo>( send_milis, this, 0);
00209          timer().template set_timer<self_type, &self_type::send_query>( 1000, this, 0);
00210       }
00211       // -----------------------------------------------------------------------
00212       void receive( node_id_t from, size_t len, block_data_t *data, const ExtendedData& exdata )
00213       {
00214          #ifdef PLTT_TRACKER_TRACKING_METRICS
00215          messages_received_periodic = messages_received_periodic + 1;
00216          messages_bytes_received_periodic = messages_bytes_received_periodic + len;
00217          messages_received = messages_received + 1;
00218          messages_bytes_received = messages_bytes_received_periodic + len;
00219          #endif
00220          message_id_t msg_id = *data;
00221          Message *message = ( Message* )data;
00222          if ( msg_id == PLTT_QUERY_REPORT_ID )
00223          {
00224             #ifdef OPT_RELIABLE_TRACKING
00225             if ( find_ack_agent_id( PLTT_Agent( message->payload() ).get_agent_id() ) == 1 ) { return; }
00226             #endif
00227             agent_ack_id_list.push_back( PLTT_Agent( message->payload() ).get_agent_id() );
00228             #ifdef ISENSE_PLTT_TRACKER_DEBUG_REPORT
00229             debug().debug( "PLTT_Tracker %x: Receive - Received agent from %x\n", self.get_id(), from );
00230             #endif
00231             #ifdef OPT_RELIABLE_TRACKING
00232             block_data_t buf[Radio::MAX_MESSAGE_LENGTH];
00233             block_data_t* buff = buf;
00234             PLTT_Agent agent = PLTT_Agent( message->payload() );
00235             agent.update_reliable_agent_id();
00236             agent.set_buffer_from( buff );
00237             send( from, PLTT_Agent( message->payload() ).get_buffer_size(), buff, PLTT_QUERY_REPORT_ACK_ID );
00238             send( from, PLTT_Agent( message->payload() ).get_buffer_size(), buff, PLTT_QUERY_REPORT_ACK_ID );
00239             send( from, PLTT_Agent( message->payload() ).get_buffer_size(), buff, PLTT_QUERY_REPORT_ACK_ID );
00240             #endif
00241             #ifdef ISENSE_PLTT_TRACKER_DEBUG_REPORT
00242             debug().debug( "Target :\n");
00243             agent.get_target().print( debug() );
00244             debug().debug( "\n");
00245             #endif
00246             #ifdef PLTT_TRACKER_TRACKING_METRICS
00247             tracking_metrics_periodic.inc_report_messages_received();
00248             tracking_metrics_periodic.inc_report_messages_bytes_received( len );
00249             tracking_metrics.inc_report_messages_received();
00250             tracking_metrics.inc_report_messages_bytes_received( len );
00251             agent.set_track_end( clock().time() );
00252             //debug().debug( " current agent id %x, end_time : sec %i mil %i ", agent.get_agent_id(), clock().seconds( clock().time() ), clock().milliseconds( clock().time() ) );
00253             #ifndef MARIOS_DEMO
00254 //          debug().debug( " AR\t%x\t%i\t%i\t%i\t%i\t%i\t%i\t%i\t%i\t%i\t%i\t%i\t%i ",
00255 //                agent.get_agent_id(),
00256 //                clock().seconds( agent.get_track_start( clock() ) ), clock().milliseconds( agent.get_track_start( clock() ) ),
00257 //                clock().seconds( agent.get_aprox_detection( clock() ) ), clock().milliseconds( agent.get_aprox_detection( clock() ) ),
00258 //                clock().seconds( agent.get_track_end( clock() ) ), clock().milliseconds( agent.get_track_end( clock() ) ),
00259 //                clock().seconds( agent.detection_duration( clock() ) ), clock().milliseconds( agent.detection_duration( clock() ) ),
00260 //                clock().seconds( agent.track_duration( clock() ) ), clock().milliseconds( agent.track_duration( clock() ) ),
00261 //                agent.get_target().get_position().get_x(), agent.get_target().get_position().get_y()
00262 //                );
00263             ++agent_received_counter;
00264             //if ( clock().seconds( agent.track_duration( clock() ) ) < 1)
00265             //{
00266             debug().debug(" %i/%i\t%x\t%i\t%i\t%i\t%i\t%i ", agent_received_counter, agent_sent_counter, agent.get_agent_id(),
00267                   clock().seconds( agent.track_duration( clock() ) ), clock().milliseconds( agent.track_duration( clock() ) ),
00268                   agent.get_target().get_position().get_x(), agent.get_target().get_position().get_y(), agent.get_reliable_agent_id() );
00269             //}
00270             #endif
00271             #endif
00272             #ifdef MARIOS_DEMO
00273             debug().debug( "%i,%i", agent.get_target().get_position().get_x(), agent.get_target().get_position().get_y() );
00274             #endif
00275             //timer().template set_timer<self_type, &self_type::send_echo>( 0, this, 0);
00276 
00277          }
00278          else if( msg_id == PLTT_TRACK_ECHO_REPLY_ID )
00279          {
00280             #ifdef ISENSE_PLTT_TRACKER_DEBUG_QUERY
00281             debug().debug( "PLTT_Tracker %x: Receive - Received echo reply from %x\n", self.get_id(), from );
00282             #endif
00283             #ifdef PLTT_TRACKER_TRACKING_METRICS
00284             tracking_metrics_periodic.inc_echo_messages_received();
00285             tracking_metrics_periodic.inc_echo_messages_bytes_received( len );
00286             tracking_metrics.inc_echo_messages_received();
00287             tracking_metrics.inc_echo_messages_bytes_received( len );
00288             #endif
00289             AgentID aid = read<Os, block_data_t, AgentID>( message->payload() );
00290             if ( ( aid == current_agent_id ) && ( exdata.link_metric() > current_link_metric ) )
00291             {
00292                #ifdef ISENSE_PLTT_TRACKER_DEBUG_QUERY
00293                debug().debug("PLTT_tracker %x: Receive - Node %x was chosen for the final candidate\n", self.get_id(), from);
00294                #endif
00295                current_query_destination = from;
00296                current_link_metric = exdata.link_metric();
00297             }
00298          }
00299       }
00300       // -----------------------------------------------------------------------
00301 #ifdef PLTT_TRACKER_TRACKING_METRICS
00302       // -----------------------------------------------------------------------
00303       void print_tracking_metrics( void* userdata = NULL )
00304       {
00305 //       debug().debug( " TMN\t%i\t%i\t%i\t%i\t%i\t%i\t%i\t%i\t%i\t%i\t%i ",
00306 //          clock().seconds( clock().time() ),
00307 //          tracking_metrics.get_echo_messages_send(),
00308 //          tracking_metrics.get_echo_messages_bytes_send(),
00309 //          tracking_metrics.get_query_messages_send(),
00310 //          tracking_metrics.get_query_messages_bytes_send(),
00311 //          messages_received,
00312 //          messages_bytes_received,
00313 //          tracking_metrics.get_echo_messages_received(),
00314 //          tracking_metrics.get_echo_messages_bytes_received(),
00315 //          tracking_metrics.get_report_messages_received(),
00316 //          tracking_metrics.get_report_messages_bytes_received() );
00317 //       debug().debug( " TMP\t%i\t%i\t%i\t%i\t%i\t%i\t%i\t%i\t%i\t%i\t%i ",
00318 //          clock().seconds( clock().time() ),
00319 //          tracking_metrics_periodic.get_echo_messages_send(),
00320 //          tracking_metrics_periodic.get_echo_messages_bytes_send(),
00321 //          tracking_metrics_periodic.get_query_messages_send(),
00322 //          tracking_metrics_periodic.get_query_messages_bytes_send(),
00323 //          messages_received_periodic,
00324 //          messages_bytes_received_periodic,
00325 //          tracking_metrics_periodic.get_echo_messages_received(),
00326 //          tracking_metrics_periodic.get_echo_messages_bytes_received(),
00327 //          tracking_metrics_periodic.get_report_messages_received(),
00328 //          tracking_metrics_periodic.get_report_messages_bytes_received() );
00329 //       messages_received_periodic = 0;
00330 //       messages_bytes_received_periodic = 0;
00331 //       tracking_metrics_periodic.reset();
00332          timer().template set_timer<self_type, &self_type::print_tracking_metrics>( metrics_timer, this, 0 );
00333       }
00334       // -----------------------------------------------------------------------
00335       void set_metrics_timer( millis_t _t )
00336       {
00337          metrics_timer = _t;
00338       }
00339       // -----------------------------------------------------------------------
00340 #endif
00341 #ifdef OPT_RELIABLE_TRACKING
00342       uint8_t find_ack_agent_id( AgentID _aid )
00343       {
00344          for ( AgentAckIDListIterator i = agent_ack_id_list.begin(); i != agent_ack_id_list.end(); ++i )
00345          {
00346             if ( _aid == *i )
00347             {
00348                return 1;
00349             }
00350          }
00351          return 0;
00352       }
00353       // -----------------------------------------------------------------------
00354       void cleanup_ack_agent_list( void* userdata = NULL )
00355       {
00356          if ( agent_ack_id_list.size() == 100 )
00357          {
00358             agent_ack_id_list.clear();
00359          }
00360          timer().template set_timer<self_type, &self_type::cleanup_ack_agent_list> ( 1000, this, 0 );
00361       }
00362       // -----------------------------------------------------------------------
00363 #endif
00364    private:
00365       Radio& radio()
00366       {
00367          return *radio_; 
00368       }
00369       Timer& timer()
00370       {
00371          return *timer_; 
00372       }
00373       Debug& debug()
00374       { 
00375          return *debug_; 
00376       }
00377       Rand& rand()
00378       {
00379          return *rand_;
00380       }
00381       Clock& clock()
00382       {
00383          return *clock_;
00384       }
00385       Clock * clock_;
00386       Radio * radio_;
00387       Timer * timer_;
00388       Rand * rand_;
00389       Debug * debug_;
00390       enum MessageIds
00391       {
00392          PLTT_QUERY_ID = 31,
00393          PLTT_QUERY_REPORT_ID = 41,
00394          PLTT_TRACK_ECHO_ID = 51,
00395          PLTT_TRACK_ECHO_REPLY_ID = 61
00396 #ifdef OPT_RELIABLE_TRACKING
00397          ,PLTT_QUERY_REPORT_ACK_ID = 81
00398 #endif
00399       };
00400       uint32_t callback_id_;
00401       millis_t send_milis;
00402       node_id_t target_id;
00403       TxPower trans_power;
00404       Node self;
00405       PLTT_Agent agent;
00406       Position topology_size;
00407       IntensityNumber target_max_inten;
00408       AgentID current_agent_id;
00409       node_id_t current_query_destination;
00410       uint16_t current_link_metric;
00411 #ifdef PLTT_TRACKER_TRACKING_METRICS
00412       PLTT_TrackerTrackingMetric tracking_metrics_periodic;
00413       PLTT_TrackerTrackingMetric tracking_metrics;
00414       uint32_t messages_received_periodic;
00415       uint32_t messages_bytes_received_periodic;
00416       uint32_t messages_received;
00417       uint32_t messages_bytes_received;
00418       millis_t metrics_timer;
00419       uint32_t agent_received_counter;
00420       uint32_t agent_sent_counter;
00421 #endif
00422 #ifdef OPT_RELIABLE_TRACKING
00423       AgentAckIDList agent_ack_id_list;
00424 #endif
00425    };
00426 }
00427 #endif