added msg size + nei id + nei pos to log

include/buzz_utility.h

@@ -78,6 +78,8 @@ void set_robot_var(int ROBOTS);
 
 int get_inmsg_size();
 
+std::vector<uint8_t*> get_inmsg_vector();
+
 std::string getuavstate();
 
 int getlogicaltime();

src/buzz_utility.cpp

@@ -565,6 +565,10 @@ int get_inmsg_size()
   return IN_MSG.size();
 }
 
+std::vector<uint8_t*> get_inmsg_vector(){
+  return IN_MSG;
+}
+
 string getuavstate()
 /*
 / return current BVM state

src/roscontroller.cpp

@@ -170,7 +170,27 @@ void roscontroller::RosControllerRun()
       log<<cur_pos.latitude << "," << cur_pos.longitude << ","
             << cur_pos.altitude << ",";
       log << (int)no_of_robots<<",";
-      log <<(int)buzz_utility::get_inmsg_size()<<",";
+      map<int, buzz_utility::Pos_struct>::iterator it =
+            neighbours_pos_map.begin();
+      log  << neighbours_pos_map.size()<< ",";
+      for (; it != neighbours_pos_map.end(); ++it)
+      {
+        log << (double)it->second.x << "," << (double)it->second.y
+            << "," << (double)it->second.z << ",";
+      }
+      std::vector<uint8_t*> in_msg = buzz_utility::get_inmsg_vector();
+      log <<(int)in_msg.size()<<",";
+      for (std::vector<uint8_t*>::iterator it = in_msg.begin() ; it != in_msg.end(); ++it){
+        uint8_t* first_INmsg = (uint8_t*)(*it);
+        size_t tot = 0;
+        //  Size is at first 2 bytes
+        uint16_t msg_size = (*(uint16_t*)first_INmsg) * sizeof(uint64_t);
+        tot += sizeof(uint16_t);
+        //  Decode neighbor Id
+        uint16_t neigh_id = *(uint16_t*)(first_INmsg + tot);
+        log<<(int)neigh_id<<","<<(int)msg_size<<",";
+      }
+
       log <<buzz_utility::getuavstate()<<std::endl;
       //  Call Step from buzz script
       buzz_utility::buzz_script_step();