Merged solo-playground into dev

.gitignore

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

include/buzz_update.h

@@ -129,6 +129,8 @@ int get_update_status();
 
 void set_read_update_status();
 
+int compile_bzz();
+
 void updates_set_robots(int robots);
 
 #endif

include/buzz_utility.h

@@ -28,13 +28,18 @@ 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 compute_users_rb();
 
-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

@@ -9,6 +9,9 @@
 #include "buzz_utility.h"
 //#include "roscontroller.h"
 
+	#define EARTH_RADIUS (double) 6371000.0
+	#define DEG2RAD(DEG) ((DEG)*((M_PI)/(180.0)))
+
 namespace buzzuav_closures{
 	typedef enum {
       		COMMAND_NIL = 0,     // Dummy command
@@ -46,12 +49,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,8 +87,7 @@ 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);
+int buzzuav_adduserRB(buzzvm_t vm);
-
 /*
  * Updates flight status and rc command in Buzz, put it in a tabel to acess it 
  * use flight.status for flight status

include/roscontroller.h

@@ -52,7 +52,7 @@ public:
 	void RosControllerRun();
 	
 private:
-        struct num_robot_count
+    struct num_robot_count
 	{
 		uint8_t history[10];
 		uint8_t index=0;
@@ -61,17 +61,16 @@ private:
 	  
 	}; typedef struct num_robot_count Num_robot_count ;
 
-        // WGS84 constants
+	struct gps
-        double equatorial_radius = 6378137.0;
+	{
-        double flattening = 1.0/298.257223563;
+		double longitude=0.0;
-        double excentrity2 = 2*flattening - flattening*flattening;
+		double latitude=0.0;
-        // default reference position
+		float  altitude=0.0;
-        double DEFAULT_REFERENCE_LATITUDE  = 45.457817;
+	}; typedef struct gps GPS ;
-        double DEFAULT_REFERENCE_LONGITUDE = -73.636075;
 	
- 	double cur_pos[3];
+	GPS target, home, cur_pos;
- 	double home[3];
  	double cur_rel_altitude;
+
 	uint64_t payload;
 	std::map< int,  buzz_utility::Pos_struct> neighbours_pos_map;
 	std::map< int,  buzz_utility::Pos_struct> raw_neighbours_pos_map;
@@ -137,7 +136,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();
@@ -158,13 +157,17 @@ private:
 	/*Puts raw neighbours position in lat.,long.,alt. inside raw_neigbours_pos_map*/
 	void raw_neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr );
 
-        /*Set the current position of the robot callback*/
+    /*Set the current position of the robot callback*/
 	void set_cur_pos(double latitude,
 			 double longitude,
 			 double altitude);
 	/*convert from spherical to cartesian coordinate system callback */
-	void cvt_rangebearing_coordinates(double neighbours_pos_payload [], double out[], double pos[]);
+	void gps_rb(GPS nei_pos, double out[]);
-	void cvt_ned_coordinates(double neighbours_pos_payload [], double out[], double pos[]);
+	void gps_ned_cur(float &ned_x, float &ned_y, GPS t);
+	void gps_ned_home(float &ned_x, float &ned_y);
+	void gps_convert_ned(float &ned_x, float &ned_y,
+			double gps_t_lon, double gps_t_lat,
+			double gps_r_lon, double gps_r_lat);
 
 	/*battery status callback*/ 
 	void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg);
@@ -177,7 +180,7 @@ private:
 	
 	/*current position callback*/
 	void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg);
-        void users_pos(const rosbuzz::neigh_pos msg);
+    void users_pos(const rosbuzz::neigh_pos msg);
 
 	/*current relative altitude callback*/
 	void current_rel_alt(const std_msgs::Float64::ConstPtr& msg);

launch/rosbuzz-solo.launch

@@ -20,23 +20,37 @@
 	<!-- set streaming rate -->
 	<!-- node pkg="rosservice" type="rosservice" name="freq" args="call /mavros/set_stream_rate 0 10 1" output="screen" / -->
 
-	<!-- run xbee -->
+	<!-- run xbee>
 	<node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" />
 	<param name="Xbee_In_From_Buzz" type="str" value="outMavlink" />
 	<param name="Xbee_Out_To_Buzz" type="str" value="inMavlink" />
+	<param name="Xbee_In_From_Controller" type="str" value="xbee_cmd" / -->
+
+	<!--  xmee_mav Drone type and Commununication Mode -->
+	<!-- node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="master solo" output="screen" -->
+	<node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" />
+	<!-- node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave solo" output="screen" -->
+	<!--node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" -->
+	<!--  xmee_mav Topics and Services Names -->
+	<param name="Xbee_In_From_Buzz" type="str" value="outMavlink" />
+	<param name="Xbee_Out_To_Buzz" type="str" value="inMavlink" />
 	<param name="Xbee_In_From_Controller" type="str" value="xbee_cmd" />
+	<param name="Xbee_Out_To_Controller" type="str" value="mav_dji_cmd" />
+	
 
 
 	<!-- run rosbuzz -->
 	<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >	
 		<rosparam file="/home/pi/ros_catkinKin_ws/src/ROSBuzz/launch/launch_config/solo.yaml"/>
-		<param name="bzzfile_name" value="/home/pi/ros_catkinKin_ws/src/ROSBuzz/src/testflockfev.bzz" />
+		<param name="bzzfile_name" value="/home/pi/ros_catkinKin_ws/src/ROSBuzz/script/testflockfev.bzz" />
 		<param name="rcclient" value="true" /> 
-		<param name="rcservice_name" value="/buzzcmd" />
+		<param name="rcservice_name" value="buzzcmd" />
-		<param name="in_payload" value="/inMavlink"/>
+		<param name="in_payload" value="inMavlink"/>
-		<param name="out_payload" value="/outMavlink"/>
+		<param name="out_payload" value="outMavlink"/>
-		<param name="xbee_status_srv" value="/xbee_status"/>
+		<param name="xbee_status_srv" value="xbee_status"/>
-		<param name="stand_by" value="/home/pi/ros_catkinKin_ws/src/ROSBuzz/src/stand_by.bo"/>
+           	<param name="xbee_plugged" value="true"/>
+                <param name="name" value="solos1"/>
+		<param name="stand_by" value="/home/pi/ros_catkinKin_ws/src/ROSBuzz/script/stand_by.bzz"/>
 	</node>
 
 </launch>

launch/rosbuzz-testing-sitl.launch

@@ -27,13 +27,15 @@
 	<!-- run rosbuzz -->
 	<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >	
 		<rosparam file="/home/ivan/catkin_ws/src/rosbuzz/launch/launch_config/solo.yaml"/>
-		<param name="bzzfile_name" value="/home/ivan/catkin_ws/src/rosbuzz/src/testflockfev.bzz" />
+		<param name="bzzfile_name" value="/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz" />
 		<param name="rcclient" value="true" /> 
-		<param name="rcservice_name" value="/buzzcmd" />
+		<param name="rcservice_name" value="buzzcmd" />
-		<param name="in_payload" value="/inMavlink"/>
+		<param name="in_payload" value="inMavlink"/>
-		<param name="out_payload" value="/outMavlink"/>
+		<param name="out_payload" value="outMavlink"/>
-		<param name="xbee_status_srv" value="/xbee_status"/>
+		<param name="xbee_status_srv" value="xbee_status"/>
-		<param name="stand_by" value="/home/ivan/catkin_ws/src/rosbuzz/src/stand_by.bo"/>
+		<param name="xbee_plugged" value="false"/>
+		<param name="name" value="solos1"/>
+		<param name="stand_by" value="/home/ivan/catkin_ws/src/rosbuzz/script/stand_by.bzz"/>
 	</node>
 	
 	<!--	

launch/rosbuzzm100.launch

@@ -1,18 +1,16 @@
 <!-- Launch file for ROSBuzz -->
 
 <launch>
-		<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >	
+	<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="rcclient" value="true" /> 
+            <param name="bzzfile_name" value="$(find rosbuzz)/script/testflockfev.bzz" />
-		<param name="rcservice_name" value="/buzzcmd" />
+            <param name="rcclient" value="true" />
-		<param name="fcclient_name" value="/dji_mavcmd" />
+            <param name="rcservice_name" value="buzzcmd" />
-		<param name="in_payload" value="/inMavlink"/>
+            <param name="in_payload" value="inMavlink"/>
-		<param name="out_payload" value="/outMavlink"/>
+            <param name="out_payload" value="outMavlink"/>
-		<param name="xbee_status_srv" value="/xbee_status"/>
+            <param name="xbee_plugged" value="true"/>
-		<param name="robot_id" value="3"/>
+            <param name="name" value="m1001"/>
-		<param name="No_of_Robots" value="3"/>
+            <param name="xbee_status_srv" value="xbee_status"/>
-		<param name="stand_by" value="$(env ROS_WS)/src/ROSBuzz/src/stand_by.bo"/>
+            <param name="stand_by" value="$(find rosbuzz)/script/stand_by.bzz"/>
-		</node>
+        </node>		
-		
-		
 </launch>

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/stand_by.bzz

@@ -9,6 +9,8 @@ if(r. data < l. data or (r. data == l. data )) return l
 else return r
 })
 
+s = swarm.create(1)
+s.join()
 }
 
 function stand_by(){

script/test1.bzz

@@ -1,4 +1,3 @@
-
 # We need this for 2D vectors
 # Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
 include "/home/ubuntu/buzz/src/include/vec2.bzz"

script/testalone.bzz

@@ -1,7 +1,8 @@
 
 # We need this for 2D vectors
 # Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
-include "/home/ubuntu/buzz/src/include/vec2.bzz"
+#include "/home/ubuntu/buzz/src/include/vec2.bzz"
+#include "vec2.bzz"
 ####################################################################################################
 # Updater related
 # This should be here for the updater to work, changing position of code will crash the updater
@@ -20,7 +21,7 @@ function init() {
 
 # Executed at each time step.
 function step() {
-
+	log("Altitude: ", position.altitude)
 	if(flight.rc_cmd==22) {
 		flight.rc_cmd=0
 		uav_takeoff(TARGET_ALTITUDE)		
@@ -31,7 +32,6 @@ function step() {
 		flight.rc_cmd=0
 		uav_goto()
 	}
-
 # test moveto cmd
 #if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0)
 #	uav_moveto(0.5, 0.5)

script/testflockfev.bzz

@@ -1,5 +1,6 @@
 # We need this for 2D vectors
 # Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
+#include "vec2.bzz"
 include "/home/ubuntu/buzz/src/include/vec2.bzz"
 ####################################################################################################
 # Updater related
@@ -11,12 +12,12 @@ function updated_neigh(){
 neighbors.broadcast(updated, update_no)
 }
 
-TARGET_ALTITUDE = 3.0
+TARGET_ALTITUDE = 5.0
 CURSTATE = "TURNEDOFF"
  
 # Lennard-Jones parameters
-TARGET     = 10.0	#0.000001001
+TARGET     = 12.0	#0.000001001
-EPSILON    = 18.0	#0.001
+EPSILON    = 3.0	#0.001
  
 # Lennard-Jones interaction magnitude
 function lj_magnitude(dist, target, epsilon) {
@@ -43,15 +44,18 @@ 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
+#  	timeW = timeW+1
-  	uav_moveto(0.0,0.0)
+#  	uav_moveto(0.06,0.0)
-  	}
+#  	} else {
+#  	timeW = timeW+1
+#  	uav_moveto(0.0,0.06)
+#  	}
 }
  
 ########################################
@@ -142,11 +146,32 @@ function land() {
 	}
 }
 
+function users_save(t) {
+	if(size(t)>0) {
+		foreach(t, function(id, tab) {
+				#log("id: ",id," Latitude ", tab.la, "Longitude ", tab.lo)
+				add_user_rb(id,tab.la,tab.lo)
+			})
+	}
+}
+
+function table_print(t) {
+  if(size(t)>0) {
+		foreach(t, function(u, tab) {
+				log("id: ",u," Range ", tab.r, "Bearing ", tab.b)
+			})
+	}
+}
+
 # Executed once at init time.
 function init() {
 	s = swarm.create(1)
-#	s.select(1)
 	s.join()
+
+	vt = stigmergy.create(5)
+	t = {}
+	vt.put("p",t)
+
 	statef=idle
 	CURSTATE = "IDLE"
 }
@@ -197,6 +222,14 @@ neighbors.listen("cmd",
 	statef()
 	log("Current state: ", CURSTATE)
 	log("Swarm size: ",ROBOTS)
+
+	# Check local users and push to v.stig
+	if(size(users.dataG)>0)
+		vt.put("p", users.dataG)
+				
+        # Save locally the users and print RG
+	users_save(vt.get("p"))
+	table_print(users.dataL)
 }
  
 # Executed once when the robot (or the simulator) is reset.

script/testflockfev.bzz.save

@@ -0,0 +1,216 @@
+# We need this for 2D vectors
+# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
+include "vec2.bzz"
+####################################################################################################
+# 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)
+}
+
+TARGET_ALTITUDE = 3.0
+CURSTATE = "TURNEDOFF"
+ 
+# Lennard-Jones parameters
+TARGET     = 12.0	#0.000001001
+EPSILON    = 6.0	#0.001
+ 
+# Lennard-Jones interaction magnitude
+function lj_magnitude(dist, target, epsilon) {
+  return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
+}
+ 
+# Neighbor data to LJ interaction vector
+function lj_vector(rid, data) {
+  return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth)
+}
+ 
+# Accumulator of neighbor LJ interactions
+function lj_sum(rid, data, accum) {
+  return math.vec2.add(data, accum)
+}
+ 
+# Calculates and actuates the flocking interaction
+function hexagon() {
+  statef=hexagon
+  CURSTATE = "HEXAGON"
+  # Calculate accumulator
+  var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
+  if(neighbors.count() > 0)
+    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)
+
+#  if(timeW>=WAIT_TIMEOUT) {	#FOR MOVETO TESTS
+#  	timeW =0
+#  	statef=land
+#  	} else if(timeW>=WAIT_TIMEOUT/2) {
+#  	timeW = timeW+1
+#  	uav_moveto(0.06,0.0)
+#  	} else {
+#  	timeW = timeW+1
+#  	uav_moveto(0.0,0.06)
+#  	}
+}
+ 
+########################################
+#
+# BARRIER-RELATED FUNCTIONS
+#
+########################################
+
+#
+# Constants
+#
+BARRIER_VSTIG = 1
+# ROBOTS = 3 # number of robots in the swarm
+
+#
+# Sets a barrier
+#
+function barrier_set(threshold, transf) {
+  statef = function() {
+    barrier_wait(threshold, transf);
+  }
+  barrier = stigmergy.create(BARRIER_VSTIG)
+}
+
+#
+# Make yourself ready
+#
+function barrier_ready() {
+  barrier.put(id, 1)
+}
+
+#
+# Executes the barrier
+#
+WAIT_TIMEOUT = 200
+timeW=0
+function barrier_wait(threshold, transf) {
+  barrier.get(id)
+  CURSTATE = "BARRIERWAIT"
+  if(barrier.size() >= threshold) {
+    barrier = nil
+    transf()
+  } else if(timeW>=WAIT_TIMEOUT) {
+    barrier = nil
+    statef=land
+    timeW=0
+  }
+  timeW = timeW+1
+}
+
+# flight status
+
+function idle() {
+statef=idle
+CURSTATE = "IDLE"
+   
+}
+
+function takeoff() {
+	CURSTATE = "TAKEOFF"
+	statef=takeoff
+	log("TakeOff: ", flight.status)
+	log("Relative position: ", position.altitude)
+	
+	if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
+		barrier_set(ROBOTS,hexagon)
+		barrier_ready()
+		#statef=hexagon
+	}
+	else {
+		log("Altitude: ", TARGET_ALTITUDE)
+		neighbors.broadcast("cmd", 22)
+		uav_takeoff(TARGET_ALTITUDE)
+	}
+}
+function land() {
+	CURSTATE = "LAND"
+	statef=land
+	log("Land: ", flight.status)
+	if(flight.status == 2 or flight.status == 3){
+		neighbors.broadcast("cmd", 21)
+		uav_land()
+	}
+	else {
+		timeW=0
+		barrier = nil
+		statef=idle
+	}
+}
+
+# Executed once at init time.
+function init() {
+	s = swarm.create(1)
+	s.join()
+	statef=idle
+	CURSTATE = "IDLE"
+	vt = stigmergy.cerate(5)
+	t = {}
+	vt.put("p",t)
+}
+
+# Executed at each time step.
+function step() {
+	if(flight.rc_cmd==22) {
+		log("cmd 22")
+		flight.rc_cmd=0
+		statef = takeoff
+		CURSTATE = "TAKEOFF"		
+		neighbors.broadcast("cmd", 22)
+	} else if(flight.rc_cmd==21) {
+		log("cmd 21")
+		log("To land")
+		flight.rc_cmd=0
+		statef = land
+		CURSTATE = "LAND"
+		neighbors.broadcast("cmd", 21)
+	} else if(flight.rc_cmd==16) {
+		flight.rc_cmd=0
+		statef = idle
+		uav_goto()
+	} else if(flight.rc_cmd==400) {
+		flight.rc_cmd=0
+		uav_arm()
+		neighbors.broadcast("cmd", 400)
+	} else if (flight.rc_cmd==401){
+		flight.rc_cmd=0
+		uav_disarm()
+		neighbors.broadcast("cmd", 401)
+	}
+neighbors.listen("cmd",
+   function(vid, value, rid) {
+      print("Got (", vid, ",", value, ") from robot #", rid)
+	if(value==22 and CURSTATE=="IDLE") {
+		statef=takeoff
+	} else if(value==21) {
+		statef=land
+	} else if(value==400 and CURSTATE=="IDLE") {
+		uav_arm()
+	} else if(value==401 and CURSTATE=="IDLE"){
+		uav_disarm()
+	}
+   }
+
+)
+	statef()
+	log("Current state: ", CURSTATE)
+	log("Swarm size: ",ROBOTS)
+	if(users.dataG)
+		vt.put("p",users.dataG)
+	table_print(users.dataL)
+}
+ 
+# Executed once when the robot (or the simulator) is reset.
+function reset() {
+}
+ 
+# Executed once at the end of experiment.
+function destroy() {
+}

script/testflocksim.bzz

@@ -1,6 +1,6 @@
 # We need this for 2D vectors
 # Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
-include "/home/ubuntu/buzz/src/include/vec2.bzz"
+include "vec2.bzz"
 ####################################################################################################
 # Updater related
 # This should be here for the updater to work, changing position of code will crash the updater
@@ -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,37 @@ function land() {
 	}
 }
 
+function users_save(t) {
+	if(size(t)>0) {
+		foreach(t, function(id, tab) {
+				#log("id: ",id," Latitude ", tab.la, "Longitude ", tab.lo)
+				add_user_rb(id,tab.la,tab.lo)
+			})
+	}
+}
+
+# printing the contents of a table: a custom function
+function table_print(t) {
+  if(size(t)>0) {
+		foreach(t, function(u, tab) {
+				log("id: ",u," Range ", tab.r, "Bearing ", tab.b)
+			})
+	}
+}
+
+########################################
+#
+# MAIN FUNCTIONS
+#
+########################################
+
 # Executed once at init time.
 function init() {
 	s = swarm.create(1)
-#	s.select(1)
 	s.join()
+	vt = stigmergy.create(5)
+	t = {}
+	vt.put("p",t)
 	statef=idle
 	CURSTATE = "IDLE"
 }
@@ -196,12 +222,17 @@ neighbors.listen("cmd",
 	statef()
 	log("Current state: ", CURSTATE)
 	log("Swarm size: ",ROBOTS)
-	log("User position: ", users.range)
 	
 	# Read a value from the structure
-        #x = usersvstig.get(46)
+				log(users)      
+				#users_print(users.dataG)
+				if(size(users.dataG)>0)
+					vt.put("p", users.dataG)
+				
         # Get the number of keys in the structure
-        #log("The vstig has ", usersvstig.size(), " elements")
+        log("The vstig has ", vt.size(), " elements")
+				users_save(vt.get("p"))
+				table_print(users.dataL)
 }
  
 # Executed once when the robot (or the simulator) is reset.

script/testsolo.bzz

@@ -0,0 +1,214 @@
+# We need this for 2D vectors
+# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
+include "/home/ubuntu/buzz/src/include/vec2.bzz"
+####################################################################################################
+# 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)
+}
+
+TARGET_ALTITUDE = 3.0
+CURSTATE = "TURNEDOFF"
+ 
+# Lennard-Jones parameters
+TARGET     = 10.0	#0.000001001
+EPSILON    = 18.0	#0.001
+ 
+# Lennard-Jones interaction magnitude
+function lj_magnitude(dist, target, epsilon) {
+  return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
+}
+ 
+# Neighbor data to LJ interaction vector
+function lj_vector(rid, data) {
+  return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth)
+}
+ 
+# Accumulator of neighbor LJ interactions
+function lj_sum(rid, data, accum) {
+  return math.vec2.add(data, accum)
+}
+ 
+# Calculates and actuates the flocking interaction
+function hexagon() {
+  statef=hexagon
+  CURSTATE = "HEXAGON"
+  # Calculate accumulator
+  var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
+  if(neighbors.count() > 0)
+    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)
+
+  log("Time: ", timeW)
+  if(timeW>=WAIT_TIMEOUT) {	#FOR MOVETO TESTS
+  	timeW =0
+  	statef=land
+  	} else {
+  		if(timeW >= (WAIT_TIMEOUT / 2)){
+  			uav_moveto(0.03,0.0)
+  		} else {
+  			uav_moveto(0.0,0.03)
+  		}
+  		timeW = timeW+1
+  	}
+}
+ 
+########################################
+#
+# BARRIER-RELATED FUNCTIONS
+#
+########################################
+
+#
+# Constants
+#
+BARRIER_VSTIG = 1
+# ROBOTS = 3 # number of robots in the swarm
+
+#
+# Sets a barrier
+#
+function barrier_set(threshold, transf) {
+  statef = function() {
+    barrier_wait(threshold, transf);
+  }
+  barrier = stigmergy.create(BARRIER_VSTIG)
+}
+
+#
+# Make yourself ready
+#
+function barrier_ready() {
+  barrier.put(id, 1)
+}
+
+#
+# Executes the barrier
+#
+WAIT_TIMEOUT = 300
+timeW=0
+function barrier_wait(threshold, transf) {
+  barrier.get(id)
+  CURSTATE = "BARRIERWAIT"
+  if(barrier.size() >= threshold) {
+    barrier = nil
+    transf()
+  } else if(timeW>=WAIT_TIMEOUT) {
+    barrier = nil
+    statef=land
+    timeW=0
+  }
+  timeW = timeW+1
+}
+
+# flight status
+
+function idle() {
+statef=idle
+CURSTATE = "IDLE"
+   
+}
+
+function takeoff() {
+	CURSTATE = "TAKEOFF"
+	statef=takeoff
+	log("TakeOff: ", flight.status)
+	log("Relative position: ", position.altitude)
+	
+	if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
+		barrier_set(ROBOTS, hexagon)
+		#barrier_set(ROBOTS, land);
+		barrier_ready()
+		#statef=hexagon
+	}
+	else {
+		log("Altitude: ", TARGET_ALTITUDE)
+		neighbors.broadcast("cmd", 22)
+		uav_takeoff(TARGET_ALTITUDE)
+	}
+}
+function land() {
+	CURSTATE = "LAND"
+	statef=land
+	log("Land: ", flight.status)
+	if(flight.status == 2 or flight.status == 3){
+		neighbors.broadcast("cmd", 21)
+		uav_land()
+	}
+	else {
+		timeW=0
+		barrier = nil
+		statef=idle
+	}
+}
+
+# Executed once at init time.
+function init() {
+	s = swarm.create(1)
+#	s.select(1)
+	s.join()
+	statef=idle
+	CURSTATE = "IDLE"
+}
+
+# Executed at each time step.
+function step() {
+	if(flight.rc_cmd==22) {
+		log("cmd 22")
+		flight.rc_cmd=0
+		statef = takeoff
+		CURSTATE = "TAKEOFF"		
+		neighbors.broadcast("cmd", 22)
+	} else if(flight.rc_cmd==21) {
+		log("cmd 21")
+		log("To land")
+		flight.rc_cmd=0
+		statef = land
+		CURSTATE = "LAND"
+		neighbors.broadcast("cmd", 21)
+	} else if(flight.rc_cmd==16) {
+		flight.rc_cmd=0
+		statef = idle
+		uav_goto()
+	} else if(flight.rc_cmd==400) {
+		flight.rc_cmd=0
+		uav_arm()
+		neighbors.broadcast("cmd", 400)
+	} else if (flight.rc_cmd==401){
+		flight.rc_cmd=0
+		uav_disarm()
+		neighbors.broadcast("cmd", 401)
+	}
+neighbors.listen("cmd",
+   function(vid, value, rid) {
+      print("Got (", vid, ",", value, ") from robot #", rid)
+	if(value==22 and CURSTATE=="IDLE") {
+		statef=takeoff
+	} else if(value==21) {
+		statef=land
+	} else if(value==400 and CURSTATE=="IDLE") {
+		uav_arm()
+	} else if(value==401 and CURSTATE=="IDLE"){
+		uav_disarm()
+	}
+   }
+
+)
+	statef()
+	log("Current state: ", CURSTATE)
+	log("Swarm size: ",ROBOTS)
+}
+ 
+# Executed once when the robot (or the simulator) is reset.
+function reset() {
+}
+ 
+# Executed once at the end of experiment.
+function destroy() {
+}

script/teststig.bzz

@@ -0,0 +1,39 @@
+include "vec2.bzz"
+####################################################################################################
+# 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() {
+}

script/update_sim/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/update_sim/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/update_sim/old_testflockfev1.bzz

@@ -0,0 +1,209 @@
+# We need this for 2D vectors
+
+# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
+include "/home/ubuntu/buzz/src/include/vec2.bzz"
+####################################################################################################
+# 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)
+}
+
+TARGET_ALTITUDE = 3.0
+CURSTATE = "TURNEDOFF"
+ 
+# Lennard-Jones parameters
+TARGET     = 10.0	#0.000001001
+EPSILON    = 18.0	#0.001
+ 
+# Lennard-Jones interaction magnitude
+function lj_magnitude(dist, target, epsilon) {
+  return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
+}
+ 
+# Neighbor data to LJ interaction vector
+function lj_vector(rid, data) {
+  return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth)
+}
+ 
+# Accumulator of neighbor LJ interactions
+function lj_sum(rid, data, accum) {
+  return math.vec2.add(data, accum)
+}
+ 
+# Calculates and actuates the flocking interaction
+function hexagon() {
+  statef=hexagon
+  CURSTATE = "HEXAGON"
+  # Calculate accumulator
+  var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
+  if(neighbors.count() > 0)
+    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)
+
+#  if(timeW>=WAIT_TIMEOUT) {	#FOR MOVETO TESTS
+#  	timeW =0
+#  	statef=land
+#  	} else {
+#  	timeW = timeW+1
+#  	uav_moveto(0.0,0.0)
+#  	}
+}
+ 
+########################################
+#
+# BARRIER-RELATED FUNCTIONS
+#
+########################################
+
+#
+# Constants
+#
+BARRIER_VSTIG = 1
+# ROBOTS = 3 # number of robots in the swarm
+
+#
+# Sets a barrier
+#
+function barrier_set(threshold, transf) {
+  statef = function() {
+    barrier_wait(threshold, transf);
+  }
+  barrier = stigmergy.create(BARRIER_VSTIG)
+}
+
+#
+# Make yourself ready
+#
+function barrier_ready() {
+  barrier.put(id, 1)
+}
+
+#
+# Executes the barrier
+#
+WAIT_TIMEOUT = 200
+timeW=0
+function barrier_wait(threshold, transf) {
+  barrier.get(id)
+  CURSTATE = "BARRIERWAIT"
+  if(barrier.size() >= threshold) {
+    barrier = nil
+    transf()
+  } else if(timeW>=WAIT_TIMEOUT) {
+    barrier = nil
+    statef=land
+    timeW=0
+  }
+  timeW = timeW+1
+}
+
+# flight status
+
+function idle() {
+statef=idle
+CURSTATE = "IDLE"
+   
+}
+
+function takeoff() {
+	CURSTATE = "TAKEOFF"
+	statef=takeoff
+	log("TakeOff: ", flight.status)
+	log("Relative position: ", position.altitude)
+	
+	if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
+		barrier_set(ROBOTS,hexagon)
+		barrier_ready()
+		#statef=hexagon
+	}
+	else {
+		log("Altitude: ", TARGET_ALTITUDE)
+		neighbors.broadcast("cmd", 22)
+		uav_takeoff(TARGET_ALTITUDE)
+	}
+}
+function land() {
+	CURSTATE = "LAND"
+	statef=land
+	log("Land: ", flight.status)
+	if(flight.status == 2 or flight.status == 3){
+		neighbors.broadcast("cmd", 21)
+		uav_land()
+	}
+	else {
+		timeW=0
+		barrier = nil
+		statef=idle
+	}
+}
+
+# Executed once at init time.
+function init() {
+	s = swarm.create(1)
+#	s.select(1)
+	s.join()
+	statef=idle
+	CURSTATE = "IDLE"
+}
+
+# Executed at each time step.
+function step() {
+	if(flight.rc_cmd==22) {
+		log("cmd 22")
+		flight.rc_cmd=0
+		statef = takeoff
+		CURSTATE = "TAKEOFF"		
+		neighbors.broadcast("cmd", 22)
+	} else if(flight.rc_cmd==21) {
+		log("cmd 21")
+		log("To land")
+		flight.rc_cmd=0
+		statef = land
+		CURSTATE = "LAND"
+		neighbors.broadcast("cmd", 21)
+	} else if(flight.rc_cmd==16) {
+		flight.rc_cmd=0
+		statef = idle
+		uav_goto()
+	} else if(flight.rc_cmd==400) {
+		flight.rc_cmd=0
+		uav_arm()
+		neighbors.broadcast("cmd", 400)
+	} else if (flight.rc_cmd==401){
+		flight.rc_cmd=0
+		uav_disarm()
+		neighbors.broadcast("cmd", 401)
+	}
+neighbors.listen("cmd",
+   function(vid, value, rid) {
+      print("Got (", vid, ",", value, ") from robot #", rid)
+	if(value==22 and CURSTATE=="IDLE") {
+		statef=takeoff
+	} else if(value==21) {
+		statef=land
+	} else if(value==400 and CURSTATE=="IDLE") {
+		uav_arm()
+	} else if(value==401 and CURSTATE=="IDLE"){
+		uav_disarm()
+	}
+   }
+
+)
+	statef()
+	log("Current state: ", CURSTATE)
+	log("Swarm size: ",ROBOTS)
+}
+ 
+# Executed once when the robot (or the simulator) is reset.
+function reset() {
+}
+ 
+# Executed once at the end of experiment.
+function destroy() {
+}

script/update_sim/old_testflockfev2.bzz

@@ -0,0 +1,208 @@
+# We need this for 2D vectors
+# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
+include "/home/ubuntu/buzz/src/include/vec2.bzz"
+####################################################################################################
+# 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)
+}
+
+TARGET_ALTITUDE = 3.0
+CURSTATE = "TURNEDOFF"
+ 
+# Lennard-Jones parameters
+TARGET     = 10.0	#0.000001001
+EPSILON    = 18.0	#0.001
+ 
+# Lennard-Jones interaction magnitude
+function lj_magnitude(dist, target, epsilon) {
+  return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
+}
+ 
+# Neighbor data to LJ interaction vector
+function lj_vector(rid, data) {
+  return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth)
+}
+ 
+# Accumulator of neighbor LJ interactions
+function lj_sum(rid, data, accum) {
+  return math.vec2.add(data, accum)
+}
+ 
+# Calculates and actuates the flocking interaction
+function hexagon() {
+  statef=hexagon
+  CURSTATE = "HEXAGON"
+  # Calculate accumulator
+  var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
+  if(neighbors.count() > 0)
+    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)
+
+#  if(timeW>=WAIT_TIMEOUT) {	#FOR MOVETO TESTS
+#  	timeW =0
+#  	statef=land
+#  	} else {
+#  	timeW = timeW+1
+#  	uav_moveto(0.0,0.0)
+#  	}
+}
+ 
+########################################
+#
+# BARRIER-RELATED FUNCTIONS
+#
+########################################
+
+#
+# Constants
+#
+BARRIER_VSTIG = 1
+# ROBOTS = 3 # number of robots in the swarm
+
+#
+# Sets a barrier
+#
+function barrier_set(threshold, transf) {
+  statef = function() {
+    barrier_wait(threshold, transf);
+  }
+  barrier = stigmergy.create(BARRIER_VSTIG)
+}
+
+#
+# Make yourself ready
+#
+function barrier_ready() {
+  barrier.put(id, 1)
+}
+
+#
+# Executes the barrier
+#
+WAIT_TIMEOUT = 200
+timeW=0
+function barrier_wait(threshold, transf) {
+  barrier.get(id)
+  CURSTATE = "BARRIERWAIT"
+  if(barrier.size() >= threshold) {
+    barrier = nil
+    transf()
+  } else if(timeW>=WAIT_TIMEOUT) {
+    barrier = nil
+    statef=land
+    timeW=0
+  }
+  timeW = timeW+1
+}
+
+# flight status
+
+function idle() {
+statef=idle
+CURSTATE = "IDLE"
+   
+}
+
+function takeoff() {
+	CURSTATE = "TAKEOFF"
+	statef=takeoff
+	log("TakeOff: ", flight.status)
+	log("Relative position: ", position.altitude)
+	
+	if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
+		barrier_set(ROBOTS,hexagon)
+		barrier_ready()
+		#statef=hexagon
+	}
+	else {
+		log("Altitude: ", TARGET_ALTITUDE)
+		neighbors.broadcast("cmd", 22)
+		uav_takeoff(TARGET_ALTITUDE)
+	}
+}
+function land() {
+	CURSTATE = "LAND"
+	statef=land
+	log("Land: ", flight.status)
+	if(flight.status == 2 or flight.status == 3){
+		neighbors.broadcast("cmd", 21)
+		uav_land()
+	}
+	else {
+		timeW=0
+		barrier = nil
+		statef=idle
+	}
+}
+
+# Executed once at init time.
+function init() {
+	s = swarm.create(1)
+#	s.select(1)
+	s.join()
+	statef=idle
+	CURSTATE = "IDLE"
+}
+
+# Executed at each time step.
+function step() {
+	if(flight.rc_cmd==22) {
+		log("cmd 22")
+		flight.rc_cmd=0
+		statef = takeoff
+		CURSTATE = "TAKEOFF"		
+		neighbors.broadcast("cmd", 22)
+	} else if(flight.rc_cmd==21) {
+		log("cmd 21")
+		log("To land")
+		flight.rc_cmd=0
+		statef = land
+		CURSTATE = "LAND"
+		neighbors.broadcast("cmd", 21)
+	} else if(flight.rc_cmd==16) {
+		flight.rc_cmd=0
+		statef = idle
+		uav_goto()
+	} else if(flight.rc_cmd==400) {
+		flight.rc_cmd=0
+		uav_arm()
+		neighbors.broadcast("cmd", 400)
+	} else if (flight.rc_cmd==401){
+		flight.rc_cmd=0
+		uav_disarm()
+		neighbors.broadcast("cmd", 401)
+	}
+neighbors.listen("cmd",
+   function(vid, value, rid) {
+      print("Got (", vid, ",", value, ") from robot #", rid)
+	if(value==22 and CURSTATE=="IDLE") {
+		statef=takeoff
+	} else if(value==21) {
+		statef=land
+	} else if(value==400 and CURSTATE=="IDLE") {
+		uav_arm()
+	} else if(value==401 and CURSTATE=="IDLE"){
+		uav_disarm()
+	}
+   }
+
+)
+	statef()
+	log("Current state: ", CURSTATE)
+	log("Swarm size: ",ROBOTS)
+}
+ 
+# Executed once when the robot (or the simulator) is reset.
+function reset() {
+}
+ 
+# Executed once at the end of experiment.
+function destroy() {
+}

script/update_sim/testflockfev1.bzz

@@ -0,0 +1,240 @@
+# We need this for 2D vectors
+# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
+include "vec2.bzz"
+####################################################################################################
+# 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)
+}
+
+TARGET_ALTITUDE = 3.0
+CURSTATE = "TURNEDOFF"
+ 
+# Lennard-Jones parameters
+TARGET     = 12.0	#0.000001001
+EPSILON    = 3.0	#0.001
+ 
+# Lennard-Jones interaction magnitude
+function lj_magnitude(dist, target, epsilon) {
+  return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
+}
+ 
+# Neighbor data to LJ interaction vector
+function lj_vector(rid, data) {
+  return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth)
+}
+ 
+# Accumulator of neighbor LJ interactions
+function lj_sum(rid, data, accum) {
+  return math.vec2.add(data, accum)
+}
+ 
+# Calculates and actuates the flocking interaction
+function hexagon() {
+  statef=hexagon
+  CURSTATE = "HEXAGON"
+  # Calculate accumulator
+  var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
+  if(neighbors.count() > 0)
+    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)
+
+#  if(timeW>=WAIT_TIMEOUT) {	#FOR MOVETO TESTS
+#  	timeW =0
+#  	statef=land
+#  	} else if(timeW>=WAIT_TIMEOUT/2) {
+#  	timeW = timeW+1
+#  	uav_moveto(0.06,0.0)
+#  	} else {
+#  	timeW = timeW+1
+#  	uav_moveto(0.0,0.06)
+#  	}
+}
+ 
+########################################
+#
+# BARRIER-RELATED FUNCTIONS
+#
+########################################
+
+#
+# Constants
+#
+BARRIER_VSTIG = 1
+# ROBOTS = 3 # number of robots in the swarm
+
+#
+# Sets a barrier
+#
+function barrier_set(threshold, transf) {
+  statef = function() {
+    barrier_wait(threshold, transf);
+  }
+  barrier = stigmergy.create(BARRIER_VSTIG)
+}
+
+#
+# Make yourself ready
+#
+function barrier_ready() {
+  barrier.put(id, 1)
+}
+
+#
+# Executes the barrier
+#
+WAIT_TIMEOUT = 200
+timeW=0
+function barrier_wait(threshold, transf) {
+  barrier.get(id)
+  CURSTATE = "BARRIERWAIT"
+  if(barrier.size() >= threshold) {
+    barrier = nil
+    transf()
+  } else if(timeW>=WAIT_TIMEOUT) {
+    barrier = nil
+    statef=land
+    timeW=0
+  }
+  timeW = timeW+1
+}
+
+# flight status
+
+function idle() {
+statef=idle
+CURSTATE = "IDLE"
+   
+}
+
+function takeoff() {
+	CURSTATE = "TAKEOFF"
+	statef=takeoff
+	log("TakeOff: ", flight.status)
+	log("Relative position: ", position.altitude)
+	
+	if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
+		barrier_set(ROBOTS,hexagon)
+		barrier_ready()
+		#statef=hexagon
+	}
+	else {
+		log("Altitude: ", TARGET_ALTITUDE)
+		neighbors.broadcast("cmd", 22)
+		uav_takeoff(TARGET_ALTITUDE)
+	}
+}
+function land() {
+	CURSTATE = "LAND"
+	statef=land
+	log("Land: ", flight.status)
+	if(flight.status == 2 or flight.status == 3){
+		neighbors.broadcast("cmd", 21)
+		uav_land()
+	}
+	else {
+		timeW=0
+		barrier = nil
+		statef=idle
+	}
+}
+
+function users_save(t) {
+	if(size(t)>0) {
+		foreach(t, function(id, tab) {
+				#log("id: ",id," Latitude ", tab.la, "Longitude ", tab.lo)
+				add_user_rb(id,tab.la,tab.lo)
+			})
+	}
+}
+
+function table_print(t) {
+  if(size(t)>0) {
+		foreach(t, function(u, tab) {
+				log("id: ",u," Range ", tab.r, "Bearing ", tab.b)
+			})
+	}
+}
+
+# Executed once at init time.
+function init() {
+	s = swarm.create(1)
+	s.join()
+
+	vt = stigmergy.create(5)
+	t = {}
+	vt.put("p",t)
+
+	statef=idle
+	CURSTATE = "IDLE"
+}
+
+# Executed at each time step.
+function step() {
+	if(flight.rc_cmd==22) {
+		log("cmd 22")
+		flight.rc_cmd=0
+		statef = takeoff
+		CURSTATE = "TAKEOFF"		
+		neighbors.broadcast("cmd", 22)
+	} else if(flight.rc_cmd==21) {
+		log("cmd 21")
+		log("To land")
+		flight.rc_cmd=0
+		statef = land
+		CURSTATE = "LAND"
+		neighbors.broadcast("cmd", 21)
+	} else if(flight.rc_cmd==16) {
+		flight.rc_cmd=0
+		statef = idle
+		uav_goto()
+	} else if(flight.rc_cmd==400) {
+		flight.rc_cmd=0
+		uav_arm()
+		neighbors.broadcast("cmd", 400)
+	} else if (flight.rc_cmd==401){
+		flight.rc_cmd=0
+		uav_disarm()
+		neighbors.broadcast("cmd", 401)
+	}
+neighbors.listen("cmd",
+   function(vid, value, rid) {
+      print("Got (", vid, ",", value, ") from robot #", rid)
+	if(value==22 and CURSTATE=="IDLE") {
+		statef=takeoff
+	} else if(value==21) {
+		statef=land
+	} else if(value==400 and CURSTATE=="IDLE") {
+		uav_arm()
+	} else if(value==401 and CURSTATE=="IDLE"){
+		uav_disarm()
+	}
+   }
+
+)
+	statef()
+	log("Current state: ", CURSTATE)
+	log("Swarm size: ",ROBOTS)
+
+	# Check local users and push to v.stig
+	if(size(users.dataG)>0)
+		vt.put("p", users.dataG)
+				
+        # Save locally the users and print RG
+	users_save(vt.get("p"))
+	table_print(users.dataL)
+}
+ 
+# Executed once when the robot (or the simulator) is reset.
+function reset() {
+}
+ 
+# Executed once at the end of experiment.
+function destroy() {
+}

script/update_sim/testflockfev2.bzz

@@ -0,0 +1,240 @@
+# We need this for 2D vectors
+# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
+include "vec2.bzz"
+####################################################################################################
+# 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)
+}
+
+TARGET_ALTITUDE = 3.0
+CURSTATE = "TURNEDOFF"
+ 
+# Lennard-Jones parameters
+TARGET     = 12.0	#0.000001001
+EPSILON    = 3.0	#0.001
+ 
+# Lennard-Jones interaction magnitude
+function lj_magnitude(dist, target, epsilon) {
+  return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
+}
+ 
+# Neighbor data to LJ interaction vector
+function lj_vector(rid, data) {
+  return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth)
+}
+ 
+# Accumulator of neighbor LJ interactions
+function lj_sum(rid, data, accum) {
+  return math.vec2.add(data, accum)
+}
+ 
+# Calculates and actuates the flocking interaction
+function hexagon() {
+  statef=hexagon
+  CURSTATE = "HEXAGON"
+  # Calculate accumulator
+  var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
+  if(neighbors.count() > 0)
+    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)
+
+#  if(timeW>=WAIT_TIMEOUT) {	#FOR MOVETO TESTS
+#  	timeW =0
+#  	statef=land
+#  	} else if(timeW>=WAIT_TIMEOUT/2) {
+#  	timeW = timeW+1
+#  	uav_moveto(0.06,0.0)
+#  	} else {
+#  	timeW = timeW+1
+#  	uav_moveto(0.0,0.06)
+#  	}
+}
+ 
+########################################
+#
+# BARRIER-RELATED FUNCTIONS
+#
+########################################
+
+#
+# Constants
+#
+BARRIER_VSTIG = 1
+# ROBOTS = 3 # number of robots in the swarm
+
+#
+# Sets a barrier
+#
+function barrier_set(threshold, transf) {
+  statef = function() {
+    barrier_wait(threshold, transf);
+  }
+  barrier = stigmergy.create(BARRIER_VSTIG)
+}
+
+#
+# Make yourself ready
+#
+function barrier_ready() {
+  barrier.put(id, 1)
+}
+
+#
+# Executes the barrier
+#
+WAIT_TIMEOUT = 200
+timeW=0
+function barrier_wait(threshold, transf) {
+  barrier.get(id)
+  CURSTATE = "BARRIERWAIT"
+  if(barrier.size() >= threshold) {
+    barrier = nil
+    transf()
+  } else if(timeW>=WAIT_TIMEOUT) {
+    barrier = nil
+    statef=land
+    timeW=0
+  }
+  timeW = timeW+1
+}
+
+# flight status
+
+function idle() {
+statef=idle
+CURSTATE = "IDLE"
+   
+}
+
+function takeoff() {
+	CURSTATE = "TAKEOFF"
+	statef=takeoff
+	log("TakeOff: ", flight.status)
+	log("Relative position: ", position.altitude)
+	
+	if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
+		barrier_set(ROBOTS,hexagon)
+		barrier_ready()
+		#statef=hexagon
+	}
+	else {
+		log("Altitude: ", TARGET_ALTITUDE)
+		neighbors.broadcast("cmd", 22)
+		uav_takeoff(TARGET_ALTITUDE)
+	}
+}
+function land() {
+	CURSTATE = "LAND"
+	statef=land
+	log("Land: ", flight.status)
+	if(flight.status == 2 or flight.status == 3){
+		neighbors.broadcast("cmd", 21)
+		uav_land()
+	}
+	else {
+		timeW=0
+		barrier = nil
+		statef=idle
+	}
+}
+
+function users_save(t) {
+	if(size(t)>0) {
+		foreach(t, function(id, tab) {
+				#log("id: ",id," Latitude ", tab.la, "Longitude ", tab.lo)
+				add_user_rb(id,tab.la,tab.lo)
+			})
+	}
+}
+
+function table_print(t) {
+  if(size(t)>0) {
+		foreach(t, function(u, tab) {
+				log("id: ",u," Range ", tab.r, "Bearing ", tab.b)
+			})
+	}
+}
+
+# Executed once at init time.
+function init() {
+	s = swarm.create(1)
+	s.join()
+
+	vt = stigmergy.create(5)
+	t = {}
+	vt.put("p",t)
+
+	statef=idle
+	CURSTATE = "IDLE"
+}
+
+# Executed at each time step.
+function step() {
+	if(flight.rc_cmd==22) {
+		log("cmd 22")
+		flight.rc_cmd=0
+		statef = takeoff
+		CURSTATE = "TAKEOFF"		
+		neighbors.broadcast("cmd", 22)
+	} else if(flight.rc_cmd==21) {
+		log("cmd 21")
+		log("To land")
+		flight.rc_cmd=0
+		statef = land
+		CURSTATE = "LAND"
+		neighbors.broadcast("cmd", 21)
+	} else if(flight.rc_cmd==16) {
+		flight.rc_cmd=0
+		statef = idle
+		uav_goto()
+	} else if(flight.rc_cmd==400) {
+		flight.rc_cmd=0
+		uav_arm()
+		neighbors.broadcast("cmd", 400)
+	} else if (flight.rc_cmd==401){
+		flight.rc_cmd=0
+		uav_disarm()
+		neighbors.broadcast("cmd", 401)
+	}
+neighbors.listen("cmd",
+   function(vid, value, rid) {
+      print("Got (", vid, ",", value, ") from robot #", rid)
+	if(value==22 and CURSTATE=="IDLE") {
+		statef=takeoff
+	} else if(value==21) {
+		statef=land
+	} else if(value==400 and CURSTATE=="IDLE") {
+		uav_arm()
+	} else if(value==401 and CURSTATE=="IDLE"){
+		uav_disarm()
+	}
+   }
+
+)
+	statef()
+	log("Current state: ", CURSTATE)
+	log("Swarm size: ",ROBOTS)
+
+	# Check local users and push to v.stig
+	if(size(users.dataG)>0)
+		vt.put("p", users.dataG)
+				
+        # Save locally the users and print RG
+	users_save(vt.get("p"))
+	table_print(users.dataL)
+}
+ 
+# Executed once when the robot (or the simulator) is reset.
+function reset() {
+}
+ 
+# Executed once at the end of experiment.
+function destroy() {
+}

script/update_sim/testflockfev5.bzz

@@ -0,0 +1,208 @@
+# We need this for 2D vectors
+# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
+include "/home/ubuntu/buzz/src/include/vec2.bzz"
+####################################################################################################
+# 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)
+}
+
+TARGET_ALTITUDE = 3.0
+CURSTATE = "TURNEDOFF"
+ 
+# Lennard-Jones parameters
+TARGET     = 10.0	#0.000001001
+EPSILON    = 18.0	#0.001
+ 
+# Lennard-Jones interaction magnitude
+function lj_magnitude(dist, target, epsilon) {
+  return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
+}
+ 
+# Neighbor data to LJ interaction vector
+function lj_vector(rid, data) {
+  return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth)
+}
+ 
+# Accumulator of neighbor LJ interactions
+function lj_sum(rid, data, accum) {
+  return math.vec2.add(data, accum)
+}
+ 
+# Calculates and actuates the flocking interaction
+function hexagon() {
+  statef=hexagon
+  CURSTATE = "HEXAGON"
+  # Calculate accumulator
+  var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
+  if(neighbors.count() > 0)
+    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)
+
+#  if(timeW>=WAIT_TIMEOUT) {	#FOR MOVETO TESTS
+#  	timeW =0
+#  	statef=land
+#  	} else {
+#  	timeW = timeW+1
+#  	uav_moveto(0.0,0.0)
+#  	}
+}
+ 
+########################################
+#
+# BARRIER-RELATED FUNCTIONS
+#
+########################################
+
+#
+# Constants
+#
+BARRIER_VSTIG = 1
+# ROBOTS = 3 # number of robots in the swarm
+
+#
+# Sets a barrier
+#
+function barrier_set(threshold, transf) {
+  statef = function() {
+    barrier_wait(threshold, transf);
+  }
+  barrier = stigmergy.create(BARRIER_VSTIG)
+}
+
+#
+# Make yourself ready
+#
+function barrier_ready() {
+  barrier.put(id, 1)
+}
+
+#
+# Executes the barrier
+#
+WAIT_TIMEOUT = 200
+timeW=0
+function barrier_wait(threshold, transf) {
+  barrier.get(id)
+  CURSTATE = "BARRIERWAIT"
+  if(barrier.size() >= threshold) {
+    barrier = nil
+    transf()
+  } else if(timeW>=WAIT_TIMEOUT) {
+    barrier = nil
+    statef=land
+    timeW=0
+  }
+  timeW = timeW+1
+}
+
+# flight status
+
+function idle() {
+statef=idle
+CURSTATE = "IDLE"
+   
+}
+
+function takeoff() {
+	CURSTATE = "TAKEOFF"
+	statef=takeoff
+	log("TakeOff: ", flight.status)
+	log("Relative position: ", position.altitude)
+	
+	if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
+		barrier_set(ROBOTS,hexagon)
+		barrier_ready()
+		#statef=hexagon
+	}
+	else {
+		log("Altitude: ", TARGET_ALTITUDE)
+		neighbors.broadcast("cmd", 22)
+		uav_takeoff(TARGET_ALTITUDE)
+	}
+}
+function land() {
+	CURSTATE = "LAND"
+	statef=land
+	log("Land: ", flight.status)
+	if(flight.status == 2 or flight.status == 3){
+		neighbors.broadcast("cmd", 21)
+		uav_land()
+	}
+	else {
+		timeW=0
+		barrier = nil
+		statef=idle
+	}
+}
+
+# Executed once at init time.
+function init() {
+	s = swarm.create(1)
+#	s.select(1)
+	s.join()
+	statef=idle
+	CURSTATE = "IDLE"
+}
+
+# Executed at each time step.
+function step() {
+	if(flight.rc_cmd==22) {
+		log("cmd 22")
+		flight.rc_cmd=0
+		statef = takeoff
+		CURSTATE = "TAKEOFF"		
+		neighbors.broadcast("cmd", 22)
+	} else if(flight.rc_cmd==21) {
+		log("cmd 21")
+		log("To land")
+		flight.rc_cmd=0
+		statef = land
+		CURSTATE = "LAND"
+		neighbors.broadcast("cmd", 21)
+	} else if(flight.rc_cmd==16) {
+		flight.rc_cmd=0
+		statef = idle
+		uav_goto()
+	} else if(flight.rc_cmd==400) {
+		flight.rc_cmd=0
+		uav_arm()
+		neighbors.broadcast("cmd", 400)
+	} else if (flight.rc_cmd==401){
+		flight.rc_cmd=0
+		uav_disarm()
+		neighbors.broadcast("cmd", 401)
+	}
+neighbors.listen("cmd",
+   function(vid, value, rid) {
+      print("Got (", vid, ",", value, ") from robot #", rid)
+	if(value==22 and CURSTATE=="IDLE") {
+		statef=takeoff
+	} else if(value==21) {
+		statef=land
+	} else if(value==400 and CURSTATE=="IDLE") {
+		uav_arm()
+	} else if(value==401 and CURSTATE=="IDLE"){
+		uav_disarm()
+	}
+   }
+
+)
+	statef()
+	log("Current state: ", CURSTATE)
+	log("Swarm size: ",ROBOTS)
+}
+ 
+# Executed once when the robot (or the simulator) is reset.
+function reset() {
+}
+ 
+# Executed once at the end of experiment.
+function destroy() {
+}

script/update_sim/testflockfev6.bzz

@@ -0,0 +1,208 @@
+# We need this for 2D vectors
+# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
+include "/home/ubuntu/buzz/src/include/vec2.bzz"
+####################################################################################################
+# 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)
+}
+
+TARGET_ALTITUDE = 3.0
+CURSTATE = "TURNEDOFF"
+ 
+# Lennard-Jones parameters
+TARGET     = 10.0	#0.000001001
+EPSILON    = 18.0	#0.001
+ 
+# Lennard-Jones interaction magnitude
+function lj_magnitude(dist, target, epsilon) {
+  return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
+}
+ 
+# Neighbor data to LJ interaction vector
+function lj_vector(rid, data) {
+  return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth)
+}
+ 
+# Accumulator of neighbor LJ interactions
+function lj_sum(rid, data, accum) {
+  return math.vec2.add(data, accum)
+}
+ 
+# Calculates and actuates the flocking interaction
+function hexagon() {
+  statef=hexagon
+  CURSTATE = "HEXAGON"
+  # Calculate accumulator
+  var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
+  if(neighbors.count() > 0)
+    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)
+
+#  if(timeW>=WAIT_TIMEOUT) {	#FOR MOVETO TESTS
+#  	timeW =0
+#  	statef=land
+#  	} else {
+#  	timeW = timeW+1
+#  	uav_moveto(0.0,0.0)
+#  	}
+}
+ 
+########################################
+#
+# BARRIER-RELATED FUNCTIONS
+#
+########################################
+
+#
+# Constants
+#
+BARRIER_VSTIG = 1
+# ROBOTS = 3 # number of robots in the swarm
+
+#
+# Sets a barrier
+#
+function barrier_set(threshold, transf) {
+  statef = function() {
+    barrier_wait(threshold, transf);
+  }
+  barrier = stigmergy.create(BARRIER_VSTIG)
+}
+
+#
+# Make yourself ready
+#
+function barrier_ready() {
+  barrier.put(id, 1)
+}
+
+#
+# Executes the barrier
+#
+WAIT_TIMEOUT = 200
+timeW=0
+function barrier_wait(threshold, transf) {
+  barrier.get(id)
+  CURSTATE = "BARRIERWAIT"
+  if(barrier.size() >= threshold) {
+    barrier = nil
+    transf()
+  } else if(timeW>=WAIT_TIMEOUT) {
+    barrier = nil
+    statef=land
+    timeW=0
+  }
+  timeW = timeW+1
+}
+
+# flight status
+
+function idle() {
+statef=idle
+CURSTATE = "IDLE"
+   
+}
+
+function takeoff() {
+	CURSTATE = "TAKEOFF"
+	statef=takeoff
+	log("TakeOff: ", flight.status)
+	log("Relative position: ", position.altitude)
+	
+	if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
+		barrier_set(ROBOTS,hexagon)
+		barrier_ready()
+		#statef=hexagon
+	}
+	else {
+		log("Altitude: ", TARGET_ALTITUDE)
+		neighbors.broadcast("cmd", 22)
+		uav_takeoff(TARGET_ALTITUDE)
+	}
+}
+function land() {
+	CURSTATE = "LAND"
+	statef=land
+	log("Land: ", flight.status)
+	if(flight.status == 2 or flight.status == 3){
+		neighbors.broadcast("cmd", 21)
+		uav_land()
+	}
+	else {
+		timeW=0
+		barrier = nil
+		statef=idle
+	}
+}
+
+# Executed once at init time.
+function init() {
+	s = swarm.create(1)
+#	s.select(1)
+	s.join()
+	statef=idle
+	CURSTATE = "IDLE"
+}
+
+# Executed at each time step.
+function step() {
+	if(flight.rc_cmd==22) {
+		log("cmd 22")
+		flight.rc_cmd=0
+		statef = takeoff
+		CURSTATE = "TAKEOFF"		
+		neighbors.broadcast("cmd", 22)
+	} else if(flight.rc_cmd==21) {
+		log("cmd 21")
+		log("To land")
+		flight.rc_cmd=0
+		statef = land
+		CURSTATE = "LAND"
+		neighbors.broadcast("cmd", 21)
+	} else if(flight.rc_cmd==16) {
+		flight.rc_cmd=0
+		statef = idle
+		uav_goto()
+	} else if(flight.rc_cmd==400) {
+		flight.rc_cmd=0
+		uav_arm()
+		neighbors.broadcast("cmd", 400)
+	} else if (flight.rc_cmd==401){
+		flight.rc_cmd=0
+		uav_disarm()
+		neighbors.broadcast("cmd", 401)
+	}
+neighbors.listen("cmd",
+   function(vid, value, rid) {
+      print("Got (", vid, ",", value, ") from robot #", rid)
+	if(value==22 and CURSTATE=="IDLE") {
+		statef=takeoff
+	} else if(value==21) {
+		statef=land
+	} else if(value==400 and CURSTATE=="IDLE") {
+		uav_arm()
+	} else if(value==401 and CURSTATE=="IDLE"){
+		uav_disarm()
+	}
+   }
+
+)
+	statef()
+	log("Current state: ", CURSTATE)
+	log("Swarm size: ",ROBOTS)
+}
+ 
+# Executed once when the robot (or the simulator) is reset.
+function reset() {
+}
+ 
+# Executed once at the end of experiment.
+function destroy() {
+}

script/update_sim/testflockfev7.bzz

@@ -0,0 +1,208 @@
+# We need this for 2D vectors
+# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
+include "/home/ubuntu/buzz/src/include/vec2.bzz"
+####################################################################################################
+# 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)
+}
+
+TARGET_ALTITUDE = 3.0
+CURSTATE = "TURNEDOFF"
+ 
+# Lennard-Jones parameters
+TARGET     = 10.0	#0.000001001
+EPSILON    = 18.0	#0.001
+ 
+# Lennard-Jones interaction magnitude
+function lj_magnitude(dist, target, epsilon) {
+  return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
+}
+ 
+# Neighbor data to LJ interaction vector
+function lj_vector(rid, data) {
+  return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth)
+}
+ 
+# Accumulator of neighbor LJ interactions
+function lj_sum(rid, data, accum) {
+  return math.vec2.add(data, accum)
+}
+ 
+# Calculates and actuates the flocking interaction
+function hexagon() {
+  statef=hexagon
+  CURSTATE = "HEXAGON"
+  # Calculate accumulator
+  var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
+  if(neighbors.count() > 0)
+    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)
+
+#  if(timeW>=WAIT_TIMEOUT) {	#FOR MOVETO TESTS
+#  	timeW =0
+#  	statef=land
+#  	} else {
+#  	timeW = timeW+1
+#  	uav_moveto(0.0,0.0)
+#  	}
+}
+ 
+########################################
+#
+# BARRIER-RELATED FUNCTIONS
+#
+########################################
+
+#
+# Constants
+#
+BARRIER_VSTIG = 1
+# ROBOTS = 3 # number of robots in the swarm
+
+#
+# Sets a barrier
+#
+function barrier_set(threshold, transf) {
+  statef = function() {
+    barrier_wait(threshold, transf);
+  }
+  barrier = stigmergy.create(BARRIER_VSTIG)
+}
+
+#
+# Make yourself ready
+#
+function barrier_ready() {
+  barrier.put(id, 1)
+}
+
+#
+# Executes the barrier
+#
+WAIT_TIMEOUT = 200
+timeW=0
+function barrier_wait(threshold, transf) {
+  barrier.get(id)
+  CURSTATE = "BARRIERWAIT"
+  if(barrier.size() >= threshold) {
+    barrier = nil
+    transf()
+  } else if(timeW>=WAIT_TIMEOUT) {
+    barrier = nil
+    statef=land
+    timeW=0
+  }
+  timeW = timeW+1
+}
+
+# flight status
+
+function idle() {
+statef=idle
+CURSTATE = "IDLE"
+   
+}
+
+function takeoff() {
+	CURSTATE = "TAKEOFF"
+	statef=takeoff
+	log("TakeOff: ", flight.status)
+	log("Relative position: ", position.altitude)
+	
+	if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
+		barrier_set(ROBOTS,hexagon)
+		barrier_ready()
+		#statef=hexagon
+	}
+	else {
+		log("Altitude: ", TARGET_ALTITUDE)
+		neighbors.broadcast("cmd", 22)
+		uav_takeoff(TARGET_ALTITUDE)
+	}
+}
+function land() {
+	CURSTATE = "LAND"
+	statef=land
+	log("Land: ", flight.status)
+	if(flight.status == 2 or flight.status == 3){
+		neighbors.broadcast("cmd", 21)
+		uav_land()
+	}
+	else {
+		timeW=0
+		barrier = nil
+		statef=idle
+	}
+}
+
+# Executed once at init time.
+function init() {
+	s = swarm.create(1)
+#	s.select(1)
+	s.join()
+	statef=idle
+	CURSTATE = "IDLE"
+}
+
+# Executed at each time step.
+function step() {
+	if(flight.rc_cmd==22) {
+		log("cmd 22")
+		flight.rc_cmd=0
+		statef = takeoff
+		CURSTATE = "TAKEOFF"		
+		neighbors.broadcast("cmd", 22)
+	} else if(flight.rc_cmd==21) {
+		log("cmd 21")
+		log("To land")
+		flight.rc_cmd=0
+		statef = land
+		CURSTATE = "LAND"
+		neighbors.broadcast("cmd", 21)
+	} else if(flight.rc_cmd==16) {
+		flight.rc_cmd=0
+		statef = idle
+		uav_goto()
+	} else if(flight.rc_cmd==400) {
+		flight.rc_cmd=0
+		uav_arm()
+		neighbors.broadcast("cmd", 400)
+	} else if (flight.rc_cmd==401){
+		flight.rc_cmd=0
+		uav_disarm()
+		neighbors.broadcast("cmd", 401)
+	}
+neighbors.listen("cmd",
+   function(vid, value, rid) {
+      print("Got (", vid, ",", value, ") from robot #", rid)
+	if(value==22 and CURSTATE=="IDLE") {
+		statef=takeoff
+	} else if(value==21) {
+		statef=land
+	} else if(value==400 and CURSTATE=="IDLE") {
+		uav_arm()
+	} else if(value==401 and CURSTATE=="IDLE"){
+		uav_disarm()
+	}
+   }
+
+)
+	statef()
+	log("Current state: ", CURSTATE)
+	log("Swarm size: ",ROBOTS)
+}
+ 
+# 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{
 
@@ -17,40 +17,139 @@ namespace buzz_utility{
 	static char*        BO_FNAME        = 0;
 	static uint8_t*     BO_BUF          = 0;
 	static buzzdebug_t  DBG_INFO        = 0;
-	static uint8_t      MSG_SIZE        = 50;   // Only 100 bytes of Buzz messages every step
+	static uint8_t      MSG_SIZE        = 250;   // 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;
-        buzzobj_t           usersvstig;
+	static std::vector<uint8_t*> IN_MSG;
-        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);
+			}
+		}/*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, "dataG", 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, "dataG", 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,44 +167,57 @@ 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 */
+   		/* Go through messages and append them to the vector */
    		uint16_t* data= u64_cvt_u16((uint64_t)payload[0]);
 		/*Size is at first 2 bytes*/
    		uint16_t size=data[0]*sizeof(uint64_t);
    		delete[] data;
    		uint8_t* pl =(uint8_t*)malloc(size);
-   		memset(pl, 0,size);
+		/* Copy packet into temporary buffer */
-   		/* Copy packet into temporary buffer */
+	   	memcpy(pl, payload ,size);
-   		memcpy(pl, payload ,size);
+   		IN_MSG.push_back(pl);
-		/*size and robot id read*/
-   		size_t tot = sizeof(uint32_t);
-		/* Go through the messages until there's nothing else to read */
-      		uint16_t unMsgSize=0;
    		
-			/*Obtain Buzz messages only when they are present*/
+	}
-	      			do {
+	
-		 			/* Get payload size */
+	void in_message_process(){
-		 			unMsgSize = *(uint16_t*)(pl + tot);
+		while(!IN_MSG.empty()){
-	   	 			tot += sizeof(uint16_t);
+			uint8_t* first_INmsg = (uint8_t*)IN_MSG.front(); 
-		 			/* Append message to the Buzz input message queue */
+			/* Go through messages and append them to the FIFO */
-		 			if(unMsgSize > 0 && unMsgSize <= size - tot ) {
+   			uint16_t* data= u64_cvt_u16((uint64_t)first_INmsg[0]);
-		    			buzzinmsg_queue_append(VM,
+			/*Size is at first 2 bytes*/
-		                        buzzmsg_payload_frombuffer(pl +tot, unMsgSize));
+   			uint16_t size=data[0]*sizeof(uint64_t);
-		    			tot += unMsgSize;
+   			delete[] data;
-		 			}
+			/*size and robot id read*/
-	      			}while(size - tot > sizeof(uint16_t) && unMsgSize > 0);
+	   		size_t tot = sizeof(uint32_t);
-		free(pl);
+			/* Go through the messages until there's nothing else to read */
-   		/* Process messages */
+	      		uint16_t unMsgSize=0;
+				/*Obtain Buzz messages push it into queue*/
+		      			do {
+			 			/* Get payload size */
+			 			unMsgSize = *(uint16_t*)(first_INmsg + tot);
+		   	 			tot += sizeof(uint16_t);
+			 			/* Append message to the Buzz input message queue */
+			 			if(unMsgSize > 0 && unMsgSize <= size - tot ) {
+			    			buzzinmsg_queue_append(VM,
+				                buzzmsg_payload_frombuffer(first_INmsg +tot, unMsgSize));
+			    			tot += unMsgSize;
+			 			}
+		      			}while(size - tot > sizeof(uint16_t) && unMsgSize > 0);
+			IN_MSG.erase(IN_MSG.begin());
+			free(first_INmsg);
+		}
+		/* Process messages VM call*/
 		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);
+		/* Process out messages */
+		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 +263,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 +289,7 @@ namespace buzz_utility{
               			 VM->pc,
       			         VM->errormsg);
    		}
-  	 	return msg;
+ 	return msg;
 	}
 
 	/****************************************/
@@ -189,15 +301,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 +324,11 @@ 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;
+   		buzzvm_pushs(VM,  buzzvm_string_register(VM, "add_user_rb", 1));
+   		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_adduserRB));
+   		buzzvm_gstore(VM);
+
+   	return VM->state;
 	}
 
 	/**************************************************/
@@ -224,15 +340,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 +363,142 @@ 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;
+   		buzzvm_pushs(VM,  buzzvm_string_register(VM, "add_user_rb", 1));
+   		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
+   		buzzvm_gstore(VM);
+
+   	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);*/
+
+		buzzobj_t t = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE);
+		buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
+		buzzvm_push(VM,t);
+		buzzvm_gstore(VM);
+		buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
+		buzzvm_gload(VM);
+		buzzvm_pushs(VM, buzzvm_string_register(VM, "dataG", 1));
+		buzzvm_pusht(VM);
+		buzzobj_t data = buzzvm_stack_at(VM, 1);
+		buzzvm_tput(VM);
+		buzzvm_push(VM, data);
+		
+		buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
+		buzzvm_gload(VM);
+		buzzvm_pushs(VM, buzzvm_string_register(VM, "dataL", 1));
+		buzzvm_pusht(VM);
+		data = buzzvm_stack_at(VM, 1);
+		buzzvm_tput(VM);
+		buzzvm_push(VM, data);
+
+   	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;
    	}
-
-   	
-buzzvm_dup(VM);
-   	// usersvstig = stigmergy.create(123)
-        buzzvm_pushs(VM, buzzvm_string_register(VM, "v", 1));
-        // 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 +508,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 +545,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 +594,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 +622,74 @@ 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 available messages*/
-	    * Update sensors
+		in_message_process();
-	    */
+		/*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
-	   //usleep(10000);
-	   /*Print swarm*/
-	   //buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
-	   //int SwarmSize = buzzdict_size(VM->swarmmembers)+1;
-	   //fprintf(stderr, "Real Swarm Size: %i\n",SwarmSize);
 		
-	   /* Check swarm state -- Not crashing thanks to test added in check_swarm_members */
+		/*Print swarm*/
-//	   int status = 1;
+		//buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
-//	   buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status);
+		//int SwarmSize = buzzdict_size(VM->swarmmembers)+1;
+		//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 */
+		//int status = 1;
+		//buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status);
 	}
 
 	/****************************************/
@@ -553,6 +729,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 +741,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;
+
 	/****************************************/
 	/****************************************/
 
@@ -74,7 +77,7 @@ namespace buzzuav_closures{
 	/*----------------------------------------/
 	/ Compute GPS destination from current position and desired Range and Bearing
 	/----------------------------------------*/
-	#define EARTH_RADIUS (double) 6371000.0
+
 	void gps_from_rb(double  range, double bearing, double out[3]) {
 		double lat = cur_pos[0]*M_PI/180.0;
 		double lon = cur_pos[1]*M_PI/180.0;
@@ -85,6 +88,16 @@ namespace buzzuav_closures{
 		out[2] = height; //constant height.
 	}
 
+	void rb_from_gps(double nei[], double out[], double cur[]){   
+       	double d_lon = nei[1] - cur[1];
+       	double d_lat = nei[0] - cur[0];
+        double ned_x = DEG2RAD(d_lat) * EARTH_RADIUS;
+        double ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0]));
+		out[0] = sqrt(ned_x*ned_x+ned_y*ned_y);
+		out[1] = atan2(ned_y,ned_x);
+		out[2] = 0.0;
+	}
+
 	// Hard coded GPS position in Park Maisonneuve, Montreal, Canada for simulation tests
 	double hcpos1[4] = {45.564489, -73.562537, 45.564140, -73.562336};
 	double hcpos2[4] = {45.564729, -73.562060, 45.564362, -73.562958};
@@ -105,17 +118,81 @@ namespace buzzuav_closures{
 	   float dx = buzzvm_stack_at(vm, 2)->f.value;
 	   double d = sqrt(dx*dx+dy*dy);	//range
 	   goto_pos[0]=dx;
-           goto_pos[1]=dy;
+       goto_pos[1]=dy;
-           goto_pos[2]=height;
+       goto_pos[2]=height;
 	   /*double b = atan2(dy,dx);		//bearing
 	   printf(" Vector for Goto: %.7f,%.7f\n",dx,dy);
 	   gps_from_rb(d, b, goto_pos);
 	   cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;*/
+       printf(" Vector for Goto: %.7f,%.7f\n",dx,dy);
 	   printf(" Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f  \n",goto_pos[0], goto_pos[1], goto_pos[2]);
 	   buzz_cmd= COMMAND_MOVETO; // TO DO what should we use
 	   return buzzvm_ret0(vm);
 	}
 
+	int users_add2localtable(buzzvm_t vm, int id, float range, float bearing) {
+		if(vm->state != BUZZVM_STATE_READY) return vm->state;
+		buzzvm_pushs(vm, buzzvm_string_register(vm, "users", 1));
+		buzzvm_gload(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, "dataL", 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, "dataL", 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 range */
+		buzzvm_push(vm, entry);
+		buzzvm_pushs(vm, buzzvm_string_register(vm, "r", 1));
+		buzzvm_pushf(vm, range);
+		buzzvm_tput(vm);
+		/* Insert longitude */
+		buzzvm_push(vm, entry);
+		buzzvm_pushs(vm, buzzvm_string_register(vm, "b", 1));
+		buzzvm_pushf(vm, bearing);
+		buzzvm_tput(vm);
+		/* Save entry into data table */
+		buzzvm_push(vm, entry);
+		buzzvm_tput(vm);
+		//printf("\tBuzz_closure saved new user: %i (%f,%f)\n", id, range, bearing);
+		return vm->state;
+	}
+
+	int buzzuav_adduserRB(buzzvm_t vm) {
+	   buzzvm_lnum_assert(vm, 3);
+	   buzzvm_lload(vm, 1); /* longitude */
+	   buzzvm_lload(vm, 2); /* latitude */
+	   buzzvm_lload(vm, 3); /* id */
+	   buzzvm_type_assert(vm, 3, BUZZTYPE_INT);
+	   buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
+	   buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
+	   double tmp[3];
+	   tmp[0] = buzzvm_stack_at(vm, 2)->f.value;
+	   tmp[1] = buzzvm_stack_at(vm, 1)->f.value;
+	   tmp[2] = 0.0;
+	   int uid = buzzvm_stack_at(vm, 3)->i.value;
+	   double rb[3];
+
+	   rb_from_gps(tmp, rb, cur_pos);
+
+	   //printf("\tGot new user from bzz stig: %i - %f, %f\n", uid, rb[0], rb[1]);
+
+	   return users_add2localtable(vm, uid, rb[0], rb[1]);
+	}
+
 	/*----------------------------------------/
 	/ Buzz closure to go directly to a GPS destination from the Mission Planner
 	/----------------------------------------*/
@@ -245,11 +322,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));
@@ -269,26 +368,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

@@ -7,13 +7,20 @@ namespace rosbzz_node{
 	---------------*/
 	roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv)	
 	{
+
+		home[0]=0.0;home[1]=0.0;home[2]=0.0;
+		target[0]=0.0;target[1]=0.0;target[2]=0.0;
+		cur_pos[0]=0.0;cur_pos[1]=0.0;cur_pos[2]=0.0;
+
 		ROS_INFO("Buzz_node");
 		/*Obtain parameters from ros parameter server*/
 	  	Rosparameters_get(n_c_priv);
 		/*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 +29,23 @@ 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);;
 		setpoint_counter = 0;
 		fcu_timeout = TIMEOUT;
-                home[0]=0.0;home[1]=0.0;home[2]=0.0;
+
+		while(cur_pos[2] == 0.0f){
+			ROS_INFO("Waiting for GPS. ");
+			ros::Duration(0.5).sleep();
+			ros::spinOnce();
+		}
+
+
+		if(xbeeplugged){
+			GetRobotId();
+		} else {
+			robot_id= strtol(robot_name.c_str() + 5, NULL, 10);
+		}
 	}
 
 	/*---------------------
@@ -47,8 +63,9 @@ namespace rosbzz_node{
 	void roscontroller::GetRobotId()
 	{
 
+		
 		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");
@@ -67,38 +84,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())
   			{
-      				/*Update neighbors position inside Buzz*/
+      			/*Update neighbors position inside Buzz*/
-     				buzz_utility::neighbour_pos_callback(neighbours_pos_map);
+     			//buzz_closure::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();
@@ -107,10 +125,10 @@ namespace rosbzz_node{
 				 else
 					fcu_timeout -= 1/10;
  				/*sleep for the mentioned loop rate*/
-    				timer_step+=1;
+    			timer_step+=1;
    				maintain_pos(timer_step);
 
-				
+   				std::cout<< "HOME: " << home[0] << ", " << home[1];
 			}
 			/* Destroy updater and Cleanup */
     			//update_routine(bcfname.c_str(), dbgfname.c_str(),1);
@@ -150,7 +168,10 @@ 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);
+
+		std::cout<< "////////////////// " << xbeesrv_name;
 
 		GetSubscriptionParameters(n_c);
 		// initialize topics to null?
@@ -218,8 +239,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);
@@ -257,28 +278,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 << "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
@@ -310,14 +338,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!");
 		}
 	}
 
@@ -333,7 +360,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));
@@ -357,7 +384,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;
@@ -393,7 +420,7 @@ namespace rosbzz_node{
 		    	payload_pub.publish(update_packets);
 		    	multi_msg=false;
 		    	delete[] payload_64;
-		}*/
+		}
 		
 	}
 	/*---------------------------------------------------------------------------------
@@ -424,12 +451,19 @@ namespace rosbzz_node{
 				break;
 			case mavros_msgs::CommandCode::NAV_TAKEOFF:
 				if(!armstate){
+
 					SetMode("LOITER", 0);
 					armstate = 1;
 					Arm();
 					ros::Duration(0.5).sleep();
-                                        // Registering HOME POINT.
+					// Registering HOME POINT.
-                                        home[0] = cur_pos[0];home[1] = cur_pos[1];home[2] = cur_pos[2];
+					if(home[0] == 0){
+						//test #1: set home only once -- ok
+						home[0] = cur_pos[0]; home[1] = cur_pos[1]; home[2] = cur_pos[2];
+						//test #2: set home mavros -- nope
+						//SetMavHomePosition(cur_pos[0], cur_pos[1], cur_pos[2]);
+
+					}
 				}
 				if(current_mode != "GUIDED")
 					SetMode("GUIDED", 2000); // for real solo, just add 2000ms delay (it should always be in loiter after arm/disarm)
@@ -490,7 +524,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));
 		}
 	/*-----------------------------------------------------------------------------------
@@ -499,7 +533,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));
 		}
 
@@ -509,18 +543,15 @@ namespace rosbzz_node{
 	void roscontroller::set_cur_pos(double latitude,
 			 double longitude,
 			 double altitude){
-		cur_pos [0] =latitude;
+		cur_pos [0] = latitude;
-		cur_pos [1] =longitude;
+		cur_pos [1] = longitude;
-		cur_pos [2] =altitude;
+		cur_pos [2] = altitude;
 	}
 
 	/*-----------------------------------------------------------
 	/ Compute Range and Bearing of a neighbor in a local reference frame
 	/ from GPS coordinates
 	----------------------------------------------------------- */
-	#define EARTH_RADIUS (double) 6371000.0
-	#define DEG2RAD(DEG) ((DEG)*((M_PI)/(180.0)))
-	
 	void ecef2ned_matrix(const double ref_ecef[3], double M[3][3]) {
             double hyp_az, hyp_el;
             double sin_el, cos_el, sin_az, cos_az;
@@ -557,95 +588,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;
 	}
 
@@ -709,7 +737,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);
                         }
 
                 }
@@ -743,12 +772,14 @@ namespace rosbzz_node{
 		moveMsg.header.frame_id = 1;
 		double local_pos[3];
 		cvt_ned_coordinates(cur_pos,local_pos,home);
-                ROS_INFO("[%i] ROSBuzz Home: %.7f, %.7f", robot_id, home[0], home[1]);
+       //         ROS_INFO("[%i] ROSBuzz Home: %.7f, %.7f", robot_id, home[0], home[1]);
-                ROS_INFO("[%i] ROSBuzz LocalPos: %.7f, %.7f", robot_id, local_pos[0], local_pos[1]);
+       //         ROS_INFO("[%i] ROSBuzz LocalPos: %.7f, %.7f", robot_id, local_pos[0], local_pos[1]);
                     
                 /*prepare the goto publish message ATTENTION: ENU FRAME FOR MAVROS STANDARD (then converted to NED)*/
-		moveMsg.pose.position.x = local_pos[1]+y;
+        target[0]+=y;
-		moveMsg.pose.position.y = local_pos[0]+x;
+		target[1]+=x;
+        moveMsg.pose.position.x = target[0];//local_pos[1]+y;
+		moveMsg.pose.position.y = target[1];
 		moveMsg.pose.position.z = z;
 
 		moveMsg.pose.orientation.x = 0;
@@ -757,7 +788,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!");
                 }
@@ -780,6 +811,7 @@ namespace rosbzz_node{
 		}
 	}
 
+
 	void roscontroller::SetStreamRate(int id, int rate, int on_off){
 		mavros_msgs::StreamRate message;
 		message.request.stream_id = id;
@@ -852,9 +884,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