787 lines
19 KiB
Plaintext
787 lines
19 KiB
Plaintext
#
|
|
# Include files
|
|
#
|
|
|
|
include "taskallocate/graphs/shapes_P.bzz"
|
|
include "taskallocate/graphs/shapes_O.bzz"
|
|
include "taskallocate/graphs/shapes_L.bzz"
|
|
include "taskallocate/graphs/shapes_Y.bzz"
|
|
|
|
ROBOT_RADIUS = 50
|
|
ROBOT_DIAMETER = 2.0*ROBOT_RADIUS
|
|
ROBOT_SAFETYDIST = 2.0*ROBOT_DIAMETER
|
|
ROOT_ID = 2
|
|
graph_id = 3
|
|
graph_loop = 1
|
|
|
|
#
|
|
# 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("GRAPH_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("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
|
|
|
|
#navigation vector
|
|
m_navigation={.x=0,.y=0}
|
|
|
|
#Current label being requested or chosen (-1 when none)
|
|
m_nLabel=-1
|
|
m_messageID={}
|
|
repeat_assign=0
|
|
assign_label=-1
|
|
assign_id=-1
|
|
m_gotjoinedparent = 0
|
|
#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
|
|
|
|
# virtual stigmergy for the LOCK barrier.
|
|
m_lockstig = 1
|
|
|
|
# Lennard-Jones parameters, may need change
|
|
EPSILON_GRAPH = 4000 #13.5 the LJ parameter for other robots
|
|
|
|
#
|
|
#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 "GRAPH_FREE"
|
|
}
|
|
else if(value==2){
|
|
return "GRAPH_ASKING"
|
|
}
|
|
else if(value==3){
|
|
return "GRAPH_JOINING"
|
|
}
|
|
else if(value==4){
|
|
return "GRAPH_JOINED"
|
|
}
|
|
else if(value==5){
|
|
return "GRAPH_LOCK"
|
|
}
|
|
}
|
|
#
|
|
#map from state to int
|
|
#
|
|
function s2i(value){
|
|
if(value=="GRAPH_FREE"){
|
|
return 1
|
|
}
|
|
else if(value=="GRAPH_ASKING"){
|
|
return 2
|
|
}
|
|
else if(value=="GRAPH_JOINING"){
|
|
return 3
|
|
}
|
|
else if(value=="GRAPH_JOINED"){
|
|
return 4
|
|
}
|
|
else if(value=="GRAPH_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 ind=nil
|
|
var i=0
|
|
while(i<size(table)){
|
|
if(table[i]==value)
|
|
ind=i
|
|
i=i+1
|
|
}
|
|
return ind
|
|
}
|
|
#
|
|
#pack message into 1 number
|
|
#
|
|
function packmessage(send_table){
|
|
var send_value
|
|
send_value=1000000*send_table.State+100000*send_table.Label+10000*send_table.ReqLabel+100*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%1000000)/1000000
|
|
recv_value=recv_value-wan*1000000
|
|
var qian=(recv_value-recv_value%100000)/100000
|
|
recv_value=recv_value-qian*100000
|
|
var bai=(recv_value-recv_value%10000)/10000
|
|
recv_value=recv_value-bai*10000
|
|
var shi=(recv_value-recv_value%100)/100
|
|
recv_value=recv_value-shi*100
|
|
var ge=recv_value
|
|
var return_table={.State=0.0,.Label=0.0,.ReqLabel=0.0,.ReqID=0.0,.Response=0.0}
|
|
return_table.State=wan
|
|
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(lj_magnitude(dis,target,EPSILON_GRAPH), 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
|
|
}
|
|
#
|
|
# starts the neighbors listener
|
|
#
|
|
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
|
|
|
|
#log(rid, " is in ", m_MessageState[m_neighbourCount], " ", m_MessageLabel[m_neighbourCount])
|
|
|
|
m_neighbourCount=m_neighbourCount+1
|
|
})
|
|
}
|
|
#
|
|
#Function used to get the station info of the sender of the message
|
|
#
|
|
function Get_DisAndAzi(t_id){
|
|
neighbors.foreach(
|
|
function(rid, data) {
|
|
if(rid==t_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]=="GRAPH_JOINED"){
|
|
m_vecNodes[m_MessageLabel[i]].State="ASSIGNED"
|
|
m_vecNodes[m_MessageLabel[i]].StateAge=m_unJoiningLostPeriod
|
|
}
|
|
else if(m_MessageState[i]=="GRAPH_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
|
|
}
|
|
}
|
|
|
|
#
|
|
# Do free
|
|
#
|
|
function DoFree() {
|
|
if(BVMSTATE!="GRAPH_FREE"){
|
|
BVMSTATE="GRAPH_FREE"
|
|
m_unWaitCount=m_unLabelSearchWaitTime
|
|
m_selfMessage.State=s2i(BVMSTATE)
|
|
}else{
|
|
UpdateGraph()
|
|
m_selfMessage.State=s2i(BVMSTATE)
|
|
|
|
#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]=="GRAPH_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){
|
|
m_nLabel=unFoundLabel
|
|
DoAsking()
|
|
return
|
|
}
|
|
|
|
#set message
|
|
m_selfMessage.State=s2i(BVMSTATE)
|
|
|
|
BroadcastGraph()
|
|
}
|
|
}
|
|
#
|
|
#Do asking
|
|
#
|
|
function DoAsking(){
|
|
if(BVMSTATE!="GRAPH_ASKING"){
|
|
BVMSTATE="GRAPH_ASKING"
|
|
#math.rng.setseed(id)
|
|
m_unRequestId=id#math.rng.uniform(0,10) #don't know why the random numbers are the same, add id to make the ReqID different
|
|
m_selfMessage.State=s2i(BVMSTATE)
|
|
m_selfMessage.ReqLabel=m_nLabel
|
|
m_selfMessage.ReqID=m_unRequestId
|
|
m_unWaitCount=m_unResponseTimeThreshold
|
|
}else {
|
|
UpdateGraph()
|
|
#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]=="GRAPH_JOINED"){
|
|
#log("received label = ",m_MessageReqLabel[i])
|
|
if(m_MessageReqLabel[i]==m_nLabel)
|
|
if(m_MessageResponse[i]!="REQ_NONE"){
|
|
psResponse=i
|
|
}}
|
|
if(m_MessageState[i]=="GRAPH_JOINING" and m_MessageLabel[i]==m_nLabel){
|
|
DoFree()
|
|
return
|
|
}
|
|
i=i+1
|
|
}
|
|
#analyse response
|
|
if(psResponse==-1){
|
|
#no response, wait
|
|
|
|
m_unWaitCount=m_unWaitCount-1
|
|
m_selfMessage.State=s2i(BVMSTATE)
|
|
m_selfMessage.ReqLabel=m_nLabel
|
|
m_selfMessage.ReqID=m_unRequestId
|
|
#if(m_unWaitCount==0){
|
|
#DoFree()
|
|
#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
|
|
DoFree()
|
|
}
|
|
if(m_MessageReqID[psResponse]==m_unRequestId){
|
|
if(m_MessageResponse[psResponse]=="REQ_GRANTED"){
|
|
DoJoining()
|
|
return
|
|
}
|
|
else{
|
|
DoAsking()
|
|
return
|
|
}
|
|
}
|
|
}
|
|
m_selfMessage.Label=m_nLabel
|
|
|
|
m_navigation.x=0.0
|
|
m_navigation.y=0.0
|
|
goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0, 0.0)
|
|
BroadcastGraph()
|
|
}
|
|
}
|
|
#
|
|
#Do joining
|
|
#
|
|
function DoJoining(){
|
|
if(BVMSTATE!="GRAPH_JOINING") {
|
|
BVMSTATE="GRAPH_JOINING"
|
|
m_selfMessage.State=s2i(BVMSTATE)
|
|
m_selfMessage.Label=m_nLabel
|
|
m_unWaitCount=m_unJoiningLostPeriod
|
|
} else {
|
|
UpdateGraph()
|
|
if(m_gotjoinedparent!=1)
|
|
set_rc_goto()
|
|
else
|
|
goto_gps(DoJoined)
|
|
#pack the communication package
|
|
m_selfMessage.State=s2i(BVMSTATE)
|
|
m_selfMessage.Label=m_nLabel
|
|
BroadcastGraph()
|
|
}
|
|
}
|
|
|
|
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]=="GRAPH_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 position of self to pred in local coordinate
|
|
m_cMeToPred.Bearing=m_MessageBearing[IDofPred]
|
|
|
|
var S2Pred=math.vec2.newp(m_cMeToPred.Range,m_cMeToPred.Bearing)
|
|
var S2Target=math.vec2.add(S2Pred,P2Target)
|
|
|
|
goal = gps_from_vec(math.vec2.newp(math.vec2.length(S2Target)/100.0, math.atan(S2Target.y,S2Target.x)))
|
|
print("Saving GPS goal (", m_nLabel, " from ", IDofPred, "): ",goal.latitude, goal.longitude, "(", S2Target.x/100, S2Target.y/100, ")")
|
|
storegoal(goal.latitude, goal.longitude, pose.position.altitude)
|
|
m_gotjoinedparent = 1
|
|
}
|
|
}
|
|
#
|
|
#Do joined
|
|
#
|
|
function DoJoined(){
|
|
if(BVMSTATE!="GRAPH_JOINED"){
|
|
BVMSTATE="GRAPH_JOINED"
|
|
m_selfMessage.State=s2i(BVMSTATE)
|
|
m_selfMessage.Label=m_nLabel
|
|
m_vecNodes[m_nLabel].State="ASSIGNED"
|
|
|
|
#write statues
|
|
barrier_create()
|
|
barrier_ready(-1)
|
|
|
|
m_navigation.x=0.0
|
|
m_navigation.y=0.0
|
|
goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0, 0.0)
|
|
} else {
|
|
UpdateGraph()
|
|
m_selfMessage.State=s2i(BVMSTATE)
|
|
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]=="GRAPH_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
|
|
j=j+1
|
|
}
|
|
}
|
|
if(m_MessageState[i]=="GRAPH_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(m_MessageState[i]=="GRAPH_JOINING" and repeat_assign==1 and m_MessageLabel[i]==assign_label){
|
|
repeat_assign=0
|
|
}
|
|
|
|
|
|
#if it is the pred
|
|
if((m_MessageState[i]=="GRAPH_JOINED" or m_MessageState[i]=="GRAPH_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
|
|
}
|
|
if(repeat_assign==0){
|
|
#get the best index, whose ReqLabel and Reqid are
|
|
ReqLabel=m_MessageReqLabel[mapRequests[ReqIndex]]
|
|
var ReqID=m_MessageReqID[mapRequests[ReqIndex]]
|
|
assign_label=ReqLabel
|
|
assign_id=ReqID
|
|
repeat_assign=1
|
|
}
|
|
m_selfMessage.ReqLabel=assign_label
|
|
m_selfMessage.ReqID=assign_id
|
|
m_selfMessage.Response=r2i("REQ_GRANTED")
|
|
#m_vecNodes[ReqLabel].State="ASSIGNING"
|
|
log("Label=",assign_label)
|
|
log("ID=",assign_id)
|
|
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){
|
|
DoFree()
|
|
return
|
|
}
|
|
}
|
|
# using JOINED as the resume state kinds of just reset the barrier timeout, should be IDLE/ALLOCATE...
|
|
barrier_wait(ROBOTS, "GRAPH_LOCK", "GRAPH_JOINED", -1)
|
|
BroadcastGraph()
|
|
}
|
|
}
|
|
#
|
|
#Do Lock
|
|
#
|
|
timeout_graph = 40
|
|
function DoLock() {
|
|
#[transition to lock...
|
|
BVMSTATE="GRAPH_LOCK"
|
|
m_selfMessage.State=s2i(BVMSTATE)
|
|
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
|
|
goto_abs(m_navigation.x, m_navigation.y, 0.0, 0.0)
|
|
|
|
#stop listening
|
|
neighbors.ignore("m")
|
|
#..]
|
|
UpdateGraph()
|
|
m_selfMessage.State=s2i(BVMSTATE)
|
|
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
|
|
|
|
# goto_abs(m_navigation.x, m_navigation.y, 0.0, 0.0)
|
|
BroadcastGraph()
|
|
|
|
if(graph_loop) {
|
|
if(timeout_graph==0) {
|
|
if(graph_id < 3)
|
|
graph_id = graph_id + 1
|
|
else
|
|
graph_id = 0
|
|
timeout_graph = 40
|
|
BVMSTATE="TASK_ALLOCATE"
|
|
}
|
|
timeout_graph = timeout_graph - 1
|
|
}
|
|
}
|
|
|
|
#
|
|
# Executed at init
|
|
#
|
|
function initGraph() {
|
|
#
|
|
# Global parameters for graph formation
|
|
#
|
|
m_unResponseTimeThreshold=10
|
|
m_unLabelSearchWaitTime=10
|
|
m_fTargetDistanceTolerance=100
|
|
m_fTargetAngleTolerance=0.1
|
|
m_unJoiningLostPeriod=100
|
|
}
|
|
#
|
|
# Executed every step (main loop)
|
|
#
|
|
function UpdateGraph() {
|
|
#update the graph
|
|
UpdateNodeInfo()
|
|
#reset message package to be sent
|
|
m_selfMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
|
|
}
|
|
|
|
function BroadcastGraph() {
|
|
#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
|
|
|
|
}
|
|
#
|
|
# Executed when reset
|
|
#
|
|
function resetGraph(){
|
|
m_receivedMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0}
|
|
m_selfMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
|
|
m_navigation={.x=0,.y=0}
|
|
m_nLabel=-1
|
|
m_messageID={}
|
|
lock_neighbor_id={}
|
|
lock_neighbor_dis={}
|
|
m_unRequestId=0
|
|
m_bias=0
|
|
m_cMeToPred={.Range=0.0,.Bearing=0.0,.GlobalBearing=0.0}
|
|
m_unWaitCount=0
|
|
repeat_assign=0
|
|
assign_label=-1
|
|
assign_id=-1
|
|
m_gotjoinedparent = 0
|
|
|
|
if(graph_id==0){
|
|
log("Loading P graph")
|
|
Read_GraphP()
|
|
} else if(graph_id==1) {
|
|
log("Loading O graph")
|
|
Read_GraphO()
|
|
} else if(graph_id==2) {
|
|
log("Loading L graph")
|
|
Read_GraphL()
|
|
} else if(graph_id==3) {
|
|
log("Loading Y graph")
|
|
Read_GraphY()
|
|
}
|
|
|
|
#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
|
|
DoJoined()
|
|
}else{ #[B]Other robots are defined as free.
|
|
DoFree()
|
|
}
|
|
}
|
|
#
|
|
# Executed upon destroy
|
|
#
|
|
function destroyGraph() {
|
|
#clear neighbour message
|
|
m_navigation.x=0.0
|
|
m_navigation.y=0.0
|
|
goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0, 0.0)
|
|
m_vecNodes={}
|
|
#stop listening
|
|
neighbors.ignore("m")
|
|
}
|