changed to old barrier and modified log for easy parsing

buzz_scripts/include/act/old_barrier.bzz

@@ -0,0 +1,69 @@
+########################################
+#
+# BARRIER-RELATED FUNCTIONS
+#
+########################################
+
+#
+# Constants
+#
+BARRIER_TIMEOUT = 1200 # in steps
+BARRIER_VSTIG = 80
+timeW = 0
+barrier = nil
+
+#
+# Sets a barrier
+#
+function barrier_create() {
+  # reset
+  timeW = 0
+  # create barrier vstig
+  #log("---> Prev. br. ", barrier, " ", BARRIER_VSTIG)
+  if(barrier!=nil) {
+    barrier=nil
+    BARRIER_VSTIG = BARRIER_VSTIG +1
+  }
+  #log("---> New. br. ", barrier, " ", BARRIER_VSTIG)
+  barrier = stigmergy.create(BARRIER_VSTIG)
+}
+
+function barrier_set(threshold, transf, resumef,bc) {
+  statef = function() {
+    barrier_wait(threshold, transf, resumef,bc);
+  }
+  BVMSTATE = "BARRIERWAIT"
+  barrier_create()
+}
+
+#
+# Make yourself ready
+#
+function barrier_ready(bc) {
+  #log("BARRIER READY -------")
+  barrier.put(id, 1)
+  barrier.put("d", 0)
+}
+
+#
+# Executes the barrier
+#
+function barrier_wait(threshold, transf, resumef,bc) {
+  barrier.put(id, 1)
+
+  barrier.get(id)
+  log("--->BS: ", barrier.size(), " (", BARRIER_VSTIG, ")")
+  if(barrier.size() - 1 >= threshold or barrier.get("d") == 1) {
+    barrier.put("d", 1)
+    timeW = 0
+    BVMSTATE = transf
+  } else if(timeW >= BARRIER_TIMEOUT) {
+    log("------> Barrier Timeout !!!!")
+    barrier=nil
+    timeW = 0
+    BVMSTATE = resumef
+  }
+
+  timeW = timeW+1
+}
+

buzz_scripts/include/act/states.bzz

@@ -4,7 +4,7 @@
 #
 ########################################
 include "utils/vec2.bzz"
-include "act/barrier.bzz"
+include "act/old_barrier.bzz"
 include "utils/conversions.bzz"
 
 TARGET_ALTITUDE = 15.0 # m.
@@ -191,8 +191,8 @@ function rc_cmd_listen() {
 	} else if(flight.rc_cmd==21) {
 		flight.rc_cmd=0
 		AUTO_LAUNCH_STATE = "TURNEDOFF"
-		barrier_set(ROBOTS, "GOHOME", BVMSTATE, 21)
+		#barrier_set(ROBOTS, "GOHOME", BVMSTATE, 21)
-		barrier_ready(21)
+		#barrier_ready(21)
 		BVMSTATE = "STOP"
 		neighbors.broadcast("cmd", 21)
 	} else if(flight.rc_cmd==20) {

src/roscontroller.cpp

@@ -170,16 +170,18 @@ void roscontroller::RosControllerRun()
       log<<cur_pos.latitude << "," << cur_pos.longitude << ","
             << cur_pos.altitude << ",";
       log << (int)no_of_robots<<",";
+      log  << neighbours_pos_map.size()<< ",";
+      std::vector<uint8_t*> in_msg = buzz_utility::get_inmsg_vector();
+      log <<(int)in_msg.size()<<",";
+      log <<buzz_utility::getuavstate()<<",";
       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;
@@ -190,8 +192,7 @@ void roscontroller::RosControllerRun()
         uint16_t neigh_id = *(uint16_t*)(first_INmsg + tot);
         log<<(int)neigh_id<<","<<(int)msg_size<<",";
       }
-
+      log<<std::endl;
-      log <<buzz_utility::getuavstate()<<std::endl;
       //  Call Step from buzz script
       buzz_utility::buzz_script_step();
       //  Prepare messages and publish them