Merge commit '02f76b8f67eab43d655ac008df19f681410a6238' into ros_drones_ws

buzz_scripts/include/act/barrier.bzz

@@ -63,14 +63,14 @@ function barrier_wait(threshold, transf, resumef, bc) {
   barrier.put(id, bc)
   barrier.get(id)
   log("--->BS: ", barrier.size(), " / ", threshold, " (", BARRIER_VSTIG, " - ", barrier.get("d"), ") t= ", timeW)
-  if(barrier.size()-1 >= threshold or barrier.get("d") == 1) {
+  #if(barrier.size()-1 >= threshold or barrier.get("d") == 1) {
     if(barrier_allgood(barrier,bc)) {
       barrier.put("d", 1)
       timeW = 0
       BVMSTATE = transf
     } else 
       barrier.put("d", 0)
-  }
+  #}
 
   if(timeW >= BARRIER_TIMEOUT) {
     log("------> Barrier Timeout !!!!")
@@ -86,13 +86,13 @@ function barrier_wait(threshold, transf, resumef, bc) {
 # Check all members state
 function barrier_allgood(barrier, bc) {
   barriergood = 1
-  barrier.foreach(
+  barrier.foreach(function(key, value, robot){
-    function(key, value, robot){
+      log("VS entry : ", key, " ", value, " ", robot)
-      #log("VS entry : ", key, " ", value, " ", robot)
+      if(key == "d"){
-      if(value != bc and key != "d"){
+        if(value == 1)
+          return 1
+      } else if(value != bc)
         barriergood = 0
-      }
+    })
-    }
-  )
   return barriergood
 }

buzz_scripts/include/act/neighborcomm.bzz

@@ -50,15 +50,22 @@ function rc_cmd_listen() {
 	} else if (flight.rc_cmd==902){
 		flight.rc_cmd=0
 		destroyGraph()
-		barrier_set(ROBOTS, "AGGREGATE", BVMSTATE, 902)
+		barrier_set(ROBOTS, "WAYPOINT", BVMSTATE, 902)
 		barrier_ready(902)
 		neighbors.broadcast("cmd", 902)
 	} else if (flight.rc_cmd==903){
 		flight.rc_cmd=0
 		destroyGraph()
-		barrier_set(ROBOTS, "FORMATION", BVMSTATE, 903)
+		resetWP()
+		barrier_set(ROBOTS, "POTENTIAL", BVMSTATE, 903)
 		barrier_ready(903)
 		neighbors.broadcast("cmd", 903)
+	} else if (flight.rc_cmd==904){
+		flight.rc_cmd=0
+		destroyGraph()
+		barrier_set(ROBOTS, "IDLE", BVMSTATE, 904)
+		barrier_ready(904)
+		neighbors.broadcast("cmd", 904)
 	}
 }
 
@@ -85,22 +92,28 @@ function nei_cmd_listen() {
 					}else if(value==900){	# Shapes
 						barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900)
 						#barrier_ready(900)
-						neighbors.broadcast("cmd", 900)
+						#neighbors.broadcast("cmd", 900)
-					} else if(value==901){	# Pursuit
+					} else if(value==901 and BVMSTATE!="BARRIERWAIT"){	# Pursuit
 						destroyGraph()
 						barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 901)
 						#barrier_ready(901)
-						neighbors.broadcast("cmd", 901)
+						#neighbors.broadcast("cmd", 901)
-					} else if(value==902){	# Agreggate
+					} else if(value==902 and BVMSTATE!="BARRIERWAIT" and BVMSTATE!="WAYPOINT"){	# Waypoint
 						destroyGraph()
-						barrier_set(ROBOTS, "AGGREGATE", BVMSTATE, 902)
+						barrier_set(ROBOTS, "WAYPOINT", BVMSTATE, 902)
 						#barrier_ready(902)
-						neighbors.broadcast("cmd", 902)
+						#neighbors.broadcast("cmd", 902)
-					} else if(value==903){	# Formation
+					} else if(value==903 and BVMSTATE!="BARRIERWAIT" and BVMSTATE!="POTENTIAL"){	# Formation
 						destroyGraph()
-						barrier_set(ROBOTS, "FORMATION", BVMSTATE, 903)
+						resetWP()
+						barrier_set(ROBOTS, "POTENTIAL", BVMSTATE, 903)
 						#barrier_ready(903)
-						neighbors.broadcast("cmd", 903)
+						#neighbors.broadcast("cmd", 903)
+					} else if(value==904 and BVMSTATE!="BARRIERWAIT" and BVMSTATE!="IDLE"){	# idle
+						destroyGraph()
+						barrier_set(ROBOTS, "IDLE", BVMSTATE, 904)
+						#barrier_ready(904)
+						#neighbors.broadcast("cmd", 904)
 					} else if(value==16 and BVMSTATE=="IDLE"){
 					#  neighbors.listen("gt",function(vid, value, rid) {
 					#    print("Got (", vid, ",", value, ") from robot #", rid)
@@ -110,3 +123,29 @@ function nei_cmd_listen() {
 				#}
     })
 }
+
+firsttimeinwp = 1
+function check_rc_wp() {
+	if(firsttimeinwp) {
+		v_wp = stigmergy.create(WP_STIG)
+		storegoal(pose.position.latitude, pose.position.longitude, pose.position.altitude)
+		firsttimeinwp = 0
+	}
+	if(rc_goto.id != -1) {
+		if(rc_goto.id == id) {
+			wpreached = 0
+			storegoal(rc_goto.latitude, rc_goto.longitude, pose.position.altitude)
+			return
+		} else {
+			var ls = packWP2i(rc_goto.latitude, rc_goto.longitude, 0)
+			v_wp.put(rc_goto.id,ls)
+			reset_rc()
+		}
+	}
+}
+
+function resetWP() {
+	firsttimeinwp = 1
+	if(v_wp!=nil)
+		v_wp.foreach(function(key, value, robot){v_wp[key]=nil})
+}

buzz_scripts/include/act/states.bzz

@@ -97,40 +97,23 @@ function stop() {
 }
 
 # State functions for individual waypoint control
-firsttimeinwp = 1
+
 wpreached = 1
 function indiWP() {
-	if(firsttimeinwp) {
-		v_wp = stigmergy.create(WP_STIG)
-		storegoal(pose.position.latitude, pose.position.longitude, pose.position.altitude)
-		firsttimeinwp = 0
-	}
 	BVMSTATE = "INDIWP"
-	if(rc_goto.id != -1) {
+	check_rc_wp()
-		if(rc_goto.id == id) {
-			wpreached = 0
-			storegoal(rc_goto.latitude, rc_goto.longitude, pose.position.altitude)
-			return
-		} else {
-			var ls = packWP2i(rc_goto.latitude, rc_goto.longitude, 0)
-			v_wp.put(rc_goto.id,ls)
-			reset_rc()
-		}
-	}
 
-  #if(vstig_buzzy == 0) {
+	wpi = v_wp.get(id)
-		wpi = v_wp.get(id)
+	if(wpi!=nil) {
-		if(wpi!=nil) {
+		wp = unpackWP2i(wpi)
-			wp = unpackWP2i(wpi)
+		if(wp.pro == 0) {
-			if(wp.pro == 0) {
+				wpreached = 0
-					wpreached = 0
+				storegoal(wp.lat, wp.lon, pose.position.altitude)
-					storegoal(wp.lat, wp.lon, pose.position.altitude)
+				var ls = packWP2i(wp.lat, wp.lon, 1)
-					var ls = packWP2i(wp.lat, wp.lon, 1)
+				v_wp.put(id,ls)
-					v_wp.put(id,ls)
+				return
-					return
+			}      
-				}      
+	}
-		}
-	#}
 
 	if(wpreached!=1)
 		goto_gps(indiWP_done)
@@ -138,7 +121,7 @@ function indiWP() {
 }
 
 function indiWP_done() {
-	BVMSTATE = "INDIWP"
+	BVMSTATE = "WAYPOINT"
 	wpreached = 1
 }
 
@@ -208,10 +191,12 @@ function pursuit() {
 }
 
 # Lennard-Jones interaction magnitude
-TARGET     = 8.0
+TARGET     = 16.0
-EPSILON    = 30.0
+EPSILON    = 30.0 #30
+GAIN_ATT	 = 50.0
+GAIN_REP	 = 30.0
 function lj_magnitude(dist, target, epsilon) {
-	lj = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
+	lj = -(epsilon / dist) * ((target / dist)^4)# - (target / dist)^2) #repulse only to avoid each other
   return lj
 }
 
@@ -225,14 +210,32 @@ function lj_sum(rid, data, accum) {
   return math.vec2.add(data, accum)
 }
 
-# Sate function that calculates and actuates the flocking interaction
+# State function that calculates and actuates LJ flocking interaction with vstig targets (attractor/repulsors)
-function formation() {
+function lennardjones() {
-  BVMSTATE="FORMATION"
+  BVMSTATE="POTENTIAL"
+	check_rc_wp()
   # Calculate accumulator
   accum_lj = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
   if(neighbors.count() > 0)
     accum_lj = math.vec2.scale(accum_lj, 1.0 / neighbors.count())
-	accum_lj = LimitSpeed(accum_lj, 1.0/3.0)
+	# Add attractor/repulsor effects
+	log(v_wp.size())
+	accum_t = math.vec2.new(0.0, 0.0);
+	v_wp.foreach(
+    function(key, value, robot){
+			wp = unpackWP2i(value)
+			dvec = vec_from_gps(wp.lat, wp.lon, 0)
+			Tdist = math.vec2.length(dvec)
+			if(key > 99 and Tdist < 40)
+				accum_t = math.vec2.sub(accum_t, math.vec2.newp(GAIN_REP*(TARGET/Tdist)^4, math.vec2.angle(dvec)))
+			else if(key > 49 and Tdist > 10)
+				accum_t = math.vec2.add(accum_t, math.vec2.newp(GAIN_ATT*(TARGET/Tdist)^2, math.vec2.angle(dvec)))
+	})
+	if(v_wp.size() > 0)
+    accum_t = math.vec2.scale(accum_t, 1.0 / v_wp.size())
+	#log(math.vec2.length(accum_t),math.vec2.length(accum_lj))
+	
+	accum_lj = LimitSpeed(math.vec2.add(accum_t,accum_lj), 1.0) #1/3
   goto_abs(accum_lj.x, accum_lj.y, 0.0, 0.0)
 }
 

buzz_scripts/include/utils/table.bzz

@@ -55,7 +55,7 @@ function i2s(value){
 	return "LAUNCH"
 	}
   else if(value==9){
-	return "STOP"
+	return "FORMATION"
 	}
   else {
 	return "UNDETERMINED"
@@ -80,7 +80,7 @@ function s2i(value){
 	else if(value=="BARRIERWAIT"){
 	return 5
 	}
-	else if(value=="INDIWP"){
+	else if(value=="WAYPOINT"){
 	return 6
 	}
 	else if(value=="TASK_ALLOCATE"){
@@ -89,7 +89,7 @@ function s2i(value){
 	else if(value=="LAUNCH"){
 	return 8
 	}
-	else if(value=="STOP"){
+	else if(value=="POTENTIAL"){
 	return 9
 	}
   else

buzz_scripts/include/utils/takeoff_heights.bzz

@@ -1,12 +1,12 @@
 takeoff_heights ={
-    .0 = 30.0,
+    .0 = 0.0,
-    .1 = 21.0,
+    .1 = 32.0,
-    .2 = 18.0,
+    .2 = 28.0,
-    .3 = 12.0,
+    .3 = 24.0,
-    .4 = 28.0,
+    .4 = 20.0,
-    .5 = 12.0,
+    .5 = 16.0,
-    .6 = 3.0,
+    .6 = 12.0,
-    .9 = 15.0,
+    .9 = 8.0,
     .11 = 6.0,
     .14 = 9.0,
     .16 = 9.0,

buzz_scripts/main.bzz

@@ -12,7 +12,7 @@ include "utils/takeoff_heights.bzz"
 
 #State launched after takeoff
 
-AUTO_LAUNCH_STATE = "INDIWP"
+AUTO_LAUNCH_STATE = "IDLE"
 TARGET     = 9.0
 EPSILON    = 30.0
 ROOT_ID = 3
@@ -78,8 +78,8 @@ function step() {
 		statef=idle
 	else if(BVMSTATE=="AGGREGATE")
 		statef=aggregate
-	else if(BVMSTATE=="FORMATION")
+	else if(BVMSTATE=="POTENTIAL")
-		statef=formation
+		statef=lennardjones
 	else if(BVMSTATE=="PURSUIT")
 		statef=pursuit
 	else if(BVMSTATE=="TASK_ALLOCATE")	# or bidding ?