Merge remote-tracking branch 'refs/remotes/origin/dev'

Conflicts:
	script/testflockfev.bzz
	src/roscontroller.cpp
	src/testflockfev.bzz

.gitignore

@@ -1,3 +1,7 @@
 src/test*
 .cproject
 .project
+.basm
+.bo
+.bdb
+.bdbg

include/buzz_utility.h

@@ -21,6 +21,12 @@ struct pos_struct
   pos_struct(){}
 };
 
+#define function_register(TABLE, FNAME, FPOINTER)               \
+   buzzvm_push(VM, (TABLE));                                    \
+   buzzvm_pushs(VM, buzzvm_string_register(VM, (FNAME), 1));    \
+   buzzvm_pushcc(VM, buzzvm_function_register(VM, (FPOINTER))); \
+   buzzvm_tput(VM);
+
 typedef struct pos_struct Pos_struct ;
 
 uint16_t* u64_cvt_u16(uint64_t u64);
@@ -28,13 +34,17 @@ uint16_t* u64_cvt_u16(uint64_t u64);
 int buzz_listen(const char* type,
                        int msg_size);
 
-void neighbour_pos_callback(std::map< int,  Pos_struct> neighbours_pos_map);
+void add_user(int id, double latitude, double longitude, float altitude);
+void update_users();
+int make_table(buzzobj_t* t);
+int buzzusers_add(int id, double latitude, double longitude, double altitude);
+int buzzusers_reset();
 
-int buzz_update_users_stigmergy(buzzobj_t tbl);
+void in_msg_append(uint64_t* payload);
 
-void in_msg_process(uint64_t* payload);
+uint64_t* obt_out_msg();
 
-uint64_t* out_msg_process();
+void update_sensors();
 
 int buzz_script_set(const char* bo_filename,
                            const char* bdbg_filename, int robot_id);

include/buzzuav_closures.h

@@ -46,12 +46,13 @@ void rc_call(int rc_cmd);
 void set_battery(float voltage,float current,float remaining);
 /* sets current position */
 void set_currentpos(double latitude, double longitude, double altitude);
-void set_userspos(double latitude, double longitude, double altitude);
 /*retuns the current go to position */
 double* getgoto();
 /* updates flight status*/
 void flight_status_update(uint8_t state);
-
+/* Update neighbors table */
+void neighbour_pos_callback(int id, float range, float bearing, float elevation);
+void update_neighbors(buzzvm_t vm);
 /*Flight status*/
 void set_obstacle_dist(float dist[]);
 
@@ -83,7 +84,6 @@ int buzzuav_update_battery(buzzvm_t vm);
  * Updates current position in Buzz
  */
 int buzzuav_update_currentpos(buzzvm_t vm);
-buzzobj_t buzzuav_update_userspos(buzzvm_t vm);
 
 /*
  * Updates flight status and rc command in Buzz, put it in a tabel to acess it 

include/roscontroller.h

@@ -137,7 +137,7 @@ private:
 	void Rosparameters_get(ros::NodeHandle& n_c_priv);
 
 	/*compiles buzz script from the specified .bzz file*/
-	void Compile_bzz();
+	std::string Compile_bzz(std::string bzzfile_name);
 
 	/*Flight controller service call*/
 	void flight_controller_service_call();

launch/rosbuzzm100.launch

@@ -2,16 +2,18 @@
 
 <launch>
 		<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >	
-		<param name="bzzfile_name" value="$(env ROS_WS)/src/ROSBuzz/src/test1.bzz" />
+		<rosparam file="$(find rosbuzz)/launch/launch_config/m100.yaml"/>
+		<param name="bzzfile_name" value="$(env ROS_WS)/src/testflockfev.bzz" />
                 <param name="rcclient" value="true" /> 
 		<param name="rcservice_name" value="/buzzcmd" />
-		<param name="fcclient_name" value="/dji_mavcmd" />
+		<!--param name="fcclient_name" value="/dji_mavcmd" -->
 		<param name="in_payload" value="/inMavlink"/>
 		<param name="out_payload" value="/outMavlink"/>
 		<param name="xbee_status_srv" value="/xbee_status"/>
-		<param name="robot_id" value="3"/>
+		<param name="xbee_plugged" value="true"/>
-		<param name="No_of_Robots" value="3"/>
+		<!--param name="robot_id" value="3"-->
-		<param name="stand_by" value="$(env ROS_WS)/src/ROSBuzz/src/stand_by.bo"/>
+		<!--param name="No_of_Robots" value="3"-->
+		<param name="stand_by" value="$(env ROS_WS)/src/stand_by.bo"/>
 		</node>
 		
 		

script/include/string.bzz

@@ -0,0 +1,92 @@
+#
+# Returns the string character at the given position.
+# PARAM s: The string
+# PARAM n: The position of the wanted character
+# RETURN The character at the wanted position, or nil
+#
+string.charat = function(s, n) {
+  return string.sub(s, n, n+1)
+}
+
+#
+# Returns the index of the first occurrence of the given string m
+# within another string s. If none is found, this function returns
+# nil.
+# PARAM s: The string
+# PARAM m: The string to match
+# RETURN: The position of the first match, or nil
+#
+string.indexoffirst = function(s, m) {
+  var ls = string.length(s)
+  var lm = string.length(m)
+  var i = 0
+  while(i < ls-lm+1) {
+    if(string.sub(s, i, i+lm) == m) return i
+    i = i + 1
+  }
+  return nil
+}
+
+#
+# Returns the index of the last occurrence of the given string m
+# within another string s. If none is found, this function returns
+# nil.
+# PARAM s: The string
+# PARAM m: The string to match
+# RETURN: The position of the last match, or nil
+#
+string.indexoflast = function(s, m) {
+  var ls = string.length(s)
+  var lm = string.length(m)
+  var i = ls - lm + 1
+  while(i >= 0) {
+    if(string.sub(s, i, i+lm) == m) return i
+    i = i - 1
+  }
+  return nil
+}
+
+# Splits a string s using the delimiters in d. The string list is
+# returned in a table indexed by value (starting at 0).
+# PARAM s: The string
+# PARAM d: A string containing the delimiters
+# RETURN: A table containing the tokens
+string.split = function(s, d) {
+  var i1 = 0  # index to move along s (token start)
+  var i2 = 0  # index to move along s (token end)
+  var c = 0   # token count
+  var t = {}  # token list
+  var ls = string.length(s)
+  var ld = string.length(d)
+  # Go through string s
+  while(i2 < ls) {
+    # Try every delimiter
+    var j = 0   # index to move along d
+    var f = nil # whether the delimiter was found or not
+    while(j < ld and (not f)) {
+      if(string.charat(s, i2) == string.charat(d, j)) {
+        # Delimiter found
+        f = 1
+        # Is it worth adding a new token?
+        if(i2 > i1) {
+          t[c] = string.sub(s, i1, i2)
+          c = c + 1
+        }
+        # Start new token
+        i1 = i2 + 1
+      }
+      else {
+        # Next delimiter
+        j = j + 1
+      }
+    }
+    # Next string character
+    i2 = i2 + 1
+  }
+  # Is it worth adding a new token?
+  if(i2 > i1) {
+    t[c] = string.sub(s, i1, i2)
+  }
+  # Return token list
+  return t;
+}

script/include/vec2.bzz

@@ -0,0 +1,107 @@
+#
+# Create a new namespace for vector2 functions
+#
+math.vec2 = {}
+
+#
+# Creates a new vector2.
+# PARAM x: The x coordinate.
+# PARAM y: The y coordinate.
+# RETURN: A new vector2.
+#
+math.vec2.new = function(x, y) {
+  return { .x = x, .y = y }
+}
+
+#
+# Creates a new vector2 from polar coordinates.
+# PARAM l: The length of the vector2.
+# PARAM a: The angle of the vector2.
+# RETURN: A new vector2.
+#
+math.vec2.newp = function(l, a) {
+  return {
+    .x = l * math.cos(a),
+    .y = l * math.sin(a)
+  }
+}
+
+#
+# Calculates the length of the given vector2.
+# PARAM v: The vector2.
+# RETURN: The length of the vector.
+#
+math.vec2.length = function(v) {
+  return math.sqrt(v.x * v.x + v.y * v.y)
+}
+
+#
+# Calculates the angle of the given vector2.
+# PARAM v: The vector2.
+# RETURN: The angle of the vector.
+#
+math.vec2.angle = function(v) {
+  return math.atan2(v.y, v.x)
+}
+
+#
+# Returns the normalized form of a vector2.
+# PARAM v: The vector2.
+# RETURN: The normalized form.
+#
+math.vec2.norm = function(v) {
+  var l = math.length(v)
+  return {
+    .x = v.x / l,
+    .y = v.y / l
+  }
+}
+
+#
+# Calculates v1 + v2.
+# PARAM v1: A vector2.
+# PARAM v2: A vector2.
+# RETURN: v1 + v2
+#
+math.vec2.add = function(v1, v2) {
+  return {
+    .x = v1.x + v2.x,
+    .y = v1.y + v2.y
+  }
+}
+
+#
+# Calculates v1 - v2.
+# PARAM v1: A vector2.
+# PARAM v2: A vector2.
+# RETURN: v1 + v2
+#
+math.vec2.sub = function(v1, v2) {
+  return {
+    .x = v1.x - v2.x,
+    .y = v1.y - v2.y
+  }
+}
+
+#
+# Scales a vector by a numeric constant.
+# PARAM v: A vector2.
+# PARAM s: A number (float or int).
+# RETURN: s * v
+#
+math.vec2.scale = function(v, s) {
+  return {
+    .x = v.x * s,
+    .y = v.y * s
+  }
+}
+
+#
+# Calculates v1 . v2 (the dot product)
+# PARAM v1: A vector2.
+# PARAM v2: A vector2.
+# RETURN: v1 . v2
+#
+math.vec2.dot = function(v1, v2) {
+  return v1.x * v2.x + v1.y * v2.y
+}

script/testflockfev.bzz

@@ -43,19 +43,15 @@ function hexagon() {
     math.vec2.scale(accum, 1.0 / neighbors.count())
   # Move according to vector
   #print("Robot ", id, "must push ",accum.length, "; ", accum.angle)
-#  uav_moveto(accum.x,accum.y)
+  uav_moveto(accum.x,accum.y)
 
-  if(timeW>=WAIT_TIMEOUT) {	#FOR MOVETO TESTS
+#  if(timeW>=WAIT_TIMEOUT) {	#FOR MOVETO TESTS
-  	timeW =0
+#  	timeW =0
-  	statef=land
+#  	statef=land
-  	} else {
+#  	} else {
-  		if(timeW >= (WAIT_TIMEOUT / 2)){
+#  	timeW = timeW+1
-  			uav_moveto(5.0,0.0)
+#  	uav_moveto(0.0,0.0)
-  		} else {
+#  	}
-  			uav_moveto(0.0,5.0)
-  		}
-  		timeW = timeW+1
-  	}
 }
  
 ########################################
@@ -90,7 +86,7 @@ function barrier_ready() {
 #
 # Executes the barrier
 #
-WAIT_TIMEOUT = 300
+WAIT_TIMEOUT = 200
 timeW=0
 function barrier_wait(threshold, transf) {
   barrier.get(id)

script/testflocksim.bzz

@@ -8,7 +8,7 @@ include "/home/ubuntu/buzz/src/include/vec2.bzz"
 updated="update_ack"
 update_no=0
 function updated_neigh(){
-neighbors.broadcast(updated, update_no)
+	neighbors.broadcast(updated, update_no)
 }
 
 TARGET_ALTITUDE = 10.0
@@ -141,11 +141,26 @@ function land() {
 	}
 }
 
+function table_print(t) {
+  foreach(t, function(key, value) {
+      log(key, " -> ", value)
+    })
+}
+
+########################################
+#
+# MAIN FUNCTIONS
+#
+########################################
+
 # Executed once at init time.
 function init() {
 	s = swarm.create(1)
-#	s.select(1)
 	s.join()
+	v = stigmergy.create(5)
+	t = {}
+	v.put("p",t)
+	v.put("u",1)
 	statef=idle
 	CURSTATE = "IDLE"
 }
@@ -196,12 +211,22 @@ neighbors.listen("cmd",
 	statef()
 	log("Current state: ", CURSTATE)
 	log("Swarm size: ",ROBOTS)
-	log("User position: ", users.range)
+	#log("User position: ", users.range)
 	
 	# Read a value from the structure
-        #x = usersvstig.get(46)
+        #t = v.get("p")
+				log(users)      
+				table_print(users)
+				if(size(users)>0){
+					tmp = {2 3}
+					v.put("p", tmp)
+					v.put("u",2)
+				}
+				
         # Get the number of keys in the structure
-        #log("The vstig has ", usersvstig.size(), " elements")
+        log("The vstig has ", v.size(), " elements")
+				table_print(v.get("p"))
+				log(v.get("u"))
 }
  
 # Executed once when the robot (or the simulator) is reset.

script/teststig.bzz

@@ -0,0 +1,38 @@
+####################################################################################################
+# Updater related
+# This should be here for the updater to work, changing position of code will crash the updater
+####################################################################################################
+updated="update_ack"
+update_no=0
+function updated_neigh(){
+neighbors.broadcast(updated, update_no)
+}
+
+function init(){
+    s = swarm.create(1)
+	s.join()
+    v = stigmergy.create(5)
+    #t= {}
+    #v.put("p",t)
+    v.put("u",1)
+}
+
+function step() {
+	log("Swarm size: ",ROBOTS)
+    log("The vstig has ", v.size(), " elements")
+    log(v.get("u"))
+    if (id==1) {
+        #tmp = { .x=3 }
+        #v.put("p",tmp)
+        v.put("u",2)
+    }
+	#log(v.get("p"))
+}
+
+# Executed once when the robot (or the simulator) is reset.
+function reset() {
+}
+ 
+# Executed once at the end of experiment.
+function destroy() {
+}

src/buzz_update.cpp

@@ -27,80 +27,72 @@ static int neigh=-1;
 static int 	    updater_msg_ready ;
 static int updated=0;
 
+/*Initialize updater*/
 void init_update_monitor(const char* bo_filename, const char* stand_by_script){
 	fprintf(stdout,"intiialized file monitor.\n");
 	fd=inotify_init1(IN_NONBLOCK);
 	if ( fd < 0 ) {
-	    perror( "inotify_init error" );
+		perror( "inotify_init error" );
-	  }
+	}
-	/* watch /.bzz file for any activity and report it back to me */
+	/* watch /.bzz file for any activity and report it back to update */
 	wd=inotify_add_watch(fd, bzz_file,IN_ALL_EVENTS );
-
+	/*load the .bo under execution into the updater*/
 	uint8_t*    BO_BUF          = 0;
 	FILE* fp = fopen(bo_filename, "rb");
-
+	if(!fp) {
-	   if(!fp) {
+		perror(bo_filename);
-	      perror(bo_filename);
+	}
-	    
+	fseek(fp, 0, SEEK_END);
-	   }
+	size_t bcode_size = ftell(fp);
-	   fseek(fp, 0, SEEK_END);
+	rewind(fp);
-
+	BO_BUF = (uint8_t*)malloc(bcode_size);
-	   size_t bcode_size = ftell(fp);
+	if(fread(BO_BUF, 1, bcode_size, fp) < bcode_size) {
-	   rewind(fp);
+		perror(bo_filename);
-
+		fclose(fp);
-	   BO_BUF = (uint8_t*)malloc(bcode_size);
+	//return 0;
-	   if(fread(BO_BUF, 1, bcode_size, fp) < bcode_size) {
+	}
-	      perror(bo_filename);
+	fclose(fp);
-	     
+	/*Load stand_by .bo file into the updater*/
-	      fclose(fp);
-	      //return 0;
-	   }
-	   fclose(fp);
 	uint8_t* STD_BO_BUF          = 0;
 	fp = fopen(stand_by_script, "rb");
+	if(!fp) {
+		perror(stand_by_script);
 
-	   if(!fp) {
+	}
-	      perror(stand_by_script);
+	fseek(fp, 0, SEEK_END);
-	    
+	size_t stdby_bcode_size = ftell(fp);
-	   }
+	rewind(fp);
-	   fseek(fp, 0, SEEK_END);
+	STD_BO_BUF = (uint8_t*)malloc(stdby_bcode_size);
-
+	if(fread(STD_BO_BUF, 1, stdby_bcode_size, fp) < stdby_bcode_size) {
-	   size_t stdby_bcode_size = ftell(fp);
+		perror(stand_by_script);
-	   rewind(fp);
+	fclose(fp);
-
+	//return 0;
-	   STD_BO_BUF = (uint8_t*)malloc(stdby_bcode_size);
+	}
-	   if(fread(STD_BO_BUF, 1, stdby_bcode_size, fp) < stdby_bcode_size) {
+	fclose(fp);
-	      perror(stand_by_script);
+	/*Create the updater*/	
-	     
+	updater = (buzz_updater_elem_t)malloc(sizeof(struct buzz_updater_elem_s));
-	      fclose(fp);
+	/*Intialize the updater with the required data*/
-	      //return 0;
+	updater->bcode = BO_BUF;
-	   }
+	updater->outmsg_queue = NULL;
-	   fclose(fp);
+	updater->inmsg_queue = NULL;
-
+	updater->bcode_size = (size_t*) malloc(sizeof(size_t));
-	 updater = (buzz_updater_elem_t)malloc(sizeof(struct buzz_updater_elem_s));
+	updater->update_no = (uint8_t*) malloc(sizeof(uint16_t));
-	  /* Create a new table for updater*/
+	*(uint16_t*)(updater->update_no) =0;
-	  updater->bcode = BO_BUF;
+	*(size_t*)(updater->bcode_size)=bcode_size;
-	  updater->outmsg_queue = NULL;
+	updater->standby_bcode = STD_BO_BUF;
-	  updater->inmsg_queue = NULL;
+	updater->standby_bcode_size = (size_t*)malloc(sizeof(size_t));
-	  updater->bcode_size = (size_t*) malloc(sizeof(size_t));
+	*(size_t*)(updater->standby_bcode_size)=stdby_bcode_size;
-	  updater->update_no = (uint8_t*) malloc(sizeof(uint16_t));
+	updater->mode=(int*)malloc(sizeof(int));
-	  *(uint16_t*)(updater->update_no) =0;
+	*(int*)(updater->mode)=CODE_RUNNING;
-	  *(size_t*)(updater->bcode_size)=bcode_size;
+	//no_of_robot=barrier;
-	  updater->standby_bcode = STD_BO_BUF;
+	updater_msg_ready=0;
-	  updater->standby_bcode_size = (size_t*)malloc(sizeof(size_t));
+	//neigh = 0;
-	  *(size_t*)(updater->standby_bcode_size)=stdby_bcode_size;
+	//updater->outmsg_queue=
-	  updater->mode=(int*)malloc(sizeof(int));
+	// update_table->barrier=nvs;
-	  *(int*)(updater->mode)=CODE_RUNNING;
-	  //no_of_robot=barrier;
-	  updater_msg_ready=0;
-	  //neigh = 0;
-	  //updater->outmsg_queue=
-	  // update_table->barrier=nvs;
 
 }
-
+/*Check for .bzz file chages*/
 int check_update(){
-        
 	struct inotify_event *event;
 	char buf[1024];
 	int check =0;
@@ -108,7 +100,6 @@ int check_update(){
 	int len=read(fd,buf,1024);
 	while(i<len){
 		event=(struct inotify_event *) &buf[i];
-
 		/* file was modified this flag is true in nano and self delet in gedit and other editors */
                 //fprintf(stdout,"inside file monitor.\n");
 		if(event->mask & (IN_MODIFY| IN_DELETE_SELF)){
@@ -123,20 +114,12 @@ int check_update(){
 			check=1;
 			old_update =1;
 			}
-
 		}
-
 		/* update index to start of next event */
 		i+=sizeof(struct inotify_event)+event->len;
-
 	}
-
 	if (!check) old_update=0;
-	/*if(update){
+return check;
-	buzz_script_set(update_bo, update_bdbg);
-	update = 0;
-	}*/
-	return check;
 }
 
 
@@ -163,57 +146,56 @@ void code_message_outqueue_append(){
 }
 
 void code_message_inqueue_append(uint8_t* msg,uint16_t size){
-updater->inmsg_queue=(updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s));
+	updater->inmsg_queue=(updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s));
-fprintf(stdout,"in ms append code size %d\n", (int) size);
+	fprintf(stdout,"in ms append code size %d\n", (int) size);
-updater->inmsg_queue->queue = (uint8_t*)malloc(size);
+	updater->inmsg_queue->queue = (uint8_t*)malloc(size);
-updater->inmsg_queue->size  = (uint8_t*)malloc(sizeof(uint16_t));
+	updater->inmsg_queue->size  = (uint8_t*)malloc(sizeof(uint16_t));
-memcpy(updater->inmsg_queue->queue, msg, size);
+	memcpy(updater->inmsg_queue->queue, msg, size);
-*(uint16_t*)(updater->inmsg_queue->size)  = size;
+	*(uint16_t*)(updater->inmsg_queue->size)  = size;
-
 }
 
 void code_message_inqueue_process(){
-int size=0;
+	int size=0;
-fprintf(stdout,"[debug]Updater mode %d \n", *(int*)(updater->mode) );
+	fprintf(stdout,"[debug]Updater mode %d \n", *(int*)(updater->mode) );
-fprintf(stdout,"[debug] %u : current update number, %u : received update no \n",( *(uint16_t*) (updater->update_no) ), (*(uint16_t*)(updater->inmsg_queue->queue)) );
+	fprintf(stdout,"[debug] %u : current update number, %u : received update no \n",( *(uint16_t*) (updater->update_no) ), (*(uint16_t*)(updater->inmsg_queue->queue)) );
-fprintf(stdout,"[debug]Updater code size %u \n",(*(uint16_t*)(updater->inmsg_queue->queue+sizeof(uint16_t)) ) );
+	fprintf(stdout,"[debug]Updater code size %u \n",(*(uint16_t*)(updater->inmsg_queue->queue+sizeof(uint16_t)) ) );
 
-if(  *(int*) (updater->mode) == CODE_RUNNING){		
+	if(  *(int*) (updater->mode) == CODE_RUNNING){		
-	//fprintf(stdout,"[debug]Inside inmsg code running");
+		//fprintf(stdout,"[debug]Inside inmsg code running");
-	if( *(uint16_t*)(updater->inmsg_queue->queue) >  *(uint16_t*) (updater->update_no)  ){
+		if( *(uint16_t*)(updater->inmsg_queue->queue) >  *(uint16_t*) (updater->update_no)  ){
-		//fprintf(stdout,"[debug]Inside update number comparision");
+			//fprintf(stdout,"[debug]Inside update number comparision");
-		uint16_t update_no=*(uint16_t*)(updater->inmsg_queue->queue);	
+			uint16_t update_no=*(uint16_t*)(updater->inmsg_queue->queue);	
-		size +=sizeof(uint16_t);	
+			size +=sizeof(uint16_t);	
-		uint16_t update_bcode_size =*(uint16_t*)(updater->inmsg_queue->queue+size);
+			uint16_t update_bcode_size =*(uint16_t*)(updater->inmsg_queue->queue+size);
-		size +=sizeof(uint16_t);	
+			size +=sizeof(uint16_t);	
-		//fprintf(stdout,"in queue process Update no %d\n", (int) update_no);
+			//fprintf(stdout,"in queue process Update no %d\n", (int) update_no);
-		//fprintf(stdout,"in queue process bcode size %d\n", (int) update_bcode_size);
+			//fprintf(stdout,"in queue process bcode size %d\n", (int) update_bcode_size);
-		//FILE *fp;
+			//FILE *fp;
-		//fp=fopen("update.bo", "wb");
+			//fp=fopen("update.bo", "wb");
-		//fwrite((updater->inmsg_queue->queue+size), update_bcode_size, 1, fp);
+			//fwrite((updater->inmsg_queue->queue+size), update_bcode_size, 1, fp);
-		//fclose(fp);
+			//fclose(fp);
-		if( test_set_code((uint8_t*)(updater->inmsg_queue->queue+size),
+			if( test_set_code((uint8_t*)(updater->inmsg_queue->queue+size),
-		   (char*) dbgf_name,(size_t)update_bcode_size) ) {
+				(char*) dbgf_name,(size_t)update_bcode_size) ) {
-		*(uint16_t*)(updater->update_no)=update_no;
+				*(uint16_t*)(updater->update_no)=update_no;
-		neigh=1;
+				neigh=1;
+			}
 		}
 	}
-}
 
-//fprintf(stdout,"in queue freed\n");
+	//fprintf(stdout,"in queue freed\n");
-delete_p(updater->inmsg_queue->queue);
+	delete_p(updater->inmsg_queue->queue);
-delete_p(updater->inmsg_queue->size);
+	delete_p(updater->inmsg_queue->size);
-delete_p(updater->inmsg_queue);
+	delete_p(updater->inmsg_queue);
 }
 void update_routine(const char* bcfname,
                            const char* dbgfname){
-dbgf_name=(char*)dbgfname;
+	dbgf_name=(char*)dbgfname;
-buzzvm_t  VM = buzz_utility::get_vm();
+	buzzvm_t  VM = buzz_utility::get_vm();
 
-buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1));
+	buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1));
-			buzzvm_pushi(VM, *(uint16_t*)(updater->update_no));
+				buzzvm_pushi(VM, *(uint16_t*)(updater->update_no));
-			buzzvm_gstore(VM);
+				buzzvm_gstore(VM);
-//fprintf(stdout,"[Debug : ]updater value = %i \n",updater->mode);
+	//fprintf(stdout,"[Debug : ]updater value = %i \n",updater->mode);
 	if(*(int*)updater->mode==CODE_RUNNING){
 		buzzvm_function_call(VM, "updated_neigh", 0);
 		if(check_update()){
@@ -226,65 +208,55 @@ buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1));
 			std::string  name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
  			name = name.substr(0,name.find_last_of("."));
 			bzzfile_in_compile << "bzzparse "<<bzzfile_name<<" "<<path<< name<<".basm";
-		
 			FILE *fp;
 			int comp=0;
 			char buf[128];
 			fprintf(stdout,"Update found \nUpdating script ...\n");
-			
 			if ((fp = popen(bzzfile_in_compile.str().c_str(), "r")) == NULL) { // to change file edit
 				fprintf(stdout,"Error opening pipe!\n");
-		 	   	}
+		 	}
-		
+ 	 	     	while (fgets(buf, 128, fp) != NULL) {
-				 while (fgets(buf, 128, fp) != NULL) {
+				fprintf(stdout,"OUTPUT: %s \n", buf);
-					fprintf(stdout,"OUTPUT: %s \n", buf);
+				comp=1;		    		
-					comp=1;		    		
+			}
-				     	}
 			bzzfile_in_compile.str("");
            		bzzfile_in_compile <<"bzzasm "<<path<<name<<".basm "<<path<<name<<".bo "<<path<<name<<".bdbg";
-
 			if ((fp = popen(bzzfile_in_compile.str().c_str(), "r")) == NULL) { // to change file edit
-			fprintf(stdout,"Error opening pipe!\n");
+				fprintf(stdout,"Error opening pipe!\n");
-		 	    }
+		 	}
-		 	 while (fgets(buf, 128, fp) != NULL) {
+		 	while (fgets(buf, 128, fp) != NULL) {
 				fprintf(stdout,"OUTPUT: %s \n", buf);
-		    	    }
+		    	}
-
+	  		if(pclose(fp) || comp) {
-	  		 if(pclose(fp) || comp)  {
+	     			fprintf(stdout,"Errors in comipilg script so staying on old script\n");
-	     		  fprintf(stdout,"Errors in comipilg script so staying on old script\n");
+	 		}
-	    		  //  return -1;
+	  		else {	
-	 		   }
-	  		 else {
-				
 				uint8_t*    BO_BUF          = 0;
 				FILE* fp = fopen(bcfname, "rb");  // to change file edit
-				   if(!fp) {
+				if(!fp) {
-				      perror(bcfname);
+					perror(bcfname);
-				      
+				}
-				   }
+				fseek(fp, 0, SEEK_END);
-				   fseek(fp, 0, SEEK_END);
+				size_t bcode_size = ftell(fp);
-				   size_t bcode_size = ftell(fp);
+				rewind(fp);
-				   rewind(fp);
+				BO_BUF = (uint8_t*)malloc(bcode_size);
-				   BO_BUF = (uint8_t*)malloc(bcode_size);
+				if(fread(BO_BUF, 1, bcode_size, fp) < bcode_size) {
-				   if(fread(BO_BUF, 1, bcode_size, fp) < bcode_size) {
+					perror(bcfname);  
-				      perror(bcfname);
+					fclose(fp);
-				      
+				}
-				      fclose(fp);
+				fclose(fp);
-				      
-				   }
-				   fclose(fp);
 				if(test_set_code(BO_BUF, dbgfname,bcode_size)){
-				 uint16_t update_no =*(uint16_t*)(updater->update_no);
+					uint16_t update_no =*(uint16_t*)(updater->update_no);
-				*(uint16_t*)(updater->update_no) =update_no +1;
+					*(uint16_t*)(updater->update_no) =update_no +1;
-				code_message_outqueue_append();
+					code_message_outqueue_append();
-				VM = buzz_utility::get_vm();
+					VM = buzz_utility::get_vm();
-				fprintf(stdout,"Update no %d\n", *(uint16_t*)(updater->update_no));
+					fprintf(stdout,"Update no %d\n", *(uint16_t*)(updater->update_no));
-				buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1));
+					buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1));
-				buzzvm_pushi(VM, *(uint16_t*)(updater->update_no));
+					buzzvm_pushi(VM, *(uint16_t*)(updater->update_no));
-				buzzvm_gstore(VM);
+					buzzvm_gstore(VM);
-				neigh=-1;
+					neigh=-1;
-				fprintf(stdout,"Sending code... \n");		
+					fprintf(stdout,"Sending code... \n");		
-			        code_message_outqueue_append();
+					code_message_outqueue_append();
 				}
 				delete_p(BO_BUF);
 		       	}
@@ -330,7 +302,6 @@ return (uint8_t*)updater->outmsg_queue->queue;
 
 uint8_t* getupdate_out_msg_size(){
 //fprintf(stdout,"[Debug  from get out size in util: ]size = %i \n",*(uint16_t*)updater->outmsg_queue->size);
-		
 return (uint8_t*)updater->outmsg_queue->size;
 }
 
@@ -397,17 +368,17 @@ int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_size ){
 }
 
 void destroy_out_msg_queue(){
-//fprintf(stdout,"out queue freed\n");
+	//fprintf(stdout,"out queue freed\n");
-delete_p(updater->outmsg_queue->queue);
+	delete_p(updater->outmsg_queue->queue);
-delete_p(updater->outmsg_queue->size);
+	delete_p(updater->outmsg_queue->size);
-delete_p(updater->outmsg_queue);
+	delete_p(updater->outmsg_queue);
-updater_msg_ready=0;
+	updater_msg_ready=0;
 }
 int get_update_status(){
 return updated;
 }
 void set_read_update_status(){
-updated=0;
+	updated=0;
 }
 int get_update_mode(){
 return (int)*(int*)(updater->mode);
@@ -420,44 +391,44 @@ buzz_updater_elem_t get_updater(){
 return updater;
 }
 void destroy_updater(){
-delete_p(updater->bcode);
+	delete_p(updater->bcode);
-delete_p(updater->bcode_size);
+	delete_p(updater->bcode_size);
-delete_p(updater->standby_bcode);
+	delete_p(updater->standby_bcode);
-delete_p(updater->standby_bcode_size);
+	delete_p(updater->standby_bcode_size);
-delete_p(updater->mode);
+	delete_p(updater->mode);
-delete_p(updater->update_no);
+	delete_p(updater->update_no);
-if(updater->outmsg_queue){
+	if(updater->outmsg_queue){
-delete_p(updater->outmsg_queue->queue);
+		delete_p(updater->outmsg_queue->queue);
-delete_p(updater->outmsg_queue->size);
+		delete_p(updater->outmsg_queue->size);
-delete_p(updater->outmsg_queue);
+		delete_p(updater->outmsg_queue);
-}
+	}
-if(updater->inmsg_queue){
+	if(updater->inmsg_queue){
-delete_p(updater->inmsg_queue->queue);
+		delete_p(updater->inmsg_queue->queue);
-delete_p(updater->inmsg_queue->size);
+		delete_p(updater->inmsg_queue->size);
-delete_p(updater->inmsg_queue);
+		delete_p(updater->inmsg_queue);
-}
+	}
-//
+	//
-inotify_rm_watch(fd,wd);
+	inotify_rm_watch(fd,wd);
-close(fd);
+	close(fd);
-}
+	}
 
 void set_bzz_file(const char* in_bzz_file){
-bzz_file=in_bzz_file;
+	bzz_file=in_bzz_file;
 }
 
 void updates_set_robots(int robots){
-no_of_robot=robots;
+	no_of_robot=robots;
 }
 
 void collect_data(){
-//fprintf(stdout,"start and end time in data collection Info : %f,%f",(double)begin,(double)end);
+	//fprintf(stdout,"start and end time in data collection Info : %f,%f",(double)begin,(double)end);
-double time_spent =   (t2.tv_sec - t1.tv_sec) * 1000.0; //(double)(end - begin) / CLOCKS_PER_SEC;
+	double time_spent =   (t2.tv_sec - t1.tv_sec) * 1000.0; //(double)(end - begin) / CLOCKS_PER_SEC;
-time_spent += (t2.tv_usec - t1.tv_usec) / 1000.0;
+	time_spent += (t2.tv_usec - t1.tv_usec) / 1000.0;
-//int bytecodesize=(int);
+	//int bytecodesize=(int);
-fprintf(stdout,"Data logger Info : %i , %i , %f , %ld , %i , %d \n",(int)no_of_robot,neigh,time_spent,*(size_t*)updater->bcode_size,(int)no_of_robot,*(uint8_t*)updater->update_no);
+	fprintf(stdout,"Data logger Info : %i , %i , %f , %ld , %i , %d \n",(int)no_of_robot,neigh,time_spent,*(size_t*)updater->bcode_size,(int)no_of_robot,*(uint8_t*)updater->update_no);
-//FILE *Fileptr;
+	//FILE *Fileptr;
-//Fileptr=fopen("/home/ubuntu/ROS_WS/update_logger.csv", "a");
+	//Fileptr=fopen("/home/ubuntu/ROS_WS/update_logger.csv", "a");
-//fprintf(Fileptr,"%i,%i,%i,%i,%i,%u\n",(int)buzz_utility::get_robotid(),neigh,timer_steps,(int)*(size_t*)updater->bcode_size,(int)no_of_robot, *(uint8_t*)updater->update_no);
+	//fprintf(Fileptr,"%i,%i,%i,%i,%i,%u\n",(int)buzz_utility::get_robotid(),neigh,timer_steps,(int)*(size_t*)updater->bcode_size,(int)no_of_robot, *(uint8_t*)updater->update_no);
-//fclose(Fileptr);
+	//fclose(Fileptr);
 }
 

src/buzz_utility.cpp

@@ -6,7 +6,7 @@
  *  @copyright 2016 MistLab. All rights reserved.
  */
 
-#include <buzz_utility.h>
+#include "buzz_utility.h"
 
 namespace buzz_utility{
 
@@ -19,38 +19,141 @@ namespace buzz_utility{
 	static buzzdebug_t  DBG_INFO        = 0;
 	static uint8_t      MSG_SIZE        = 50;   // Only 100 bytes of Buzz messages every step
 	static int          MAX_MSG_SIZE    = 10000; // Maximum Msg size for sending update packets 
-	static int 	    Robot_id        = 0;
+	static int 	    	Robot_id        = 0;
-        buzzobj_t           usersvstig;
+
-        buzzobj_t           key;
+	std::map< int,  Pos_struct> users_map;
 	
 	/****************************************/
-	/*adds neighbours position*/
+
-	void neighbour_pos_callback(std::map< int,  Pos_struct> neighbours_pos_map){
+	void add_user(int id, double latitude, double longitude, float altitude)
-		/* Reset neighbor information */
+	{
-    		buzzneighbors_reset(VM);
+		Pos_struct pos_arr;
-  		/* Get robot id and update neighbor information */
+		pos_arr.x=latitude;
-  	  	map< int, Pos_struct >::iterator it;
+		pos_arr.y=longitude;
-    		for (it=neighbours_pos_map.begin(); it!=neighbours_pos_map.end(); ++it){
+		pos_arr.z=altitude;
-    			buzzneighbors_add(VM,
+		map< int, buzz_utility::Pos_struct >::iterator it = users_map.find(id);
-                        		  it->first,
+		if(it!=users_map.end())
-                        		  (it->second).x,
+			users_map.erase(it);
-                        		  (it->second).y,
+		users_map.insert(make_pair(id, pos_arr));
-                        		  (it->second).z);
+		//ROS_INFO("Buzz_utility got updated/new user: %i (%f,%f,%f)", id, latitude, longitude, altitude);
-    		}
+	}
+
+	void update_users(){
+		if(users_map.size()>0) {
+			/* Reset users information */
+			buzzusers_reset();
+			/* Get user id and update user information */
+			map< int, Pos_struct >::iterator it;
+			for (it=users_map.begin(); it!=users_map.end(); ++it){
+				//ROS_INFO("Buzz_utility will save user %i.", it->first);
+				buzzusers_add(it->first,
+									(it->second).x,
+									(it->second).y,
+									(it->second).z);
+				buzzusers_add(it->first+1,
+									(it->second).x,
+									(it->second).y,
+									(it->second).z);
+			}
+		}else
+			ROS_INFO("[%i] No new users",Robot_id);
+	}
+
+	int buzzusers_reset() {
+		if(VM->state != BUZZVM_STATE_READY) return VM->state;
+		/* Make new table */
+		buzzobj_t t = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE);
+		//make_table(&t);
+		if(VM->state != BUZZVM_STATE_READY) return VM->state;
+		/* Register table as global symbol */
+		/*buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
+		buzzvm_gload(VM);
+		buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1));
+        buzzvm_tget(VM);*/
+        buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
+		buzzvm_push(VM, t);
+		buzzvm_gstore(VM);
+        //buzzvm_pushi(VM, 2);
+		//buzzvm_callc(VM);
+		return VM->state;
+	}
+
+	int buzzusers_add(int id, double latitude, double longitude, double altitude) {
+		if(VM->state != BUZZVM_STATE_READY) return VM->state;
+		/* Get users "p" table */
+		/*buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
+		buzzvm_gload(VM);
+		buzzvm_pushs(VM, buzzvm_string_register(VM, "get", 1));
+        buzzvm_tget(VM);*/
+		buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
+		buzzvm_gload(VM);
+        //buzzvm_pushi(VM, 1);
+		//buzzvm_callc(VM);
+		buzzvm_type_assert(VM, 1, BUZZTYPE_TABLE);
+		buzzobj_t nbr = buzzvm_stack_at(VM, 1);
+		/* Get "data" field */
+		buzzvm_pushs(VM, buzzvm_string_register(VM, "data", 1));
+		buzzvm_tget(VM);
+		if(buzzvm_stack_at(VM, 1)->o.type == BUZZTYPE_NIL) {
+			ROS_INFO("Empty data, create a new table");
+			buzzvm_pop(VM);
+			buzzvm_push(VM, nbr);
+			buzzvm_pushs(VM, buzzvm_string_register(VM, "data", 1));
+			buzzvm_pusht(VM);
+			buzzobj_t data = buzzvm_stack_at(VM, 1);
+			buzzvm_tput(VM);
+			buzzvm_push(VM, data);
+		}
+		/* When we get here, the "data" table is on top of the stack */
+		/* Push user id */
+		buzzvm_pushi(VM, id);
+		/* Create entry table */
+		buzzobj_t entry = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE);
+		/* Insert latitude */
+		buzzvm_push(VM, entry);
+		buzzvm_pushs(VM, buzzvm_string_register(VM, "la", 1));
+		buzzvm_pushf(VM, latitude);
+		buzzvm_tput(VM);
+		/* Insert longitude */
+		buzzvm_push(VM, entry);
+		buzzvm_pushs(VM, buzzvm_string_register(VM, "lo", 1));
+		buzzvm_pushf(VM, longitude);
+		buzzvm_tput(VM);
+		/* Insert altitude */
+		buzzvm_push(VM, entry);
+		buzzvm_pushs(VM, buzzvm_string_register(VM, "al", 1));
+		buzzvm_pushf(VM, altitude);
+		buzzvm_tput(VM);
+		/* Save entry into data table */
+		buzzvm_push(VM, entry);
+		buzzvm_tput(VM);
+		ROS_INFO("Buzz_utility saved new user: %i (%f,%f,%f)", id, latitude, longitude, altitude);
+		// forcing the new table into the stigmergy....
+		/*buzzobj_t newt = buzzvm_stack_at(VM, 0);
+		buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
+		buzzvm_gload(VM);
+		buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1));
+        buzzvm_tget(VM);
+		buzzvm_pushs(VM, buzzvm_string_register(VM, "p", 1));
+        buzzvm_push(VM, nbr);
+        buzzvm_pushi(VM, 2);
+		buzzvm_callc(VM);*/
+		//buzzvm_gstore(VM);
+		return VM->state;
 	}
 	/**************************************************************************/
 	/*Deserializes uint64_t into 4 uint16_t, freeing out is left to the user  */
 	/**************************************************************************/
 	uint16_t* u64_cvt_u16(uint64_t u64){
-   	uint16_t* out = new uint16_t[4];
+		uint16_t* out = new uint16_t[4];
-   	uint32_t int32_1 = u64 & 0xFFFFFFFF;
+		uint32_t int32_1 = u64 & 0xFFFFFFFF;
-   	uint32_t int32_2 = (u64 & 0xFFFFFFFF00000000 ) >> 32;
+		uint32_t int32_2 = (u64 & 0xFFFFFFFF00000000 ) >> 32;
-   	 out[0] = int32_1 & 0xFFFF;
+		out[0] = int32_1 & 0xFFFF;
-   	 out[1] = (int32_1 & (0xFFFF0000) ) >> 16;
+		out[1] = (int32_1 & (0xFFFF0000) ) >> 16;
-   	 out[2] = int32_2 & 0xFFFF;
+		out[2] = int32_2 & 0xFFFF;
-   	 out[3] = (int32_2 & (0xFFFF0000) ) >> 16;
+		out[3] = (int32_2 & (0xFFFF0000) ) >> 16;
-   	//cout << " values " <<out[0] <<"  "<<out[1] <<"  "<<out[2] <<"  "<<out[3] <<"  ";
+		//cout << " values " <<out[0] <<"  "<<out[1] <<"  "<<out[2] <<"  "<<out[3] <<"  ";
-	return out;
+		return out;
 	}
 
 	int get_robotid() {
@@ -68,7 +171,7 @@ namespace buzz_utility{
 	/*|__________________________________________________________________________|____________________________________|*/
 	/*******************************************************************************************************************/
 
-	void in_msg_process(uint64_t* payload){
+	void in_msg_append(uint64_t* payload){
 
    		/* Go through messages and add them to the FIFO */
    		uint16_t* data= u64_cvt_u16((uint64_t)payload[0]);
@@ -97,15 +200,14 @@ namespace buzz_utility{
 		 			}
 	      			}while(size - tot > sizeof(uint16_t) && unMsgSize > 0);
 		free(pl);
-   		/* Process messages */
+   		
-		buzzvm_process_inmsgs(VM);
 	}
 	/***************************************************/
 	/*Obtains messages from buzz out message Queue*/
 	/***************************************************/
 
-   	uint64_t* out_msg_process(){
+   	uint64_t* obt_out_msg(){
-   		buzzvm_process_outmsgs(VM);
+
    		uint8_t* buff_send =(uint8_t*)malloc(MAX_MSG_SIZE);
    		memset(buff_send, 0, MAX_MSG_SIZE);
 		/*Taking into consideration the sizes included at the end*/
@@ -151,8 +253,8 @@ namespace buzz_utility{
    		cout<<" payload from out msg  "<<*(payload_64+i)<<endl;
    		}*/
    		/* Send message */
-		return payload_64;
+	return payload_64;
-		}
+	}
 
 	/****************************************/
 	/*Buzz script not able to load*/
@@ -177,7 +279,7 @@ namespace buzz_utility{
               			 VM->pc,
       			         VM->errormsg);
    		}
-  	 	return msg;
+ 	return msg;
 	}
 
 	/****************************************/
@@ -189,15 +291,15 @@ namespace buzz_utility{
    		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
    		buzzvm_gstore(VM);
    		buzzvm_pushs(VM,  buzzvm_string_register(VM, "log", 1));
-                buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
+        buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
-                buzzvm_gstore(VM);
+        buzzvm_gstore(VM);
-                buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_moveto", 1));
+        buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_moveto", 1));
    		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_moveto));
    		buzzvm_gstore(VM);
-                buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_goto", 1));
+        buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_goto", 1));
    		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_goto));
    		buzzvm_gstore(VM);
-                buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_arm", 1));
+        buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_arm", 1));
    		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_arm));
    		buzzvm_gstore(VM);
 		buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_disarm", 1));
@@ -212,7 +314,8 @@ namespace buzz_utility{
    		buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_land", 1));
    		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_land));
    		buzzvm_gstore(VM);
-   	return BUZZVM_STATE_READY;
+
+   	return VM->state;
 	}
 
 	/**************************************************/
@@ -224,15 +327,15 @@ namespace buzz_utility{
    		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
    		buzzvm_gstore(VM);
    		buzzvm_pushs(VM,  buzzvm_string_register(VM, "log", 1));
-                buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
+        buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
-                buzzvm_gstore(VM);
+        buzzvm_gstore(VM);
-                buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_moveto", 1));
+        buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_moveto", 1));
    		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
    		buzzvm_gstore(VM);
-                buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_goto", 1));
+        buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_goto", 1));
    		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
    		buzzvm_gstore(VM);
-                buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_arm", 1));
+        buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_arm", 1));
    		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
    		buzzvm_gstore(VM);
 		buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_disarm", 1));
@@ -247,86 +350,124 @@ namespace buzz_utility{
    		buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_land", 1));
    		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
    		buzzvm_gstore(VM);
-   	return BUZZVM_STATE_READY;
+
+   	return VM->state;
 	}
 
+static int create_stig_tables() {
 
+   		// usersvstig = stigmergy.create(123)
+        buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
+        // get the stigmergy table from the global scope
+        buzzvm_pushs(VM, buzzvm_string_register(VM, "stigmergy", 1));
+        buzzvm_gload(VM);
+        // get the create method from the stigmergy table
+        buzzvm_pushs(VM, buzzvm_string_register(VM, "create", 1));
+        buzzvm_tget(VM);
+        // value of the stigmergy id
+        buzzvm_pushi(VM, 5);
+        // call the stigmergy.create() method
+		buzzvm_dump(VM);
+//        buzzvm_closure_call(VM, 1);
+		buzzvm_pushi(VM, 1);
+		buzzvm_callc(VM);
+        buzzvm_gstore(VM);
+		buzzvm_dump(VM);
+
+		//buzzusers_reset();
+		buzzobj_t t = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE);
+
+		buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
+		buzzvm_gload(VM);
+		buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1));
+        buzzvm_tget(VM);
+		buzzvm_pushs(VM, buzzvm_string_register(VM, "p", 1));
+		buzzvm_push(VM, t);
+		buzzvm_dump(VM);
+//        buzzvm_closure_call(VM, 2);
+        buzzvm_pushi(VM, 2);
+		buzzvm_callc(VM);
+        //buzzvm_gstore(VM);
+		//buzzvm_dump(VM);
+
+		/*buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
+        buzzvm_gload(VM);
+        buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1));
+        buzzvm_tget(VM);
+        buzzvm_pushs(VM, buzzvm_string_register(VM, "u", 1));
+        buzzvm_pusht(VM);
+        buzzvm_pushi(VM, 2);
+		buzzvm_call(VM, 0);
+        buzzvm_gstore(VM);*/
+
+   	return VM->state;
+}
 	/****************************************/
 	/*Sets the .bzz and .bdbg file*/
 	/****************************************/
-
 	int buzz_script_set(const char* bo_filename,
 	                    const char* bdbg_filename, int robot_id) {
-   	cout << " Robot ID: " <<robot_id<< endl;
+	   	ROS_INFO(" Robot ID: " , robot_id);
-   	/* Reset the Buzz VM */
+	   	/* Reset the Buzz VM */
-   	if(VM) buzzvm_destroy(&VM);
+	   	if(VM) buzzvm_destroy(&VM);
-	Robot_id = robot_id;
+		Robot_id = robot_id;
-   	VM = buzzvm_new((int)robot_id);
+	   	VM = buzzvm_new((int)robot_id);
-   	/* Get rid of debug info */
+	   	/* Get rid of debug info */
-   	if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
+	   	if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
-   	DBG_INFO = buzzdebug_new();
+	   	DBG_INFO = buzzdebug_new();
-   	/* Read bytecode and fill in data structure */
+	   	/* Read bytecode and fill in data structure */
-   	FILE* fd = fopen(bo_filename, "rb");
+	   	FILE* fd = fopen(bo_filename, "rb");
-   	if(!fd) {
+	   	if(!fd) {
-      		perror(bo_filename);
+	      		perror(bo_filename);
       		return 0;
-   	}
+	   	}
-   	fseek(fd, 0, SEEK_END);
+	   	fseek(fd, 0, SEEK_END);
-   	size_t bcode_size = ftell(fd);
+	   	size_t bcode_size = ftell(fd);
-   	rewind(fd);
+	   	rewind(fd);
-   	BO_BUF = (uint8_t*)malloc(bcode_size);
+	   	BO_BUF = (uint8_t*)malloc(bcode_size);
-   	if(fread(BO_BUF, 1, bcode_size, fd) < bcode_size) {
+	   	if(fread(BO_BUF, 1, bcode_size, fd) < bcode_size) {
-      		perror(bo_filename);
+	      		perror(bo_filename);
-      		buzzvm_destroy(&VM);
+	      		buzzvm_destroy(&VM);
-      		buzzdebug_destroy(&DBG_INFO);
+	      		buzzdebug_destroy(&DBG_INFO);
-      		fclose(fd);
+	      		fclose(fd);
       		return 0;
-   	}
+	   	}
-   	fclose(fd);
+	   	fclose(fd);
-   	/* Read debug information */
+	   	/* Read debug information */
-   	if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) {
+	   	if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) {
-      		buzzvm_destroy(&VM);
+	      		buzzvm_destroy(&VM);
-      		buzzdebug_destroy(&DBG_INFO);
+	      		buzzdebug_destroy(&DBG_INFO);
-      		perror(bdbg_filename);
+	      		perror(bdbg_filename);
       		return 0;
    	}
    	/* Set byte code */
    	if(buzzvm_set_bcode(VM, BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
       		buzzvm_destroy(&VM);
       		buzzdebug_destroy(&DBG_INFO);
-      		fprintf(stderr, "%s: Error loading Buzz script\n\n", bo_filename);
+      		ROS_ERROR("%s: Error loading Buzz script", bo_filename);
       		return 0;
    	}
    	/* Register hook functions */
    	if(buzz_register_hooks() != BUZZVM_STATE_READY) {
       		buzzvm_destroy(&VM);
       		buzzdebug_destroy(&DBG_INFO);
-      		fprintf(stderr, "%s: Error registering hooks\n\n", bo_filename);
+      		ROS_ERROR("[%i] Error registering hooks", Robot_id);
       		return 0;
    	}
+   	/* Create vstig tables 
+	if(create_stig_tables() != BUZZVM_STATE_READY) {
+      		buzzvm_destroy(&VM);
+      		buzzdebug_destroy(&DBG_INFO);
+      		ROS_ERROR("[%i] Error creating stigmergy tables", Robot_id);
+			//cout << "ERROR!!!!   ----------  " << VM->errormsg << endl;
+			//cout << "ERROR!!!!   ----------  " << buzzvm_strerror(VM) << endl;
+      		return 0;
+   	}*/
 
-   	
+	buzzobj_t t = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE);
-buzzvm_dup(VM);
+	buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
-   	// usersvstig = stigmergy.create(123)
+	buzzvm_push(VM, t);
-        buzzvm_pushs(VM, buzzvm_string_register(VM, "v", 1));
+	buzzvm_gstore(VM);
-        // value of the stigmergy id
-        buzzvm_pushi(VM, 5);
-        // get the stigmergy table from the global scope
-//        buzzvm_pushs(VM, buzzvm_string_register(VM, "stigmergy", 1));
-//        buzzvm_gload(VM);
-        // get the create method from the stigmergy table
-//        buzzvm_pushs(VM, buzzvm_string_register(VM, "create", 1));
-//        buzzvm_tget(VM);
-        // call the stigmergy.create() method
-//        buzzvm_closure_call(VM, 1);
-        // now the stigmergy is at the top of the stack - save it for future reference
-//        usersvstig = buzzvm_stack_at(VM, 0);
-//buzzvm_pop(VM);
-        // assign the virtual stigmergy to the global symbol v
-        // create also a global variable for it, so the garbage collector does not remove it
-//buzzvm_pushs(VM, buzzvm_string_register(VM, "v", 1));
-//buzzvm_push(VM, usersvstig);
-        buzzvm_gstore(VM);
         
    	/* Save bytecode file name */
    	BO_FNAME = strdup(bo_filename);
@@ -336,29 +477,16 @@ buzzvm_dup(VM);
    	buzzvm_function_call(VM, "init", 0);
    	/* All OK */
 
+    ROS_INFO("[%i] INIT DONE!!!", Robot_id);
+
    	return 1;//buzz_update_set(BO_BUF, bdbg_filename, bcode_size);
 	}
         
-        int buzz_update_users_stigmergy(buzzobj_t tbl){
-            // push the key (here it's an int with value 46)
-            buzzvm_pushi(VM, 46);
-            // push the table
-            buzzvm_push(VM, tbl);
-            // the stigmergy is stored in the vstig variable
-            // let's push it on the stack
-            buzzvm_push(VM, usersvstig);
-            // get the put method from myvstig
-            buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1));
-            buzzvm_tget(VM);
-            // call the vstig.put() method
-            buzzvm_closure_call(VM, 2);
-            return 1;
-        }
 	/****************************************/
 	/*Sets a new update                     */
 	/****************************************/
 	int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){
-/*	// Reset the Buzz VM
+	// Reset the Buzz VM
    	if(VM) buzzvm_destroy(&VM);
    	VM = buzzvm_new(Robot_id);
    	// Get rid of debug info
@@ -386,20 +514,28 @@ buzzvm_dup(VM);
       		fprintf(stdout, "%s: Error registering hooks\n\n", BO_FNAME);
         	return 0;
    	}
-
+   	/* Create vstig tables */
+	if(create_stig_tables() != BUZZVM_STATE_READY) {
+      		buzzvm_destroy(&VM);
+      		buzzdebug_destroy(&DBG_INFO);
+      		ROS_ERROR("[%i] Error creating stigmergy tables", Robot_id);
+			//cout << "ERROR!!!!   ----------  " << VM->errormsg << endl;
+			//cout << "ERROR!!!!   ----------  " << buzzvm_strerror(VM) << endl;
+      		return 0;
+   	}
    	// Execute the global part of the script
    	buzzvm_execute_script(VM);
    	// Call the Init() function
    	buzzvm_function_call(VM, "init", 0);
    	// All OK
-*/   	return 1;
+   	return 1;
 	}
 
 	/****************************************/
 	/*Performs a initialization test        */
 	/****************************************/
 	int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){
-/*	// Reset the Buzz VM
+	// Reset the Buzz VM
    	if(VM) buzzvm_destroy(&VM);
    	VM = buzzvm_new(Robot_id);
    	// Get rid of debug info
@@ -427,13 +563,21 @@ buzzvm_dup(VM);
       		fprintf(stdout, "%s: Error registering hooks\n\n", BO_FNAME);
         	return 0;
    	}
-
+   	/* Create vstig tables */
+	if(create_stig_tables() != BUZZVM_STATE_READY) {
+      		buzzvm_destroy(&VM);
+      		buzzdebug_destroy(&DBG_INFO);
+      		ROS_ERROR("[%i] Error creating stigmergy tables", Robot_id);
+			//cout << "ERROR!!!!   ----------  " << VM->errormsg << endl;
+			//cout << "ERROR!!!!   ----------  " << buzzvm_strerror(VM) << endl;
+      		return 0;
+   	}
    	// Execute the global part of the script
    	buzzvm_execute_script(VM);
    	// Call the Init() function
    	buzzvm_function_call(VM, "init", 0);
    	// All OK
-*/   	return 1;
+   	return 1;
 	}
 
 	/****************************************/
@@ -447,73 +591,75 @@ buzzvm_dup(VM);
 	typedef struct buzzswarm_elem_s* buzzswarm_elem_t;
 
 	void check_swarm_members(const void* key, void* data, void* params) {
-	   buzzswarm_elem_t e = *(buzzswarm_elem_t*)data;
+		buzzswarm_elem_t e = *(buzzswarm_elem_t*)data;
-	   int* status = (int*)params;
+		int* status = (int*)params;
-	   if(*status == 3) return;
+		if(*status == 3) return;
-	fprintf(stderr, "CHECKING SWARM MEMBERS:%i\n",buzzdarray_get(e->swarms, 0, uint16_t));
+		fprintf(stderr, "CHECKING SWARM :%i, member: %i, age: %i \n",
-	   if(buzzdarray_size(e->swarms) != 1) {
+			buzzdarray_get(e->swarms, 0, uint16_t), *(uint16_t*)key, e->age);
-	      fprintf(stderr, "Swarm list size is not 1\n");
+		if(buzzdarray_size(e->swarms) != 1) {
-	      *status = 3;
+			fprintf(stderr, "Swarm list size is not 1\n");
-	   }
+			*status = 3;
-	   else {
+		}
-	      int sid = 1;
+		else {
-              if(!buzzdict_isempty(VM->swarms)) {
+			int sid = 1;
-                if(*buzzdict_get(VM->swarms, &sid, uint8_t) &&
+			if(!buzzdict_isempty(VM->swarms)) {
-                    buzzdarray_get(e->swarms, 0, uint16_t) != sid) {
+				if(*buzzdict_get(VM->swarms, &sid, uint8_t) &&
-                    fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n",
+				    buzzdarray_get(e->swarms, 0, uint16_t) != sid) {
-                            sid,
+				    fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n",
-                            buzzdarray_get(e->swarms, 0, uint16_t));
+					    sid,
-                    *status = 3;
+					    buzzdarray_get(e->swarms, 0, uint16_t));
-                    return;
+				    *status = 3;
-                }
+				return;
-              }
+				}
-              if(buzzdict_size(VM->swarms)>1) {
+			}
-                sid = 2;
+			if(buzzdict_size(VM->swarms)>1) {
-                if(*buzzdict_get(VM->swarms, &sid, uint8_t) &&
+				sid = 2;
-                    buzzdarray_get(e->swarms, 0, uint16_t) != sid) {
+				if(*buzzdict_get(VM->swarms, &sid, uint8_t) &&
-                    fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n",
+					buzzdarray_get(e->swarms, 0, uint16_t) != sid) {
-                            sid,
+					fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n",
-                            buzzdarray_get(e->swarms, 0, uint16_t));
+					    sid,
-                    *status = 3;
+					    buzzdarray_get(e->swarms, 0, uint16_t));
-                    return;
+					*status = 3;
-                }
+				return;
-              }
+				}
-	   }
+			}
+		}
 	}
+
 	/*Step through the buzz script*/
+   	void update_sensors(){
+		/* Update sensors*/
+		buzzuav_closures::buzzuav_update_battery(VM);
+	   	buzzuav_closures::buzzuav_update_prox(VM);
+	   	buzzuav_closures::buzzuav_update_currentpos(VM);
+	   	buzzuav_closures::update_neighbors(VM);
+	   	update_users();
+	   	buzzuav_closures::buzzuav_update_flight_status(VM);
+	}
 
 	void buzz_script_step() {
-	   /*
+		/* Process messages */
-	    * Update sensors
+		buzzvm_process_inmsgs(VM);
-	    */
+		/*Update sensors*/
-	   buzzuav_closures::buzzuav_update_battery(VM);
+		update_sensors();
-	   buzzuav_closures::buzzuav_update_prox(VM);
+		/* Call Buzz step() function */
-	   buzzuav_closures::buzzuav_update_currentpos(VM);
+		if(buzzvm_function_call(VM, "step", 0) != BUZZVM_STATE_READY) {
-	   buzzobj_t tbl = buzzuav_closures::buzzuav_update_userspos(VM);
+		fprintf(stderr, "%s: execution terminated abnormally: %s\n\n",
-	   buzzuav_closures::buzzuav_update_flight_status(VM);
-           
-           //buzz_update_users_stigmergy(tbl);
-           
-           
-	    /*
-	    * Call Buzz step() function
-	    */
-	   if(buzzvm_function_call(VM, "step", 0) != BUZZVM_STATE_READY) {
-	      fprintf(stderr, "%s: execution terminated abnormally: %s\n\n",
 		      BO_FNAME,
 		      buzz_error_info());
-	      buzzvm_dump(VM);
+		buzzvm_dump(VM);
-	   }
+		}
-	   //buzzvm_process_outmsgs(VM); //--> done in  out_msg_process() function called each step
+		/* Process out messages */
-	   //usleep(10000);
+		buzzvm_process_outmsgs(VM); 
-	   /*Print swarm*/
+		/*Print swarm*/
-	   //buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
+		//buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
-	   //int SwarmSize = buzzdict_size(VM->swarmmembers)+1;
+		//int SwarmSize = buzzdict_size(VM->swarmmembers)+1;
-	   //fprintf(stderr, "Real Swarm Size: %i\n",SwarmSize);
+		//fprintf(stderr, "Real Swarm Size: %i\n",SwarmSize);
+		set_robot_var(buzzdict_size(VM->swarmmembers)+1);
 
-	   /* Check swarm state -- Not crashing thanks to test added in check_swarm_members */
+		/* Check swarm state -- Not crashing thanks to test added in check_swarm_members */
-//	   int status = 1;
+		//int status = 1;
-//	   buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status);
+		//buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status);
 	}
 
 	/****************************************/
@@ -553,6 +699,8 @@ buzzvm_dup(VM);
 		buzzuav_closures::buzzuav_update_battery(VM);
 		buzzuav_closures::buzzuav_update_prox(VM);
 		buzzuav_closures::buzzuav_update_currentpos(VM);
+	   	buzzuav_closures::update_neighbors(VM);
+	   	update_users();
 		buzzuav_closures::buzzuav_update_flight_status(VM);
 
 		int a = buzzvm_function_call(VM, "step", 0);
@@ -563,26 +711,15 @@ buzzvm_dup(VM);
 		return a == BUZZVM_STATE_READY;
 	}
 
-/*	uint16_t get_robotid() {
-		// Get hostname
-		char hstnm[30];
-		gethostname(hstnm, 30);
-		// Make numeric id from hostname
-		// NOTE: here we assume that the hostname is in the format Knn
-		int id = strtol(hstnm + 4, NULL, 10);
-		//fprintf(stdout, "Robot id from get rid buzz util:  %i\n",id);
-		return (uint16_t)id;
-	}*/
-
 	buzzvm_t get_vm() {
 		return VM;
 	}
 
-void set_robot_var(int ROBOTS){
+	void set_robot_var(int ROBOTS){
-	buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1));
+		buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1));
-	buzzvm_pushi(VM, ROBOTS);
+		buzzvm_pushi(VM, ROBOTS);
-	buzzvm_gstore(VM);
+		buzzvm_gstore(VM);
-}
+	}
 }
 
 

src/buzzuav_closures.cpp

@@ -7,7 +7,7 @@
  */
 //#define _GNU_SOURCE
 #include "buzzuav_closures.h"
-//#include "roscontroller.h"
+
 namespace buzzuav_closures{
 
 	// TODO: Minimize the required global variables and put them in the header
@@ -19,12 +19,15 @@ namespace buzzuav_closures{
 	static float batt[3];
 	static float obst[5]={0,0,0,0,0};
 	static double cur_pos[3];
-	static double users_pos[3];
 	static uint8_t status;
 	static int cur_cmd = 0;
 	static int rc_cmd=0;
 	static int buzz_cmd=0;
 	static float height=0;
+
+
+	std::map< int,  buzz_utility::Pos_struct> neighbors_map;
+
 	/****************************************/
 	/****************************************/
 
@@ -246,11 +249,33 @@ namespace buzzuav_closures{
 	   cur_pos[1]=longitude;
 	   cur_pos[2]=altitude;
 	}
-	void set_userspos(double latitude, double longitude, double altitude){
+	/*adds neighbours position*/
-	   users_pos[0]=latitude;
+	void neighbour_pos_callback(int id, float range, float bearing, float elevation){
-	   users_pos[1]=longitude;
+		buzz_utility::Pos_struct pos_arr;
-	   users_pos[2]=altitude;
+		pos_arr.x=range;
+		pos_arr.y=bearing;
+		pos_arr.z=elevation;
+		map< int, buzz_utility::Pos_struct >::iterator it = neighbors_map.find(id);
+		if(it!=neighbors_map.end())
+			neighbors_map.erase(it);
+		neighbors_map.insert(make_pair(id, pos_arr));
 	}
+
+	/* update at each step the VM table */
+	void update_neighbors(buzzvm_t vm){
+		/* Reset neighbor information */
+    	buzzneighbors_reset(vm);
+  		/* Get robot id and update neighbor information */
+  	  	map< int, buzz_utility::Pos_struct >::iterator it;
+		for (it=neighbors_map.begin(); it!=neighbors_map.end(); ++it){
+			buzzneighbors_add(vm,
+								it->first,
+								(it->second).x,
+								(it->second).y,
+								(it->second).z);
+		}
+	}
+
 	/****************************************/
 	int buzzuav_update_currentpos(buzzvm_t vm) {
 	   buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 1));
@@ -270,26 +295,6 @@ namespace buzzuav_closures{
 	   buzzvm_gstore(vm);
 	   return vm->state;
 	}
-	buzzobj_t buzzuav_update_userspos(buzzvm_t vm) {
-	   buzzvm_pushs(vm, buzzvm_string_register(vm, "users", 1));
-	   buzzvm_pusht(vm);
-	   buzzvm_dup(vm);
-	   buzzvm_pushs(vm, buzzvm_string_register(vm, "range", 1));
-	   buzzvm_pushf(vm, users_pos[0]);
-	   buzzvm_tput(vm);
-	   buzzvm_dup(vm);
-	   buzzvm_pushs(vm, buzzvm_string_register(vm, "bearing", 1));
-	   buzzvm_pushf(vm, users_pos[1]);
-	   buzzvm_tput(vm);
-	   buzzvm_dup(vm);
-	   buzzvm_pushs(vm, buzzvm_string_register(vm, "height", 1));
-	   buzzvm_pushf(vm, users_pos[2]);
-	   buzzvm_tput(vm);
-	   buzzvm_gstore(vm);
-           
-           return buzzvm_stack_at(vm, 0);
-	   //return vm->state;
-	}
 
 	void flight_status_update(uint8_t state){
 	   status=state;

src/roscontroller.cpp

@@ -13,7 +13,9 @@ namespace rosbzz_node{
 		/*Initialize publishers, subscribers and client*/
   		Initialize_pub_sub(n_c);
 		/*Compile the .bzz file to .basm, .bo and .bdbg*/
- 		Compile_bzz();
+		std::string fname = Compile_bzz(bzzfile_name);
+ 		bcfname = fname + ".bo";
+ 		dbgfname = fname + ".bdb";
 		set_bzz_file(bzzfile_name.c_str());
 		/*Initialize variables*/
 		// Solo things
@@ -22,14 +24,15 @@ namespace rosbzz_node{
 		multi_msg = true;
 		// set stream rate - wait for the FC to be started
 		SetStreamRate(0, 10, 1);
-		/// Get Robot Id - wait for Xbee to be started
+		// Get Robot Id - wait for Xbee to be started
 		if(xbeeplugged)
 			GetRobotId();
 		else
-			robot_id=strtol(robot_name.c_str() + 5, NULL, 10);;
+			robot_id= strtol(robot_name.c_str() + 5, NULL, 10);
 		setpoint_counter = 0;
 		fcu_timeout = TIMEOUT;
-                home[0]=0.0;home[1]=0.0;home[2]=0.0;
+        
+		home[0]=0.0;home[1]=0.0;home[2]=0.0;
 	}
 
 	/*---------------------
@@ -49,7 +52,7 @@ namespace rosbzz_node{
 
 		/*
 		mavros_msgs::ParamGet::Request robot_id_srv_request; robot_id_srv_request.param_id="id";
-        	mavros_msgs::ParamGet::Response robot_id_srv_response;
+        mavros_msgs::ParamGet::Response robot_id_srv_response;
 		while(!xbeestatus_srv.call(robot_id_srv_request,robot_id_srv_response)){
 			ros::Duration(0.1).sleep();
 			ROS_ERROR("Waiting for Xbee to respond to get device ID");
@@ -68,39 +71,39 @@ namespace rosbzz_node{
 		/* Set the Buzz bytecode */
 		if(buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(),robot_id)) {
 			fprintf(stdout, "Bytecode file found and set\n");
-			//init_update_monitor(bcfname.c_str(),stand_by.c_str());
+			std::string standby_bo = Compile_bzz(stand_by) + ".bo";
+			init_update_monitor(bcfname.c_str(),standby_bo.c_str());
                         ///////////////////////////////////////////////////////
                         // MAIN LOOP
                         //////////////////////////////////////////////////////
 			while (ros::ok() && !buzz_utility::buzz_script_done())
   			{
-				std::cout<<std::setprecision(17)<<"Home: " << home[0] << home[1] << std::endl;
+      			/*Update neighbors position inside Buzz*/
-      				/*Update neighbors position inside Buzz*/
+     			//buzz_closure::neighbour_pos_callback(neighbours_pos_map);
-     				buzz_utility::neighbour_pos_callback(neighbours_pos_map);
 				/*Neighbours of the robot published with id in respective topic*/
 				neighbours_pos_publisher();
 				/*Check updater state and step code*/
-  				//update_routine(bcfname.c_str(), dbgfname.c_str());
+  				update_routine(bcfname.c_str(), dbgfname.c_str());
 				/*Step buzz script */
-      				buzz_utility::buzz_script_step();
+      			buzz_utility::buzz_script_step();
 				/*Prepare messages and publish them in respective topic*/
 		  		prepare_msg_and_publish();
 				/*call flight controler service to set command long*/
 				flight_controller_service_call();
 				/*Set multi message available after update*/
-				/*if(get_update_status()){
+				if(get_update_status()){
 					set_read_update_status();
 					multi_msg=true;
-				}*/
+				}
 				/*Set ROBOTS variable for barrier in .bzz from neighbours count*/
 				//no_of_robots=get_number_of_robots();
 				get_number_of_robots();
 				//if(neighbours_pos_map.size() >0) no_of_robots =neighbours_pos_map.size()+1;
-				buzz_utility::set_robot_var(no_of_robots);
+				//buzz_utility::set_robot_var(no_of_robots);
 				/*Set no of robots for updates*/
 				updates_set_robots(no_of_robots);
-    				/*run once*/
+    			/*run once*/
-    				ros::spinOnce();
+    			ros::spinOnce();
 				/*loop rate of ros*/
 				 ros::Rate loop_rate(10);
 				 loop_rate.sleep();
@@ -109,10 +112,9 @@ namespace rosbzz_node{
 				 else
 					fcu_timeout -= 1/10;
  				/*sleep for the mentioned loop rate*/
-    				timer_step+=1;
+    			timer_step+=1;
    				maintain_pos(timer_step);
 								
-				
 			}
 			/* Destroy updater and Cleanup */
     			//update_routine(bcfname.c_str(), dbgfname.c_str(),1);
@@ -152,7 +154,8 @@ namespace rosbzz_node{
   		if(!xbeeplugged){
                     if(n_c.getParam("name", robot_name));
                     else {ROS_ERROR("Provide the xbee plugged boolean in Launch file"); system("rosnode kill rosbuzz_node");}
-                }
+                }else
+			n_c.getParam("xbee_status_srv", xbeesrv_name);
 
 		GetSubscriptionParameters(n_c);
 		// initialize topics to null?
@@ -220,8 +223,8 @@ namespace rosbzz_node{
 		arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient);
 		mode_client =  n_c.serviceClient<mavros_msgs::SetMode>(modeclient);
 		mav_client = n_c.serviceClient<mavros_msgs::CommandLong>(fcclient_name);
-		if(rcclient==true)
+        if(rcclient==true)
-			service = n_c.advertiseService(rcservice_name, &roscontroller::rc_callback,this);
+        	service = n_c.advertiseService(rcservice_name, &roscontroller::rc_callback,this);
 		ROS_INFO("Ready to receive Mav Commands from RC client");
 		xbeestatus_srv = n_c.serviceClient<mavros_msgs::ParamGet>(xbeesrv_name);
 		stream_client = n_c.serviceClient<mavros_msgs::StreamRate>(stream_client_name);
@@ -259,28 +262,35 @@ namespace rosbzz_node{
 	/*--------------------------------------------------------
 	/ Create Buzz bytecode from the bzz script inputed
 	/-------------------------------------------------------*/
-	void roscontroller::Compile_bzz(){
+	std::string roscontroller::Compile_bzz(std::string bzzfile_name){
 		/*TODO: change to bzzc instead of bzzparse and also add -I for includes*/
 		/*Compile the buzz code .bzz to .bo*/
 		stringstream bzzfile_in_compile;
-	        std::string  path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/"));
+	    std::string  path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
-		bzzfile_in_compile<<path<<"/";
+		//bzzfile_in_compile << path << "/";
-		path = bzzfile_in_compile.str();
+		//path = bzzfile_in_compile.str();
-		bzzfile_in_compile.str("");
+		//bzzfile_in_compile.str("");
 		std::string  name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
  		name = name.substr(0,name.find_last_of("."));
-		bzzfile_in_compile << "bzzparse "<<bzzfile_name<<" "<<path<< name<<".basm";
+		bzzfile_in_compile << "bzzc -I " << path << "script/include/"; //<<" "<<path<< name<<".basm";
-   		system(bzzfile_in_compile.str().c_str());
+		//bzzfile_in_compile.str("");
-		bzzfile_in_compile.str("");
+        //bzzfile_in_compile <<"bzzasm "<<path<<name<<".basm "<<path<<name<<".bo "<<path<<name<<".bdbg";
-           	bzzfile_in_compile <<"bzzasm "<<path<<name<<".basm "<<path<<name<<".bo "<<path<<name<<".bdbg";
+   		//system(bzzfile_in_compile.str().c_str());
-   		system(bzzfile_in_compile.str().c_str());
+		//bzzfile_in_compile.str("");
-		bzzfile_in_compile.str("");
+		bzzfile_in_compile << " -b " << path << name << ".bo";
-		bzzfile_in_compile <<path<<name<<".bo";
+		//bcfname = bzzfile_in_compile.str();
-		bcfname = bzzfile_in_compile.str();
+		//std::string tmp_bcfname = path + name + ".bo";
-		bzzfile_in_compile.str("");
+		//bzzfile_in_compile.str("");
-		bzzfile_in_compile <<path<<name<<".bdbg";
+		bzzfile_in_compile << " -d " << path << name << ".bdb ";
-		dbgfname = bzzfile_in_compile.str();
+		//bzzfile_in_compile << " -a " << path << name << ".asm ";
+		bzzfile_in_compile << bzzfile_name;
+		//std::string tmp_dbgfname = path + name + ".bdb";
 
+		ROS_WARN("Launching buzz compilation: %s", bzzfile_in_compile.str().c_str());
+
+   		system(bzzfile_in_compile.str().c_str());
+
+		return path + name;
 	}
 	/*----------------------------------------------------
 	/ Publish neighbours pos and id in neighbours pos topic
@@ -312,14 +322,13 @@ namespace rosbzz_node{
 	void roscontroller::Arm(){
 		mavros_msgs::CommandBool arming_message;
 		arming_message.request.value = armstate;
-		ROS_INFO("FC Arm Service called!------------------------------------------------------");
 		if(arm_client.call(arming_message)) {
 			if(arming_message.response.success==1)
-				ROS_INFO("FC Arm Service called!");
+				ROS_WARN("FC Arm Service called!");
 			else
-				ROS_INFO("FC Arm Service call failed!");
+				ROS_WARN("FC Arm Service call failed!");
 		} else {
-			ROS_INFO("FC Arm Service call failed!");
+			ROS_WARN("FC Arm Service call failed!");
 		}
 	}
 
@@ -335,7 +344,7 @@ namespace rosbzz_node{
 	/*----------------------------------------------------------------------------------------------------*/	
 	void roscontroller::prepare_msg_and_publish(){
     		/*obtain Pay load to be sent*/  
-   		uint64_t* payload_out_ptr= buzz_utility::out_msg_process();
+   		uint64_t* payload_out_ptr= buzz_utility::obt_out_msg();
     		uint64_t  position[3];
   		/*Appened current position to message*/
     		memcpy(position, cur_pos, 3*sizeof(uint64_t));
@@ -359,7 +368,7 @@ namespace rosbzz_node{
     		delete[] out;
     		delete[] payload_out_ptr;
 		/*Check for updater message if present send*/
-		/*if((int)get_update_mode()!=CODE_RUNNING && is_msg_present()==1 && multi_msg){
+		if((int)get_update_mode()!=CODE_RUNNING && is_msg_present()==1 && multi_msg){
 			uint8_t* buff_send = 0;
 	   		uint16_t updater_msgSize=*(uint16_t*) (getupdate_out_msg_size());;
 			int tot=0;
@@ -395,7 +404,7 @@ namespace rosbzz_node{
 		    	payload_pub.publish(update_packets);
 		    	multi_msg=false;
 		    	delete[] payload_64;
-		}*/
+		}
 		
 	}
 	/*---------------------------------------------------------------------------------
@@ -499,7 +508,7 @@ namespace rosbzz_node{
 	void roscontroller::neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr ){
 		map< int, buzz_utility::Pos_struct >::iterator it = neighbours_pos_map.find(id);
 		if(it!=neighbours_pos_map.end())
-		neighbours_pos_map.erase(it);
+			neighbours_pos_map.erase(it);
 		neighbours_pos_map.insert(make_pair(id, pos_arr));
 		}
 	/*-----------------------------------------------------------------------------------
@@ -508,7 +517,7 @@ namespace rosbzz_node{
 	void roscontroller::raw_neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr ){
 		map< int, buzz_utility::Pos_struct >::iterator it = raw_neighbours_pos_map.find(id);
 		if(it!=raw_neighbours_pos_map.end())
-		raw_neighbours_pos_map.erase(it);
+			raw_neighbours_pos_map.erase(it);
 		raw_neighbours_pos_map.insert(make_pair(id, pos_arr));
 		}
 
@@ -566,95 +575,92 @@ namespace rosbzz_node{
 
 	void roscontroller::cvt_rangebearing_coordinates(double nei[], double out[], double cur[]){
             
-  // calculate earth radii
+		// calculate earth radii
-  double temp = 1.0 / (1.0 - excentrity2 * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)) * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)));
+		/*double temp = 1.0 / (1.0 - excentrity2 * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)) * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)));
-  double prime_vertical_radius = equatorial_radius * sqrt(temp);
+		double prime_vertical_radius = equatorial_radius * sqrt(temp);
-  double radius_north = prime_vertical_radius * (1 - excentrity2) * temp;
+		double radius_north = prime_vertical_radius * (1 - excentrity2) * temp;
-  double radius_east  = prime_vertical_radius * cos(DEG2RAD(DEFAULT_REFERENCE_LATITUDE));
+		double radius_east  = prime_vertical_radius * cos(DEG2RAD(DEFAULT_REFERENCE_LATITUDE));*/
   
  		/*double d_lon = nei[1] - cur[1];
-        	double d_lat = nei[0] - cur[0];
+		double d_lat = nei[0] - cur[0];
-                double ned[3];
+		double ned[3];
-        	ned[0] = DEG2RAD(d_lat) * radius_north;//EARTH_RADIUS;
+		ned[0] = DEG2RAD(d_lat) * radius_north;//EARTH_RADIUS;
-        	ned[1] = -DEG2RAD(d_lon) * radius_east; //EARTH_RADIUS
+		ned[1] = -DEG2RAD(d_lon) * radius_east; //EARTH_RADIUS
-                double ecef[3];
+		double ecef[3];
-                double llh[3];llh[0]=DEG2RAD(cur[0]);llh[1]=DEG2RAD(cur[1]);llh[2]=cur[2];
+		double llh[3];llh[0]=DEG2RAD(cur[0]);llh[1]=DEG2RAD(cur[1]);llh[2]=cur[2];
-                double d = WGS84_E * sin(llh[0]);
+		double d = WGS84_E * sin(llh[0]);
-                double N = WGS84_A / sqrt(1. - d*d);
+		double N = WGS84_A / sqrt(1. - d*d);
-                ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]);
+		ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]);
-                ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]);
+		ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]);
-                ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]);
+		ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]);
-                double ref_ecef[3];
+		double ref_ecef[3];
-                llh[0]=DEG2RAD(nei[0]);llh[1]=DEG2RAD(nei[1]);llh[2]=nei[2];
+		llh[0]=DEG2RAD(nei[0]);llh[1]=DEG2RAD(nei[1]);llh[2]=nei[2];
-                d = WGS84_E * sin(llh[0]);
+		d = WGS84_E * sin(llh[0]);
-                N = WGS84_A / sqrt(1. - d*d);
+		N = WGS84_A / sqrt(1. - d*d);
-                ref_ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]);
+		ref_ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]);
-                ref_ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]);
+		ref_ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]);
-                ref_ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]);
+		ref_ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]);
-                double M[3][3];
+		double M[3][3];
-                ecef2ned_matrix(ref_ecef, M);
+		ecef2ned_matrix(ref_ecef, M);
-                double ned[3];
+		double ned[3];
-                matrix_multiply(3, 3, 1, (double *)M, ecef, ned);   
+		matrix_multiply(3, 3, 1, (double *)M, ecef, ned);   
 		
-                out[0] = sqrt(ned[0]*ned[0]+ned[1]*ned[1]);
+		out[0] = sqrt(ned[0]*ned[0]+ned[1]*ned[1]);
-                out[0] = std::floor(out[0] * 1000000) / 1000000;
+		out[0] = std::floor(out[0] * 1000000) / 1000000;
 		out[1] = atan2(ned[1],ned[0]);
                 out[1] = std::floor(out[1] * 1000000) / 1000000;
 		out[2] = 0.0;*/
                 
-                double d_lon = nei[1] - cur[1];
+       	double d_lon = nei[1] - cur[1];
-        	double d_lat = nei[0] - cur[0];
+       	double d_lat = nei[0] - cur[0];
-        	double ned_x = DEG2RAD(d_lat) * EARTH_RADIUS;
+        double ned_x = DEG2RAD(d_lat) * EARTH_RADIUS;
-        	double ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0]));
+        double ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0]));
 		out[0] = sqrt(ned_x*ned_x+ned_y*ned_y);
-                //out[0] = std::floor(out[0] * 1000000) / 1000000;
+        //out[0] = std::floor(out[0] * 1000000) / 1000000;
 		out[1] = atan2(ned_y,ned_x);
-                //out[1] = std::floor(out[1] * 1000000) / 1000000;
+        //out[1] = std::floor(out[1] * 1000000) / 1000000;
 		out[2] = 0.0;
-                
-
-
 	}
 
 	void roscontroller::cvt_ned_coordinates(double nei[], double out[], double cur[]){
-                // calculate earth radii
+		// calculate earth radii
-                /*double temp = 1.0 / (1.0 - excentrity2 * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)) * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)));
+		/*double temp = 1.0 / (1.0 - excentrity2 * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)) * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)));
-                double prime_vertical_radius = equatorial_radius * sqrt(temp);
+		double prime_vertical_radius = equatorial_radius * sqrt(temp);
-                double radius_north = prime_vertical_radius * (1 - excentrity2) * temp;
+		double radius_north = prime_vertical_radius * (1 - excentrity2) * temp;
-                double radius_east  = prime_vertical_radius * cos(DEG2RAD(DEFAULT_REFERENCE_LATITUDE));
+		double radius_east  = prime_vertical_radius * cos(DEG2RAD(DEFAULT_REFERENCE_LATITUDE));
 
- 		double d_lon = nei[1] - cur[1];
+		double d_lon = nei[1] - cur[1];
-        	double d_lat = nei[0] - cur[0];
+		double d_lat = nei[0] - cur[0];
-        	out[0] = DEG2RAD(d_lat) * radius_north;//EARTH_RADIUS;
+		out[0] = DEG2RAD(d_lat) * radius_north;//EARTH_RADIUS;
-                out[0] = std::floor(out[0] * 1000000) / 1000000;
+		out[0] = std::floor(out[0] * 1000000) / 1000000;
-        	out[1] = -DEG2RAD(d_lon) * radius_east; //EARTH_RADIUS
+		out[1] = -DEG2RAD(d_lon) * radius_east; //EARTH_RADIUS
-                out[1] = std::floor(out[1] * 1000000) / 1000000;
+		out[1] = std::floor(out[1] * 1000000) / 1000000;
 		out[2] = cur[2];
-                // Using functions of the library Swift Nav (https://github.com/swift-nav/libswiftnav)
+		// Using functions of the library Swift Nav (https://github.com/swift-nav/libswiftnav)
-                double ecef[3];
+		double ecef[3];
-                double llh[3];llh[0]=DEG2RAD(cur[0]);llh[1]=DEG2RAD(cur[1]);llh[2]=cur[2];
+		double llh[3];llh[0]=DEG2RAD(cur[0]);llh[1]=DEG2RAD(cur[1]);llh[2]=cur[2];
-                double d = WGS84_E * sin(llh[0]);
+		double d = WGS84_E * sin(llh[0]);
-                double N = WGS84_A / sqrt(1. - d*d);
+		double N = WGS84_A / sqrt(1. - d*d);
-                ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]);
+		ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]);
-                ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]);
+		ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]);
-                ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]);
+		ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]);
-                double ref_ecef[3];
+		double ref_ecef[3];
-                llh[0]=DEG2RAD(nei[0]);llh[1]=DEG2RAD(nei[1]);llh[2]=nei[2];
+		llh[0]=DEG2RAD(nei[0]);llh[1]=DEG2RAD(nei[1]);llh[2]=nei[2];
-                d = WGS84_E * sin(llh[0]);
+		d = WGS84_E * sin(llh[0]);
-                N = WGS84_A / sqrt(1. - d*d);
+		N = WGS84_A / sqrt(1. - d*d);
-                ref_ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]);
+		ref_ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]);
-                ref_ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]);
+		ref_ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]);
-                ref_ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]);
+		ref_ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]);
-                double M[3][3];
+		double M[3][3];
-                ecef2ned_matrix(ref_ecef, M);
+		ecef2ned_matrix(ref_ecef, M);
-                matrix_multiply(3, 3, 1, (double *)M, ecef, out);*/
+		matrix_multiply(3, 3, 1, (double *)M, ecef, out);*/
 		
-                double d_lon = nei[1] - cur[1];
+		double d_lon = nei[1] - cur[1];
-        	double d_lat = nei[0] - cur[0];
+		double d_lat = nei[0] - cur[0];
-        	out[0] = DEG2RAD(d_lat) * EARTH_RADIUS;
+		out[0] = DEG2RAD(d_lat) * EARTH_RADIUS;
-                //out[0] = std::floor(out[0] * 1000000) / 1000000;
+		//out[0] = std::floor(out[0] * 1000000) / 1000000;
-        	out[1] = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0]));
+		out[1] = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0]));
-                //out[1] = std::floor(out[1] * 1000000) / 1000000;
+		//out[1] = std::floor(out[1] * 1000000) / 1000000;
 		out[2] = 0.0;
 	}
 
@@ -718,7 +724,8 @@ namespace rosbzz_node{
                                 us[2] = data.pos_neigh[it].altitude;
                                 double out[3];
                                 cvt_rangebearing_coordinates(us, out, cur_pos);
-                                buzzuav_closures::set_userspos(out[0], out[1], out[2]);
+                                //buzzuav_closures::set_userspos(out[0], out[1], out[2]);
+								buzz_utility::add_user(data.pos_neigh[it].position_covariance_type,data.pos_neigh[it].latitude, data.pos_neigh[it].longitude, data.pos_neigh[it].altitude);
                         }
 
                 }
@@ -766,7 +773,7 @@ namespace rosbzz_node{
 		moveMsg.pose.orientation.w = 1;
 
                 // To prevent drifting from stable position.
-		if(fabs(x)>0.1 || fabs(y)>0.1) {
+		if(fabs(x)>0.01 || fabs(y)>0.01) {
                     localsetpoint_nonraw_pub.publish(moveMsg);
                     ROS_INFO("Sent local NON RAW position message!");
                 }
@@ -862,9 +869,11 @@ namespace rosbzz_node{
 			buzz_utility::Pos_struct n_pos(cvt_neighbours_pos_payload[0],cvt_neighbours_pos_payload[1],cvt_neighbours_pos_payload[2]);
 			/*Put RID and pos*/
 			raw_neighbours_pos_put((int)out[1],raw_neigh_pos);
+			/* TODO: remove roscontroller local map array for neighbors */	
 			neighbours_pos_put((int)out[1],n_pos);
+			buzzuav_closures::neighbour_pos_callback((int)out[1],n_pos.x,n_pos.y,n_pos.z);
 			delete[] out;
-			buzz_utility::in_msg_process((message_obt+3));
+			buzz_utility::in_msg_append((message_obt+3));
 		}
 
 	}

src/stand_by.basm

@@ -1,144 +0,0 @@
-!19
-'updated
-'update_ack
-'init
-'barrier
-'stigmergy
-'create
-'put
-'id
-'barrier_val
-'onconflict
-'data
-'stand_by
-'get
-'size
-'neighbors
-'listen
-'update_no
-'ROBOTS
-'step
-
-	pushs 2
-	pushcn @__label_1
-	gstore
-	pushs 11
-	pushcn @__label_5
-	gstore
-	pushs 18
-	pushcn @__label_9
-	gstore
-	nop
-
-@__label_0
-	pushs 0	|1,7,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	pushs 1	|1,8,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	gstore	|1,20,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-
-@__exitpoint
-	done
-
-@__label_1
-	pushs 3	|4,8,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	pushs 4	|4,19,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	gload	|4,19,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	pushs 5	|4,20,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	tget	|4,26,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	pushi 101	|4,27,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	pushi 1	|4,31,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	callc	|4,31,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	gstore	|4,31,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	pushs 3	|5,7,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	gload	|5,7,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	pushs 6	|5,8,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	tget	|5,11,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	pushs 7	|5,14,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	gload	|5,14,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	pushi 1	|5,15,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	pushi 2	|5,17,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	callc	|5,17,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	pushs 8	|6,11,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	pushi 0	|6,12,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	gstore	|6,13,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	pushs 3	|7,7,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	gload	|7,7,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	pushs 9	|7,8,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	tget	|7,18,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	pushl @__label_2	|7,19,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	pushi 1	|10,2,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	callc	|10,2,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	ret0	|12,1,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-
-@__label_2
-	lload 3	|8,4,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	pushs 10	|8,6,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	tget	|8,11,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	lload 2	|8,14,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	pushs 10	|8,16,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	tget	|8,21,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	lt	|8,21,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	lload 3	|8,26,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	pushs 10	|8,28,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	tget	|8,33,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	lload 2	|8,37,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	pushs 10	|8,39,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	tget	|8,44,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	eq	|8,44,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	or	|8,45,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	jumpz @__label_3	|8,47,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	lload 2	|8,55,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	ret1	|8,55,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	jump @__label_4	|9,5,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-@__label_3	|9,5,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	lload 3	|9,13,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	ret1	|9,13,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-@__label_4	|9,13,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	ret0	|10,1,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-
-@__label_5
-	pushs 3	|16,7,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	gload	|16,7,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	pushs 12	|16,8,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	tget	|16,11,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	pushs 7	|16,14,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	gload	|16,14,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	pushi 1	|16,15,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	callc	|16,15,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	pushs 8	|17,12,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	pushs 3	|17,21,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	gload	|17,21,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	pushs 13	|17,22,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	tget	|17,26,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	pushi 0	|17,28,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	callc	|17,28,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	gstore	|17,28,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	pushs 14	|19,9,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	gload	|19,9,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	pushs 15	|19,10,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	tget	|19,16,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	pushs 0	|19,24,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	gload	|19,24,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	pushl @__label_6	|20,3,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	pushi 2	|23,1,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	callc	|23,1,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	ret0	|25,1,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-
-@__label_6
-	lload 2	|21,14,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	pushs 16	|21,25,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	gload	|21,25,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	eq	|21,25,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	jumpz @__label_7	|21,27,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	pushs 8	|21,38,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	pushs 17	|21,45,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	gload	|21,45,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	gstore	|21,45,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-@__label_7	|22,3,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	ret0	|22,4,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-
-@__label_9
-	pushs 11	|30,8,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	gload	|30,8,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	pushi 0	|30,10,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	callc	|30,10,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
-	ret0	|32,1,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz