vstig tables work

include/buzz_utility.h

@@ -21,6 +21,12 @@ struct pos_struct
   pos_struct(){}
 };
 
+#define function_register(TABLE, FNAME, FPOINTER)               \
+   buzzvm_push(VM, (TABLE));                                    \
+   buzzvm_pushs(VM, buzzvm_string_register(VM, (FNAME), 1));    \
+   buzzvm_pushcc(VM, buzzvm_function_register(VM, (FPOINTER))); \
+   buzzvm_tput(VM);
+
 typedef struct pos_struct Pos_struct ;
 
 uint16_t* u64_cvt_u16(uint64_t u64);
@@ -29,6 +35,12 @@ int buzz_listen(const char* type,
                        int msg_size);
 
 void neighbour_pos_callback(std::map< int,  Pos_struct> neighbours_pos_map);
+void update_neighbors();
+void add_user(int id, double latitude, double longitude, float altitude);
+void update_users();
+int make_table(buzzobj_t* t);
+int buzzusers_add(int id, double latitude, double longitude, double altitude);
+int buzzusers_reset();
 
 int buzz_update_users_stigmergy(buzzobj_t tbl);
 

src/buzz_utility.cpp

@@ -6,7 +6,7 @@
  *  @copyright 2016 MistLab. All rights reserved.
  */
 
-#include <buzz_utility.h>
+#include "buzz_utility.h"
 
 namespace buzz_utility{
 
@@ -19,38 +19,146 @@ namespace buzz_utility{
 	static buzzdebug_t  DBG_INFO        = 0;
 	static uint8_t      MSG_SIZE        = 50;   // Only 100 bytes of Buzz messages every step
 	static int          MAX_MSG_SIZE    = 10000; // Maximum Msg size for sending update packets 
-	static int 	    Robot_id        = 0;
+	static int 	    	Robot_id        = 0;
-        buzzobj_t           usersvstig;
+
-        buzzobj_t           key;
+	std::map< int,  Pos_struct> users_map;
+	std::map< int,  Pos_struct> neighbors_map;
 	
 	/****************************************/
 	/*adds neighbours position*/
 	void neighbour_pos_callback(std::map< int,  Pos_struct> neighbours_pos_map){
+		neighbors_map.clear();
+		map< int, Pos_struct >::iterator it;
+		for (it=neighbours_pos_map.begin(); it!=neighbours_pos_map.end(); ++it){
+			neighbors_map.insert(make_pair(it->first,it->second));
+		}
+	}
+
+	/* update at each step the VM table */
+	void update_neighbors(){
 		/* Reset neighbor information */
-    		buzzneighbors_reset(VM);
+    	buzzneighbors_reset(VM);
   		/* Get robot id and update neighbor information */
   	  	map< int, Pos_struct >::iterator it;
-    		for (it=neighbours_pos_map.begin(); it!=neighbours_pos_map.end(); ++it){
+		for (it=neighbors_map.begin(); it!=neighbors_map.end(); ++it){
-    			buzzneighbors_add(VM,
+			buzzneighbors_add(VM,
-                        		  it->first,
+								it->first,
-                        		  (it->second).x,
+								(it->second).x,
-                        		  (it->second).y,
+								(it->second).y,
-                        		  (it->second).z);
+								(it->second).z);
-    		}
+		}
+	}
+
+	void add_user(int id, double latitude, double longitude, float altitude)
+	{
+		Pos_struct pos_arr;
+		pos_arr.x=latitude;
+		pos_arr.y=longitude;
+		pos_arr.z=altitude;
+		map< int, buzz_utility::Pos_struct >::iterator it = users_map.find(id);
+		if(it!=users_map.end())
+			users_map.erase(it);
+		ROS_INFO("Buzz_utility got new user: %i", id);
+		users_map.insert(make_pair(id, pos_arr));
+	}
+
+	void update_users(){
+		/* Reset neighbor information */
+    	buzzusers_reset();
+  		/* Get robot id and update neighbor information */
+  	  	map< int, Pos_struct >::iterator it;
+		for (it=users_map.begin(); it!=users_map.end(); ++it){
+			buzzusers_add(it->first,
+								(it->second).x,
+								(it->second).y,
+								(it->second).z);
+		}
+	}
+
+	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, "p", 1));
+		//buzzvm_gload(VM);
+		buzzvm_push(VM, t);
+        buzzvm_pushi(VM, 2);
+		buzzvm_call(VM, 0);
+		//buzzvm_gstore(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, "p", 1));
+		buzzvm_gload(VM);
+        buzzvm_pushi(VM, 1);
+		buzzvm_call(VM, 0);
+		buzzvm_type_assert(VM, 1, BUZZTYPE_TABLE);
+		buzzobj_t nbr = buzzvm_stack_at(VM, 1);
+		/* Get "data" field */
+		buzzvm_pushs(VM, buzzvm_string_register(VM, "data", 1));
+		buzzvm_tget(VM);
+		if(buzzvm_stack_at(VM, 1)->o.type == BUZZTYPE_NIL) {
+			/* Empty data, create a new table */
+			buzzvm_pop(VM);
+			buzzvm_push(VM, nbr);
+			buzzvm_pushs(VM, buzzvm_string_register(VM, "data", 1));
+			buzzvm_pusht(VM);
+			buzzobj_t data = buzzvm_stack_at(VM, 1);
+			buzzvm_tput(VM);
+			buzzvm_push(VM, data);
+		}
+		/* When we get here, the "data" table is on top of the stack */
+		/* Push user id */
+		buzzvm_pushi(VM, id);
+		/* Create entry table */
+		buzzobj_t entry = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE);
+		/* Insert latitude */
+		buzzvm_push(VM, entry);
+		buzzvm_pushs(VM, buzzvm_string_register(VM, "la", 1));
+		buzzvm_pushf(VM, latitude);
+		buzzvm_tput(VM);
+		/* Insert longitude */
+		buzzvm_push(VM, entry);
+		buzzvm_pushs(VM, buzzvm_string_register(VM, "lo", 1));
+		buzzvm_pushf(VM, longitude);
+		buzzvm_tput(VM);
+		/* Insert altitude */
+		buzzvm_push(VM, entry);
+		buzzvm_pushs(VM, buzzvm_string_register(VM, "al", 1));
+		buzzvm_pushf(VM, altitude);
+		buzzvm_tput(VM);
+		/* Save entry into data table */
+		buzzvm_push(VM, entry);
+		buzzvm_tput(VM);
+		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() {
@@ -189,15 +297,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 +320,8 @@ namespace buzz_utility{
    		buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_land", 1));
    		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_land));
    		buzzvm_gstore(VM);
-   	return BUZZVM_STATE_READY;
+
+   	return VM->state;
 	}
 
 	/**************************************************/
@@ -224,15 +333,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,17 +356,59 @@ namespace buzz_utility{
    		buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_land", 1));
    		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
    		buzzvm_gstore(VM);
-   	return BUZZVM_STATE_READY;
+
+   	return VM->state;
 	}
 
+static int create_stig_tables() {
 
+   		// usersvstig = stigmergy.create(123)
+        buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
+        //buzzvm_gstore(VM);
+        // 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_closure_call(VM, 1);
+		buzzvm_pushi(VM, 1);
+		buzzvm_call(VM, 0);
+        buzzvm_gstore(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, "p", 1));
+        buzzvm_pusht(VM);
+        buzzvm_pushi(VM, 2);
+		buzzvm_call(VM, 0);
+        //buzzvm_gstore(VM);*/
+		buzzusers_reset();
+
+		/*buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
+        buzzvm_gload(VM);
+        buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1));
+        buzzvm_tget(VM);
+        buzzvm_pushs(VM, buzzvm_string_register(VM, "u", 1));
+        buzzvm_pusht(VM);
+        buzzvm_pushi(VM, 2);
+		buzzvm_call(VM, 0);
+        buzzvm_gstore(VM);*/
+
+   	return VM->state;
+}
 	/****************************************/
 	/*Sets the .bzz and .bdbg file*/
 	/****************************************/
 
 	int buzz_script_set(const char* bo_filename,
 	                    const char* bdbg_filename, int robot_id) {
-   	cout << " Robot ID: " <<robot_id<< endl;
+   	ROS_INFO(" Robot ID: %i", robot_id);
    	/* Reset the Buzz VM */
    	if(VM) buzzvm_destroy(&VM);
 	Robot_id = robot_id;
@@ -294,39 +445,25 @@ namespace buzz_utility{
    	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;
    	}
 
-   	
+	if(create_stig_tables() != BUZZVM_STATE_READY) {
-buzzvm_dup(VM);
+      		buzzvm_destroy(&VM);
-   	// usersvstig = stigmergy.create(123)
+      		buzzdebug_destroy(&DBG_INFO);
-        buzzvm_pushs(VM, buzzvm_string_register(VM, "v", 1));
+      		ROS_ERROR("[%i] Error creating stigmergy tables", Robot_id);
-        // value of the stigmergy id
+			//cout << "ERROR!!!!   ----------  " << VM->errormsg << endl;
-        buzzvm_pushi(VM, 5);
+			//cout << "ERROR!!!!   ----------  " << buzzvm_strerror(VM) << endl;
-        // get the stigmergy table from the global scope
+      		return 0;
-//        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,24 +473,47 @@ 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)
+	/*Update users positions                */
+	/****************************************/
+
+    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);
+            buzzvm_push(VM, peoplevstig);
             // 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);
+            buzzvm_closure_call(VM, 2);*/
+
+
+			// put something in it....
+        //buzzvm_push(VM, peoplevstig);
+        buzzvm_pushs(VM, buzzvm_string_register(VM, "p", 1));
+        buzzvm_gload(VM);
+        buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1));
+        buzzvm_tget(VM);
+        //buzzvm_pushs(VM, buzzvm_string_register(VM, "id", 1));
+        buzzvm_pushi(VM, Robot_id);
+        //buzzvm_gload(VM);
+        buzzvm_push(VM, tbl);
+        buzzvm_pushi(VM, 2);
+		buzzvm_call(VM, 0);
+        //buzzvm_closure_call(VM, 2);
+        //buzzvm_gstore(VM);
             return 1;
         }
+
 	/****************************************/
 	/*Sets a new update                     */
 	/****************************************/
@@ -489,10 +649,12 @@ buzzvm_dup(VM);
 	   buzzuav_closures::buzzuav_update_battery(VM);
 	   buzzuav_closures::buzzuav_update_prox(VM);
 	   buzzuav_closures::buzzuav_update_currentpos(VM);
-	   buzzobj_t tbl = buzzuav_closures::buzzuav_update_userspos(VM);
+	   update_neighbors();
+	   update_users();
+	   buzzuav_closures::buzzuav_update_userspos(VM);
 	   buzzuav_closures::buzzuav_update_flight_status(VM);
-           
+	   
-           //buzz_update_users_stigmergy(tbl);
+	   //buzz_update_users_stigmergy(tbl);
            
            
 	    /*

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
@@ -246,6 +246,14 @@ namespace buzzuav_closures{
 	   cur_pos[2]=altitude;
 	}
 	void set_userspos(double latitude, double longitude, double altitude){
+		/*buzz_utility::Pos_struct pos_arr;
+		pos_arr.x=latitude;
+		pos_arr.y=longitude;
+		pos_arr.z=altitude;
+		map< int, buzz_utility::Pos_struct >::iterator it = users_map.find(id);
+		if(it!=users_map.end())
+			users_map.erase(it);
+		users_map.insert(make_pair(id, pos_arr));*/
 	   users_pos[0]=latitude;
 	   users_pos[1]=longitude;
 	   users_pos[2]=altitude;
@@ -270,6 +278,9 @@ namespace buzzuav_closures{
 	   return vm->state;
 	}
 	buzzobj_t buzzuav_update_userspos(buzzvm_t vm) {
+		/*map< int, buzz_utility::Pos_struct >::iterator it;
+		for (it=users_map.begin(); it!=users_map.end(); ++it){
+		}*/
 	   buzzvm_pushs(vm, buzzvm_string_register(vm, "users", 1));
 	   buzzvm_pusht(vm);
 	   buzzvm_dup(vm);
@@ -284,9 +295,10 @@ namespace buzzuav_closures{
 	   buzzvm_pushs(vm, buzzvm_string_register(vm, "height", 1));
 	   buzzvm_pushf(vm, users_pos[2]);
 	   buzzvm_tput(vm);
+	   buzzobj_t tbl = buzzvm_stack_at(vm, 0);
 	   buzzvm_gstore(vm);
            
-           return buzzvm_stack_at(vm, 0);
+           return tbl;
 	   //return vm->state;
 	}
 

src/roscontroller.cpp

@@ -29,7 +29,8 @@ namespace rosbzz_node{
 			robot_id=strtol(robot_name.c_str() + 5, NULL, 10);;
 		setpoint_counter = 0;
 		fcu_timeout = TIMEOUT;
-                home[0]=0.0;home[1]=0.0;home[2]=0.0;
+        
+		home[0]=0.0;home[1]=0.0;home[2]=0.0;
 	}
 
 	/*---------------------
@@ -48,7 +49,7 @@ namespace rosbzz_node{
 	{
 
 		mavros_msgs::ParamGet::Request robot_id_srv_request; robot_id_srv_request.param_id="id";
-        	mavros_msgs::ParamGet::Response robot_id_srv_response;
+        mavros_msgs::ParamGet::Response robot_id_srv_response;
 		while(!xbeestatus_srv.call(robot_id_srv_request,robot_id_srv_response)){
 			ros::Duration(0.1).sleep();
 			ROS_ERROR("Waiting for Xbee to respond to get device ID");
@@ -73,14 +74,14 @@ namespace rosbzz_node{
                         //////////////////////////////////////////////////////
 			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_utility::neighbour_pos_callback(neighbours_pos_map);
 				/*Neighbours of the robot published with id in respective topic*/
 				neighbours_pos_publisher();
 				/*Check updater state and step code*/
   				//update_routine(bcfname.c_str(), dbgfname.c_str());
 				/*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*/
@@ -97,8 +98,8 @@ namespace rosbzz_node{
 				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 +108,9 @@ namespace rosbzz_node{
 				 else
 					fcu_timeout -= 1/10;
  				/*sleep for the mentioned loop rate*/
-    				timer_step+=1;
+    			timer_step+=1;
    				maintain_pos(timer_step);
-				
+								
-				
 			}
 			/* Destroy updater and Cleanup */
     			//update_routine(bcfname.c_str(), dbgfname.c_str(),1);
@@ -218,8 +218,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);
@@ -261,7 +261,7 @@ namespace rosbzz_node{
 		/*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<<"/";
 		path = bzzfile_in_compile.str();
 		bzzfile_in_compile.str("");
@@ -270,7 +270,7 @@ namespace rosbzz_node{
 		bzzfile_in_compile << "bzzparse "<<bzzfile_name<<" "<<path<< name<<".basm";
    		system(bzzfile_in_compile.str().c_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());
 		bzzfile_in_compile.str("");
 		bzzfile_in_compile <<path<<name<<".bo";
@@ -490,7 +490,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 +499,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));
 		}
 
@@ -709,7 +709,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);
                         }
 
                 }
@@ -757,7 +758,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!");
                 }

src/testflocksim.bzz

@@ -141,6 +141,12 @@ function land() {
 	}
 }
 
+function table_print(t) {
+  foreach(t, function(key, value) {
+      log(key, " -> ", value)
+    })
+}
+
 # Executed once at init time.
 function init() {
 	s = swarm.create(1)
@@ -199,9 +205,13 @@ neighbors.listen("cmd",
 	log("User position: ", users.range)
 	
 	# Read a value from the structure
-        #x = usersvstig.get(46)
+        t = vt.get("p")
+				log(t)
+				#table_print(t)        
+				#t = vt.get("u")
+				#table_print(t)
         # Get the number of keys in the structure
-        #log("The vstig has ", usersvstig.size(), " elements")
+        log("The vstig has ", vt.size(), " elements")
 }
  
 # Executed once when the robot (or the simulator) is reset.