From ded07aa5e881343223359fe60b6cd74b4249df1b Mon Sep 17 00:00:00 2001
From: David St-Onge <david.st-onge@polymtl.ca>
Date: Sun, 10 Sep 2017 23:30:57 -0400
Subject: [PATCH] addition of highlevel packet drop

---
 src/roscontroller.cpp | 16 +++++++++++++++-
 1 file changed, 15 insertions(+), 1 deletion(-)

diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index 49ceb38..2e4a284 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -195,6 +195,8 @@ void roscontroller::RosControllerRun()
   /* Set the Buzz bytecode */
   if (buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(),
                                     robot_id)) {
+    int packet_loss_tmp,time_step=0;
+    double cur_packet_loss=0;
     ROS_INFO("[%i] Bytecode file found and set", robot_id);
     std::string standby_bo = Compile_bzz(stand_by) + ".bo";
     //init_update_monitor(bcfname.c_str(), standby_bo.c_str());
@@ -233,7 +235,19 @@ void roscontroller::RosControllerRun()
       neighbours_pos_publisher();
       send_MPpayload();
       /*Check updater state and step code*/
-     // update_routine(bcfname.c_str(), dbgfname.c_str());
+      // update_routine(bcfname.c_str(), dbgfname.c_str());
+      if(time_step==BUZZRATE){
+         time_step=0;
+         cur_packet_loss= 1 -( (double)packet_loss_tmp/(BUZZRATE*( (int)no_of_robots-1 )) );
+         packet_loss_tmp=0;
+	if(cur_packet_loss<0) cur_packet_loss=0;
+        else if (cur_packet_loss>1) cur_packet_loss=1;
+      }
+      else{
+         packet_loss_tmp+=(int)buzz_utility::get_inmsg_size();
+         time_step++;
+      }
+      ROS_WARN("CURRENT PACKET DROP : %f ",cur_packet_loss);
       /*Log In Msg queue size*/
       log<<(int)buzz_utility::get_inmsg_size()<<",";
       /*Step buzz script */