add height table and fix large ID bug in ACF

buzz_scripts/include/act/barrier.bzz

@@ -11,7 +11,8 @@ BARRIER_TIMEOUT = 200 # in steps
 BARRIER_VSTIG = 80
 timeW = 0
 barrier = nil
-bc = 0;
+hvs = 0;
+
 
 #
 # Sets a barrier
@@ -23,7 +24,13 @@ function barrier_create() {
   #log("---> Prev. br. ", barrier, " ", BARRIER_VSTIG)
   if(barrier!=nil) {
     barrier=nil
-  #  BARRIER_VSTIG = BARRIER_VSTIG +1
+    if(hvs) {
+      BARRIER_VSTIG = BARRIER_VSTIG -1
+      hvs = 0
+    }else{
+      BARRIER_VSTIG = BARRIER_VSTIG +1
+      hvs = 1
+    }
   }
   #log("---> New. br. ", barrier, " ", BARRIER_VSTIG)
   barrier = stigmergy.create(BARRIER_VSTIG)

buzz_scripts/include/act/states.bzz

@@ -12,9 +12,9 @@ TARGET_ALTITUDE = 15.0 # m.
 BVMSTATE = "TURNEDOFF"
 PICTURE_WAIT = 20 # steps
 
-GOTO_MAXVEL = 1.5 # m/steps
+GOTO_MAXVEL = 0.85 # m/steps
 GOTO_MAXDIST = 150 # m.
-GOTODIST_TOL = 0.8  # m.
+GOTODIST_TOL = 0.4  # m.
 GOTOANG_TOL = 0.1 # rad.
 path_it = 0
 pic_time = 0
@@ -270,7 +270,6 @@ function nei_cmd_listen() {
 						AUTO_LAUNCH_STATE = "IDLE"
 						BVMSTATE = "GOHOME"
 					} else if(value==21 and BVMSTATE!="TURNEDOFF") {
-						AUTO_LAUNCH_STATE = "TURNEDOFF"
 						BVMSTATE = "STOP"
 					} else if(value==400 and BVMSTATE=="TURNEDOFF") {
 						arm()

buzz_scripts/include/taskallocate/graphformGPS.bzz

@@ -176,7 +176,7 @@ function find(table,value){
 #
 function packmessage(send_table){
     var send_value
-    send_value=100000*send_table.State+10000*send_table.Label+1000*send_table.ReqLabel+10*send_table.ReqID+send_table.Response
+    send_value=1000000*send_table.State+100000*send_table.Label+10000*send_table.ReqLabel+100*send_table.ReqID+send_table.Response
     return send_value
 }
 #
@@ -200,14 +200,14 @@ function pack_guide_msg(send_table){
 #unpack message
 #
 function unpackmessage(recv_value){
-    var wan=(recv_value-recv_value%100000)/100000
+    var wan=(recv_value-recv_value%1000000)/1000000
-    recv_value=recv_value-wan*100000
+    recv_value=recv_value-wan*1000000
-    var qian=(recv_value-recv_value%10000)/10000
+    var qian=(recv_value-recv_value%100000)/100000
-    recv_value=recv_value-qian*10000
+    recv_value=recv_value-qian*100000
-    var bai=(recv_value-recv_value%1000)/1000
+    var bai=(recv_value-recv_value%10000)/10000
-    recv_value=recv_value-bai*1000
+    recv_value=recv_value-bai*10000
-    var shi=(recv_value-recv_value%10)/10
+    var shi=(recv_value-recv_value%100)/100
-    recv_value=recv_value-shi*10
+    recv_value=recv_value-shi*100
     var ge=recv_value
     var return_table={.State=0.0,.Label=0.0,.ReqLabel=0.0,.ReqID=0.0,.Response=0.0}
     return_table.State=wan
@@ -310,10 +310,10 @@ neighbors.listen("m",
 #
 #Function used to get the station info of the sender of the message
 #
-function Get_DisAndAzi(id){
+function Get_DisAndAzi(t_id){
 	neighbors.foreach(
 	function(rid, data) {
-		if(rid==id){
+		if(rid==t_id){
 		m_receivedMessage.Range=data.distance*100.0
 		m_receivedMessage.Bearing=data.azimuth
 		}
@@ -411,7 +411,8 @@ function DoFree() {
 function DoAsking(){
 	if(BVMSTATE!="GRAPH_ASKING"){
 	  BVMSTATE="GRAPH_ASKING"
-	  m_unRequestId=id	#don't know why the random numbers are the same, add id to make the ReqID different
+	  #math.rng.setseed(id)
+	  m_unRequestId=id#math.rng.uniform(0,10)	#don't know why the random numbers are the same, add id to make the ReqID different
 	  m_selfMessage.State=s2i(BVMSTATE)
 	  m_selfMessage.ReqLabel=m_nLabel
 	  m_selfMessage.ReqID=m_unRequestId
@@ -513,17 +514,17 @@ function set_rc_goto() {
 		m_unWaitCount=m_unJoiningLostPeriod#if see pred, reset the timer
 
 		var P2Target=math.vec2.newp(m_vecNodes[m_nLabel].distance,m_vecNodes[m_nLabel].bearing)
-		m_cMeToPred.Range=m_MessageRange[IDofPred]#the poition of self to pred in local coordinate
+		m_cMeToPred.Range=m_MessageRange[IDofPred]#the position of self to pred in local coordinate
 		m_cMeToPred.Bearing=m_MessageBearing[IDofPred]
 
-    var S2Pred=math.vec2.newp(m_cMeToPred.Range,m_cMeToPred.Bearing)
+		var S2Pred=math.vec2.newp(m_cMeToPred.Range,m_cMeToPred.Bearing)
-    var S2Target=math.vec2.add(S2Pred,P2Target)
+		var S2Target=math.vec2.add(S2Pred,P2Target)
 
-    goal = gps_from_vec(math.vec2.newp(math.vec2.length(S2Target)/100.0, math.atan(S2Target.y,S2Target.x)))
+		goal = gps_from_vec(math.vec2.newp(math.vec2.length(S2Target)/100.0, math.atan(S2Target.y,S2Target.x)))
-	print("Saving GPS goal: ",goal.latitude, goal.longitude)
+		print("Saving GPS goal (", m_nLabel, " from ", IDofPred, "): ",goal.latitude, goal.longitude, "(", S2Target.x/100, S2Target.y/100, ")")
-    storegoal(goal.latitude, goal.longitude, pose.position.altitude)
+		storegoal(goal.latitude, goal.longitude, pose.position.altitude)
-    m_gotjoinedparent = 1
+		m_gotjoinedparent = 1
-  }
+  	}
 }
 #
 #Do joined

buzz_scripts/include/utils/takeoff_heights.bzz

@@ -0,0 +1,10 @@
+takeoff_heights ={
+    .1 = 21.0,
+    .2 = 18.0,
+    .3 = 12.0,
+    .5 = 24.0,
+    .6 = 3.0,
+    .9 = 15.0,
+    .11 = 6.0,
+    .16 = 9.0
+}

buzz_scripts/main.bzz

@@ -6,6 +6,7 @@ include "plan/rrtstar.bzz"
 include "taskallocate/graphformGPS.bzz"
 include "vstigenv.bzz"
 include "timesync.bzz"
+include "utils/takeoff_heights.bzz"
 
 #State launched after takeoff
 AUTO_LAUNCH_STATE = "TASK_ALLOCATE"
@@ -14,7 +15,7 @@ AUTO_LAUNCH_STATE = "TASK_ALLOCATE"
 LOWEST_ROBOT_ID = 0
 TARGET     = 9.0
 EPSILON    = 30.0
-ROOT_ID = 2
+ROOT_ID = 3
 graph_id = 3
 graph_loop = 0
 
@@ -37,7 +38,7 @@ function init() {
 
 #	initGraph()
 
-	TARGET_ALTITUDE = 4 + (id-LOWEST_ROBOT_ID)*3.0 # m
+	TARGET_ALTITUDE = takeoff_heights[id] #4 + (id-LOWEST_ROBOT_ID)*3.0 # m
 
 	# start the swarm command listener
 	nei_cmd_listen()

src/roscontroller.cpp

@@ -1024,7 +1024,7 @@ void roscontroller::SetLocalPosition(float x, float y, float z, float yaw)
 
   // To prevent drifting from stable position, uncomment
   //if(fabs(x)>0.005 || fabs(y)>0.005) {
-  //  localsetpoint_nonraw_pub.publish(moveMsg);
+    localsetpoint_nonraw_pub.publish(moveMsg);
   //}
 }