added batt/state of all neighbor in webcontrol, enhance serialization for vstig

buzz_scripts/include/act/states.bzz

@@ -13,6 +13,7 @@ include "act/neighborcomm.bzz"
 TARGET_ALTITUDE = 15.0 # m.
 BVMSTATE = "TURNEDOFF"
 PICTURE_WAIT = 20 # steps
+WP_STIG = 8
 
 path_it = 0
 pic_time = 0
@@ -100,7 +101,7 @@ firsttimeinwp = 1
 wpreached = 1
 function indiWP() {
 	if(firsttimeinwp) {
-		v_wp = stigmergy.create(8)
+		v_wp = stigmergy.create(WP_STIG)
 		storegoal(pose.position.latitude, pose.position.longitude, pose.position.altitude)
 		firsttimeinwp = 0
 	}
@@ -111,21 +112,27 @@ function indiWP() {
 			storegoal(rc_goto.latitude, rc_goto.longitude, pose.position.altitude)
 			return
 		} else {
-			v_wp.put(rc_goto.id,{.lat=rc_goto.latitude, .lon=rc_goto.longitude, .pro=0})
+			var ls = packWP2i(rc_goto.latitude, rc_goto.longitude, 0)
+			v_wp.put(rc_goto.id,ls)
 			reset_rc()
 		}
 	}
 
-  wp = v_wp.get(id)
+  #if(vstig_buzzy == 0) {
-	if(wp!=nil) {
+		wpi = v_wp.get(id)
-		#log(wp.pro,wpreached)
+		if(wpi!=nil) {
-		if(wp.pro == 0) {
+			#log(wp.pro,wpreached)
-				wpreached = 0
+			wp = unpackWP2i(wpi)
-        storegoal(wp.lat, wp.lon, pose.position.altitude)
+			table_print(wp)
-				v_wp.put(id,{.lat=wp.lat, .lon=wp.lon, .pro=1})
+			if(wp.pro == 0) {
-				return
+					wpreached = 0
-			}      
+					storegoal(wp.lat, wp.lon, pose.position.altitude)
-	}
+					var ls = packWP2i(wp.lat, wp.lon, 1)
+					v_wp.put(id,ls)
+					return
+				}      
+		}
+	#}
 
 	if(wpreached!=1)
 		goto_gps(indiWP_done)

buzz_scripts/include/plan/mapmatrix.bzz

@@ -102,21 +102,6 @@ function is_in(dictionary, x, y){
   return 0
 }
 
-
-function table_print(t) {
-  foreach(t, function(key, value) {
-      log(key, " -> ", value)
-    })
-}
-
-function table_copy(t) {
-  var t2 = {}
-  foreach(t, function(key, value) {
-      t2[key] = value
-    })
-  return t2
-}
-
 function print_pos(t) {
   var ir=1
   while(ir <= size(t)) {

buzz_scripts/include/taskallocate/graphformGPS.bzz

@@ -6,6 +6,7 @@ include "taskallocate/graphs/shapes_P.bzz"
 include "taskallocate/graphs/shapes_O.bzz"
 include "taskallocate/graphs/shapes_L.bzz"
 include "taskallocate/graphs/shapes_Y.bzz"
+#include "utils/table.bzz"
 
 ROBOT_RADIUS = 50
 ROBOT_DIAMETER = 2.0*ROBOT_RADIUS
@@ -82,60 +83,6 @@ m_lockstig = 1
 # Lennard-Jones parameters, may need change
 EPSILON_GRAPH	= 4000 #13.5 the LJ parameter for other robots
 
-#
-#return the number of value in table
-#
-function count(table,value){
-	var number=0
-	var i=0
-	while(i<size(table)){
-		if(table[i]==value){
-			number=number+1
-		}
-		i=i+1
-	}
-	return number
-}
-#
-#map from int to state
-#
-function i2s(value){
-	if(value==1){
-	return "GRAPH_FREE"
-	}
-	else if(value==2){
-	return "GRAPH_ASKING"
-	}
-	else if(value==3){
-	return "GRAPH_JOINING"
-	}
-	else if(value==4){
-	return "GRAPH_JOINED"
-	}
-	else if(value==5){
-	return "GRAPH_LOCK"
-	}
-}
-#
-#map from state to int
-#
-function s2i(value){
-	if(value=="GRAPH_FREE"){
-	return 1
-	}
-	else if(value=="GRAPH_ASKING"){
-	return 2
-	}
-	else if(value=="GRAPH_JOINING"){
-	return 3
-	}
-	else if(value=="GRAPH_JOINED"){
-	return 4
-	}
-	else if(value=="GRAPH_LOCK"){
-	return 5
-	}
-}
 #
 #map form int to response
 #

buzz_scripts/include/utils/conversions.bzz

@@ -61,3 +61,19 @@ function gps_from_vec(vec) {
 
   return Lgoal
 }
+
+GPSoffset = {.lat=45.50, .lon=-73.62}
+
+function packWP2i(in_lat, in_long, processed) {
+  var dlat = math.round((in_lat - GPSoffset.lat)*1000000)
+  var dlon = math.round((in_long - GPSoffset.lon)*1000000)
+  return dlat*100000+dlon*10+processed
+}
+
+function unpackWP2i(wp_int){
+  dlat = (wp_int-wp_int%100000)/100000
+  wp_int=wp_int-dlat*100000
+  dlon = (wp_int-wp_int%10)/10
+  wp_int=wp_int-dlon*10
+  return {.lat=dlat/1000000.0+GPSoffset.lat, .lon=dlon/1000000.0+GPSoffset.lon, .pro=wp_int}
+}

buzz_scripts/include/utils/table.bzz

@@ -0,0 +1,115 @@
+function table_print(t) {
+  foreach(t, function(key, value) {
+      log(key, " -> ", value)
+    })
+}
+
+function table_copy(t) {
+  var t2 = {}
+  foreach(t, function(key, value) {
+      t2[key] = value
+    })
+  return t2
+}
+
+#
+#return the number of value in table
+#
+function count(table,value){
+	var number=0
+	var i=0
+	while(i<size(table)){
+		if(table[i]==value){
+			number=number+1
+		}
+		i=i+1
+	}
+	return number
+}
+#
+#map from int to state
+#
+function i2s(value){
+	if(value==1){
+	return "GRAPH_FREE"
+	}
+	else if(value==2){
+	return "GRAPH_ASKING"
+	}
+	else if(value==3){
+	return "GRAPH_JOINING"
+	}
+	else if(value==4){
+	return "GRAPH_JOINED"
+	}
+	else if(value==5){
+	return "GRAPH_LOCK"
+	}
+	else if(value==6){
+	return "TURNEDOFF"
+	}
+	else if(value==7){
+	return "LAND"
+	}
+	else if(value==8){
+	return "BARRIERWAIT"
+	}
+  else if(value==9){
+	return "INDIWP"
+	}
+  else if(value==10){
+	return "TASK_ALLOCATE"
+	}
+  else if(value==11){
+	return "LAUNCH"
+	}
+  else if(value==12){
+	return "STOP"
+	}
+  else {
+	return "UNDETERMINED"
+	}
+}
+#
+#map from state to int
+#
+function s2i(value){
+	if(value=="GRAPH_FREE"){
+	return 1
+	}
+	else if(value=="GRAPH_ASKING"){
+	return 2
+	}
+	else if(value=="GRAPH_JOINING"){
+	return 3
+	}
+	else if(value=="GRAPH_JOINED"){
+	return 4
+	}
+	else if(value=="GRAPH_LOCK"){
+	return 5
+	}
+	else if(value=="TURNEDOFF"){
+	return 6
+	}
+	else if(value=="LAND"){
+	return 7
+	}
+	else if(value=="BARRIERWAIT"){
+	return 8
+	}
+	else if(value=="INDIWP"){
+	return 9
+	}
+	else if(value=="TASK_ALLOCATE"){
+	return 10
+	}
+	else if(value=="LAUNCH"){
+	return 11
+	}
+	else if(value=="STOP"){
+	return 12
+	}
+  else
+    return 0
+}

buzz_scripts/include/vstigenv.bzz

@@ -8,15 +8,14 @@
 #
 STATUS_VSTIG = 20
 GROUND_VSTIG = 21
-HIGHEST_ROBOTID = 14
+WAIT4STEP = 5
-WAIT4STEP = 10
+vstig_buzzy = 0
 
 #
 # Init var
 #
 var v_status = {}
 var v_ground = {}
-b_updating = 0
 vstig_counter = WAIT4STEP
 
 
@@ -27,36 +26,36 @@ function init_swarm() {
 
 function init_stig() {
 	v_status = stigmergy.create(STATUS_VSTIG)
-	v_ground = stigmergy.create(GROUND_VSTIG)
+	#v_ground = stigmergy.create(GROUND_VSTIG)
 }
 
 function uav_updatestig() {
-	# TODO: Push values on update only.
+	# TODO: Push values on update only?
 	if(vstig_counter<=0) {
-		b_updating=1
+		vstig_buzzy = 1
-		#var ls={.1=0,.2=battery.capacity,.3=xbee_status.rssi,.4=flight.status}
+		var ls = battery.capacity*10 + s2i(BVMSTATE)
-		ls = 50*1000000 + battery.capacity*1000 + xbee_status.rssi*10 + flight.status
+	    #log("Pushing ", ls, "on vstig with id:", id)
-	    log("Pushing ", ls, "on vstig with id:", id)
 	    v_status.put(id, ls)
 		vstig_counter=WAIT4STEP
+	} else if(vstig_counter==WAIT4STEP-1){ # ensure comm. delay between fetch and update stig
+		vstig_buzzy = 1
+		vstig_counter=vstig_counter-1
+		stattab_send()
 	} else {
-		b_updating=0
+		vstig_buzzy = 0
 		vstig_counter=vstig_counter-1
 	}
 }
 
-function unpackstatus(recv_value){
+function unpackstatus(recv_value,state_struct){
-    gps=(recv_value-recv_value%1000000)/1000000
+    #state_struct.gp=(recv_value-recv_value%1000000)/1000000
-    recv_value=recv_value-gps*1000000
+    #recv_value=recv_value-state_struct.gp*1000000
-    batt=(recv_value-recv_value%1000)/1000
+    state_struct.ba=(recv_value-recv_value%10)/10
-    recv_value=recv_value-batt*1000
+    recv_value=recv_value-state_struct.ba*10
-    xbee=(recv_value-recv_value%10)/10
+    #state_struct.xb=(recv_value-recv_value%10)/10
-    recv_value=recv_value-xbee*10
+    #recv_value=recv_value-state_struct.xb*10
-    fc=recv_value
+    state_struct.st=recv_value
-	log("- GPS ", gps)
+	return state_struct
-	log("- Battery ", batt)
-	log("- Xbee ", xbee)
-	log("- Status ", fc)
 }
 	
 function checkusers() {
@@ -95,37 +94,29 @@ function usertab_print(t) {
 
 function stattab_print() {
 	if(v_status.size()>0) {
-		if(b_updating==0) {
+		var state_struct = {.id = -1, .gp = -1, .ba = -1, .xb = -1, .st = -1}
-			u=0
+		neighbors.foreach(function(rid, data) {
-			while(u<HIGHEST_ROBOTID){
+			var nei_state = v_status.get(rid)
-				tab = v_status.get(u)
+			if(nei_state!=nil){
-				if(tab!=nil)
+					state_struct.id=u
-					unpackstatus(tab)
+					state_struct = unpackstatus(nei_state, state_struct)
+					table_print(state_struct)
+				}					
 				u=u+1
-			}
+			} )
-		}
 	}
 }
 
 function stattab_send() {
 	if(v_status.size()>0) {
-		if(b_updating==0) {
+			var state_struct = {.id = -1, .gp = -1, .ba = -1, .xb = -1, .st = 0}
-			u=0
+			neighbors.foreach(function(rid, data) {
-			while(u<HIGHEST_ROBOTID){
+				var nei_state = v_status.get(rid)
-				tab = v_status.get(u)
+				if(nei_state!=nil){
-				if(tab!=nil){
+					state_struct.id=rid
-					recv_value=tab
+					state_struct = unpackstatus(nei_state,state_struct)
-					gps=(recv_value-recv_value%1000000)/1000000
+					add_neighborStatus(state_struct)
-					recv_value=recv_value-gps*1000000
-					batt=(recv_value-recv_value%1000)/1000
-					recv_value=recv_value-batt*1000
-					xbee=(recv_value-recv_value%10)/10
-					recv_value=recv_value-xbee*10
-					fc=recv_value
-					add_neighborStatus(u,gps,batt,xbee,fc)
 				}
-				u=u+1
+			} )
-			}
-		}
 	}
 }

buzz_scripts/main.bzz

@@ -2,6 +2,7 @@ include "update.bzz"
 # don't use a stigmergy id=11 with this header, for barrier 
 # it requires an 'action' function to be defined here.
 include "act/states.bzz"
+include "utils/table.bzz"
 include "plan/rrtstar.bzz"
 include "taskallocate/graphformGPS.bzz"
 include "taskallocate/bidding.bzz"
@@ -27,9 +28,6 @@ LAND_AFTER_BARRIER_EXPIRE = 1 # if set to be 1 , the robots will land after barr
 # 3 -> indoor wheeled vehicle
 V_TYPE = 0
 
-goal_list = {
-	.0={.x = 45.5088103899, .y = -73.1540826153, .z = 2.5}
-}
 
 # Executed once at init time.
 function init() {
@@ -53,7 +51,7 @@ function step() {
 	rc_cmd_listen()
 
 	# update the vstig (status/net/batt/...)
-  	# uav_updatestig()
+  	#uav_updatestig()
 
 	#
 	# State machine

src/buzz_utility.cpp

@@ -291,6 +291,12 @@ static int testing_buzz_register_hooks()
   buzzvm_pushs(VM, buzzvm_string_register(VM, "add_neighborStatus", 1));
   buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
   buzzvm_gstore(VM);
+  buzzvm_pushs(VM, buzzvm_string_register(VM, "export_map", 1));
+  buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
+  buzzvm_gstore(VM);
+  buzzvm_pushs(VM, buzzvm_string_register(VM, "reset_rc", 1));
+  buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
+  buzzvm_gstore(VM);
 
   return VM->state;
 }

src/buzzuav_closures.cpp

@@ -54,7 +54,7 @@ int buzzros_print(buzzvm_t vm)
 ----------------------------------------------------------- */
 {
   std::ostringstream buffer(std::ostringstream::ate);
-  buffer << "[" << buzz_utility::get_robotid() << "] ";
+  buffer << std::fixed << std::setprecision(6) << "[" << buzz_utility::get_robotid() << "] ";
   for (uint32_t index = 1; index < buzzdarray_size(vm->lsyms->syms); ++index)
   {
     buzzvm_lload(vm, index);
@@ -288,23 +288,42 @@ int buzzuav_addNeiStatus(buzzvm_t vm)
 / closure to add neighbors status to the BVM
 /----------------------------------------*/
 {
-  buzzvm_lnum_assert(vm, 5);
+  buzzvm_lnum_assert(vm, 1);
-  buzzvm_lload(vm, 1);  // fc
+  buzzvm_lload(vm, 1);  // state table
-  buzzvm_lload(vm, 2);  // xbee
+  buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE);
-  buzzvm_lload(vm, 3);  // batt
+  buzzobj_t t = buzzvm_stack_at(vm, 1);
-  buzzvm_lload(vm, 4);  // gps
+  if(buzzdict_size(t->t.value) != 5) {
-  buzzvm_lload(vm, 5);  // id
+    ROS_WARN("Wrong neighbor status size.");
-  buzzvm_type_assert(vm, 5, BUZZTYPE_INT);
+    return vm->state;
-  buzzvm_type_assert(vm, 4, BUZZTYPE_INT);
+  }
-  buzzvm_type_assert(vm, 3, BUZZTYPE_INT);
+
-  buzzvm_type_assert(vm, 2, BUZZTYPE_INT);
-  buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
   buzz_utility::neighbors_status newRS;
-  uint8_t id = buzzvm_stack_at(vm, 5)->i.value;
+  buzzvm_dup(vm);
-  newRS.gps_strenght = buzzvm_stack_at(vm, 4)->i.value;
+  buzzvm_pushs(vm, buzzvm_string_register(vm, "id", 1));
-  newRS.batt_lvl = buzzvm_stack_at(vm, 3)->i.value;
+  buzzvm_tget(vm);
-  newRS.xbee = buzzvm_stack_at(vm, 2)->i.value;
+  uint8_t id = buzzvm_stack_at(vm, 1)->i.value;
+  buzzvm_pop(vm);
+  buzzvm_dup(vm);
+  buzzvm_pushs(vm, buzzvm_string_register(vm, "ba", 1));
+  buzzvm_tget(vm);
+  newRS.batt_lvl = buzzvm_stack_at(vm, 1)->i.value;
+  buzzvm_pop(vm);
+  buzzvm_dup(vm);
+  buzzvm_pushs(vm, buzzvm_string_register(vm, "gp", 1));
+  buzzvm_tget(vm);
+  newRS.gps_strenght = buzzvm_stack_at(vm, 1)->i.value;
+  buzzvm_pop(vm);
+  buzzvm_dup(vm);
+  buzzvm_pushs(vm, buzzvm_string_register(vm, "xb", 1));
+  buzzvm_tget(vm);
+  newRS.xbee = buzzvm_stack_at(vm, 1)->i.value;
+  buzzvm_pop(vm);
+  buzzvm_dup(vm);
+  buzzvm_pushs(vm, buzzvm_string_register(vm, "st", 1));
+  buzzvm_tget(vm);
   newRS.flight_status = buzzvm_stack_at(vm, 1)->i.value;
+  buzzvm_pop(vm);
+  
   map<int, buzz_utility::neighbors_status>::iterator it = neighbors_status_map.find(id);
   if (it != neighbors_status_map.end())
     neighbors_status_map.erase(it);
@@ -417,8 +436,10 @@ void set_gpsgoal(double goal[3])
   rb_from_gps(goal, rb, cur_pos);
   if (fabs(rb[0]) < 150.0) {
     goto_gpsgoal[0] = goal[0];goto_gpsgoal[1] = goal[1];goto_gpsgoal[2] = goal[2];
-    ROS_INFO("Set GPS GOAL TO ---- %f %f %f (%f %f, %f %f)", goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]);
+    ROS_INFO("[%i] Set GPS GOAL TO ---- %f %f %f (%f %f, %f %f)", buzz_utility::get_robotid(), goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]);
-  }
+  } else
+    ROS_WARN("[%i] GPS GOAL TOO FAR !!-- %f %f %f (%f %f, %f %f)", buzz_utility::get_robotid(), goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]);
+
 }
 
 int buzzuav_arm(buzzvm_t vm)