enhance barrier, simplify gps-graph and add multi-WP csv load

buzz_scripts/graphformGPS.bzz

@@ -6,7 +6,7 @@ include "vec2.bzz"
 include "update.bzz"
 
 include "vstigenv.bzz" # reserve stigmergy id=20 and 21 for this header.
-include "barrier.bzz" # reserve stigmergy id=11 for this header.
+include "barrier.bzz" # reserve stigmergy id=80 (auto-increment up) for this header.
 include "uavstates.bzz"	# require an 'action' function to be defined here.
 
 include "graphs/shapes_Y.bzz"
@@ -268,14 +268,14 @@ function target4label(nei_id){
 #calculate LJ vector for neibhor stored in i
 function LJ_vec(i){
     var dis=m_MessageRange[i]
-    var bearing=m_MessageBearing[i]
+    var ljbearing=m_MessageBearing[i]
     var nei_id=m_messageID[i]
     var target=target4label(nei_id)
     var cDir={.x=0.0,.y=0.0}
     if(target!="miss"){
-    	cDir=math.vec2.newp(FlockInteraction(dis,target,EPSILON),bearing)   
+    	cDir=math.vec2.newp(FlockInteraction(dis,target,EPSILON),ljbearing)   
     }
-       #log(id,"dis=",dis,"target=",target,"label=",nei_id)
+    #log(id,"dis=",dis,"target=",target,"label=",nei_id)
 		#log("x=",cDir.x,"y=",cDir.y)  
     return cDir
 }
@@ -383,65 +383,11 @@ function TransitionToAsking(un_label){
 #
 #Transistion to state joining
 #
-function set_rc_goto() {
-  #get information of pred
-	var i=0
-	var IDofPred=-1
-	while(i<m_neighbourCount and IDofPred==-1){
-		if(m_MessageLabel[i]==m_vecNodes[m_nLabel].Pred and m_MessageState[i]=="STATE_JOINED")
-			IDofPred=i
-		i=i+1
-	}
-
-	#found pred
-	if(IDofPred!=-1){
-		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.Bearing=m_MessageBearing[IDofPred]
-
-		#attention, m_cMeToPred.GlobalBearing is the bearing of self wrt pred in global coordinate
-		var S2PGlobalBearing=0
-
-		m_cMeToPred.GlobalBearing=LimitAngle(m_cMeToPred.GlobalBearing)
-
-		if(m_cMeToPred.GlobalBearing>math.pi)
-			S2PGlobalBearing=m_cMeToPred.GlobalBearing-math.pi
-		else
-			S2PGlobalBearing=m_cMeToPred.GlobalBearing+math.pi
-
-		var S2Pred=math.vec2.newp(m_cMeToPred.Range,S2PGlobalBearing)
-
-		#the vector from self to target in global coordinate
-		var S2Target=math.vec2.add(S2Pred,P2Target)
-		#change the vector to local coordinate of self
-		var S2Target_bearing=math.atan(S2Target.y, S2Target.x)
-		m_bias=m_cMeToPred.Bearing-S2PGlobalBearing
-		S2Target_bearing=S2Target_bearing+m_bias
-
-    gps_from_rb(math.vec2.length(S2Target)/100.0, S2Target_bearing)
-    uav_storegoal(goal.latitude, goal.longitude, position.altitude)
-  }
-}
-
 function TransitionToJoining(){
 	UAVSTATE="STATE_JOINING"
 	m_selfMessage.State=s2i(UAVSTATE) 
 	m_selfMessage.Label=m_nLabel
 	m_unWaitCount=m_unJoiningLostPeriod
-	
-	set_rc_goto()
-
-	neighbors.listen("r",
-        function(vid,value,rid){
-        var recv_table={.Label=0,.Bearing=0.0}      
-        recv_table=unpack_guide_msg(value)
-        #store the received message
-        if(recv_table.Label==m_nLabel){
-        	m_cMeToPred.GlobalBearing=recv_table.Bearing
-        }
-    })
 }
 
 #
@@ -452,11 +398,10 @@ function TransitionToJoined(){
 	m_selfMessage.State=s2i(UAVSTATE)
 	m_selfMessage.Label=m_nLabel
 	m_vecNodes[m_nLabel].State="ASSIGNED"
-	neighbors.ignore("r")
 
 	#write statues
 	#v_tag.put(m_nLabel, m_lockstig)
-  barrier_create(m_lockstig)
+  barrier_create()
 	barrier_ready()
 
 	m_navigation.x=0.0
@@ -591,13 +536,47 @@ function DoAsking(){
 #
 #Do joining
 #
+m_gotjoinedparent = 0
 function DoJoining(){
-	gotoWP(TransitionToJoined)
+
+  if(m_gotjoinedparent!=1)
+    set_rc_goto()
+	else
+    gotoWP(TransitionToJoined)
+
 	#pack the communication package
 	m_selfMessage.State=s2i(UAVSTATE)
 	m_selfMessage.Label=m_nLabel
 }
 
+function set_rc_goto() {
+  #get information of pred
+	var i=0
+	var IDofPred=-1
+	while(i<m_neighbourCount and IDofPred==-1){
+		if(m_MessageLabel[i]==m_vecNodes[m_nLabel].Pred and m_MessageState[i]=="STATE_JOINED")
+			IDofPred=i
+		i=i+1
+	}
+
+	#found pred
+	if(IDofPred!=-1){
+		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.Bearing=m_MessageBearing[IDofPred]
+
+    var S2Pred=math.vec2.newp(m_cMeToPred.Range,m_cMeToPred.Bearing)
+
+    var S2Target=math.vec2.add(S2Pred,P2Target)
+
+    gps_from_vec(math.vec2.newp(math.vec2.length(S2Target)/100.0, math.atan(S2Target.y,S2Target.x)))
+    uav_storegoal(goal.latitude, goal.longitude, position.altitude)
+    m_gotjoinedparent = 1
+  }
+}
+
 #
 #Do joined
 #
@@ -630,7 +609,6 @@ function DoJoined(){
 			if(m_nLabel==m_vecNodes[JoiningLabel].Pred){
 				##joining wrt this dot,send the global bearing
 				var m_messageForJoining={.Label=JoiningLabel,.Bearing=m_MessageBearing[i]-m_bias}
-				neighbors.broadcast("r",pack_guide_msg(m_messageForJoining))
 			}
 		}
 		#if it is the pred
@@ -679,7 +657,7 @@ function DoJoined(){
 	#if(v_tag.size()==ROBOTS){
 	#	TransitionToLock()
 	#}
-  barrier_wait(ROBOTS, TransitionToLock, DoJoined)
+  barrier_wait(ROBOTS, TransitionToLock, DoJoined, -1)
 }
 
 #

buzz_scripts/include/barrier.bzz

@@ -8,31 +8,38 @@
 # Constants
 #
 BARRIER_TIMEOUT = 200 # in steps
+BARRIER_VSTIG = 80
 timeW = 0
+barrier = nil
 
 #
 # Sets a barrier
 #
-function barrier_create(vstig_nb) {
+function barrier_create() {
   # reset
   timeW = 0
   # create barrier vstig
-  barrier = stigmergy.create(vstig_nb)
+  #log("---> Prev. br. ", barrier, " ", BARRIER_VSTIG)
+  if(barrier!=nil) {
+    barrier=nil
+    BARRIER_VSTIG = BARRIER_VSTIG +1
+  }
+  #log("---> New. br. ", barrier, " ", BARRIER_VSTIG)
+  barrier = stigmergy.create(BARRIER_VSTIG)
 }
 
-function barrier_set(threshold, transf, resumef, vstig_nb) {
+function barrier_set(threshold, transf, resumef, bdt) {
   statef = function() {
-    barrier_wait(threshold, transf, resumef);
+    barrier_wait(threshold, transf, resumef, bdt);
   }
   UAVSTATE = "BARRIERWAIT"
-  barrier_create(vstig_nb)
+  barrier_create()
 }
 
 #
 # Make yourself ready
 #
 function barrier_ready() {
-  log("BARRIER READY -------")
   barrier.put(id, 1)
   barrier.put("d", 0)
 }
@@ -40,21 +47,24 @@ function barrier_ready() {
 #
 # Executes the barrier
 #
-function barrier_wait(threshold, transf, resumef) {
+function barrier_wait(threshold, transf, resumef, bdt) {
   barrier.put(id, 1)
 
   barrier.get(id)
-  if(barrier.size() >= threshold or barrier.get("d")==1) {
+  #log("----->BS: ", barrier.size())
+  if(barrier.size() - 1 >= threshold or barrier.get("d") == 1) {
     barrier.put("d", 1)
-#    barrier = nil
     timeW = 0
     transf()
   } else if(timeW >= BARRIER_TIMEOUT) {
+    log("------> Barrier Timeout !!!!")
     timeW = 0
-#    barrier = nil
     resumef()
   }
 
+  if(bdt!=-1)
+    neighbors.broadcast("cmd", bdt)
+
   timeW = timeW+1
 }
 

buzz_scripts/include/graphs/waypointlist.csv

@@ -0,0 +1,89 @@
+0,-73.1531935978886,45.5084960903092,15,15
+1,-73.1530989420915,45.5085624255498,15,15
+2,-73.1530042862771,45.5086287608025,15,15
+3,-73.1529096304626,45.5086950960552,15,15
+4,-73.1529458247399,45.5087204611798,15,15
+5,-73.1530404805543,45.5086541259271,15,15
+6,-73.1531351363515,45.5085877906865,15,15
+7,-73.1532297921486,45.508521455446,15,15
+8,-73.1533244479457,45.5084551202054,15,15
+9,-73.1533606422273,45.508480485333,15,15
+10,-73.1532659864302,45.5085468205736,15,15
+11,-73.153171330633,45.5086131558142,15,15
+12,-73.1530766748359,45.5086794910548,15,15
+13,-73.1529820190215,45.5087458263075,15,15
+14,-73.1530182132901,45.5087711914261,15,15
+15,-73.1531128691045,45.5087048561733,15,15
+16,-73.1532075249016,45.5086385209327,15,15
+17,-73.1533021806988,45.5085721856922,15,15
+18,-73.1533968364959,45.5085058504516,15,15
+19,-73.1534330307645,45.5085312155701,15,15
+20,-73.1533383749674,45.5085975508107,15,15
+21,-73.1532437191702,45.5086638860513,15,15
+22,-73.1531490633731,45.5087302212919,15,15
+23,-73.1530544075587,45.5087965565446,15,15
+24,-73.1530906018273,45.5088219216632,15,15
+25,-73.1531852576417,45.5087555864105,15,15
+26,-73.1532799134389,45.5086892511699,15,15
+27,-73.153374569236,45.5086229159293,15,15
+28,-73.1534692250331,45.5085565806887,15,15
+29,-73.1535054193017,45.5085819458072,15,15
+30,-73.1534107635046,45.5086482810478,15,15
+31,-73.1533161077075,45.5087146162884,15,15
+32,-73.1532214519103,45.508780951529,15,15
+33,-73.1531267960959,45.5088472867817,15,15
+34,-73.1531629903645,45.5088726519003,15,15
+35,-73.1532576461789,45.5088063166476,15,15
+36,-73.1533523019761,45.508739981407,15,15
+37,-73.1534469577732,45.5086736461664,15,15
+38,-73.1535416135703,45.5086073109258,15,15
+39,-73.1535778078389,45.5086326760444,15,15
+40,-73.1534831520418,45.5086990112849,15,15
+41,-73.1533884962447,45.5087653465255,15,15
+42,-73.1532938404476,45.5088316817661,15,15
+43,-73.1531991846331,45.5088980170188,15,15
+44,-73.1532353789017,45.5089233821374,15,15
+45,-73.1533300347162,45.5088570468847,15,15
+46,-73.1534246905133,45.5087907116441,15,15
+47,-73.1535193463104,45.5087243764035,15,15
+48,-73.1536140021075,45.5086580411629,15,15
+49,-73.1536501963762,45.5086834062815,15,15
+50,-73.153555540579,45.508749741522,15,15
+51,-73.1534608847819,45.5088160767626,15,15
+52,-73.1533662289848,45.5088824120032,15,15
+53,-73.1532715731703,45.508948747256,15,15
+54,-73.1533077674389,45.5089741123745,15,15
+55,-73.1534024232534,45.5089077771218,15,15
+56,-73.1534970790505,45.5088414418812,15,15
+57,-73.1535917348476,45.5087751066406,15,15
+58,-73.1536863906448,45.5087087714,15,15
+59,-73.1537225849134,45.5087341365186,15,15
+60,-73.1536279291163,45.5088004717592,15,15
+61,-73.1535332733191,45.5088668069997,15,15
+62,-73.153438617522,45.5089331422403,15,15
+63,-73.1533439617076,45.5089994774931,15,15
+64,-73.1533801559762,45.5090248426116,15,15
+65,-73.1534748117906,45.5089585073589,15,15
+66,-73.1535694675877,45.5088921721183,15,15
+67,-73.1536641233849,45.5088258368777,15,15
+68,-73.153758779182,45.5087595016371,15,15
+69,-73.1537949734506,45.5087848667557,15,15
+70,-73.1537003176535,45.5088512019963,15,15
+71,-73.1536056618563,45.5089175372369,15,15
+72,-73.1535110060592,45.5089838724775,15,15
+73,-73.1534163502448,45.5090502077302,15,15
+74,-73.1534525445134,45.5090755728487,15,15
+75,-73.1535472003278,45.509009237596,15,15
+76,-73.1536418561249,45.5089429023554,15,15
+77,-73.1537365119221,45.5088765671148,15,15
+78,-73.1538311677192,45.5088102318742,15,15
+79,-73.1538673619878,45.5088355969928,15,15
+80,-73.1537727061907,45.5089019322334,15,15
+81,-73.1536780503936,45.508968267474,15,15
+82,-73.1535833945964,45.5090346027146,15,15
+83,-73.153488738782,45.5091009379673,15,15
+84,-73.1535249330852,45.5091263031101,15,15
+85,-73.1536195888996,45.5090599678574,15,15
+86,-73.1537142446968,45.5089936326168,15,15
+87,-73.1538089004939,45.5089272973762,15,15
+88,-73.153903556291,45.5088609621356,15,15

buzz_scripts/include/uavstates.bzz

@@ -3,10 +3,13 @@
 # FLIGHT-RELATED FUNCTIONS
 #
 ########################################
-TARGET_ALTITUDE = 5.0
+TARGET_ALTITUDE = 5.0 # m.
 UAVSTATE = "TURNEDOFF"
-GOTO_MAXVEL = 2
+PICTURE_WAIT = 40 # steps
-TAKEOFF_VSTIG = 11
+GOTO_MAXVEL = 2 # m/steps
+GOTO_MAXDIST = 150 # m.
+GOTODIST_TOL = 0.5  # m.
+GOTOANG_TOL = 0.1 # rad.
 goal = {.range=0, .bearing=0, .latitude=0, .longitude=0}
 
 function uav_initswarm() {
@@ -24,18 +27,13 @@ function idle() {
     UAVSTATE = "IDLE"
 }
 
-firstpassT = 1
 function takeoff() {
 	UAVSTATE = "TAKEOFF"
 	statef=takeoff
 	
-  if(firstpassT) {
-    barrier_create(TAKEOFF_VSTIG)
-    firstpassT = 0
-  }
-	
 	if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
-    barrier_wait(ROBOTS, action, land)
+		barrier_set(ROBOTS, action, land, -1)
+		barrier_ready()
 	} else {
     log("Altitude: ", TARGET_ALTITUDE)
 	  neighbors.broadcast("cmd", 22)
@@ -43,55 +41,65 @@ function takeoff() {
   }
 }
 
-firstpassL = 1
 function land() {
 	UAVSTATE = "LAND"
 	statef=land
 
-	if(firstpassL) {
-    barrier_create(TAKEOFF_VSTIG+1)
-    firstpassL = 0
-  }
-
 	neighbors.broadcast("cmd", 21)
 	uav_land()
 	
 	if(flight.status != 2 and flight.status != 3){
-		barrier_wait(ROBOTS,turnedoff,land)
+    barrier_set(ROBOTS,turnedoff,land, 21)
+		barrier_ready()
 	}
 }
 
-function goto() {
+function set_goto(transf) {
 	UAVSTATE = "GOTO"
-	statef=goto
+	statef=function() {
+    gotoWP(transf);
+  }
 
 	if(rc_goto.id==id){
-		s.leave()
+		if(s!=nil){
-		gotoWP(picture)
+      if(s.in())
+        s.leave()      
+    }
 	} else {
 		neighbors.broadcast("cmd", 16)
 		neighbors.broadcast("gt", rc_goto.id+rc_goto.longitude+rc_goto.latitude)
 	}
 }
 
+ptime=0
 function picture() {
+  statef=picture
+  UAVSTATE="PICTURE"
 	#TODO: change gimbal orientation
-	uav_takepicture()
+	if(ptime==PICTURE_WAIT/2)
-	statef=idle
+	  uav_takepicture()
+  else if(ptime>=PICTURE_WAIT) {
+    statef=action
+    ptime=0 
+  }
+  ptime=ptime+1
 }
 
 function gotoWP(transf) {
 	# print(rc_goto.id, " is going alone to lat=", rc_goto.latitude)
 	rb_from_gps(rc_goto.latitude, rc_goto.longitude)
-	print(id, " has to move ", goal.range)
+	print(" has to move ", goal.range)
 	m_navigation = math.vec2.newp(goal.range, goal.bearing)
-	if(math.vec2.length(m_navigation)>GOTO_MAXVEL) {
+
+	if(math.vec2.length(m_navigation)>GOTO_MAXDIST)
+    log("Sorry this is too far.")
+  else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity
 		m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation))
 		uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude)
-	} else if(math.vec2.length(m_navigation)>0.25)
+	} else if(goal.range < GOTODIST_TOL and goal.bearing < GOTOANG_TOL) # reached destination
-		uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude)
-	else
 		transf()
+	else
+		uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude)
 }
 
 function follow() {
@@ -145,16 +153,18 @@ function uav_rccmd() {
 function uav_neicmd() {
     neighbors.listen("cmd",
 			function(vid, value, rid) {
-				print("Got (", vid, ",", value, ") from robot #", rid)
+				print("Got (", vid, ",", value, ") from robot #", rid, "(", UAVSTATE, ")")
-				if(value==22 and UAVSTATE!="TAKEOFF") {
+				if(value==22 and UAVSTATE!="TAKEOFF" and UAVSTATE!="BARRIERWAIT") {
 					statef=takeoff
-				} else if(value==21) {
+		      UAVSTATE = "TAKEOFF"
+				} else if(value==21 and UAVSTATE!="LAND" and UAVSTATE!="BARRIERWAIT") {
 					statef=land
-				} else if(value==400 and UAVSTATE=="IDLE") {
+		      UAVSTATE = "LAND"
+				} else if(value==400 and UAVSTATE=="TURNEDOFF") {
 					uav_arm()
-				} else if(value==401 and UAVSTATE=="IDLE"){
+				} else if(value==401 and UAVSTATE=="TURNEDOFF"){
 					uav_disarm()
-				} else if(value==16){
+				} else if(value==16 and UAVSTATE=="IDLE"){
 				#  neighbors.listen("gt",function(vid, value, rid) {
 				#    print("Got (", vid, ",", value, ") from robot #", rid)
 				#    # if(gt.id == id) statef=goto
@@ -173,7 +183,7 @@ function LimitAngle(angle){
 }
 
 function rb_from_gps(lat, lon) {
-  print("gps2rb: ",lat,lon,position.latitude,position.longitude)
+#  print("gps2rb: ",lat,lon,position.latitude,position.longitude)
 	d_lon = lon - position.longitude
   d_lat = lat - position.latitude
   ned_x = d_lat/180*math.pi * 6371000.0;
@@ -182,12 +192,18 @@ function rb_from_gps(lat, lon) {
 	goal.bearing = LimitAngle(math.atan(ned_y,ned_x));
 }
 
-function gps_from_rb(range, bearing) {
+function gps_from_vec(vec) {
-  print("rb2gps: ",range,bearing,position.latitude,position.longitude)
+  Vrange = math.vec2.length(vec)
+  Vbearing = LimitAngle(math.atan(vec.y, vec.x))
+#  print("rb2gps: ",Vrange,Vbearing,position.latitude,position.longitude)
   latR = position.latitude*math.pi/180.0;
   lonR = position.longitude*math.pi/180.0;
-  target_lat = math.asin(math.sin(latR) * math.cos(range/6371000.0) + math.cos(latR) * math.sin(range/6371000.0) * math.cos(bearing));
+  target_lat = math.asin(math.sin(latR) * math.cos(Vrange/6371000.0) + math.cos(latR) * math.sin(Vrange/6371000.0) * math.cos(Vbearing));
-  target_lon = lonR + math.atan(math.sin(bearing) * math.sin(range/6371000.0) * math.cos(latR), math.cos(range/6371000.0) - math.sin(latR) * math.sin(target_lat));
+  target_lon = lonR + math.atan(math.sin(Vbearing) * math.sin(Vrange/6371000.0) * math.cos(latR), math.cos(Vrange/6371000.0) - math.sin(latR) * math.sin(target_lat));
   goal.latitude = target_lat*180.0/math.pi;
   goal.longitude = target_lon*180.0/math.pi;
+  #d_lat = (vec.x / 6371000.0)*180.0/math.pi;
+  #goal.latitude = d_lat + position.latitude;
+  #d_lon = (vec.y / (6371000.0 * math.cos(goal.latitude*math.pi/180.0)))*180.0/math.pi;
+  #goal.longitude = d_lon + position.longitude;
 }

buzz_scripts/testaloneWP.bzz

@@ -0,0 +1,35 @@
+include "vec2.bzz"
+include "update.bzz"
+include "barrier.bzz"	# don't use a stigmergy id=80 (auto-increment up) with this header.
+include "uavstates.bzz"	# require an 'action' function to be defined here.
+include "vstigenv.bzz"
+
+function action() {
+  uav_storegoal(-1.0,-1.0,-1.0)
+  set_goto(picture)
+}
+
+# Executed once at init time.
+function init() {
+	statef=turnedoff
+	UAVSTATE = "TURNEDOFF"
+  	uav_initstig()
+	TARGET_ALTITUDE = 15.0
+}
+
+# Executed at each time step.
+function step() {
+	uav_rccmd()
+
+	statef()
+
+	log("Current state: ", UAVSTATE)
+}
+ 
+# Executed once when the robot (or the simulator) is reset.
+function reset() {
+}
+ 
+# Executed once at the end of experiment.
+function destroy() {
+}

include/buzz_utility.h

@@ -23,8 +23,8 @@ struct pos_struct
 typedef struct pos_struct Pos_struct;
 struct rb_struct
 {
-  double r,b,la,lo;
+  double r,b,latitude,longitude,altitude;
-  rb_struct(double la, double lo, double r,double b):la(la),lo(lo),r(r),b(b){};
+  rb_struct(double la, double lo, double al, double r,double b):latitude(la),longitude(lo),altitude(al),r(r),b(b){};
   rb_struct(){}
 };
 typedef struct rb_struct RB_struct;

include/buzzuav_closures.h

@@ -9,8 +9,8 @@
 #include "ros/ros.h"
 #include "buzz_utility.h"
 
-	#define EARTH_RADIUS (double) 6371000.0
+#define EARTH_RADIUS (double) 6371000.0
-	#define DEG2RAD(DEG) ((DEG)*((M_PI)/(180.0)))
+#define DEG2RAD(DEG) ((DEG)*((M_PI)/(180.0)))
 
 namespace buzzuav_closures{
 	typedef enum {
@@ -31,13 +31,14 @@ namespace buzzuav_closures{
  * The command to use in Buzz is buzzros_print takes any available datatype in Buzz
  */
 int buzzros_print(buzzvm_t vm);
-
+void setWPlist(std::string path);
 /*
  * buzzuav_goto(latitude,longitude,altitude) function in Buzz
  * commands the UAV to go to a position supplied
  */
 int buzzuav_moveto(buzzvm_t vm);
 int buzzuav_storegoal(buzzvm_t vm);
+void parse_gpslist();
 int buzzuav_takepicture(buzzvm_t vm);
 /* Returns the current command from local variable*/
 int getcmd();

src/buzzuav_closures.cpp

@@ -32,6 +32,7 @@ namespace buzzuav_closures{
 	static float raw_packet_loss = 0.0;
 	static int filtered_packet_loss = 0;
 	static float api_rssi = 0.0;
+  string WPlistname = "";
 
 	std::map< int,  buzz_utility::RB_struct> targets_map;
 	std::map< int,  buzz_utility::Pos_struct> neighbors_map;
@@ -83,6 +84,10 @@ namespace buzzuav_closures{
 	   return buzzvm_ret0(vm);
 	}
 
+  void setWPlist(string path){
+    WPlistname = path + "include/graphs/waypointlist.csv";
+  }
+
 	/*----------------------------------------/
 	/ Compute GPS destination from current position and desired Range and Bearing
 	/----------------------------------------*/
@@ -105,10 +110,10 @@ namespace buzzuav_closures{
 		}
 
 	void rb_from_gps(double nei[], double out[], double cur[]){
-       		double d_lon = nei[1] - cur[1];
+    double d_lon = nei[1] - cur[1];
-       		double d_lat = nei[0] - cur[0];
+    double d_lat = nei[0] - cur[0];
-        	double ned_x = DEG2RAD(d_lat) * EARTH_RADIUS;
+    double ned_x = DEG2RAD(d_lat) * EARTH_RADIUS;
-        	double ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0]));
+    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] = constrainAngle(atan2(ned_y,ned_x));
 		out[2] = 0.0;
@@ -119,6 +124,52 @@ namespace buzzuav_closures{
 	double hcpos2[4] = {45.564729, -73.562060, 45.564362, -73.562958};
 	double hcpos3[4] = {45.564969, -73.562838, 45.564636, -73.563677};
 
+	void parse_gpslist()
+	{
+		// Open file:
+    ROS_INFO("WP list file: %s", WPlistname.c_str());
+		std::ifstream fin(WPlistname.c_str()); // Open in text-mode.
+
+		// Opening may fail, always check.
+		if (!fin) { 
+			ROS_ERROR("GPS list parser, could not open file."); 
+			return;
+		}
+
+    // Prepare a C-string buffer to be used when reading lines.
+    const int MAX_LINE_LENGTH = 1024; // Choose this large enough for your need.
+    char buffer[MAX_LINE_LENGTH] = {}; 
+    const char * DELIMS = "\t ,"; // Tab, space or comma.
+
+		// Read the file and load the data:
+		double lat, lon;
+    int alt, tilt, tid;
+		buzz_utility::RB_struct RB_arr;
+    // Read one line at a time.
+    while ( fin.getline(buffer, MAX_LINE_LENGTH) ) {
+      // Extract the tokens:
+      tid = atoi(strtok( buffer, DELIMS ));
+      lon = atof(strtok( NULL,   DELIMS ));
+      lat = atof(strtok( NULL,   DELIMS ));
+      alt = atoi(strtok( NULL,   DELIMS ));
+      tilt = atoi(strtok( NULL,   DELIMS ));
+      //ROS_INFO("%.6f, %.6f, %i %i %i",lat, lon, alt, tilt, tid);
+			RB_arr.latitude=lat;
+			RB_arr.longitude=lon;
+			RB_arr.altitude=alt;
+      // Insert elements.
+      map< int, buzz_utility::RB_struct >::iterator it = targets_map.find(tid);
+      if(it!=targets_map.end())
+        targets_map.erase(it);
+      targets_map.insert(make_pair(tid, RB_arr));
+    }
+
+    ROS_INFO("----->Saved %i waypoints.", targets_map.size());
+
+		// Close the file:
+		fin.close();
+	}
+
 	/*----------------------------------------/
 	/ Buzz closure to move following a 2D vector
 	/----------------------------------------*/
@@ -158,9 +209,9 @@ namespace buzzuav_closures{
 		double rb[3], tmp[3];
 		map< int, buzz_utility::RB_struct >::iterator it;
 		for (it=targets_map.begin(); it!=targets_map.end(); ++it){
-			tmp[0]=(it->second).la;tmp[1]=(it->second).lo;tmp[2]=height;
+			tmp[0]=(it->second).latitude; tmp[1]=(it->second).longitude; tmp[2]=height;
-	   		rb_from_gps(tmp, rb, cur_pos);
+	   	rb_from_gps(tmp, rb, cur_pos);
-			ROS_WARN("----------Pushing target id %i (%f,%f)", rb[0], rb[1]);
+			//ROS_WARN("----------Pushing target id %i (%f,%f)", it->first, rb[0], rb[1]);
 			buzzvm_push(vm, targettbl);
 			/* When we get here, the "targets" table is on top of the stack */
 			//ROS_INFO("Buzz_utility will save user %i.", it->first);
@@ -206,8 +257,9 @@ namespace buzzuav_closures{
 	   if(fabs(rb[0])<100.0) {
 			//printf("\tGot new user from bzz stig: %i - %f, %f\n", uid, rb[0], rb[1]);
 			buzz_utility::RB_struct RB_arr;
-			RB_arr.la=tmp[0];
+			RB_arr.latitude=tmp[0];
-			RB_arr.lo=tmp[1];
+			RB_arr.longitude=tmp[1];
+			RB_arr.altitude=tmp[2];
 			RB_arr.r=rb[0];
 			RB_arr.b=rb[1];
 			map< int, buzz_utility::RB_struct >::iterator it = targets_map.find(uid);
@@ -217,7 +269,7 @@ namespace buzzuav_closures{
 			//ROS_INFO("Buzz_utility got updated/new user: %i (%f,%f,%f)", id, latitude, longitude, altitude);
 			return vm->state;
 	   } else
-		printf(" ---------- Target too far %f\n",rb[0]);
+		ROS_WARN(" ---------- Target too far %f",rb[0]);
 
 		return 0;
 	}
@@ -249,7 +301,7 @@ namespace buzzuav_closures{
 
 	mavros_msgs::Mavlink get_status(){
 		mavros_msgs::Mavlink payload_out;
-    	map< int, buzz_utility::neighbors_status >::iterator it;
+    map< int, buzz_utility::neighbors_status >::iterator it;
 		for (it= neighbors_status_map.begin(); it!= neighbors_status_map.end(); ++it){
 			payload_out.payload64.push_back(it->first);
 			payload_out.payload64.push_back(it->second.gps_strenght);
@@ -284,11 +336,26 @@ namespace buzzuav_closures{
 	  buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT);
 	  buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
 	  buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
-    float lat = buzzvm_stack_at(vm, 3)->f.value;
+    double goal[3];
-    float lon = buzzvm_stack_at(vm, 2)->f.value;
+		goal[0] = buzzvm_stack_at(vm, 3)->f.value;
-    float alt = buzzvm_stack_at(vm, 1)->f.value;
+		goal[1] = buzzvm_stack_at(vm, 2)->f.value;
-    ROS_ERROR("DEBUG ---- %f %f %f",lat,lon,alt);
+		goal[2] = buzzvm_stack_at(vm, 1)->f.value;
-    rc_set_goto((int)buzz_utility::get_robotid(), lat, lon, alt);
+    if(goal[0]==-1 && goal[1]==-1 && goal[2]==-1){
+      if(targets_map.size()<=0)
+        parse_gpslist();
+      goal[0] = targets_map.begin()->second.latitude;
+      goal[1] = targets_map.begin()->second.longitude;
+      goal[2] = targets_map.begin()->second.altitude;
+      targets_map.erase(targets_map.begin()->first);
+    }
+
+    double rb[3];
+
+    rb_from_gps(goal, rb, cur_pos);
+    if(fabs(rb[0])<150.0)
+		  rc_set_goto((int)buzz_utility::get_robotid(), goal[0], goal[1], goal[2]);
+
+		ROS_ERROR("DEBUG ---- %f %f %f (%f %f) %f %f",goal[0],goal[1],goal[2],cur_pos[0],cur_pos[1],rb[0],rb[1]);
 	  return buzzvm_ret0(vm);
 	}
 

src/roscontroller.cpp

@@ -17,6 +17,7 @@ roscontroller::roscontroller(ros::NodeHandle &n_c, ros::NodeHandle &n_c_priv)
   bcfname = fname + ".bo";
   dbgfname = fname + ".bdb";
   set_bzz_file(bzzfile_name.c_str());
+  buzzuav_closures::setWPlist(bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/");
   /*Initialize variables*/
   // Solo things
   SetMode("LOITER", 0);
@@ -751,8 +752,8 @@ void roscontroller::flight_controller_service_call()
   } else if (tmp == buzzuav_closures::COMMAND_MOVETO) { /*Buzz call for moveto*/
     roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], 0);
   } else if (tmp == buzzuav_closures::COMMAND_PICTURE) { /* TODO: Buzz call to take a picture*/
-    ROS_INFO("TAKING A PICTURE HERE!! --------------")
+    ROS_INFO("TAKING A PICTURE HERE!! --------------");
-    ;
+    ros::Duration(0.2).sleep();
   }
 }
 /*----------------------------------------------
@@ -1092,7 +1093,7 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr &msg) {
     gps_rb(nei_pos, cvt_neighbours_pos_payload);
     /*Extract robot id of the neighbour*/
     uint16_t *out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + 3));
-    ROS_WARN("RAB of %i: %f, %f", (int)out[1], cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1]);
+//    ROS_WARN("RAB of %i: %f, %f", (int)out[1], cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1]);
     /*pass neighbour position to local maintaner*/
     buzz_utility::Pos_struct n_pos(cvt_neighbours_pos_payload[0],
                                    cvt_neighbours_pos_payload[1],