compute user rb

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

script/testflocksim.bzz

@@ -141,15 +141,24 @@ function land() {
 	}
 }
 
-function users_print(t) {
+function users_save(t) {
 	if(size(t)>0) {
 		foreach(t, function(id, tab) {
-				log("id: ",id," Latitude ", tab.la, "Longitude ", tab.lo)
+				#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(key, value) {
+				log(key, " -> ", value)
+			})
+	}
+}
+
 ########################################
 #
 # MAIN FUNCTIONS
@@ -216,15 +225,14 @@ neighbors.listen("cmd",
 	
 	# Read a value from the structure
 				log(users)      
-				if(size(users)>0){
+				#users_print(users.dataG)
-					#users_print(users.dataG)
+				if(size(users.dataG)>0)
-					if(size(users.dataG)>0)
+					vt.put("p", users.dataG)
-						vt.put("p", users.dataG)
-				}
 				
         # Get the number of keys in the structure
         log("The vstig has ", vt.size(), " elements")
-				users_print(vt.get("p"))
+				users_save(vt.get("p"))
+				table_print(users.dataL)
 }
  
 # Executed once when the robot (or the simulator) is reset.

src/buzz_utility.cpp

@@ -57,61 +57,6 @@ namespace buzz_utility{
 			}
 		}else
 			ROS_INFO("[%i] No new users",Robot_id);
-
-		//compute_users_rb();
-	}
-
-	int compute_users_rb() {
-		if(VM->state != BUZZVM_STATE_READY) return VM->state;
-		/* Get users "userG" stigmergy 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_pushi(VM, 1);
-		buzzvm_callc(VM);
-		buzzvm_type_assert(VM, 1, BUZZTYPE_TABLE);
-		buzzobj_t nbr = buzzvm_stack_at(VM, 1);
-		buzzvm_pushs(VM, buzzvm_string_register(VM, "al", 1));
-		buzzvm_tget(VM);
-		buzzvm_type_assert(VM, 1, BUZZTYPE_INT);
-		int gid = buzzvm_stack_at(VM, 1)->i.value;
-		ROS_WARN("GOT ID %i FROM V.STIG", gid);
-		/* Get "data" field */
-		buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
-		buzzvm_gload(VM);
-		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, gid);
-		/* 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, 0);
-		buzzvm_tput(VM);
-		/* Insert bearing */
-		buzzvm_push(VM, entry);
-		buzzvm_pushs(VM, buzzvm_string_register(VM, "b", 1));
-		buzzvm_pushf(VM, 0);
-		buzzvm_tput(VM);
-		/* Save entry into data table */
-		buzzvm_push(VM, entry);
-		buzzvm_tput(VM);
-		return VM->state;
 	}
 
 	int buzzusers_reset() {
@@ -416,7 +361,7 @@ namespace buzz_utility{
 	}
 
 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
@@ -461,6 +406,24 @@ static int create_stig_tables() {
 		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, "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;
 }
 	/****************************************/
@@ -515,7 +478,7 @@ static int create_stig_tables() {
       		ROS_ERROR("[%i] Error registering hooks", Robot_id);
       		return 0;
    	}
-   	/* Create vstig tables 
+   	/* Create vstig tables */
 	if(create_stig_tables() != BUZZVM_STATE_READY) {
       		buzzvm_destroy(&VM);
       		buzzdebug_destroy(&DBG_INFO);
@@ -523,12 +486,7 @@ static int create_stig_tables() {
 			//cout << "ERROR!!!!   ----------  " << VM->errormsg << endl;
 			//cout << "ERROR!!!!   ----------  " << buzzvm_strerror(VM) << endl;
       		return 0;
-   	}*/
+   	}
-
-	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);
         
    	/* Save bytecode file name */
    	BO_FNAME = strdup(bo_filename);

src/buzzuav_closures.cpp

@@ -77,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;
@@ -88,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};
@@ -119,6 +129,47 @@ namespace buzzuav_closures{
 	   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 */
@@ -127,13 +178,18 @@ namespace buzzuav_closures{
 	   buzzvm_type_assert(vm, 3, BUZZTYPE_INT);
 	   buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
 	   buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
-	   float lon = buzzvm_stack_at(vm, 1)->f.value;
+	   double tmp[3];
-	   float lat = buzzvm_stack_at(vm, 2)->f.value;
+	   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];
 
-	   printf("\tGot new user from bzz stig: %i - %f, %f\n", uid, lat, lon);
+	   rb_from_gps(tmp, rb, cur_pos);
 
-	   return buzzvm_ret0(vm);
+	   //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]);
 	}
 
 	/*----------------------------------------/

src/roscontroller.cpp

@@ -528,9 +528,6 @@ namespace rosbzz_node{
 	/ 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;