ROSBuzz_MISTLab/buzz_scripts/graphform.bzz

920 lines
21 KiB
Plaintext
Raw Normal View History

2017-06-16 20:31:54 -03:00
#
# Include files
#
include "string.bzz"
include "vec2.bzz"
include "update.bzz"
2017-06-19 17:15:18 -03:00
include "barrier.bzz" # don't use a stigmergy id=11 with this header.
include "uavstates.bzz" # require an 'action' function to be defined here.
2017-07-26 11:10:15 -03:00
include "vstigenv.bzz"
2017-06-16 20:31:54 -03:00
include "graphs/shapes_Y.bzz"
2017-07-07 01:04:40 -03:00
2017-06-26 15:09:33 -03:00
ROBOT_RADIUS=50
2017-06-16 20:31:54 -03:00
ROBOT_DIAMETER=2.0*ROBOT_RADIUS
2017-06-19 14:21:19 -03:00
ROBOT_SAFETYDIST=2.0*ROBOT_DIAMETER
2017-06-16 20:31:54 -03:00
# max velocity in cm/step
ROBOT_MAXVEL = 250.0
2017-06-16 20:31:54 -03:00
#
# Global variables
#
#
# Save message from all neighours
2017-06-16 20:31:54 -03:00
#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
2017-06-16 20:31:54 -03:00
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
2017-06-16 20:31:54 -03:00
#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}
2017-07-07 01:04:40 -03:00
2017-06-16 20:31:54 -03:00
#
#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")}
2017-06-16 20:31:54 -03:00
#Current robot state
m_eState="STATE_FREE"
#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
2017-07-17 10:49:41 -03:00
m_messageID={}
#neighbor distance to lock the current pattern
lock_neighbor_id={}
lock_neighbor_dis={}
2017-06-16 20:31:54 -03:00
#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
m_lockstig = 1
2017-06-16 20:31:54 -03:00
2017-07-07 01:04:40 -03:00
# Lennard-Jones parameters, may need change
EPSILON = 4000 #13.5 the LJ parameter for other robots
2017-06-16 20:31:54 -03:00
# Lennard-Jones interaction magnitude
function FlockInteraction(dist,target,epsilon){
2017-06-26 15:09:33 -03:00
var mag = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
return mag
2017-06-16 20:31:54 -03:00
}
function LimitAngle(angle){
2017-06-26 15:09:33 -03:00
if(angle>2*math.pi)
return angle-2*math.pi
else if (angle<0)
return angle+2*math.pi
else
return angle
2017-06-16 20:31:54 -03:00
}
#
# Calculates the angle of the given vector2.
# PARAM v: The vector2.
# RETURN: The angle of the vector.
#
Angle = function(v) {
return math.atan(v.y, v.x)
}
#
#return the number of value in table
#
function count(table,value){
2017-06-26 15:09:33 -03:00
var number=0
var i=0
while(i<size(table)){
if(table[i]==value){
number=number+1
}
i=i+1
}
return number
2017-06-16 20:31:54 -03:00
}
2017-07-07 01:04:40 -03:00
#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
}
}
2017-06-16 20:31:54 -03:00
#
#return the index of value
#
function find(table,value){
2017-06-26 15:09:33 -03:00
var index=nil
var i=0
while(i<size(table)){
if(table[i]==value)
index=i
i=i+1
}
return index
2017-06-16 20:31:54 -03:00
}
function pow(base,exponent){
2017-06-26 15:09:33 -03:00
var i=0
var renturn_val=1
if(exponent==0)
return 1
else{
while(i<exponent){
renturn_val=renturn_val*base
i=i+1
}
return renturn_val
}
2017-06-16 20:31:54 -03:00
}
2017-07-17 10:49:41 -03:00
#
#pack message into 1 number
#
function packmessage(send_table){
var send_value
send_value=10000*send_table.State+1000*send_table.Label+100*send_table.ReqLabel+10*send_table.ReqID+send_table.Response
2017-07-17 10:49:41 -03:00
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%10000)/10000
recv_value=recv_value-wan*10000
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 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}
2017-07-17 10:49:41 -03:00
return_table.State=wan
return_table.Label=qian
return_table.ReqLabel=bai
2017-07-17 10:49:41 -03:00
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
2017-07-17 10:49:41 -03:00
}
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
2017-07-17 10:49:41 -03:00
}
return return_val
}
#calculate LJ vector for neibhor stored in i
function LJ_vec(i){
var dis=m_MessageRange[i]
var bearing=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)
2017-07-17 10:49:41 -03:00
}
#log(id,"dis=",dis,"target=",target,"label=",nei_id)
#log("x=",cDir.x,"y=",cDir.y)
2017-07-17 10:49:41 -03:00
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
2017-07-17 10:49:41 -03:00
}
m_vector=math.vec2.scale(m_vector,1.0/m_neighbourCount)
#log(id,"fnal=","x=",m_vector.x,"y=",m_vector.y)
2017-07-17 10:49:41 -03:00
return m_vector
}
2017-06-16 20:31:54 -03:00
function start_listen(){
2017-07-07 01:04:40 -03:00
neighbors.listen("m",
2017-06-16 20:31:54 -03:00
function(vid,value,rid){
#store the received message
var temp_id=rid
2017-07-17 10:49:41 -03:00
var recv_val=unpackmessage(value)
2017-06-16 20:31:54 -03:00
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
2017-06-16 20:31:54 -03:00
})
}
#
#Function used to get the station info of the sender of the message
function Get_DisAndAzi(id){
2017-06-26 15:09:33 -03:00
neighbors.foreach(
function(rid, data) {
if(rid==id){
m_receivedMessage.Range=data.distance*100.0
m_receivedMessage.Bearing=data.azimuth
}
})
2017-06-16 20:31:54 -03:00
}
#
#Update node info according to neighbour robots
#
function UpdateNodeInfo(){
2017-06-26 15:09:33 -03:00
#Collect informaiton
#Update information
var i=0
2017-06-16 20:31:54 -03:00
while(i<m_neighbourCount){
2017-06-26 15:09:33 -03:00
if(m_MessageState[i]=="STATE_JOINED"){
m_vecNodes[m_MessageLabel[i]].State="ASSIGNED"
m_vecNodes[m_MessageLabel[i]].StateAge=m_unJoiningLostPeriod
2017-06-26 15:09:33 -03:00
}
else if(m_MessageState[i]=="STATE_JOINING"){
m_vecNodes[m_MessageLabel[i]].State="ASSIGNING"
m_vecNodes[m_MessageLabel[i]].StateAge=m_unJoiningLostPeriod
2017-06-26 15:09:33 -03:00
}
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
2017-06-16 20:31:54 -03:00
}
}
#
#Transistion to state free
#
function TransitionToFree(){
2017-06-26 15:09:33 -03:00
m_eState="STATE_FREE"
m_unWaitCount=m_unLabelSearchWaitTime
2017-07-07 01:04:40 -03:00
m_selfMessage.State=s2i(m_eState)
2017-06-16 20:31:54 -03:00
}
#
#Transistion to state asking
#
function TransitionToAsking(un_label){
2017-06-26 15:09:33 -03:00
m_eState="STATE_ASKING"
m_nLabel=un_label
2017-07-17 10:49:41 -03:00
m_unRequestId=id #don't know why the random numbers are the same, add id to make the ReqID different
2017-07-07 01:04:40 -03:00
m_selfMessage.State=s2i(m_eState)
m_selfMessage.ReqLabel=m_nLabel
2017-06-26 15:09:33 -03:00
m_selfMessage.ReqID=m_unRequestId
2017-06-16 20:31:54 -03:00
2017-06-26 15:09:33 -03:00
m_unWaitCount=m_unResponseTimeThreshold
2017-06-16 20:31:54 -03:00
}
#
#Transistion to state joining
#
function TransitionToJoining(){
2017-06-26 15:09:33 -03:00
m_eState="STATE_JOINING"
2017-07-07 01:04:40 -03:00
m_selfMessage.State=s2i(m_eState)
m_selfMessage.Label=m_nLabel
2017-06-26 15:09:33 -03:00
m_unWaitCount=m_unJoiningLostPeriod
2017-06-16 20:31:54 -03:00
2017-07-07 01:04:40 -03:00
neighbors.listen("r",
2017-07-17 10:49:41 -03:00
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
}
})
2017-06-16 20:31:54 -03:00
}
#
#Transistion to state joined
#
function TransitionToJoined(){
2017-06-26 15:09:33 -03:00
m_eState="STATE_JOINED"
2017-07-07 01:04:40 -03:00
m_selfMessage.State=s2i(m_eState)
m_selfMessage.Label=m_nLabel
2017-06-26 15:09:33 -03:00
m_vecNodes[m_nLabel].State="ASSIGNED"
2017-07-07 01:04:40 -03:00
neighbors.ignore("r")
2017-06-16 20:31:54 -03:00
2017-06-26 15:09:33 -03:00
#write statues
v_tag.put(m_nLabel, m_lockstig)
2017-06-16 20:31:54 -03:00
2017-06-26 15:09:33 -03:00
m_navigation.x=0.0
m_navigation.y=0.0
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
2017-06-16 20:31:54 -03:00
}
2017-07-17 10:49:41 -03:00
#
#Transistion to state Lock, lock the current formation
#
2017-06-16 20:31:54 -03:00
#
#Transistion to state Lock, lock the current formation
#
function TransitionToLock(){
2017-07-17 10:49:41 -03:00
m_eState="STATE_LOCK"
m_selfMessage.State=s2i(m_eState)
m_selfMessage.Label=m_nLabel
2017-07-17 10:49:41 -03:00
m_vecNodes[m_nLabel].State="ASSIGNED"
#record neighbor distance
lock_neighbor_id={}
lock_neighbor_dis={}
var i=0
while(i<m_neighbourCount){
2017-07-17 10:49:41 -03:00
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)
2017-06-16 20:31:54 -03:00
}
#
# Do free
#
function DoFree() {
2017-07-07 01:04:40 -03:00
m_selfMessage.State=s2i(m_eState)
#wait for a while before looking for a Label
2017-06-16 20:31:54 -03:00
if(m_unWaitCount>0)
m_unWaitCount=m_unWaitCount-1
#find a set of joined robots
var setJoinedLabels={}
2017-06-16 20:31:54 -03:00
var setJoinedIndexes={}
var i=0
var j=0
while(i<m_neighbourCount){
2017-06-16 20:31:54 -03:00
if(m_MessageState[i]=="STATE_JOINED"){
setJoinedLabels[j]=m_MessageLabel[i]
2017-06-16 20:31:54 -03:00
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
2017-06-16 20:31:54 -03:00
i=1
while(i<size(m_vecNodes) and (unFoundLabel==0)){
2017-06-16 20:31:54 -03:00
#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)
2017-06-16 20:31:54 -03:00
}
i=i+1
}
if(unFoundLabel>0){
TransitionToAsking(unFoundLabel)
2017-06-16 20:31:54 -03:00
return
}
#navigation
#if there is a joined robot within sight, move around joined robots
#else, gather with other free robots
if(size(setJoinedIndexes)>0){
var tempvec_P={.x=0.0,.y=0.0}
var tempvec_N={.x=0.0,.y=0.0}
i=0
while(i<size(setJoinedIndexes)){
var index=setJoinedIndexes[i]
tempvec_P=math.vec2.add(tempvec_P,math.vec2.newp(m_MessageRange[index],m_MessageBearing[index]+0.5*math.pi))
tempvec_N=math.vec2.add(tempvec_N,math.vec2.newp(m_MessageRange[index]-5.0*ROBOT_SAFETYDIST,m_MessageBearing[index]))
i=i+1
}
tempvec_P=math.vec2.scale(tempvec_P,size(setJoinedIndexes))
tempvec_N=math.vec2.scale(tempvec_N,size(setJoinedIndexes))
m_navigation=math.vec2.add(tempvec_P,tempvec_N)
# Limit the mvt
if(math.vec2.length(m_navigation)>ROBOT_MAXVEL*2)
m_navigation = math.vec2.scale(m_navigation, ROBOT_MAXVEL*2/math.vec2.length(m_navigation))
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
2017-06-16 20:31:54 -03:00
}else{ #no joined robots in sight
i=0
var tempvec={.x=0.0,.y=0.0}
while(i<m_neighbourCount){
2017-06-16 20:31:54 -03:00
tempvec=math.vec2.add(tempvec,math.vec2.newp(m_MessageRange[i]-2.0*ROBOT_SAFETYDIST,m_MessageBearing[i]))
i=i+1
}
m_navigation=math.vec2.scale(tempvec,1.0/i)
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
2017-06-16 20:31:54 -03:00
}
#jump the first step
if(step_cunt<=1){
m_navigation.x=0.0
m_navigation.y=0.0
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
2017-06-16 20:31:54 -03:00
}
#set message
2017-07-07 01:04:40 -03:00
m_selfMessage.State=s2i(m_eState)
2017-06-16 20:31:54 -03:00
}
#
#Do asking
#
function DoAsking(){
#look for response from predecessor
var i=0
var psResponse=-1
while(i<m_neighbourCount and psResponse==-1){
2017-06-16 20:31:54 -03:00
#the respond robot in joined state
#the request Label be the same as requesed
2017-06-16 20:31:54 -03:00
#get a respond
if(m_MessageState[i]=="STATE_JOINED"){
if(m_MessageReqLabel[i]==m_nLabel)
2017-06-16 20:31:54 -03:00
if(m_MessageResponse[i]!="REQ_NONE"){
psResponse=i
}}
i=i+1
}
#analyse response
if(psResponse==-1){
#no response, wait
m_unWaitCount=m_unWaitCount-1
2017-07-07 01:04:40 -03:00
m_selfMessage.State=s2i(m_eState)
m_selfMessage.ReqLabel=m_nLabel
2017-06-16 20:31:54 -03:00
m_selfMessage.ReqID=m_unRequestId
if(m_unWaitCount==0){
TransitionToFree()
return
}
}
else{
if(m_MessageReqID[psResponse]!=m_unRequestId){
m_vecNodes[m_nLabel].State="ASSIGNING"
m_vecNodes[m_nLabel].StateAge=m_unJoiningLostPeriod
TransitionToFree()
}
2017-06-16 20:31:54 -03:00
if(m_MessageReqID[psResponse]==m_unRequestId){
if(m_MessageResponse[psResponse]=="REQ_GRANTED"){
TransitionToJoining()
#TransitionToJoined()
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)
2017-06-16 20:31:54 -03:00
}
#
#Do joining
#
function DoJoining(){
#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")
2017-06-16 20:31:54 -03:00
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
2017-07-07 01:04:40 -03:00
m_cMeToPred.GlobalBearing=LimitAngle(m_cMeToPred.GlobalBearing)
2017-06-16 20:31:54 -03:00
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=Angle(S2Target)
m_bias=m_cMeToPred.Bearing-S2PGlobalBearing
S2Target_bearing=S2Target_bearing+m_bias
# Limit the mvt
if(math.vec2.length(S2Target)>ROBOT_MAXVEL)
S2Target = math.vec2.scale(S2Target, ROBOT_MAXVEL/math.vec2.length(S2Target))
2017-06-26 15:09:33 -03:00
m_navigation=math.vec2.newp(math.vec2.length(S2Target),S2Target_bearing)
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
2017-06-16 20:31:54 -03:00
2017-06-16 20:31:54 -03:00
#test if is already in desired position
if(math.vec2.length(S2Target)<m_fTargetDistanceTolerance and S2Target_bearing<m_fTargetAngleTolerance){
log("JOINED! ", math.vec2.length(S2Target), ", ", S2Target_bearing)
2017-06-26 15:09:33 -03:00
TransitionToJoined()
2017-06-16 20:31:54 -03:00
return
}
} else{ #miss pred, there is a change the another robot block the sight, keep moving as before for sometime
m_unWaitCount=m_unWaitCount-1
}
if(m_unWaitCount==0){
TransitionToFree()
return
}
#pack the communication package
2017-07-07 01:04:40 -03:00
m_selfMessage.State=s2i(m_eState)
m_selfMessage.Label=m_nLabel
2017-06-16 20:31:54 -03:00
}
#
#Do joined
#
function DoJoined(){
2017-07-07 01:04:40 -03:00
m_selfMessage.State=s2i(m_eState)
m_selfMessage.Label=m_nLabel
2017-06-16 20:31:54 -03:00
#collect all requests
var mapRequests={}
var i=0
var j=0
var ReqLabel
var JoiningLabel
2017-06-16 20:31:54 -03:00
var seenPred=0
while(i<m_neighbourCount){
2017-06-16 20:31:54 -03:00
if(m_MessageState[i]=="STATE_ASKING"){
ReqLabel=m_MessageReqLabel[i]
if(m_vecNodes[ReqLabel].State=="UNASSIGNED")
if(m_nLabel==m_vecNodes[ReqLabel].Pred){
2017-06-16 20:31:54 -03:00
#is a request, store the index
mapRequests[j]=i
j=j+1
}
}
if(m_MessageState[i]=="STATE_JOINING"){
JoiningLabel=m_MessageLabel[i]
if(m_nLabel==m_vecNodes[JoiningLabel].Pred){
2017-06-16 20:31:54 -03:00
##joining wrt this dot,send the global bearing
var m_messageForJoining={.Label=JoiningLabel,.Bearing=m_MessageBearing[i]-m_bias}
2017-07-17 10:49:41 -03:00
neighbors.broadcast("r",pack_guide_msg(m_messageForJoining))
2017-06-16 20:31:54 -03:00
}
}
#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){
2017-06-16 20:31:54 -03:00
seenPred=1
m_unWaitCount=m_unJoiningLostPeriod
2017-06-16 20:31:54 -03:00
}
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]]
2017-06-16 20:31:54 -03:00
var ReqID=m_MessageReqID[mapRequests[ReqIndex]]
m_selfMessage.ReqLabel=ReqLabel
2017-06-16 20:31:54 -03:00
m_selfMessage.ReqID=ReqID
2017-07-07 01:04:40 -03:00
m_selfMessage.Response=r2i("REQ_GRANTED")
m_vecNodes[ReqLabel].State="ASSIGNING"
m_vecNodes[ReqLabel].StateAge=m_unJoiningLostPeriod
2017-06-16 20:31:54 -03:00
}
#lost pred, wait for some time and transit to free
if(seenPred==0){
m_unWaitCount=m_unWaitCount-1
if(m_unWaitCount==0){
TransitionToFree()
return
}
}
2017-06-16 20:31:54 -03:00
m_navigation.x=0.0
m_navigation.y=0.0
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
2017-06-16 20:31:54 -03:00
#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()
}
2017-06-16 20:31:54 -03:00
}
#
#Do Lock
#
function DoLock(){
2017-07-07 01:04:40 -03:00
m_selfMessage.State=s2i(m_eState)
m_selfMessage.Label=m_nLabel
2017-06-16 20:31:54 -03:00
m_navigation.x=0.0
m_navigation.y=0.0
#calculate motion vection
if(m_nLabel==0){
m_navigation.x=0.25 #change value so that robot 0 will move
2017-07-17 10:49:41 -03:00
m_navigation.y=0.0
}
if(m_nLabel!=0){
m_navigation=motion_vector()
2017-06-16 20:31:54 -03:00
}
#move
uav_moveto(m_navigation.x, m_navigation.y, 0.0)
2017-06-19 17:15:18 -03:00
}
function action(){
statef=action
2017-06-26 15:09:33 -03:00
UAVSTATE="GRAPH"
2017-06-16 20:31:54 -03:00
}
#
# Executed at init
#
function init() {
#
#Adjust parameters here
#
m_unResponseTimeThreshold=10
m_unLabelSearchWaitTime=10
m_fTargetDistanceTolerance=50
m_fTargetAngleTolerance=0.1
2017-06-16 20:31:54 -03:00
m_unJoiningLostPeriod=100
#
# Join Swarm
#
2017-06-19 17:15:18 -03:00
uav_initswarm()
v_tag = stigmergy.create(m_lockstig)
2017-07-26 11:10:15 -03:00
uav_initstig()
2017-07-22 15:14:06 -03:00
Reset()
TARGET_ALTITUDE = 2.5 + id * 1.5
2017-06-16 20:31:54 -03:00
}
#
# Executed every step
#
function step(){
2017-06-19 17:15:18 -03:00
uav_rccmd()
uav_neicmd()
2017-07-26 11:10:15 -03:00
uav_updatestig()
2017-06-16 20:31:54 -03:00
#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")}
2017-06-16 20:31:54 -03:00
#
#act according to current state
#
2017-06-26 15:09:33 -03:00
if(UAVSTATE=="GRAPH"){
2017-06-16 20:31:54 -03:00
if(m_eState=="STATE_FREE")
DoFree()
else if(m_eState=="STATE_ESCAPE")
DoEscape()
else if(m_eState=="STATE_ASKING")
DoAsking()
else if(m_eState=="STATE_JOINING")
DoJoining()
else if(m_eState=="STATE_JOINED")
DoJoined()
else if(m_eState=="STATE_LOCK")
DoLock()
}
statef()
debug(m_eState,m_nLabel)
log("Current state: ", UAVSTATE)
log("Swarm size: ", ROBOTS)
# if(id==0)
# stattab_print()
2017-07-26 11:10:15 -03:00
2017-06-16 20:31:54 -03:00
#navigation
#broadcast messag
2017-07-17 10:49:41 -03:00
neighbors.broadcast("m",packmessage(m_selfMessage))
2017-06-16 20:31:54 -03:00
#
#clean message storage
m_MessageState={}#store received neighbour message
m_MessageLabel={}#store received neighbour message
m_MessageReqLabel={}#store received neighbour message
2017-06-16 20:31:54 -03:00
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
2017-06-16 20:31:54 -03:00
#step cunt+1
step_cunt=step_cunt+1
}
#
# 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
2017-07-17 10:49:41 -03:00
# m_vecNodes_fixed={}
# m_vecNodes_fixed=parse_graph_fixed("/home/dave/ROS_WS/src/rosbuzz/buzz_scripts/include/Graph_fixed.graph")
Read_Graph()
2017-06-16 20:31:54 -03:00
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==0){
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)
2017-06-16 20:31:54 -03:00
m_vecNodes={}
#stop listening
2017-07-07 01:04:40 -03:00
neighbors.ignore("m")
}