enhanced barrier with foreach

buzz_scripts/include/act/barrier.bzz

@@ -55,13 +55,7 @@ function barrier_wait(threshold, transf, resumef, bc) {
   var allgood = 0
   log("--->BS: ", barrier.size(), " / ", threshold, " (", BARRIER_VSTIG, " - ", barrier.get("d"), ") t= ", timeW)
   if(barrier.size() - 1 >= threshold or barrier.get("d") == 1) {
-    var bi = LOWEST_ROBOT_ID
+    allgood = barrier_allgood(barrier,bc)
-    allgood = 1
-    while (bi<LOWEST_ROBOT_ID+threshold) {
-      if(barrier.get(bi) != bc)
-        allgood = 0
-      bi = bi + 1
-    }
   }
   
   if(allgood) {
@@ -78,3 +72,19 @@ function barrier_wait(threshold, transf, resumef, bc) {
 
   timeW = timeW+1
 }
+
+barriergood = 1
+
+# Barrer check all entries
+function barrier_allgood(barrier, bc) {
+  barriergood = 1
+  barrier.foreach(
+    function(key, value, robot){
+      #log("VS entry : ", key, " ", value, " ", robot)
+      if(value != bc and key != "d"){
+        barriergood = 0
+      }
+    }
+  )
+  return barriergood
+}

src/roscontroller.cpp

@@ -59,6 +59,18 @@ roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv)
   {
     robot_id = strtol(robot_name.c_str() + 5, NULL, 10);
   }
+  // Create log dir and open log file
+  std::string path =
+      bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
+  path = path.substr(0, bzzfile_name.find_last_of("\\/"))+"/log/";
+  std::string folder_check="mkdir -p "+path;
+  system(folder_check.c_str());
+  for(int i=5;i>0;i--){
+    rename((path +"logger_"+ std::to_string((uint8_t)robot_id)+"_"+std::to_string(i-1)+".log").c_str(),
+     (path +"logger_"+ std::to_string((uint8_t)robot_id)+"_"+std::to_string(i)+".log").c_str());
+  }
+  path += "logger_"+std::to_string(robot_id)+"_0.log";
+  log.open(path.c_str(), std::ios_base::trunc | std::ios_base::out);
 }
 
 roscontroller::~roscontroller()
@@ -68,6 +80,8 @@ roscontroller::~roscontroller()
 {
   // Destroy the BVM
   buzz_utility::buzz_script_destroy();
+  // Close the data logging file
+  log.close();
 }
 
 void roscontroller::GetRobotId()
@@ -137,6 +151,13 @@ void roscontroller::RosControllerRun()
       }
       if (debug)
         ROS_WARN("CURRENT PACKET DROP : %f ", cur_packet_loss);
+      // log data
+      log<<ros::Time::now()<<",";
+      log<<cur_pos.latitude << "," << cur_pos.longitude << ","
+            << cur_pos.altitude << ",";
+      log << (int)no_of_robots<<",";
+      log <<(int)buzz_utility::get_inmsg_size()<<",";
+      log <<buzz_utility::getuavstate()<<std::endl;
       //  Call Step from buzz script
       buzz_utility::buzz_script_step();
       //  Prepare messages and publish them
@@ -153,7 +174,6 @@ void roscontroller::RosControllerRun()
       buzz_utility::set_robot_var(no_of_robots);
       if(update) buzz_update::updates_set_robots(no_of_robots);
       // get_xbee_status();  // commented out because it may slow down the node too much, to be tested
-
       ros::spinOnce();
       loop_rate.sleep();
       // make sure to sleep for the rest of the loop time