removed old buzz_scripts
This commit is contained in:
dave
parent
3576b624aa
commit
4c2dfe655d
|
|
@ -1,896 +0,0 @@
|
|||
#
|
||||
# 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=11 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 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)
|
||||
}
|
||||
#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
|
||||
|
||||
neighbors.listen("r",
|
||||
function(vid,value,rid){
|
||||
var recv_table={.Label=0,.Bearing=0.0}
|
||||
recv_table=unpack_guide_msg(value)
|
||||
#store the received message
|
||||
if(recv_table.Label==m_nLabel){
|
||||
m_cMeToPred.GlobalBearing=recv_table.Bearing
|
||||
}
|
||||
})
|
||||
|
||||
}
|
||||
|
||||
#
|
||||
#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"
|
||||
neighbors.ignore("r")
|
||||
|
||||
#write statues
|
||||
v_tag.put(m_nLabel, m_lockstig)
|
||||
|
||||
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
|
||||
#
|
||||
#
|
||||
#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
|
||||
}
|
||||
|
||||
#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)
|
||||
}else{ #no joined robots in sight
|
||||
i=0
|
||||
var tempvec={.x=0.0,.y=0.0}
|
||||
|
||||
while(i<m_neighbourCount){
|
||||
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)
|
||||
}
|
||||
|
||||
|
||||
|
||||
#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)
|
||||
}
|
||||
#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
|
||||
#
|
||||
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")
|
||||
IDofPred=i
|
||||
i=i+1
|
||||
}
|
||||
|
||||
#found pred
|
||||
if(IDofPred!=-1){
|
||||
m_unWaitCount=m_unJoiningLostPeriod#if see pred, reset the timer
|
||||
|
||||
var P2Target=math.vec2.newp(m_vecNodes[m_nLabel].distance,m_vecNodes[m_nLabel].bearing)
|
||||
m_cMeToPred.Range=m_MessageRange[IDofPred]#the poition of self to pred in local coordinate
|
||||
m_cMeToPred.Bearing=m_MessageBearing[IDofPred]
|
||||
|
||||
#attention, m_cMeToPred.GlobalBearing is the bearing of self wrt pred in global coordinate
|
||||
var S2PGlobalBearing=0
|
||||
|
||||
m_cMeToPred.GlobalBearing=LimitAngle(m_cMeToPred.GlobalBearing)
|
||||
|
||||
if(m_cMeToPred.GlobalBearing>math.pi)
|
||||
S2PGlobalBearing=m_cMeToPred.GlobalBearing-math.pi
|
||||
else
|
||||
S2PGlobalBearing=m_cMeToPred.GlobalBearing+math.pi
|
||||
|
||||
var S2Pred=math.vec2.newp(m_cMeToPred.Range,S2PGlobalBearing)
|
||||
|
||||
#the vector from self to target in global coordinate
|
||||
var S2Target=math.vec2.add(S2Pred,P2Target)
|
||||
#change the vector to local coordinate of self
|
||||
var S2Target_bearing=math.atan(S2Target.y, S2Target.x)
|
||||
m_bias=m_cMeToPred.Bearing-S2PGlobalBearing
|
||||
S2Target_bearing=S2Target_bearing+m_bias
|
||||
|
||||
# Limit the mvt
|
||||
if(math.vec2.length(S2Target)>ROBOT_MAXVEL)
|
||||
S2Target = math.vec2.scale(S2Target, ROBOT_MAXVEL/math.vec2.length(S2Target))
|
||||
|
||||
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)
|
||||
|
||||
|
||||
|
||||
#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)
|
||||
TransitionToJoined()
|
||||
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
|
||||
m_selfMessage.State=s2i(UAVSTATE)
|
||||
m_selfMessage.Label=m_nLabel
|
||||
|
||||
}
|
||||
|
||||
#
|
||||
#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}
|
||||
neighbors.broadcast("r",pack_guide_msg(m_messageForJoining))
|
||||
}
|
||||
}
|
||||
#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
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
m_navigation.x=0.0
|
||||
m_navigation.y=0.0
|
||||
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
|
||||
|
||||
|
||||
#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()
|
||||
}
|
||||
}
|
||||
|
||||
#
|
||||
#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 = 7.5 #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")
|
||||
}
|
||||
|
|
@ -1,269 +0,0 @@
|
|||
include "vec2.bzz"
|
||||
include "update.bzz"
|
||||
include "barrier.bzz" # don't use a stigmergy id=11 with this header.
|
||||
include "uavstates.bzz" # require an 'action' function to be defined here.
|
||||
|
||||
# Lennard-Jones parameters
|
||||
TARGET = 12.0
|
||||
EPSILON = 10.0
|
||||
|
||||
#################################################
|
||||
### UTILITY FUNCTIONS ###########################
|
||||
#################################################
|
||||
|
||||
# Write a table as if it was a matrix
|
||||
function write_knowledge(k, row, col, val) {
|
||||
var key = string.concat(string.tostring(row),"-",string.tostring(col))
|
||||
k[key] = val
|
||||
log("Writing knowledge:", val, " to ", row, " ", col)
|
||||
}
|
||||
|
||||
# Read a table as if it was a matrix
|
||||
function read_knowledge(k, row, col) {
|
||||
var key = string.concat(string.tostring(row),"-",string.tostring(col))
|
||||
if (k[key] == nil) {
|
||||
log("Warning: reading 'nil' value from the knowledge table at", row, " ", col, ", returning -1")
|
||||
return -1
|
||||
} else {
|
||||
return k[key]
|
||||
}
|
||||
}
|
||||
|
||||
# Int to String
|
||||
function itos(i) {
|
||||
|
||||
log("Use 'string.tostring(OJB)' instead")
|
||||
|
||||
if (i==0) { return "0" }
|
||||
if (i==1) { return "1" }
|
||||
if (i==2) { return "2" }
|
||||
if (i==3) { return "3" }
|
||||
if (i==4) { return "4" }
|
||||
if (i==5) { return "5" }
|
||||
if (i==6) { return "6" }
|
||||
if (i==7) { return "7" }
|
||||
if (i==8) { return "8" }
|
||||
if (i==9) { return "9" }
|
||||
|
||||
log("Function 'itos' out of bounds, returning the answer (42)")
|
||||
return "42"
|
||||
}
|
||||
|
||||
# String to Int
|
||||
function stoi(s) {
|
||||
if (s=='0') { return 0 }
|
||||
if (s=='1') { return 1 }
|
||||
if (s=='2') { return 2 }
|
||||
if (s=='3') { return 3 }
|
||||
if (s=='4') { return 4 }
|
||||
if (s=='5') { return 5 }
|
||||
if (s=='6') { return 6 }
|
||||
if (s=='7') { return 7 }
|
||||
if (s=='8') { return 8 }
|
||||
if (s=='9') { return 9 }
|
||||
|
||||
log("Function 'stoi' out of bounds, returning the answer (42)")
|
||||
return 42
|
||||
|
||||
}
|
||||
|
||||
# Rads to degrees
|
||||
function rtod(r) {
|
||||
return (r*(180.0/math.pi))
|
||||
}
|
||||
|
||||
# Degrees to rads
|
||||
function dtor(d) {
|
||||
return (math.pi*(d/180.0))
|
||||
}
|
||||
|
||||
# Force angles in the (-180,180) interval
|
||||
function degrees_interval(a) {
|
||||
var temp = a
|
||||
while ((temp>360.0) or (temp<0.0)) {
|
||||
if (temp > 360.0) {
|
||||
temp = temp - 360.0
|
||||
} else if (temp < 0.0){
|
||||
temp = temp + 360.0
|
||||
}
|
||||
}
|
||||
if (temp > 180.0) {
|
||||
temp = temp - 360.0
|
||||
}
|
||||
return temp
|
||||
}
|
||||
|
||||
# Force angles in the (-pi,pi) interval
|
||||
function radians_interval(a) {
|
||||
var temp = a
|
||||
while ((temp>2.0*math.pi) or (temp<0.0)) {
|
||||
if (temp > 2.0*math.pi) {
|
||||
temp = temp - 2.0*math.pi
|
||||
} else if (temp < 0.0){
|
||||
temp = temp + 2.0*math.pi
|
||||
}
|
||||
}
|
||||
if (temp > math.pi) {
|
||||
temp = temp - 2.0*math.pi
|
||||
}
|
||||
return temp
|
||||
}
|
||||
|
||||
#################################################
|
||||
### MOVEMENT/COMMUNICATION PRIMITIVES ###########
|
||||
#################################################
|
||||
|
||||
# Lennard-Jones interaction magnitude
|
||||
function lj_magnitude(dist, target, epsilon) {
|
||||
return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
|
||||
}
|
||||
# Neighbor data to LJ interaction vector
|
||||
function lj_vector(rid, data) {
|
||||
return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth)
|
||||
}
|
||||
# Accumulator of neighbor LJ interactions
|
||||
function lj_sum(rid, data, accum) {
|
||||
return math.vec2.add(data, accum)
|
||||
}
|
||||
|
||||
# Calculates and actuates the flocking interaction
|
||||
function hexagon() {
|
||||
# Calculate accumulator
|
||||
var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
|
||||
if(neighbors.count() > 0)
|
||||
math.vec2.scale(accum, 1.0 / neighbors.count())
|
||||
# Move according to vector
|
||||
moveto(accum.x, accum.y)
|
||||
}
|
||||
|
||||
function inform_your_neighborhood() {
|
||||
# Reset to 0 the visibility of all neighbors
|
||||
foreach(knowledge, function(key, value) {
|
||||
column = string.sub(key, string.length(key)-1,string.length(key))
|
||||
if (column=='3') {
|
||||
knowledge[key] = 0
|
||||
}
|
||||
})
|
||||
neighbors.foreach( function(rid, data) {
|
||||
# For each neighbor, send a message with its azimuth, as seen by the broadcasting robot
|
||||
message_id = string.tostring(rid)
|
||||
neighbors.broadcast(message_id, rtod(data.azimuth))
|
||||
# Record the neighbor azimuth in my own knowledge table
|
||||
write_knowledge(knowledge, rid, 0, rtod(data.azimuth))
|
||||
# Record the neighbor distance in my own knowledge table
|
||||
write_knowledge(knowledge, rid, 2, data.distance)
|
||||
# Set neighbor as visible
|
||||
write_knowledge(knowledge, rid, 3, 1)
|
||||
})
|
||||
# Send a message with the desired direction, as seen by the broadcasting robot
|
||||
neighbors.broadcast("direction", local_dir)
|
||||
|
||||
}
|
||||
|
||||
function listen_to_your_neighborhood() {
|
||||
# For all "senders" in my neighborhood, record my azimuth, as seen by them
|
||||
message_id = string.tostring(id)
|
||||
neighbors.listen(message_id, function(vid, value, rid) {
|
||||
write_knowledge(knowledge, rid, 1, value)
|
||||
})
|
||||
}
|
||||
|
||||
#################################################
|
||||
### ACTUAL CONTROLLERS ##########################
|
||||
#################################################
|
||||
|
||||
function zero() {
|
||||
# Do not move
|
||||
moveto(0.0,0.0)
|
||||
# Tell the neighbors of the center where to go
|
||||
inform_your_neighborhood()
|
||||
}
|
||||
|
||||
function onetwo() {
|
||||
if (id == 1) {
|
||||
angle = 45
|
||||
} else {
|
||||
angle = -135
|
||||
}
|
||||
# Broadcast information
|
||||
inform_your_neighborhood()
|
||||
# If the center 0 is in sight
|
||||
if (read_knowledge(knowledge, 0, 3) == 1) {
|
||||
arm_offset = degrees_interval(read_knowledge(knowledge, 0, 1) - angle)
|
||||
if (arm_offset<3 and arm_offset>(-3)) {
|
||||
hexagon() # Underlying Lennard-Jones potential behavior
|
||||
} else {
|
||||
|
||||
local_rotation = degrees_interval( read_knowledge(knowledge, 0, 1) + (180.0 - read_knowledge(knowledge, 0, 0)) )
|
||||
local_arm = degrees_interval(angle - local_rotation)
|
||||
|
||||
if (read_knowledge(knowledge, 0, 2) > 250.0) {
|
||||
x_mov = math.cos(dtor(read_knowledge(knowledge, 0, 0)))
|
||||
y_mov = math.sin(dtor(read_knowledge(knowledge, 0, 0)))
|
||||
} else if (read_knowledge(knowledge, 0, 2) < 30.0) {
|
||||
x_mov = -math.cos(dtor(read_knowledge(knowledge, 0, 0)))
|
||||
y_mov = -math.sin(dtor(read_knowledge(knowledge, 0, 0)))
|
||||
} else {
|
||||
spiraling = 2.0+(id/10.0) # Fun stuff but be careful with this, it affects how a robots turns around a central node, use random number generation, eventually
|
||||
if (arm_offset > 0) { # Clockwise
|
||||
|
||||
x_mov = -math.sin(dtor(read_knowledge(knowledge, 0, 0)))
|
||||
y_mov = math.cos(dtor(read_knowledge(knowledge, 0, 0))) * spiraling
|
||||
} else { # Counterclockwise
|
||||
x_mov = math.sin(dtor(read_knowledge(knowledge, 0, 0)))
|
||||
y_mov = -math.cos(dtor(read_knowledge(knowledge, 0, 0))) * spiraling
|
||||
}
|
||||
}
|
||||
speed = 100
|
||||
moveto(speed * x_mov,speed * y_mov)
|
||||
}
|
||||
} else {
|
||||
hexagon()
|
||||
}
|
||||
}
|
||||
|
||||
#################################################
|
||||
### BUZZ FUNCTIONS ##############################
|
||||
#################################################
|
||||
|
||||
function action(){
|
||||
if (id == 0)
|
||||
statef=zero
|
||||
else
|
||||
statef=onetwo
|
||||
|
||||
UAVSTATE="TENTACLES"
|
||||
}
|
||||
|
||||
# Executed at init time
|
||||
function init() {
|
||||
uav_initswarm()
|
||||
|
||||
# Local knowledge table
|
||||
knowledge = {}
|
||||
# Update local knowledge with information from the neighbors
|
||||
listen_to_your_neighborhood()
|
||||
# Variables initialization
|
||||
iteration = 0
|
||||
|
||||
}
|
||||
|
||||
# Executed every time step
|
||||
function step() {
|
||||
uav_rccmd()
|
||||
uav_neicmd()
|
||||
|
||||
statef()
|
||||
log("Current state: ", CURSTATE)
|
||||
log("Swarm size: ",ROBOTS)
|
||||
|
||||
# Count the number of steps
|
||||
iteration = iteration + 1
|
||||
}
|
||||
|
||||
# Executed once when the robot (or the simulator) is reset.
|
||||
function reset() {
|
||||
}
|
||||
# Execute at exit
|
||||
function destroy() {
|
||||
}
|
||||
|
|
@ -1,119 +0,0 @@
|
|||
include "vec2.bzz"
|
||||
include "update.bzz"
|
||||
include "barrier.bzz" # don't use a stigmergy id=11 with this header.
|
||||
include "uavstates.bzz" # require an 'action' function to be defined here.
|
||||
include "vstigenv.bzz"
|
||||
|
||||
TARGET_ALTITUDE = 10.0
|
||||
|
||||
# Lennard-Jones parameters
|
||||
TARGET = 12.0
|
||||
EPSILON = 14.0
|
||||
|
||||
# Lennard-Jones interaction magnitude
|
||||
function lj_magnitude(dist, target, epsilon) {
|
||||
return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
|
||||
#return -(4 * epsilon) * ((target / dist)^12 - (target / dist)^6)
|
||||
}
|
||||
|
||||
# Neighbor data to LJ interaction vector
|
||||
function lj_vector(rid, data) {
|
||||
return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth)
|
||||
}
|
||||
|
||||
# Accumulator of neighbor LJ interactions
|
||||
function lj_sum(rid, data, accum) {
|
||||
return math.vec2.add(data, accum)
|
||||
}
|
||||
|
||||
function user_attract(t) {
|
||||
fus = math.vec2.new(0.0, 0.0)
|
||||
if(size(t)>0) {
|
||||
foreach(t, function(u, tab) {
|
||||
#log("id: ",u," Range ", tab.r, "Bearing ", tab.b)
|
||||
fus = math.vec2.add(fus, math.vec2.newp(lj_magnitude(tab.r, 3 * TARGET / 4.0, EPSILON * 2.0), tab.b))
|
||||
})
|
||||
math.vec2.scale(fus, 1.0 / size(t))
|
||||
}
|
||||
#print("User attract:", fus.x," ", fus.y, " [", size(t), "]")
|
||||
return fus
|
||||
}
|
||||
|
||||
# Calculates and actuates the flocking interaction
|
||||
function action() {
|
||||
statef=action
|
||||
# Calculate accumulator
|
||||
var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
|
||||
if(neighbors.count() > 0)
|
||||
accum = math.vec2.scale(accum, 1.0 / neighbors.count())
|
||||
|
||||
accum = math.vec2.add(accum, user_attract(users.dataL))
|
||||
accum = math.vec2.scale(accum, 1.0 / 2.0)
|
||||
|
||||
if(math.vec2.length(accum) > 1.0) {
|
||||
accum = math.vec2.scale(accum, 1.0 / math.vec2.length(accum))
|
||||
}
|
||||
|
||||
# Move according to vector
|
||||
print("Robot ", id, "must push ", math.vec2.length(accum) )#, "; ", math.vec2.angle(accum))
|
||||
uav_moveto(accum.x, accum.y)
|
||||
UAVSTATE = "LENNARDJONES"
|
||||
|
||||
# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS
|
||||
# timeW =0
|
||||
# statef=land
|
||||
# } else if(timeW>=WAIT_TIMEOUT/2) {
|
||||
# UAVSTATE ="GOEAST"
|
||||
# timeW = timeW+1
|
||||
# uav_moveto(0.0,5.0)
|
||||
# } else {
|
||||
# UAVSTATE ="GONORTH"
|
||||
# timeW = timeW+1
|
||||
# uav_moveto(5.0,0.0)
|
||||
# }
|
||||
}
|
||||
|
||||
|
||||
########################################
|
||||
#
|
||||
# MAIN FUNCTIONS
|
||||
#
|
||||
########################################
|
||||
|
||||
# Executed once at init time.
|
||||
function init() {
|
||||
uav_initswarm()
|
||||
|
||||
vt = stigmergy.create(5)
|
||||
t = {}
|
||||
vt.put("p",t)
|
||||
}
|
||||
|
||||
# Executed at each time step.
|
||||
function step() {
|
||||
uav_rccmd()
|
||||
uav_neicmd()
|
||||
|
||||
statef()
|
||||
log("Current state: ", UAVSTATE)
|
||||
log("Swarm size: ",ROBOTS)
|
||||
|
||||
# Read a value from the structure
|
||||
# log(users)
|
||||
#users_print(users.dataG)
|
||||
if(size(users.dataG)>0)
|
||||
vt.put("p", users.dataG)
|
||||
|
||||
# Get the number of keys in the structure
|
||||
#log("The vstig has ", vt.size(), " elements")
|
||||
users_save(vt.get("p"))
|
||||
table_print(users.dataL)
|
||||
}
|
||||
|
||||
# Executed once when the robot (or the simulator) is reset.
|
||||
function reset() {
|
||||
}
|
||||
|
||||
# Executed once at the end of experiment.
|
||||
function destroy() {
|
||||
}
|
||||
|
|
@ -1,537 +0,0 @@
|
|||
# Include files
|
||||
include "string.bzz"
|
||||
include "vec2.bzz"
|
||||
include "utilities.bzz"
|
||||
include "barrier.bzz"
|
||||
|
||||
############################################
|
||||
|
||||
# Global variables
|
||||
|
||||
RANGE = 200 # rab range in cm, get from argos file
|
||||
|
||||
N_SONS = 10 # Maximum number of sons
|
||||
|
||||
TRIGGER_VSTIG = 101 # ID of trigger virtual stigmergy
|
||||
BARRIER_VSTIG = 102 # ID of barrier virtual stigmergy
|
||||
|
||||
################################################################
|
||||
################################################################
|
||||
|
||||
# Tree utility functions
|
||||
|
||||
function parent_select() {
|
||||
|
||||
|
||||
# Selects potential parents
|
||||
var candidates = {}
|
||||
candidates = neighbors.filter(function(rid, data) {
|
||||
return ((knowledge.level[rid] > 0) and (data.distance < (RANGE - 10.0)) and (knowledge.free[rid] == 1))})
|
||||
# and (data.distance > 10.0)
|
||||
|
||||
# Selects closest parent candidate as parent
|
||||
var temp = {}
|
||||
temp = candidates.reduce(function(rid, data, accum) {
|
||||
accum.min = math.min(accum.min, data.distance)
|
||||
return accum
|
||||
}, {.min = RANGE})
|
||||
var min = temp.min
|
||||
|
||||
|
||||
var flag = 0
|
||||
foreach(knowledge.distance, function(key, value) {
|
||||
if ((flag == 0) and (candidates.data[key] != nil)) {
|
||||
if (value == min) {
|
||||
tree.parent.id = key
|
||||
tree.parent.distance = value
|
||||
tree.parent.azimuth = knowledge.azimuth[key]
|
||||
flag = 1
|
||||
}
|
||||
}
|
||||
#break (when implemented)
|
||||
})
|
||||
|
||||
}
|
||||
|
||||
####################################
|
||||
|
||||
function count() {
|
||||
|
||||
if (nb_sons == 0) {
|
||||
eq_level = level
|
||||
}
|
||||
|
||||
else if (nb_sons >= 1) {
|
||||
var temp = {}
|
||||
temp = sons.reduce(function(rid, data, accum) {
|
||||
accum.sum = accum.sum + tree.sons[rid].eq_level
|
||||
return accum
|
||||
}, {.sum = 0})
|
||||
eq_level = temp.sum - (nb_sons - 1) * level
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
####################################
|
||||
|
||||
function change_frame(p01, p1, theta) {
|
||||
|
||||
var p0 = {
|
||||
.x = math.cos(theta) * p1.x + math.sin(theta) * p1.y + p01.x,
|
||||
.y = -math.sin(theta) * p1.x + math.cos(theta) * p1.y + p01.y
|
||||
}
|
||||
return p0
|
||||
|
||||
}
|
||||
|
||||
####################################
|
||||
|
||||
transform_accum = function(rid, data) {
|
||||
|
||||
# Son contribution in frame F1
|
||||
var p1 = {
|
||||
.x = tree.sons[rid].accum_x,
|
||||
.y = tree.sons[rid].accum_y
|
||||
}
|
||||
|
||||
# Rotation angle between F0 and F1
|
||||
var theta = tree.sons[rid].angle_parent - data.azimuth - math.pi
|
||||
|
||||
# Translation vector from F0 to F1
|
||||
var p01 = {
|
||||
.x = 0,
|
||||
.y = 0
|
||||
}
|
||||
|
||||
var p0 = {}
|
||||
|
||||
if (tree.sons[rid].angle_parent != nil) {
|
||||
var rot_angle = radians_interval(theta)
|
||||
p0 = change_frame(p01, p1, rot_angle)
|
||||
}
|
||||
else {
|
||||
p0.x = p1.x
|
||||
p0.y = p1.y
|
||||
}
|
||||
|
||||
return p0
|
||||
}
|
||||
|
||||
####################################
|
||||
|
||||
function centroid() {
|
||||
|
||||
# If the robot has a parent
|
||||
if ((tree.parent != nil) or (root == 1)) {
|
||||
var sum_F1 = { .x = 0, .y = 0}
|
||||
|
||||
# If the robot has at least one son
|
||||
if (nb_sons > 0) {
|
||||
var temp = {}
|
||||
# Expresses son contrib (in F1) in its own reference frame (F0)
|
||||
temp = sons.map(transform_accum)
|
||||
# Sums son contributions expressed in F0 frame
|
||||
sum_F1 = temp.reduce(function(rid, data, accum) {
|
||||
accum.x = accum.x + data.x
|
||||
accum.y = accum.y + data.y
|
||||
#log("id ", rid, " sum_x ", accum.x, " sum_y ", accum.y)
|
||||
return accum
|
||||
}, {.x = 0, .y = 0})
|
||||
}
|
||||
|
||||
# If the robot is not the root
|
||||
if ((root == 0) and (tree.parent.id != nil)) {
|
||||
#var nb_descendants = eq_level - level
|
||||
var p0 = knowledge.distance[tree.parent.id]#tree.parent.distance
|
||||
var theta = knowledge.azimuth[tree.parent.id]#tree.parent.azimuth
|
||||
# Adds current robot contribution to centroid sum
|
||||
accum_x = sum_F1.x - (nb_descendants + 1) * p0 * math.cos(theta)
|
||||
accum_y = sum_F1.y - (nb_descendants + 1) * p0 * math.sin(theta)
|
||||
}
|
||||
# If the robot is the root
|
||||
else if ((root == 1) and (robot_count != 0)) {
|
||||
# Centroid coordinates in root ref frame
|
||||
accum_x = sum_F1.x / robot_count
|
||||
accum_y = sum_F1.y / robot_count
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
################################################################
|
||||
################################################################
|
||||
|
||||
# Tree reconfiguration functions
|
||||
|
||||
function tree_config() {
|
||||
statef()
|
||||
}
|
||||
|
||||
function end_fun() {
|
||||
if (root == 1) {
|
||||
log("centroid X: ", accum_x, " Y ", accum_y)
|
||||
}
|
||||
}
|
||||
|
||||
####################################
|
||||
|
||||
function root_select() {
|
||||
|
||||
log(id," root_select")
|
||||
if (tree.parent.id != nil) {
|
||||
if(neighbors.data[tree.parent.id] != nil) {
|
||||
angle_parent = neighbors.data[tree.parent.id].azimuth
|
||||
}
|
||||
}
|
||||
|
||||
if (root == 1) {
|
||||
|
||||
# Listens for new root acknowledgment
|
||||
|
||||
foreach(knowledge.ackn, function(key, value) {
|
||||
if (value == better_root) {
|
||||
#log(id, " got it")
|
||||
root = 0
|
||||
level = 0
|
||||
setleds(0,0,0)
|
||||
}
|
||||
})
|
||||
|
||||
if (ack == 1) {
|
||||
# Triggers transition to new state
|
||||
trigger.put("a", 1)
|
||||
}
|
||||
else if ((root == 1) and (better_root != id) and (trigger.get("a") != 1)) {
|
||||
setleds(255,0,0)
|
||||
better_root = id
|
||||
|
||||
# Centroid position in root reference frame (F0)
|
||||
var c0 = math.vec2.new(accum_x, accum_y)
|
||||
|
||||
# Distance from current root to centroid
|
||||
var dist_rc = math.vec2.length(c0)
|
||||
#log("root ", id, " dist_centr ", dist_rc)
|
||||
|
||||
# Distances from neighbors to centroid
|
||||
var dist_centroid = {}
|
||||
dist_centroid = neighbors.map(function(rid, data) {
|
||||
# Neighbour position in frame F0
|
||||
var p0 = math.vec2.newp(data.distance, data.azimuth)
|
||||
# Difference vector between p0 and c0
|
||||
var v = math.vec2.sub(p0, c0)
|
||||
# Distance between robot and centroid
|
||||
return math.vec2.length(v)
|
||||
})
|
||||
|
||||
# Minimal distance to centroid
|
||||
var temp = {}
|
||||
temp = dist_centroid.reduce(function(rid, data, accum) {
|
||||
accum.min = math.min(accum.min, data)
|
||||
return accum
|
||||
}, {.min = dist_rc})
|
||||
var min = temp.min
|
||||
#log("min ", min)
|
||||
|
||||
# If the current root is the closest to the centroid
|
||||
if(dist_rc == min) {
|
||||
ack = 1
|
||||
}
|
||||
# Otherwise
|
||||
else {
|
||||
var flag = 0
|
||||
# Selects closest robot to centroid as better root
|
||||
foreach(dist_centroid.data, function(key, value) {
|
||||
if(flag == 0) {
|
||||
if(value == min) {
|
||||
better_root = key
|
||||
flag = 1
|
||||
}
|
||||
# break (when implemented)
|
||||
}
|
||||
})
|
||||
|
||||
|
||||
#log(id, " better root : ", better_root)
|
||||
#log("X : ", accum_x, "Y : ", accum_y)
|
||||
var angle = neighbors.data[better_root].azimuth
|
||||
# Broadcasts
|
||||
var message = {
|
||||
.better_root = better_root,
|
||||
.centroid_x = accum_x,
|
||||
.centroid_y = accum_y,
|
||||
.angle_old_root = angle
|
||||
}
|
||||
neighbors.broadcast("msg1", message)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
else if (better_root == nil) {
|
||||
# Listen for old root message
|
||||
foreach(knowledge.better_root, function(rid, value) {
|
||||
if(value == id) {
|
||||
|
||||
var theta = neighbors.data[rid].azimuth
|
||||
|
||||
var p1 = {
|
||||
.x = knowledge.centroid_x[rid],
|
||||
.y = knowledge.centroid_y[rid]
|
||||
}
|
||||
|
||||
var p01 = {
|
||||
.x = neighbors.data[rid].distance * math.cos(theta),
|
||||
.y = neighbors.data[rid].distance * math.sin(theta)
|
||||
}
|
||||
|
||||
var p0 = {}
|
||||
|
||||
if (knowledge.angle_old_root[rid] != nil) {
|
||||
var rot_angle = radians_interval(knowledge.angle_old_root[rid] - theta - math.pi)
|
||||
p0 = change_frame(p01, p1, rot_angle)
|
||||
}
|
||||
else {
|
||||
p0.x = p1.x
|
||||
p0.y = p1.y
|
||||
}
|
||||
|
||||
accum_x = p0.x
|
||||
accum_y = p0.y
|
||||
|
||||
centroid_x = accum_x
|
||||
centroid_y = accum_y
|
||||
|
||||
root = 1
|
||||
neighbors.broadcast("got_it", id)
|
||||
}
|
||||
})
|
||||
}
|
||||
|
||||
# Transitions to new state when all robots are ready
|
||||
if (trigger.get("a") == 1) {
|
||||
barrier_set(ROBOTS, end_fun)
|
||||
barrier_ready()
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
####################################
|
||||
|
||||
function tree_select() {
|
||||
|
||||
log(id, " tree_select")
|
||||
|
||||
neighbors.map(function(rid, data) {
|
||||
knowledge.distance[rid] = data.distance
|
||||
knowledge.azimuth[rid] = data.azimuth
|
||||
return 1
|
||||
})
|
||||
|
||||
if (level == 0) {
|
||||
# Finds a parent
|
||||
parent_select()
|
||||
# Updates robot level
|
||||
if (tree.parent.id != nil) {
|
||||
#log(" ")
|
||||
#log("selected")
|
||||
#log("son ", id)
|
||||
#log("parent ", tree.parent.id)
|
||||
#log(" ")
|
||||
|
||||
level = knowledge.level[tree.parent.id] + 1
|
||||
angle_parent = neighbors.data[tree.parent.id].azimuth
|
||||
}
|
||||
}
|
||||
else {
|
||||
# Updates list of sons
|
||||
foreach(knowledge.parent, function(key, value) {
|
||||
if(value == id) {
|
||||
#log(value)
|
||||
if(tree.sons[key] == nil) {
|
||||
# Updates robot nb_sons
|
||||
nb_sons = nb_sons + 1
|
||||
# Updates robot free status
|
||||
if (nb_sons >= N_SONS) {
|
||||
free = 0
|
||||
}
|
||||
}
|
||||
tree.sons[key] = {
|
||||
.distance = knowledge.distance[key],
|
||||
.azimuth = knowledge.azimuth[key],
|
||||
.eq_level = knowledge.eq_level[key],
|
||||
.accum_x = knowledge.accum_x[key],
|
||||
.accum_y = knowledge.accum_y[key],
|
||||
.angle_parent = knowledge.angle_parent[key]
|
||||
}
|
||||
}
|
||||
})
|
||||
}
|
||||
|
||||
# Creates a subset of neighbors to get the sons
|
||||
# and parent
|
||||
sons = {}
|
||||
sons = neighbors.filter(function(rid, data) {
|
||||
return (tree.sons[rid] != nil)
|
||||
})
|
||||
parent = {}
|
||||
parent = neighbors.filter(function(rid, data) {
|
||||
return (tree.parent.id == rid)
|
||||
})
|
||||
|
||||
# Counts robots (updates eq_level)
|
||||
count()
|
||||
|
||||
# Updates count of robots in the tree
|
||||
if (root == 1) {
|
||||
robot_count = eq_level
|
||||
}
|
||||
|
||||
nb_descendants = eq_level - level
|
||||
|
||||
|
||||
# Computes centroid (updates accum_x, accum_y)
|
||||
centroid()
|
||||
|
||||
# Broadcast information to other robots
|
||||
var message = {
|
||||
.level = level,
|
||||
.parent = tree.parent.id,
|
||||
.eq_level = eq_level,
|
||||
.free = free,
|
||||
.accum_x = accum_x,
|
||||
.accum_y = accum_y,
|
||||
.angle_parent = angle_parent
|
||||
}
|
||||
neighbors.broadcast("msg2", message)
|
||||
|
||||
# Triggers transition to new state if root count = ROBOTS
|
||||
if (root == 1) {
|
||||
if(robot_count == ROBOTS) {
|
||||
log("centroid X: ", accum_x, " Y ", accum_y)
|
||||
trigger.put("b", 1)
|
||||
}
|
||||
}
|
||||
|
||||
# Transitions to new state when all robots are ready
|
||||
if (trigger.get("b") == 1) {
|
||||
barrier_set(ROBOTS, root_select)
|
||||
barrier_ready()
|
||||
}
|
||||
}
|
||||
|
||||
################################################################
|
||||
################################################################
|
||||
|
||||
|
||||
# This function is executed every time you press the 'execute' button
|
||||
function init() {
|
||||
|
||||
trigger = stigmergy.create(TRIGGER_VSTIG)
|
||||
barrier = stigmergy.create(BARRIER_VSTIG)
|
||||
|
||||
# Trees
|
||||
old_tree = {}
|
||||
tree = old_tree
|
||||
old_tree.parent = {}
|
||||
old_tree.sons = {}
|
||||
|
||||
# Boolean variables
|
||||
root = 0 # Root status
|
||||
free = 1 # Node status (true if accepts sons)
|
||||
ack = 0
|
||||
|
||||
# Tree variables
|
||||
level = 0
|
||||
eq_level = 0
|
||||
nb_sons = 0
|
||||
nb_descendants = 0
|
||||
|
||||
# Update root status
|
||||
if (id == 0) {
|
||||
root = 1 # True if robot is the ro ot
|
||||
level = 1
|
||||
robot_count = 0
|
||||
}
|
||||
|
||||
statef = tree_select
|
||||
|
||||
knowledge = {
|
||||
.level = {},
|
||||
.parent = {},
|
||||
.eq_level = {},
|
||||
.free = {},
|
||||
.accum_x = {},
|
||||
.accum_y = {},
|
||||
.angle_parent = {},
|
||||
.distance = {},
|
||||
.azimuth = {},
|
||||
.better_root = {},
|
||||
.centroid_x = {},
|
||||
.centroid_y = {},
|
||||
.angle_old_root = {},
|
||||
.ackn = {}
|
||||
}
|
||||
|
||||
# Broadcast information to other robots
|
||||
var message = {
|
||||
.level = level,
|
||||
.parent = old_tree.parent.id,
|
||||
.eq_level = eq_level,
|
||||
.free = free,
|
||||
.accum_x = accum_x,
|
||||
.accum_y = accum_y,
|
||||
.angle_parent = 0.0
|
||||
}
|
||||
neighbors.broadcast("msg2", message)
|
||||
|
||||
# Listen to information from other robots for root_select
|
||||
neighbors.listen("msg1", function(vid, value, rid) {
|
||||
knowledge.better_root[rid] = value.better_root
|
||||
knowledge.centroid_x[rid] = value.centroid_x
|
||||
knowledge.centroid_y[rid] = value.centroid_y
|
||||
knowledge.angle_old_root[rid] = value.angle_old_root
|
||||
knowledge.angle_parent[rid] = value.angle_parent
|
||||
})
|
||||
|
||||
# Listen to information from other robots for tree_select
|
||||
neighbors.listen("msg2", function(vid, value, rid) {
|
||||
knowledge.level[rid] = value.level
|
||||
knowledge.parent[rid] = value.parent
|
||||
knowledge.eq_level[rid] = value.eq_level
|
||||
knowledge.free[rid] = value.free
|
||||
knowledge.accum_x[rid] = value.accum_x
|
||||
knowledge.accum_y[rid] = value.accum_y
|
||||
knowledge.angle_parent[rid] = value.angle_parent
|
||||
})
|
||||
|
||||
neighbors.listen("got_it", function(vid, value, rid) {
|
||||
knowledge.ackn[rid] = value
|
||||
})
|
||||
|
||||
}
|
||||
|
||||
############################################
|
||||
|
||||
# This function is executed at each time step
|
||||
function step() {
|
||||
|
||||
tree_config()
|
||||
goal = math.vec2.new(0.0, 0.0)
|
||||
|
||||
}
|
||||
############################################
|
||||
|
||||
# This function is executed every time you press the 'reset'
|
||||
# button in the GUI.
|
||||
function reset() {
|
||||
# put your code here
|
||||
}
|
||||
|
||||
############################################
|
||||
|
||||
# This function is executed only once, when the robot is removed
|
||||
# from the simulation
|
||||
function destroy() {
|
||||
# put your code here
|
||||
}
|
||||
|
||||
################
|
||||
|
|
@ -93,7 +93,7 @@ Service
|
|||
-------
|
||||
|
||||
* Remote Controller:
|
||||
The package offers a mavros_msgs/CommandLong service "buzzcmd" to control its state.
|
||||
The package offers a mavros_msgs/CommandLong service "buzzcmd" to control its state. In the "misc" folder a bash script shows how to control the Buzz states from the command line.
|
||||
|
||||
References
|
||||
------
|
||||
|
|
|
|||
Loading…
Reference in New Issue