Merge commit 'b1514752b9c5ce3dc4dbade62bb7014dee79abe1' into ros_drones_ws

buzz_scripts/graphform.bzz

@@ -44,7 +44,7 @@ m_receivedMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Respo
 m_selfMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
 
 #Current robot state
-m_eState="STATE_FREE"
+# m_eState="STATE_FREE" # replace with default UAVSTATE
 
 #navigation vector
 m_navigation={.x=0,.y=0}
@@ -361,19 +361,19 @@ function UpdateNodeInfo(){
 #Transistion to state free
 #
 function TransitionToFree(){
-	m_eState="STATE_FREE"
+	UAVSTATE="STATE_FREE"
 	m_unWaitCount=m_unLabelSearchWaitTime
-	m_selfMessage.State=s2i(m_eState)
+	m_selfMessage.State=s2i(UAVSTATE)
 }
 
 #
 #Transistion to state asking
 #
 function TransitionToAsking(un_label){
-	m_eState="STATE_ASKING"
+	UAVSTATE="STATE_ASKING"
 	m_nLabel=un_label
 	m_unRequestId=id	#don't know why the random numbers are the same, add id to make the ReqID different
-	m_selfMessage.State=s2i(m_eState)
+	m_selfMessage.State=s2i(UAVSTATE)
 	m_selfMessage.ReqLabel=m_nLabel
 	m_selfMessage.ReqID=m_unRequestId
 
@@ -384,8 +384,8 @@ function TransitionToAsking(un_label){
 #Transistion to state joining
 #
 function TransitionToJoining(){
-	m_eState="STATE_JOINING"
+	UAVSTATE="STATE_JOINING"
-	m_selfMessage.State=s2i(m_eState) 
+	m_selfMessage.State=s2i(UAVSTATE) 
 	m_selfMessage.Label=m_nLabel
 	m_unWaitCount=m_unJoiningLostPeriod
 
@@ -405,8 +405,8 @@ function TransitionToJoining(){
 #Transistion to state joined
 #
 function TransitionToJoined(){
-	m_eState="STATE_JOINED"
+	UAVSTATE="STATE_JOINED"
-	m_selfMessage.State=s2i(m_eState)
+	m_selfMessage.State=s2i(UAVSTATE)
 	m_selfMessage.Label=m_nLabel
 	m_vecNodes[m_nLabel].State="ASSIGNED"
 	neighbors.ignore("r")
@@ -426,8 +426,8 @@ function TransitionToJoined(){
 #Transistion to state Lock, lock the current formation
 #
 function TransitionToLock(){
-    m_eState="STATE_LOCK"
+    UAVSTATE="STATE_LOCK"
-    m_selfMessage.State=s2i(m_eState)
+    m_selfMessage.State=s2i(UAVSTATE)
     m_selfMessage.Label=m_nLabel
     m_vecNodes[m_nLabel].State="ASSIGNED"
     #record neighbor distance
@@ -448,7 +448,7 @@ while(i<m_neighbourCount){
 # Do free
 #
 function DoFree() {
-	m_selfMessage.State=s2i(m_eState)
+	m_selfMessage.State=s2i(UAVSTATE)
 
 	#wait for a while before looking for a Label
 	if(m_unWaitCount>0)
@@ -527,7 +527,7 @@ function DoFree() {
 		uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
 	}
 	#set message
-	m_selfMessage.State=s2i(m_eState)
+	m_selfMessage.State=s2i(UAVSTATE)
 
 }
 
@@ -558,7 +558,7 @@ function DoAsking(){
 	if(psResponse==-1){
 		#no response, wait
 		m_unWaitCount=m_unWaitCount-1
-		m_selfMessage.State=s2i(m_eState)
+		m_selfMessage.State=s2i(UAVSTATE)
 		m_selfMessage.ReqLabel=m_nLabel
 		m_selfMessage.ReqID=m_unRequestId
 		if(m_unWaitCount==0){
@@ -576,7 +576,6 @@ function DoAsking(){
 	if(m_MessageReqID[psResponse]==m_unRequestId){
 		if(m_MessageResponse[psResponse]=="REQ_GRANTED"){
 			TransitionToJoining()
-			#TransitionToJoined()
 			return
 		}
 		else{
@@ -656,7 +655,7 @@ function DoJoining(){
 	}
 
 	#pack the communication package
-	m_selfMessage.State=s2i(m_eState)
+	m_selfMessage.State=s2i(UAVSTATE)
 	m_selfMessage.Label=m_nLabel
 
 }
@@ -665,7 +664,7 @@ function DoJoining(){
 #Do joined
 #
 function DoJoined(){
-	m_selfMessage.State=s2i(m_eState)
+	m_selfMessage.State=s2i(UAVSTATE)
 	m_selfMessage.Label=m_nLabel
 
 	#collect all requests
@@ -754,7 +753,7 @@ function DoJoined(){
 #Do Lock
 #
 function DoLock(){
-m_selfMessage.State=s2i(m_eState)
+m_selfMessage.State=s2i(UAVSTATE)
 m_selfMessage.Label=m_nLabel
 m_navigation.x=0.0
 m_navigation.y=0.0
@@ -772,7 +771,10 @@ uav_moveto(m_navigation.x, m_navigation.y, 0.0)
 
 function action(){
   statef=action
-  UAVSTATE="GRAPH"
+  UAVSTATE="STATE_FREE"
+
+  # reset the graph
+  Reset()
 }
 
 #
@@ -792,27 +794,24 @@ function init() {
 	# Join Swarm
 	#
 	uav_initswarm()
-    v_tag = stigmergy.create(m_lockstig)
+  v_tag = stigmergy.create(m_lockstig)
-  	uav_initstig()
+  uav_initstig()
 	# go to diff. height since no collision avoidance implemented yet
 	TARGET_ALTITUDE = 7.5 #2.5 + id * 1.5
 	statef=turnedoff
 	UAVSTATE = "TURNEDOFF"
-
-	# reset the graph
-	Reset()
 }
 
 #
 # Executed every step
 #
-function step(){
+function step() {
 	# listen to potential RC
 	uav_rccmd()
 	# get the swarm commands
 	uav_neicmd()
 	# update the vstig (status/net/batt)
-  	uav_updatestig()
+  uav_updatestig()
 
 	#update the graph
 	UpdateNodeInfo()
@@ -821,27 +820,23 @@ function step(){
 	#
 	# graph state machine
 	#
-	if(UAVSTATE=="GRAPH"){
+	if(UAVSTATE=="STATE_FREE")
-		if(m_eState=="STATE_FREE")
+		statef=DoFree
-			DoFree()
+	else if(UAVSTATE=="STATE_ESCAPE")
-		else if(m_eState=="STATE_ESCAPE")
+		statef=DoEscape
-			DoEscape()
+	else if(UAVSTATE=="STATE_ASKING")
-		else if(m_eState=="STATE_ASKING")
+		statef=DoAsking
-			DoAsking()
+	else if(UAVSTATE=="STATE_JOINING")
-		else if(m_eState=="STATE_JOINING")
+		statef=DoJoining
-			DoJoining()
+	else if(UAVSTATE=="STATE_JOINED")
-		else if(m_eState=="STATE_JOINED")
+		statef=DoJoined
-			DoJoined()
+	else if(UAVSTATE=="STATE_LOCK")
-		else if(m_eState=="STATE_LOCK")
+		statef=DoLock
-			DoLock()
-	}
 
 	# high level UAV state machine
 	statef()
 
-
+	log("Current state: ", UAVSTATE, " and label: ", m_nLabel)
-	debug(m_eState,m_nLabel)
-	log("Current state: ", UAVSTATE)
 	log("Swarm size: ", ROBOTS)
 	
 	#navigation
@@ -868,12 +863,8 @@ function step(){
 # Executed when reset
 #
 function Reset(){
-#    m_vecNodes={}
-#    m_vecNodes = parse_graph("/home/dave/ROS_WS/src/rosbuzz/buzz_scripts/include/Graph_drone.graph")#change the .graph file when necessary
-#    m_vecNodes_fixed={}
-#    m_vecNodes_fixed=parse_graph_fixed("/home/dave/ROS_WS/src/rosbuzz/buzz_scripts/include/Graph_fixed.graph")
 	Read_Graph()
-    m_nLabel=-1
+  m_nLabel=-1
 
 	#start listening
 	start_listen()

buzz_scripts/graphformGPS.bzz

@@ -0,0 +1,806 @@
+#
+# Include files
+#
+include "string.bzz"
+include "vec2.bzz"
+include "update.bzz"
+
+include "vstigenv.bzz" # reserve stigmergy id=20 and 21 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"
+
+ROBOT_RADIUS = 50
+ROBOT_DIAMETER = 2.0*ROBOT_RADIUS
+ROBOT_SAFETYDIST = 2.0*ROBOT_DIAMETER
+ROOT_ID = 2
+
+# max velocity in cm/step
+ROBOT_MAXVEL = 150.0
+
+#
+# Global variables
+#
+
+#
+# Save message from all neighours
+#the indexes are as 1,2,3..., while each value is a table that store the information of a neighbour robot
+m_MessageState={}#store received neighbour message
+m_MessageLabel={}#store received neighbour message
+m_MessageReqLabel={}#store received neighbour message
+m_MessageReqID={}#store received neighbour message
+m_MessageResponse={}#store received neighbour message
+m_MessageRange={}#store received neighbour message
+m_MessageBearing={}#store received neighbour message
+m_neighbourCount=0#used to cunt neighbours
+#Save message from one neighbour
+#the indexes are as State(received state),Label(received Label),ReqLabel,ReqID,Response,Range,Bearing
+m_receivedMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0}
+
+#
+#Save the message to send
+#The keys of the talbe is State(current state),Label(current Label),ReqLabel(requested Label),ReqID(request id),Response(reply message{REQ_NONE,REQ_GRANTED,REQ_RESEND})
+m_selfMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
+
+#Current robot state
+# m_eState="STATE_FREE" # replace with default UAVSTATE
+
+#navigation vector
+m_navigation={.x=0,.y=0}
+
+#Debug message to be displayed in qt-opengl
+#m_ossDebugMsg
+
+#Debug vector to draw
+#CVector2 m_cDebugVector
+
+#Current label being requested or chosen (-1 when none)
+m_nLabel=-1
+m_messageID={}
+#neighbor distance to lock the current pattern
+lock_neighbor_id={}
+lock_neighbor_dis={}
+
+#Label request id
+m_unRequestId=0
+
+#Global bias, used to map local coordinate to global coordinate
+m_bias=0
+
+#Vector to predecessor,range is the distance between robots, bearing is the angle of pred wrt self in local coordinate of self, globalbearing is the angle of self wrt pred in global coordinate
+m_cMeToPred={.Range=0.0,.Bearing=0.0,.GlobalBearing=0.0}
+
+#Counter to wait for something to happen
+m_unWaitCount=0
+
+#Number of steps to wait before looking for a free label
+m_unLabelSearchWaitTime=0
+
+#Number of steps to wait for an answer to be received
+m_unResponseTimeThreshold=0
+
+#Number of steps to wait until giving up joining
+m_unJoiningLostPeriod=0
+
+#Tolerance distance to a target location
+m_fTargetDistanceTolerance=0
+
+#step cunt
+step_cunt=0
+
+# virtual stigmergy for the LOCK barrier.
+m_lockstig = 1
+
+# Lennard-Jones parameters, may need change
+EPSILON	= 1800 #13.5 the LJ parameter for other robots
+
+# Lennard-Jones interaction magnitude
+
+function FlockInteraction(dist,target,epsilon){
+	var mag = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
+	return mag
+}
+
+#
+#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 "STATE_FREE"
+	}
+	else if(value==2){
+	return "STATE_ASKING"
+	}
+	else if(value==3){
+	return "STATE_JOINING"
+	}
+	else if(value==4){
+	return "STATE_JOINED"
+	}
+	else if(value==5){
+	return "STATE_LOCK"
+	}
+}
+
+#map from state to int
+function s2i(value){
+	if(value=="STATE_FREE"){
+	return 1
+	}
+	else if(value=="STATE_ASKING"){
+	return 2
+	}
+	else if(value=="STATE_JOINING"){
+	return 3
+	}
+	else if(value=="STATE_JOINED"){
+	return 4
+	}
+	else if(value=="STATE_LOCK"){
+	return 5
+	}
+}
+#map form int to response
+function i2r(value){
+	if(value==1){
+	return "REQ_NONE"
+	}
+	else if(value==2){
+	return "REQ_GRANTED"
+	}
+}
+#map from response to int
+function r2i(value){
+	if(value=="REQ_NONE"){
+	return 1
+	}
+	else if(value=="REQ_GRANTED"){
+	return 2
+	}
+}
+
+
+#
+#return the index of value
+#
+function find(table,value){
+	var index=nil
+	var i=0
+	while(i<size(table)){
+		if(table[i]==value)
+			index=i
+		i=i+1
+	}
+	return index
+}
+
+#
+#pack message into 1 number
+#
+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
+    return send_value
+}
+#
+#pack guide message into 1 number
+#
+function pack_guide_msg(send_table){
+    var send_value
+    var r_id=send_table.Label#id of target robot
+    var pon#positive or negative ,0 postive, 1 negative
+    if(send_table.Bearing>=0){
+    pon=0
+    }
+    else{
+    pon=1
+    }
+    var b=math.abs(send_table.Bearing) 
+    send_value=r_id*1000+pon*100+b
+    return send_value
+}
+#
+#unpack message
+#
+function unpackmessage(recv_value){
+    var wan=(recv_value-recv_value%100000)/100000
+    recv_value=recv_value-wan*100000
+    var qian=(recv_value-recv_value%10000)/10000
+    recv_value=recv_value-qian*10000
+    var bai=(recv_value-recv_value%1000)/1000
+    recv_value=recv_value-bai*1000
+    var shi=(recv_value-recv_value%10)/10
+    recv_value=recv_value-shi*10
+    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
+    return_table.Label=qian
+    return_table.ReqLabel=bai
+    return_table.ReqID=shi
+    return_table.Response=ge
+    return return_table
+}
+#
+#unpack guide message
+#
+function unpack_guide_msg(recv_value){
+log(id,"I pass value=",recv_value)
+    var qian=(recv_value-recv_value%1000)/1000
+    recv_value=recv_value-qian*1000
+    var bai=(recv_value-recv_value%100)/100
+    recv_value=recv_value-bai*100
+    var b=recv_value
+    var return_table={.Label=0.0,.Bearing=0.0}
+    return_table.Label=qian
+    if(bai==1){
+    	b=b*-1.0
+    }
+    return_table.Bearing=b
+    return return_table
+}
+
+#get the target distance to neighbr nei_id
+function target4label(nei_id){
+    var return_val="miss"
+    var i=0
+    while(i<size(lock_neighbor_id)){
+		if(lock_neighbor_id[i]==nei_id){
+			return_val=lock_neighbor_dis[i]
+		}
+		i=i+1
+    }
+    return return_val
+}
+#calculate LJ vector for neibhor stored in i
+function LJ_vec(i){
+    var dis=m_MessageRange[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),ljbearing)   
+    }
+    #log(id,"dis=",dis,"target=",target,"label=",nei_id)
+		#log("x=",cDir.x,"y=",cDir.y)  
+    return cDir
+}
+#calculate the motion vector
+function motion_vector(){
+    var i=0
+    var m_vector={.x=0.0,.y=0.0}
+    while(i<m_neighbourCount){
+		#calculate and add the motion vector
+		m_vector=math.vec2.add(m_vector,LJ_vec(i))
+		#log(id,"x=",m_vector.x,"y=",m_vector.y)    
+		i=i+1
+    }
+    m_vector=math.vec2.scale(m_vector,1.0/m_neighbourCount)
+	#log(id,"fnal=","x=",m_vector.x,"y=",m_vector.y)    
+    return m_vector
+}
+
+function start_listen(){
+neighbors.listen("m",
+    function(vid,value,rid){
+    #store the received message
+    var temp_id=rid
+    var recv_val=unpackmessage(value)
+    Get_DisAndAzi(temp_id)
+    #add the received message
+    #
+    m_MessageState[m_neighbourCount]=i2s(recv_val.State)
+    m_MessageLabel[m_neighbourCount]=recv_val.Label
+    m_MessageReqLabel[m_neighbourCount]=recv_val.ReqLabel
+    m_MessageReqID[m_neighbourCount]=recv_val.ReqID
+    m_MessageResponse[m_neighbourCount]=i2r(recv_val.Response)
+    m_MessageRange[m_neighbourCount]=m_receivedMessage.Range
+    m_MessageBearing[m_neighbourCount]=m_receivedMessage.Bearing
+	m_messageID[m_neighbourCount]=rid
+    m_neighbourCount=m_neighbourCount+1
+})
+}
+#
+#Function used to get the station info of the sender of the message
+function Get_DisAndAzi(id){
+	neighbors.foreach(
+	function(rid, data) {
+		if(rid==id){
+		m_receivedMessage.Range=data.distance*100.0
+		m_receivedMessage.Bearing=data.azimuth
+		}
+		})
+}
+
+#
+#Update node info according to neighbour robots
+#
+function UpdateNodeInfo(){
+	#Collect informaiton
+	#Update information
+	var i=0
+
+	while(i<m_neighbourCount){
+		if(m_MessageState[i]=="STATE_JOINED"){
+			m_vecNodes[m_MessageLabel[i]].State="ASSIGNED"
+			m_vecNodes[m_MessageLabel[i]].StateAge=m_unJoiningLostPeriod
+		}
+		else if(m_MessageState[i]=="STATE_JOINING"){
+			m_vecNodes[m_MessageLabel[i]].State="ASSIGNING"
+			m_vecNodes[m_MessageLabel[i]].StateAge=m_unJoiningLostPeriod
+		}
+		i=i+1
+	}
+	#Forget old information
+	i=0
+	while(i<size(m_vecNodes)){
+		if((m_vecNodes[i].StateAge>0) and (m_vecNodes[i].State=="ASSIGNING")){
+		m_vecNodes[i].StateAge=m_vecNodes[i].StateAge-1
+		if(m_vecNodes[i].StateAge==0)
+			m_vecNodes[i].State="UNASSIGNED"
+		}
+	i=i+1
+	}
+}
+
+#
+#Transistion to state free
+#
+function TransitionToFree(){
+	UAVSTATE="STATE_FREE"
+	m_unWaitCount=m_unLabelSearchWaitTime
+	m_selfMessage.State=s2i(UAVSTATE)
+}
+
+#
+#Transistion to state asking
+#
+function TransitionToAsking(un_label){
+	UAVSTATE="STATE_ASKING"
+	m_nLabel=un_label
+	m_unRequestId=id	#don't know why the random numbers are the same, add id to make the ReqID different
+	m_selfMessage.State=s2i(UAVSTATE)
+	m_selfMessage.ReqLabel=m_nLabel
+	m_selfMessage.ReqID=m_unRequestId
+
+	m_unWaitCount=m_unResponseTimeThreshold
+}
+
+#
+#Transistion to state joining
+#
+function TransitionToJoining(){
+	UAVSTATE="STATE_JOINING"
+	m_selfMessage.State=s2i(UAVSTATE) 
+	m_selfMessage.Label=m_nLabel
+	m_unWaitCount=m_unJoiningLostPeriod
+}
+
+#
+#Transistion to state joined
+#
+function TransitionToJoined(){
+	UAVSTATE="STATE_JOINED"
+	m_selfMessage.State=s2i(UAVSTATE)
+	m_selfMessage.Label=m_nLabel
+	m_vecNodes[m_nLabel].State="ASSIGNED"
+
+	#write statues
+	#v_tag.put(m_nLabel, m_lockstig)
+  barrier_create()
+	barrier_ready()
+
+	m_navigation.x=0.0
+	m_navigation.y=0.0
+	uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
+}
+
+#
+#Transistion to state Lock, lock the current formation
+#
+function TransitionToLock(){
+  UAVSTATE="STATE_LOCK"
+  m_selfMessage.State=s2i(UAVSTATE)
+  m_selfMessage.Label=m_nLabel
+  m_vecNodes[m_nLabel].State="ASSIGNED"
+  #record neighbor distance
+  lock_neighbor_id={}
+  lock_neighbor_dis={}
+  var i=0
+  while(i<m_neighbourCount){
+    lock_neighbor_id[i]=m_messageID[i]
+    lock_neighbor_dis[i]=m_MessageRange[i]
+    i=i+1
+  }
+  m_navigation.x=0.0
+  m_navigation.y=0.0
+  uav_moveto(m_navigation.x, m_navigation.y, 0.0)
+}
+
+#
+# Do free
+#
+function DoFree() {
+	m_selfMessage.State=s2i(UAVSTATE)
+
+	#wait for a while before looking for a Label
+	if(m_unWaitCount>0)
+		m_unWaitCount=m_unWaitCount-1
+
+	#find a set of joined robots
+	var setJoinedLabels={}
+	var setJoinedIndexes={}
+	var i=0
+	var j=0
+	while(i<m_neighbourCount){
+		if(m_MessageState[i]=="STATE_JOINED"){
+		setJoinedLabels[j]=m_MessageLabel[i]
+		setJoinedIndexes[j]=i
+		j=j+1
+		}
+		i=i+1
+	}
+
+	#go through the graph to look for a proper Label
+	var unFoundLabel=0
+#	var IDofPred=0
+	i=1
+	while(i<size(m_vecNodes) and (unFoundLabel==0)){
+		#if the node is unassigned and the predecessor is insight
+		if(m_vecNodes[i].State=="UNASSIGNED" and count(setJoinedLabels,m_vecNodes[i].Pred)==1){
+			unFoundLabel=m_vecNodes[i].Label
+#			IDofPred=find(m_MessageLabel,m_vecNodes[unFoundLabel].Pred)
+		}
+		i=i+1
+	}
+
+	if(unFoundLabel>0){
+		TransitionToAsking(unFoundLabel)
+		return
+	}
+
+	#set message
+	m_selfMessage.State=s2i(UAVSTATE)
+
+}
+
+#
+#Do asking
+#
+function DoAsking(){
+	#look for response from predecessor
+	var i=0
+	var psResponse=-1
+	while(i<m_neighbourCount and psResponse==-1){
+		#the respond robot in joined state
+		#the request Label be the same as requesed
+		#get a respond
+		if(m_MessageState[i]=="STATE_JOINED"){
+			log("received label = ",m_MessageReqLabel[i])
+			if(m_MessageReqLabel[i]==m_nLabel)
+				if(m_MessageResponse[i]!="REQ_NONE"){
+				log("get response")
+				psResponse=i
+		}}
+		i=i+1
+	}
+	#analyse response
+	if(psResponse==-1){
+		#no response, wait
+		m_unWaitCount=m_unWaitCount-1
+		m_selfMessage.State=s2i(UAVSTATE)
+		m_selfMessage.ReqLabel=m_nLabel
+		m_selfMessage.ReqID=m_unRequestId
+		if(m_unWaitCount==0){
+			TransitionToFree()
+			return
+		}
+	}
+	else{
+		log("respond id=",m_MessageReqID[psResponse])
+		if(m_MessageReqID[psResponse]!=m_unRequestId){
+			m_vecNodes[m_nLabel].State="ASSIGNING"
+			m_vecNodes[m_nLabel].StateAge=m_unJoiningLostPeriod
+			TransitionToFree()
+    	}
+	if(m_MessageReqID[psResponse]==m_unRequestId){
+		if(m_MessageResponse[psResponse]=="REQ_GRANTED"){
+			TransitionToJoining()
+			return
+		}
+		else{
+			TransitionToAsking(m_nLabel)
+			return
+		}
+	}
+	}
+	m_navigation.x=0.0
+	m_navigation.y=0.0
+	uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
+}
+
+#
+#Do joining
+#
+m_gotjoinedparent = 0
+function DoJoining(){
+
+  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
+#
+function DoJoined(){
+	m_selfMessage.State=s2i(UAVSTATE)
+	m_selfMessage.Label=m_nLabel
+
+	#collect all requests
+	var mapRequests={}
+	var i=0
+	var j=0
+	var ReqLabel
+	var JoiningLabel
+	var seenPred=0
+	while(i<m_neighbourCount){
+		if(m_MessageState[i]=="STATE_ASKING"){
+			ReqLabel=m_MessageReqLabel[i]
+			log("ReqLabel var:",ReqLabel)
+			log("M_vec var",m_vecNodes[ReqLabel].State)
+			if(m_vecNodes[ReqLabel].State=="UNASSIGNED")
+				if(m_nLabel==m_vecNodes[ReqLabel].Pred){
+					#is a request, store the index
+					mapRequests[j]=i
+					log("Into if m_nLabel")
+					j=j+1
+				}
+		}
+		if(m_MessageState[i]=="STATE_JOINING"){
+			JoiningLabel=m_MessageLabel[i]
+			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}
+			}
+		}
+		#if it is the pred
+		if((m_MessageState[i]=="STATE_JOINED" or m_MessageState[i]=="STATE_LOCK") and m_MessageLabel[i]==m_vecNodes[m_nLabel].Pred){
+			seenPred=1
+			m_unWaitCount=m_unJoiningLostPeriod
+		}
+		i=i+1
+	}
+
+	#get request
+	if(size(mapRequests)!=0){
+		i=1
+		var ReqIndex=0
+		while(i<size(mapRequests)){
+			#compare the distance
+			if(m_MessageRange[mapRequests[ReqIndex]]>m_MessageRange[mapRequests[i]])
+				ReqIndex=i
+			i=i+1
+		}
+		#get the best index, whose ReqLabel and Reqid are
+		ReqLabel=m_MessageReqLabel[mapRequests[ReqIndex]]
+		var ReqID=m_MessageReqID[mapRequests[ReqIndex]]
+		m_selfMessage.ReqLabel=ReqLabel
+		m_selfMessage.ReqID=ReqID
+		m_selfMessage.Response=r2i("REQ_GRANTED")
+		m_vecNodes[ReqLabel].State="ASSIGNING"
+		log("Label=",ReqLabel)
+		log("ID=",ReqID)
+    	m_vecNodes[ReqLabel].StateAge=m_unJoiningLostPeriod
+	}
+
+	#lost pred, wait for some time and transit to free
+	if(seenPred==0){
+	   m_unWaitCount=m_unWaitCount-1
+	   if(m_unWaitCount==0){
+	      TransitionToFree()
+	      return
+	   }
+	}
+
+	#check if should to transists to lock
+	#write statues
+  #v_tag.get(m_nLabel)
+	#log(v_tag.size(), " of ", ROBOTS, " ready to lock")
+	#if(v_tag.size()==ROBOTS){
+	#	TransitionToLock()
+	#}
+  barrier_wait(ROBOTS, TransitionToLock, DoJoined, -1)
+}
+
+#
+#Do Lock
+#
+function DoLock(){
+  m_selfMessage.State=s2i(UAVSTATE)
+  m_selfMessage.Label=m_nLabel
+  m_navigation.x=0.0
+  m_navigation.y=0.0
+  #calculate motion vection
+  if(m_nLabel==0){
+      m_navigation.x=0.0	#change value so that robot 0 will move
+      m_navigation.y=0.0
+  }
+  if(m_nLabel!=0){
+      m_navigation=motion_vector()
+  }
+  #move
+  uav_moveto(m_navigation.x, m_navigation.y, 0.0)
+}
+
+function action(){
+  statef=action
+  UAVSTATE="STATE_FREE"
+
+  # reset the graph
+  Reset()
+}
+
+#
+# Executed at init
+#
+function init() {
+	#
+	# Global parameters for graph formation
+	#
+	m_unResponseTimeThreshold=10
+	m_unLabelSearchWaitTime=10
+	m_fTargetDistanceTolerance=100
+	m_fTargetAngleTolerance=0.1
+	m_unJoiningLostPeriod=100
+
+	#
+	# Join Swarm
+	#
+	uav_initswarm()
+  #v_tag = stigmergy.create(m_lockstig)
+  uav_initstig()
+	# go to diff. height since no collision avoidance implemented yet
+	TARGET_ALTITUDE = 2.5 + id * 1.5
+	statef=turnedoff
+	UAVSTATE = "TURNEDOFF"
+}
+
+#
+# Executed every step
+#
+function step() {
+	# listen to potential RC
+	uav_rccmd()
+	# get the swarm commands
+	uav_neicmd()
+	# update the vstig (status/net/batt)
+  uav_updatestig()
+
+	#update the graph
+	UpdateNodeInfo()
+	#reset message package to be sent
+	m_selfMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
+	#
+	# graph state machine
+	#
+	if(UAVSTATE=="STATE_FREE")
+		statef=DoFree
+	else if(UAVSTATE=="STATE_ESCAPE")
+		statef=DoEscape
+	else if(UAVSTATE=="STATE_ASKING")
+		statef=DoAsking
+	else if(UAVSTATE=="STATE_JOINING")
+		statef=DoJoining
+	else if(UAVSTATE=="STATE_JOINED")
+		statef=DoJoined
+	else if(UAVSTATE=="STATE_LOCK")
+		statef=DoLock
+
+	# high level UAV state machine
+	statef()
+
+	log("Current state: ", UAVSTATE, " and label: ", m_nLabel)
+	log("Swarm size: ", ROBOTS)
+	
+	#navigation
+	#broadcast message
+	neighbors.broadcast("m",packmessage(m_selfMessage))
+
+	#
+	#clean message storage
+	m_MessageState={}#store received neighbour message
+	m_MessageLabel={}#store received neighbour message
+	m_MessageReqLabel={}#store received neighbour message
+	m_MessageReqID={}#store received neighbour message
+	m_MessageResponse={}#store received neighbour message
+	m_MessageRange={}#store received neighbour message
+	m_MessageBearing={}#store received neighbour message
+	m_neighbourCount=0
+
+
+	#step cunt+1
+	step_cunt=step_cunt+1
+}
+
+#
+# Executed when reset
+#
+function Reset(){
+	Read_Graph()
+  m_nLabel=-1
+
+	#start listening
+	start_listen()
+	#
+	#set initial state, only one robot choose [A], while the rest choose [B]
+	#
+	#[A]The robot used to triger the formation process is defined as joined,
+	if(id==ROOT_ID){
+		m_nLabel=0
+		TransitionToJoined()
+	}
+	#[B]Other robots are defined as free.
+	else{
+		TransitionToFree()
+	}
+}
+
+#
+# Executed upon destroy
+#
+function destroy() {
+	#clear neighbour message
+	m_navigation.x=0.0
+	m_navigation.y=0.0
+	uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
+	m_vecNodes={}
+	#stop listening 
+	neighbors.ignore("m")
+}

buzz_scripts/include/barrier.bzz

@@ -7,43 +7,63 @@
 #
 # Constants
 #
-BARRIER_VSTIG = 11
 BARRIER_TIMEOUT = 200 # in steps
+BARRIER_VSTIG = 80
+timeW = 0
+barrier = nil
 
 #
 # Sets a barrier
 #
+function barrier_create() {
+  # reset
+  timeW = 0
+  # create barrier vstig
+  #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, bdt) {
   statef = function() {
-    barrier_wait(threshold, transf, resumef, bdt);
+    barrier_wait(threshold, transf, resumef);
   }
-  barrier = stigmergy.create(BARRIER_VSTIG)
+  UAVSTATE = "BARRIERWAIT"
+  barrier_create()
 }
 
 #
 # Make yourself ready
 #
 function barrier_ready() {
+  log("BARRIER READY -------")
   barrier.put(id, 1)
+  barrier.put("d", 0)
 }
 
 #
 # Executes the barrier
 #
-timeW=0
 function barrier_wait(threshold, transf, resumef, bdt) {
   barrier.put(id, 1)
-  UAVSTATE = "BARRIERWAIT"
 
   barrier.get(id)
-  if(barrier.size() >= threshold) {
+  #log("----->BS: ", barrier.size())
-#    getlowest()
+  if(barrier.size() - 1 >= threshold or barrier.get("d") == 1) {
+    barrier.put("d", 1)
+    timeW = 0
     transf()
   } else if(timeW >= BARRIER_TIMEOUT) {
-    barrier = nil
+    log("------> Barrier Timeout !!!!")
+    timeW = 0
     resumef()
-    timeW=0
+  }
-  } else if(bdt!=-1)
+
+  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,14 @@
 # FLIGHT-RELATED FUNCTIONS
 #
 ########################################
-TARGET_ALTITUDE = 5.0
+TARGET_ALTITUDE = 5.0 # m.
 UAVSTATE = "TURNEDOFF"
-GOTO_MAXVEL = 2
+PICTURE_WAIT = 40 # steps
-goal = {.range=0, .bearing=0}
+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() {
 	s = swarm.create(1)
@@ -23,60 +27,88 @@ 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_set(ROBOTS,action,land,-1)
+		barrier_set(ROBOTS, action, land, -1)
 		barrier_ready()
-	}
+	} else {
-	else {
+    log("Altitude: ", TARGET_ALTITUDE)
-		log("Altitude: ", TARGET_ALTITUDE)
+	  neighbors.broadcast("cmd", 22)
-		neighbors.broadcast("cmd", 22)
+	  uav_takeoff(TARGET_ALTITUDE)
-		uav_takeoff(TARGET_ALTITUDE)
+  }
-	}
 }
 
+firstpassL = 1
 function land() {
 	UAVSTATE = "LAND"
 	statef=land
 
-	if(flight.status == 2 or flight.status == 3){
+	neighbors.broadcast("cmd", 21)
-		neighbors.broadcast("cmd", 21)
+	uav_land()
-		uav_land()
+	
-	}
+	if(flight.status != 2 and flight.status != 3){
-	else {
+    barrier_set(ROBOTS,turnedoff,land, 21)
-		barrier_set(ROBOTS,turnedoff,land,21)
 		barrier_ready()
-		timeW=0
-		#barrier = nil
-		#statef=idle
 	}
 }
 
-function goto() {
+function set_goto(transf) {
 	UAVSTATE = "GOTO"
-	statef=goto
+	statef=function() {
-	# print(rc_goto.id, " is going alone to lat=", rc_goto.latitude)
+    gotoWP(transf);
+  }
+
 	if(rc_goto.id==id){
-		s.leave()
+		if(s!=nil){
-		rb_from_gps(rc_goto.latitude, rc_goto.longitude)
+      if(s.in())
-		print(id, " has to move ", goal.range)
+        s.leave()      
-		m_navigation = math.vec2.newp(goal.range, goal.bearing)
+    }
-		if(math.vec2.length(m_navigation)>GOTO_MAXVEL) {
-			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)
-			uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude)
-		else
-			statef = idle
 	} 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
+	if(ptime==PICTURE_WAIT/2)
+	  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(" has to move ", goal.range)
+	m_navigation = math.vec2.newp(goal.range, goal.bearing)
+
+	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(goal.range < GOTODIST_TOL and goal.bearing < GOTOANG_TOL) # reached destination
+		transf()
+	else
+		uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude)
+}
+
 function follow() {
 	if(size(targets)>0) {
 		UAVSTATE = "FOLLOW"
@@ -128,16 +160,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
@@ -156,11 +190,27 @@ function LimitAngle(angle){
 }
 
 function rb_from_gps(lat, lon) {
-  print("rb dbg: ",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;
   ned_y = d_lon/180*math.pi * 6371000.0 * math.cos(lat/180*math.pi);
 	goal.range = math.sqrt(ned_x*ned_x+ned_y*ned_y);
 	goal.bearing = LimitAngle(math.atan(ned_y,ned_x));
+}
+
+function gps_from_vec(vec) {
+  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(Vrange/6371000.0) + math.cos(latR) * math.sin(Vrange/6371000.0) * math.cos(Vbearing));
+  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 {
@@ -22,6 +22,7 @@ namespace buzzuav_closures{
 		COMMAND_DISARM,
 		COMMAND_GOTO,
 		COMMAND_MOVETO,
+		COMMAND_PICTURE,
 	 } Custom_CommandCode;
 
 /*
@@ -30,13 +31,15 @@ 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();
 /*Sets goto position */

include/roscontroller.h

@@ -55,6 +55,8 @@ public:
   //void RosControllerInit();
   void RosControllerRun();
 
+  static const string CAPTURE_SRV_DEFAULT_NAME;
+
 private:
   struct num_robot_count
   {
@@ -70,7 +72,7 @@ private:
     double latitude=0.0;
     float  altitude=0.0;
   }; typedef struct gps GPS ; // not useful in cpp
-  
+
   GPS target, home, cur_pos;
   double cur_rel_altitude;
 
@@ -92,7 +94,19 @@ private:
   /*tmp to be corrected*/
   uint8_t no_cnt=0;
   uint8_t old_val=0	;
-  std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,obstacles_topic,stand_by,xbeesrv_name, setpoint_name;
+  std::string bzzfile_name;
+  std::string fcclient_name;
+  std::string armclient;
+  std::string modeclient;
+  std::string rcservice_name;
+  std::string bcfname,dbgfname;
+  std::string out_payload;
+  std::string in_payload;
+  std::string obstacles_topic;
+  std::string stand_by;
+  std::string xbeesrv_name;
+  std::string capture_srv_name;
+  std::string setpoint_name;
   std::string stream_client_name;
   std::string relative_altitude_sub_name;
   std::string setpoint_nonraw;
@@ -102,6 +116,7 @@ private:
   Num_robot_count count_robots;
   ros::ServiceClient mav_client;
   ros::ServiceClient xbeestatus_srv;
+  ros::ServiceClient capture_srv;
   ros::Publisher payload_pub;
   ros::Publisher MPpayload_pub;
   ros::Publisher neigh_pos_pub;

src/buzz_utility.cpp

@@ -304,9 +304,12 @@ void in_message_process(){
         buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_moveto", 1));
    		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_moveto));
    		buzzvm_gstore(VM);
-        buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_storegoal", 1));
+		buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_storegoal", 1));
    		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_storegoal));
    		buzzvm_gstore(VM);
+        buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_takepicture", 1));
+   		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_takepicture));
+   		buzzvm_gstore(VM);
         buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_arm", 1));
    		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_arm));
    		buzzvm_gstore(VM);
@@ -349,7 +352,7 @@ void in_message_process(){
         buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_moveto", 1));
    		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
    		buzzvm_gstore(VM);
-        buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_storegoal", 1));
+        buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_takepicture", 1));
    		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
    		buzzvm_gstore(VM);
         buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_arm", 1));

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
 	/----------------------------------------*/
@@ -142,7 +193,7 @@ namespace buzzuav_closures{
     cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;*/
       //printf(" Vector for Goto: %.7f,%.7f\n",dx,dy);
     //ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0], goto_pos[1], goto_pos[2]);
-    buzz_cmd= COMMAND_MOVETO; // TO DO what should we use
+    buzz_cmd = COMMAND_MOVETO; // TO DO what should we use
     return buzzvm_ret0(vm);
 	}
 
@@ -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);
@@ -264,19 +316,47 @@ namespace buzzuav_closures{
 		message_number++;*/
 		return payload_out;
 	}
+	/*----------------------------------------/
+	/ Buzz closure to take a picture here.
+	/----------------------------------------*/
+	int buzzuav_takepicture(buzzvm_t vm) {
+	  cur_cmd = mavros_msgs::CommandCode::CMD_DO_SET_CAM_TRIGG_DIST;
+	  buzz_cmd = COMMAND_PICTURE;
+	  return buzzvm_ret0(vm);
+	}
+
 	/*----------------------------------------/
 	/ Buzz closure to store locally a GPS destination from the fleet
 	/----------------------------------------*/
 	int buzzuav_storegoal(buzzvm_t vm) {
 	  buzzvm_lnum_assert(vm, 3);
-	   buzzvm_lload(vm, 1); // latitude
+	  buzzvm_lload(vm, 1); // altitude
-	   buzzvm_lload(vm, 2); // longitude
+	  buzzvm_lload(vm, 2); // longitude
-	   buzzvm_lload(vm, 3); // altitude
+	  buzzvm_lload(vm, 3); // latitude
-	   buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT);
+	  buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT);
-	   buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
+	  buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
-	   buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
+	  buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
-     rc_set_goto(buzzvm_stack_at(vm, 1)->f.value, buzzvm_stack_at(vm, 2)->f.value, buzzvm_stack_at(vm, 3)->f.value, (int)buzz_utility::get_robotid());
+    double goal[3];
-	   return buzzvm_ret0(vm);
+		goal[0] = buzzvm_stack_at(vm, 3)->f.value;
+		goal[1] = buzzvm_stack_at(vm, 2)->f.value;
+		goal[2] = buzzvm_stack_at(vm, 1)->f.value;
+    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

@@ -2,6 +2,8 @@
 #include <thread>
 namespace rosbzz_node {
 
+const string roscontroller::CAPTURE_SRV_DEFAULT_NAME = "/image_sender/capture_image";
+
 /*---------------
 /Constructor
 ---------------*/
@@ -17,6 +19,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);
@@ -200,7 +203,7 @@ void roscontroller::RosControllerRun()
     while (ros::ok() && !buzz_utility::buzz_script_done()) {
       /*Update neighbors position inside Buzz*/
       // buzz_closure::neighbour_pos_callback(neighbours_pos_map);
-      
+
       /*log ROS Time stamp, pos (lat,long,alt), Swarm size, no. of neigh,
         neigh pos, RSSI val, Packet loss, filtered packet loss*/
       log<<ros::Time::now()<<",";
@@ -215,17 +218,17 @@ void roscontroller::RosControllerRun()
           log << (double)it->second.x << "," << (double)it->second.y
               << "," << (double)it->second.z << ",";
         }
-      const uint8_t shrt_id= 0xFF; 
+      const uint8_t shrt_id= 0xFF;
       float result;
-      if ( GetAPIRssi(shrt_id, result) ) 
+      if ( GetAPIRssi(shrt_id, result) )
       	log<<result<<",";
       else
         log<<"0,";
-      if ( GetRawPacketLoss(shrt_id, result) ) 
+      if ( GetRawPacketLoss(shrt_id, result) )
       	log<<result<<",";
       else
         log<<"0,";
-      if ( GetFilteredPacketLoss(shrt_id, result) ) 
+      if ( GetFilteredPacketLoss(shrt_id, result) )
       	log<<result<<",";
       else
         log<<"0,";
@@ -264,15 +267,15 @@ void roscontroller::RosControllerRun()
       // no_of_robots=get_number_of_robots();
       get_number_of_robots();
       buzz_utility::set_robot_var(no_of_robots);
-      /*Retrive the state of the graph and uav and log TODO WARNING :PLEASE REMOVE IF 
+      /*Retrive the state of the graph and uav and log TODO WARNING :PLEASE REMOVE IF
 	SCRIPT IS NOT graphform.bzz*/
       static buzzvm_t     VM = buzz_utility::get_vm();
-      buzzvm_pushs(VM, buzzvm_string_register(VM, "m_eState",1));
+      std::stringstream state_buff;
-            	buzzvm_gload(VM);
+      //buzzvm_pushs(VM, buzzvm_string_register(VM, "m_eState",1));
-            	buzzobj_t graph_state = buzzvm_stack_at(VM, 1);
+      //      	buzzvm_gload(VM);
-            	buzzvm_pop(VM);
+      //      	buzzobj_t graph_state = buzzvm_stack_at(VM, 1);
-		std::stringstream state_buff;
+      //      	buzzvm_pop(VM);
-		state_buff<< graph_state->s.value.str<<",";
+      //	state_buff<< graph_state->s.value.str<<",";
       buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE",1));
             	buzzvm_gload(VM);
             	buzzobj_t uav_state = buzzvm_stack_at(VM, 1);
@@ -366,6 +369,11 @@ void roscontroller::Rosparameters_get(ros::NodeHandle &n_c)
   } else
     n_c.getParam("xbee_status_srv", xbeesrv_name);
 
+  if(!n_c.getParam("capture_image_srv", capture_srv_name))
+  {
+    capture_srv_name = CAPTURE_SRV_DEFAULT_NAME;
+  }
+
   GetSubscriptionParameters(n_c);
   // initialize topics to null?
 }
@@ -474,6 +482,7 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle &n_c)
     service = n_c.advertiseService(rcservice_name, &roscontroller::rc_callback, this);
   ROS_INFO("Ready to receive Mav Commands from RC client");
   xbeestatus_srv = n_c.serviceClient<mavros_msgs::ParamGet>(xbeesrv_name);
+  capture_srv = n_c.serviceClient<mavros_msgs::CommandBool>(capture_srv_name);
   stream_client = n_c.serviceClient<mavros_msgs::StreamRate>(stream_client_name);
 
   users_sub = n_c.subscribe("users_pos", 5, &roscontroller::users_pos, this);
@@ -750,9 +759,10 @@ 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*/
-    // roscontroller::SetLocalPositionNonRaw(goto_pos[1], goto_pos[0],
+    ROS_INFO("TAKING A PICTURE HERE!! --------------");
-    // goto_pos[2], 0);
+    mavros_msgs::CommandBool capture_command;
+    capture_srv.call(capture_command);
   }
 }
 /*----------------------------------------------
@@ -1092,7 +1102,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],