re-fix msgOut (vivek), debug bandwidth-optimised graph and add vstig of uav status
This commit is contained in:
parent
efe1c762b0
commit
b9c0dfb117
|
@ -24,19 +24,6 @@ function lj_sum(rid, data, accum) {
|
||||||
return math.vec2.add(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
|
# Calculates and actuates the flocking interaction
|
||||||
function action() {
|
function action() {
|
||||||
statef=action
|
statef=action
|
||||||
|
@ -45,9 +32,6 @@ function action() {
|
||||||
if(neighbors.count() > 0)
|
if(neighbors.count() > 0)
|
||||||
accum = math.vec2.scale(accum, 1.0 / neighbors.count())
|
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) {
|
if(math.vec2.length(accum) > 1.0) {
|
||||||
accum = math.vec2.scale(accum, 1.0 / math.vec2.length(accum))
|
accum = math.vec2.scale(accum, 1.0 / math.vec2.length(accum))
|
||||||
}
|
}
|
||||||
|
@ -80,6 +64,7 @@ function action() {
|
||||||
# Executed once at init time.
|
# Executed once at init time.
|
||||||
function init() {
|
function init() {
|
||||||
uav_initswarm()
|
uav_initswarm()
|
||||||
|
uav_initstig()
|
||||||
}
|
}
|
||||||
|
|
||||||
# Executed at each time step.
|
# Executed at each time step.
|
||||||
|
@ -88,9 +73,11 @@ function step() {
|
||||||
uav_neicmd()
|
uav_neicmd()
|
||||||
|
|
||||||
statef()
|
statef()
|
||||||
|
uav_updatestig()
|
||||||
log("Current state: ", UAVSTATE)
|
log("Current state: ", UAVSTATE)
|
||||||
log("Swarm size: ",ROBOTS)
|
log("Swarm size: ",ROBOTS)
|
||||||
|
if(id==0)
|
||||||
|
stattab_print()
|
||||||
}
|
}
|
||||||
|
|
||||||
# Executed once when the robot (or the simulator) is reset.
|
# Executed once when the robot (or the simulator) is reset.
|
||||||
|
|
|
@ -7,12 +7,15 @@ include "update.bzz"
|
||||||
include "barrier.bzz" # don't use a stigmergy id=11 with this header.
|
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 "uavstates.bzz" # require an 'action' function to be defined here.
|
||||||
|
|
||||||
include "graphs/shapes_square.bzz"
|
include "graphs/shapes_L.bzz"
|
||||||
|
|
||||||
ROBOT_RADIUS=50
|
ROBOT_RADIUS=50
|
||||||
ROBOT_DIAMETER=2.0*ROBOT_RADIUS
|
ROBOT_DIAMETER=2.0*ROBOT_RADIUS
|
||||||
ROBOT_SAFETYDIST=2.0*ROBOT_DIAMETER
|
ROBOT_SAFETYDIST=2.0*ROBOT_DIAMETER
|
||||||
|
|
||||||
|
# max velocity in cm/step
|
||||||
|
ROBOT_MAXVEL = 500.0
|
||||||
|
|
||||||
#
|
#
|
||||||
# Global variables
|
# Global variables
|
||||||
#
|
#
|
||||||
|
@ -21,21 +24,21 @@ ROBOT_SAFETYDIST=2.0*ROBOT_DIAMETER
|
||||||
# Save message from all neighours
|
# 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
|
#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_MessageState={}#store received neighbour message
|
||||||
m_MessageLable={}#store received neighbour message
|
m_MessageLabel={}#store received neighbour message
|
||||||
m_MessageReqLable={}#store received neighbour message
|
m_MessageReqLabel={}#store received neighbour message
|
||||||
m_MessageReqID={}#store received neighbour message
|
m_MessageReqID={}#store received neighbour message
|
||||||
m_MessageResponse={}#store received neighbour message
|
m_MessageResponse={}#store received neighbour message
|
||||||
m_MessageRange={}#store received neighbour message
|
m_MessageRange={}#store received neighbour message
|
||||||
m_MessageBearing={}#store received neighbour message
|
m_MessageBearing={}#store received neighbour message
|
||||||
m_neighbourCunt=0#used to cunt neighbours
|
m_neighbourCount=0#used to cunt neighbours
|
||||||
#Save message from one neighbour
|
#Save message from one neighbour
|
||||||
#the indexes are as State(received state),Lable(received lable),ReqLable,ReqID,Response,Range,Bearing
|
#the indexes are as State(received state),Label(received Label),ReqLabel,ReqID,Response,Range,Bearing
|
||||||
m_receivedMessage={.State=s2i("STATE_FREE"),.Lable=0,.ReqLable=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0}
|
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
|
#Save the message to send
|
||||||
#The keys of the talbe is State(current state),Lable(current lable),ReqLable(requested lable),ReqID(request id),Response(reply message{REQ_NONE,REQ_GRANTED,REQ_RESEND})
|
#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"),.Lable=0,.ReqLable=0,.ReqID=0,.Response=r2i("REQ_NONE")}
|
m_selfMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
|
||||||
|
|
||||||
#Current robot state
|
#Current robot state
|
||||||
m_eState="STATE_FREE"
|
m_eState="STATE_FREE"
|
||||||
|
@ -84,7 +87,7 @@ m_fTargetDistanceTolerance=0
|
||||||
step_cunt=0
|
step_cunt=0
|
||||||
|
|
||||||
#virtual stigmergy
|
#virtual stigmergy
|
||||||
v_tag = stigmergy.create(1)
|
m_lockstig = 1
|
||||||
|
|
||||||
# Lennard-Jones parameters, may need change
|
# Lennard-Jones parameters, may need change
|
||||||
EPSILON = 13.5 #the LJ parameter for other robots
|
EPSILON = 13.5 #the LJ parameter for other robots
|
||||||
|
@ -220,7 +223,7 @@ function pow(base,exponent){
|
||||||
#
|
#
|
||||||
function packmessage(send_table){
|
function packmessage(send_table){
|
||||||
var send_value
|
var send_value
|
||||||
send_value=10000*send_table.State+1000*send_table.Lable+100*send_table.ReqLable+10*send_table.ReqID+send_table.Response
|
send_value=10000*send_table.State+1000*send_table.Label+100*send_table.ReqLabel+10*send_table.ReqID+send_table.Response
|
||||||
return send_value
|
return send_value
|
||||||
}
|
}
|
||||||
#
|
#
|
||||||
|
@ -253,10 +256,10 @@ function unpackmessage(recv_value){
|
||||||
var shi=(recv_value-recv_value%10)/10
|
var shi=(recv_value-recv_value%10)/10
|
||||||
recv_value=recv_value-shi*10
|
recv_value=recv_value-shi*10
|
||||||
var ge=recv_value
|
var ge=recv_value
|
||||||
var return_table={.State=0.0,.Lable=0.0,.Reqlable=0.0,.ReqID=0.0,.Response=0.0}
|
var return_table={.State=0.0,.Label=0.0,.ReqLabel=0.0,.ReqID=0.0,.Response=0.0}
|
||||||
return_table.State=wan
|
return_table.State=wan
|
||||||
return_table.Lable=qian
|
return_table.Label=qian
|
||||||
return_table.ReqLable=bai
|
return_table.ReqLabel=bai
|
||||||
return_table.ReqID=shi
|
return_table.ReqID=shi
|
||||||
return_table.Response=ge
|
return_table.Response=ge
|
||||||
return return_table
|
return return_table
|
||||||
|
@ -309,13 +312,13 @@ function LJ_vec(i){
|
||||||
function motion_vector(){
|
function motion_vector(){
|
||||||
var i=0
|
var i=0
|
||||||
var m_vector={.x=0.0,.y=0.0}
|
var m_vector={.x=0.0,.y=0.0}
|
||||||
while(i<m_neighbourCunt){
|
while(i<m_neighbourCount){
|
||||||
#calculate and add the motion vector
|
#calculate and add the motion vector
|
||||||
m_vector=math.vec2.add(m_vector,LJ_vec(i))
|
m_vector=math.vec2.add(m_vector,LJ_vec(i))
|
||||||
#log(id,"x=",m_vector.x,"y=",m_vector.y)
|
#log(id,"x=",m_vector.x,"y=",m_vector.y)
|
||||||
i=i+1
|
i=i+1
|
||||||
}
|
}
|
||||||
m_vector=math.vec2.scale(m_vector,1000.0)
|
m_vector=math.vec2.scale(m_vector,1.0/m_neighbourCount)
|
||||||
return m_vector
|
return m_vector
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -328,15 +331,15 @@ neighbors.listen("m",
|
||||||
Get_DisAndAzi(temp_id)
|
Get_DisAndAzi(temp_id)
|
||||||
#add the received message
|
#add the received message
|
||||||
#
|
#
|
||||||
m_MessageState[m_neighbourCunt]=i2s(recv_val.State)
|
m_MessageState[m_neighbourCount]=i2s(recv_val.State)
|
||||||
m_MessageLable[m_neighbourCunt]=recv_val.Lable
|
m_MessageLabel[m_neighbourCount]=recv_val.Label
|
||||||
m_MessageReqLable[m_neighbourCunt]=recv_val.ReqLable
|
m_MessageReqLabel[m_neighbourCount]=recv_val.ReqLabel
|
||||||
m_MessageReqID[m_neighbourCunt]=recv_val.ReqID
|
m_MessageReqID[m_neighbourCount]=recv_val.ReqID
|
||||||
m_MessageResponse[m_neighbourCunt]=i2r(recv_val.Response)
|
m_MessageResponse[m_neighbourCount]=i2r(recv_val.Response)
|
||||||
m_MessageRange[m_neighbourCunt]=m_receivedMessage.Range
|
m_MessageRange[m_neighbourCount]=m_receivedMessage.Range
|
||||||
m_MessageBearing[m_neighbourCunt]=m_receivedMessage.Bearing
|
m_MessageBearing[m_neighbourCount]=m_receivedMessage.Bearing
|
||||||
m_messageID[m_neighbourCunt]=rid
|
m_messageID[m_neighbourCount]=rid
|
||||||
m_neighbourCunt=m_neighbourCunt+1
|
m_neighbourCount=m_neighbourCount+1
|
||||||
})
|
})
|
||||||
}
|
}
|
||||||
#
|
#
|
||||||
|
@ -359,14 +362,14 @@ function UpdateNodeInfo(){
|
||||||
#Update information
|
#Update information
|
||||||
var i=0
|
var i=0
|
||||||
|
|
||||||
while(i<m_neighbourCunt){
|
while(i<m_neighbourCount){
|
||||||
if(m_MessageState[i]=="STATE_JOINED"){
|
if(m_MessageState[i]=="STATE_JOINED"){
|
||||||
m_vecNodes[m_MessageLable[i]].State="ASSIGNED"
|
m_vecNodes[m_MessageLabel[i]].State="ASSIGNED"
|
||||||
m_vecNodes[m_MessageLable[i]].StateAge=m_unJoiningLostPeriod
|
m_vecNodes[m_MessageLabel[i]].StateAge=m_unJoiningLostPeriod
|
||||||
}
|
}
|
||||||
else if(m_MessageState[i]=="STATE_JOINING"){
|
else if(m_MessageState[i]=="STATE_JOINING"){
|
||||||
m_vecNodes[m_MessageLable[i]].State="ASSIGNING"
|
m_vecNodes[m_MessageLabel[i]].State="ASSIGNING"
|
||||||
m_vecNodes[m_MessageLable[i]].StateAge=m_unJoiningLostPeriod
|
m_vecNodes[m_MessageLabel[i]].StateAge=m_unJoiningLostPeriod
|
||||||
}
|
}
|
||||||
i=i+1
|
i=i+1
|
||||||
}
|
}
|
||||||
|
@ -399,7 +402,7 @@ function TransitionToAsking(un_label){
|
||||||
m_nLabel=un_label
|
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_unRequestId=id #don't know why the random numbers are the same, add id to make the ReqID different
|
||||||
m_selfMessage.State=s2i(m_eState)
|
m_selfMessage.State=s2i(m_eState)
|
||||||
m_selfMessage.ReqLable=m_nLabel
|
m_selfMessage.ReqLabel=m_nLabel
|
||||||
m_selfMessage.ReqID=m_unRequestId
|
m_selfMessage.ReqID=m_unRequestId
|
||||||
|
|
||||||
m_unWaitCount=m_unResponseTimeThreshold
|
m_unWaitCount=m_unResponseTimeThreshold
|
||||||
|
@ -411,7 +414,7 @@ function TransitionToAsking(un_label){
|
||||||
function TransitionToJoining(){
|
function TransitionToJoining(){
|
||||||
m_eState="STATE_JOINING"
|
m_eState="STATE_JOINING"
|
||||||
m_selfMessage.State=s2i(m_eState)
|
m_selfMessage.State=s2i(m_eState)
|
||||||
m_selfMessage.Lable=m_nLabel
|
m_selfMessage.Label=m_nLabel
|
||||||
m_unWaitCount=m_unJoiningLostPeriod
|
m_unWaitCount=m_unJoiningLostPeriod
|
||||||
|
|
||||||
neighbors.listen("r",
|
neighbors.listen("r",
|
||||||
|
@ -432,12 +435,12 @@ function TransitionToJoining(){
|
||||||
function TransitionToJoined(){
|
function TransitionToJoined(){
|
||||||
m_eState="STATE_JOINED"
|
m_eState="STATE_JOINED"
|
||||||
m_selfMessage.State=s2i(m_eState)
|
m_selfMessage.State=s2i(m_eState)
|
||||||
m_selfMessage.Lable=m_nLabel
|
m_selfMessage.Label=m_nLabel
|
||||||
m_vecNodes[m_nLabel].State="ASSIGNED"
|
m_vecNodes[m_nLabel].State="ASSIGNED"
|
||||||
neighbors.ignore("r")
|
neighbors.ignore("r")
|
||||||
|
|
||||||
#write statues
|
#write statues
|
||||||
v_tag.put(m_nLabel, 1)
|
v_tag.put(m_nLabel, m_lockstig)
|
||||||
|
|
||||||
m_navigation.x=0.0
|
m_navigation.x=0.0
|
||||||
m_navigation.y=0.0
|
m_navigation.y=0.0
|
||||||
|
@ -453,13 +456,13 @@ function TransitionToJoined(){
|
||||||
function TransitionToLock(){
|
function TransitionToLock(){
|
||||||
m_eState="STATE_LOCK"
|
m_eState="STATE_LOCK"
|
||||||
m_selfMessage.State=s2i(m_eState)
|
m_selfMessage.State=s2i(m_eState)
|
||||||
m_selfMessage.Lable=m_nLabel
|
m_selfMessage.Label=m_nLabel
|
||||||
m_vecNodes[m_nLabel].State="ASSIGNED"
|
m_vecNodes[m_nLabel].State="ASSIGNED"
|
||||||
#record neighbor distance
|
#record neighbor distance
|
||||||
lock_neighbor_id={}
|
lock_neighbor_id={}
|
||||||
lock_neighbor_dis={}
|
lock_neighbor_dis={}
|
||||||
var i=0
|
var i=0
|
||||||
while(i<m_neighbourCunt){
|
while(i<m_neighbourCount){
|
||||||
lock_neighbor_id[i]=m_messageID[i]
|
lock_neighbor_id[i]=m_messageID[i]
|
||||||
lock_neighbor_dis[i]=m_MessageRange[i]
|
lock_neighbor_dis[i]=m_MessageRange[i]
|
||||||
i=i+1
|
i=i+1
|
||||||
|
@ -475,39 +478,39 @@ while(i<m_neighbourCunt){
|
||||||
function DoFree() {
|
function DoFree() {
|
||||||
m_selfMessage.State=s2i(m_eState)
|
m_selfMessage.State=s2i(m_eState)
|
||||||
|
|
||||||
#wait for a while before looking for a lable
|
#wait for a while before looking for a Label
|
||||||
if(m_unWaitCount>0)
|
if(m_unWaitCount>0)
|
||||||
m_unWaitCount=m_unWaitCount-1
|
m_unWaitCount=m_unWaitCount-1
|
||||||
|
|
||||||
#find a set of joined robots
|
#find a set of joined robots
|
||||||
var setJoinedLables={}
|
var setJoinedLabels={}
|
||||||
var setJoinedIndexes={}
|
var setJoinedIndexes={}
|
||||||
var i=0
|
var i=0
|
||||||
var j=0
|
var j=0
|
||||||
while(i<m_neighbourCunt){
|
while(i<m_neighbourCount){
|
||||||
if(m_MessageState[i]=="STATE_JOINED"){
|
if(m_MessageState[i]=="STATE_JOINED"){
|
||||||
setJoinedLables[j]=m_MessageLable[i]
|
setJoinedLabels[j]=m_MessageLabel[i]
|
||||||
setJoinedIndexes[j]=i
|
setJoinedIndexes[j]=i
|
||||||
j=j+1
|
j=j+1
|
||||||
}
|
}
|
||||||
i=i+1
|
i=i+1
|
||||||
}
|
}
|
||||||
|
|
||||||
#go through the graph to look for a proper lable
|
#go through the graph to look for a proper Label
|
||||||
var unFoundLable=0
|
var unFoundLabel=0
|
||||||
var IDofPred=0
|
# var IDofPred=0
|
||||||
i=1
|
i=1
|
||||||
while(i<size(m_vecNodes) and (unFoundLable==0)){
|
while(i<size(m_vecNodes) and (unFoundLabel==0)){
|
||||||
#if the node is unassigned and the predecessor is insight
|
#if the node is unassigned and the predecessor is insight
|
||||||
if(m_vecNodes[i].State=="UNASSIGNED" and count(setJoinedLables,m_vecNodes[i].Pred)==1){
|
if(m_vecNodes[i].State=="UNASSIGNED" and count(setJoinedLabels,m_vecNodes[i].Pred)==1){
|
||||||
unFoundLable=m_vecNodes[i].Lable
|
unFoundLabel=m_vecNodes[i].Label
|
||||||
IDofPred=find(m_MessageLable,m_vecNodes[unFoundLable].Pred)
|
# IDofPred=find(m_MessageLabel,m_vecNodes[unFoundLabel].Pred)
|
||||||
}
|
}
|
||||||
i=i+1
|
i=i+1
|
||||||
}
|
}
|
||||||
|
|
||||||
if(unFoundLable>0){
|
if(unFoundLabel>0){
|
||||||
TransitionToAsking(unFoundLable)
|
TransitionToAsking(unFoundLabel)
|
||||||
return
|
return
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -527,12 +530,15 @@ function DoFree() {
|
||||||
tempvec_P=math.vec2.scale(tempvec_P,size(setJoinedIndexes))
|
tempvec_P=math.vec2.scale(tempvec_P,size(setJoinedIndexes))
|
||||||
tempvec_N=math.vec2.scale(tempvec_N,size(setJoinedIndexes))
|
tempvec_N=math.vec2.scale(tempvec_N,size(setJoinedIndexes))
|
||||||
m_navigation=math.vec2.add(tempvec_P,tempvec_N)
|
m_navigation=math.vec2.add(tempvec_P,tempvec_N)
|
||||||
|
# Limit the mvt
|
||||||
|
if(math.vec2.length(m_navigation)>ROBOT_MAXVEL)
|
||||||
|
m_navigation = math.vec2.scale(m_navigation, ROBOT_MAXVEL/math.vec2.length(m_navigation))
|
||||||
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
|
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
|
||||||
}else{ #no joined robots in sight
|
}else{ #no joined robots in sight
|
||||||
i=0
|
i=0
|
||||||
var tempvec={.x=0.0,.y=0.0}
|
var tempvec={.x=0.0,.y=0.0}
|
||||||
|
|
||||||
while(i<m_neighbourCunt){
|
while(i<m_neighbourCount){
|
||||||
tempvec=math.vec2.add(tempvec,math.vec2.newp(m_MessageRange[i]-2.0*ROBOT_SAFETYDIST,m_MessageBearing[i]))
|
tempvec=math.vec2.add(tempvec,math.vec2.newp(m_MessageRange[i]-2.0*ROBOT_SAFETYDIST,m_MessageBearing[i]))
|
||||||
i=i+1
|
i=i+1
|
||||||
}
|
}
|
||||||
|
@ -544,7 +550,9 @@ function DoFree() {
|
||||||
|
|
||||||
#jump the first step
|
#jump the first step
|
||||||
if(step_cunt<=1){
|
if(step_cunt<=1){
|
||||||
uav_moveto(0.0,0.0)
|
m_navigation.x=0.0
|
||||||
|
m_navigation.y=0.0
|
||||||
|
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
|
||||||
}
|
}
|
||||||
#set message
|
#set message
|
||||||
m_selfMessage.State=s2i(m_eState)
|
m_selfMessage.State=s2i(m_eState)
|
||||||
|
@ -561,12 +569,12 @@ function DoAsking(){
|
||||||
#look for response from predecessor
|
#look for response from predecessor
|
||||||
var i=0
|
var i=0
|
||||||
var psResponse=-1
|
var psResponse=-1
|
||||||
while(i<m_neighbourCunt and psResponse==-1){
|
while(i<m_neighbourCount and psResponse==-1){
|
||||||
#the respond robot in joined state
|
#the respond robot in joined state
|
||||||
#the request lable be the same as requesed
|
#the request Label be the same as requesed
|
||||||
#get a respond
|
#get a respond
|
||||||
if(m_MessageState[i]=="STATE_JOINED"){
|
if(m_MessageState[i]=="STATE_JOINED"){
|
||||||
if(m_MessageReqLable[i]==m_nLabel)
|
if(m_MessageReqLabel[i]==m_nLabel)
|
||||||
if(m_MessageResponse[i]!="REQ_NONE"){
|
if(m_MessageResponse[i]!="REQ_NONE"){
|
||||||
psResponse=i
|
psResponse=i
|
||||||
}}
|
}}
|
||||||
|
@ -577,7 +585,7 @@ function DoAsking(){
|
||||||
#no response, wait
|
#no response, wait
|
||||||
m_unWaitCount=m_unWaitCount-1
|
m_unWaitCount=m_unWaitCount-1
|
||||||
m_selfMessage.State=s2i(m_eState)
|
m_selfMessage.State=s2i(m_eState)
|
||||||
m_selfMessage.ReqLable=m_nLabel
|
m_selfMessage.ReqLabel=m_nLabel
|
||||||
m_selfMessage.ReqID=m_unRequestId
|
m_selfMessage.ReqID=m_unRequestId
|
||||||
if(m_unWaitCount==0){
|
if(m_unWaitCount==0){
|
||||||
TransitionToFree()
|
TransitionToFree()
|
||||||
|
@ -599,7 +607,9 @@ function DoAsking(){
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
uav_moveto(0.0,0.0)
|
m_navigation.x=0.0
|
||||||
|
m_navigation.y=0.0
|
||||||
|
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
|
||||||
}
|
}
|
||||||
|
|
||||||
#
|
#
|
||||||
|
@ -609,8 +619,8 @@ function DoJoining(){
|
||||||
#get information of pred
|
#get information of pred
|
||||||
var i=0
|
var i=0
|
||||||
var IDofPred=-1
|
var IDofPred=-1
|
||||||
while(i<m_neighbourCunt and IDofPred==-1){
|
while(i<m_neighbourCount and IDofPred==-1){
|
||||||
if(m_MessageLable[i]==m_vecNodes[m_nLabel].Pred and m_MessageState[i]=="STATE_JOINED")
|
if(m_MessageLabel[i]==m_vecNodes[m_nLabel].Pred and m_MessageState[i]=="STATE_JOINED")
|
||||||
IDofPred=i
|
IDofPred=i
|
||||||
i=i+1
|
i=i+1
|
||||||
}
|
}
|
||||||
|
@ -640,7 +650,12 @@ function DoJoining(){
|
||||||
#change the vector to local coordinate of self
|
#change the vector to local coordinate of self
|
||||||
var S2Target_bearing=Angle(S2Target)
|
var S2Target_bearing=Angle(S2Target)
|
||||||
m_bias=m_cMeToPred.Bearing-S2PGlobalBearing
|
m_bias=m_cMeToPred.Bearing-S2PGlobalBearing
|
||||||
S2Target_bearing=S2Target_bearing+m_bias # commented out by DS'06/17
|
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)
|
m_navigation=math.vec2.newp(math.vec2.length(S2Target),S2Target_bearing)
|
||||||
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
|
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
|
||||||
|
|
||||||
|
@ -648,7 +663,7 @@ function DoJoining(){
|
||||||
|
|
||||||
#test if is already in desired position
|
#test if is already in desired position
|
||||||
if(math.abs(S2Target.x)<m_fTargetDistanceTolerance and math.abs(S2Target.y)<m_fTargetDistanceTolerance){
|
if(math.abs(S2Target.x)<m_fTargetDistanceTolerance and math.abs(S2Target.y)<m_fTargetDistanceTolerance){
|
||||||
#log(S2Target_dis,S2Target_bearing)
|
log("LOCKING! ", math.vec2.length(S2Target), ", ", S2Target_bearing)
|
||||||
TransitionToJoined()
|
TransitionToJoined()
|
||||||
return
|
return
|
||||||
}
|
}
|
||||||
|
@ -664,7 +679,7 @@ function DoJoining(){
|
||||||
|
|
||||||
#pack the communication package
|
#pack the communication package
|
||||||
m_selfMessage.State=s2i(m_eState)
|
m_selfMessage.State=s2i(m_eState)
|
||||||
m_selfMessage.Lable=m_nLabel
|
m_selfMessage.Label=m_nLabel
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -673,35 +688,35 @@ function DoJoining(){
|
||||||
#
|
#
|
||||||
function DoJoined(){
|
function DoJoined(){
|
||||||
m_selfMessage.State=s2i(m_eState)
|
m_selfMessage.State=s2i(m_eState)
|
||||||
m_selfMessage.Lable=m_nLabel
|
m_selfMessage.Label=m_nLabel
|
||||||
|
|
||||||
#collect all requests
|
#collect all requests
|
||||||
var mapRequests={}
|
var mapRequests={}
|
||||||
var i=0
|
var i=0
|
||||||
var j=0
|
var j=0
|
||||||
var ReqLable
|
var ReqLabel
|
||||||
var JoiningLable
|
var JoiningLabel
|
||||||
var seenPred=0
|
var seenPred=0
|
||||||
while(i<m_neighbourCunt){
|
while(i<m_neighbourCount){
|
||||||
if(m_MessageState[i]=="STATE_ASKING"){
|
if(m_MessageState[i]=="STATE_ASKING"){
|
||||||
ReqLable=m_MessageReqLable[i]
|
ReqLabel=m_MessageReqLabel[i]
|
||||||
if(m_vecNodes[ReqLable].State=="UNASSIGNED")
|
if(m_vecNodes[ReqLabel].State=="UNASSIGNED")
|
||||||
if(m_nLabel==m_vecNodes[ReqLable].Pred){
|
if(m_nLabel==m_vecNodes[ReqLabel].Pred){
|
||||||
#is a request, store the index
|
#is a request, store the index
|
||||||
mapRequests[j]=i
|
mapRequests[j]=i
|
||||||
j=j+1
|
j=j+1
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
if(m_MessageState[i]=="STATE_JOINING"){
|
if(m_MessageState[i]=="STATE_JOINING"){
|
||||||
JoiningLable=m_MessageLable[i]
|
JoiningLabel=m_MessageLabel[i]
|
||||||
if(m_nLabel==m_vecNodes[JoiningLable].Pred){
|
if(m_nLabel==m_vecNodes[JoiningLabel].Pred){
|
||||||
##joining wrt this dot,send the global bearing
|
##joining wrt this dot,send the global bearing
|
||||||
var m_messageForJoining={.Label=JoiningLable,.Bearing=m_MessageBearing[i]-m_bias}
|
var m_messageForJoining={.Label=JoiningLabel,.Bearing=m_MessageBearing[i]-m_bias}
|
||||||
neighbors.broadcast("r",pack_guide_msg(m_messageForJoining))
|
neighbors.broadcast("r",pack_guide_msg(m_messageForJoining))
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
#if it is the pred
|
#if it is the pred
|
||||||
if(m_MessageState[i]=="STATE_JOINED" and m_MessageLable[i]==m_vecNodes[m_nLabel].Pred){
|
if(m_MessageState[i]=="STATE_JOINED" and m_MessageLabel[i]==m_vecNodes[m_nLabel].Pred){
|
||||||
seenPred=1
|
seenPred=1
|
||||||
m_unWaitCount=m_unJoiningLostPeriod
|
m_unWaitCount=m_unJoiningLostPeriod
|
||||||
}
|
}
|
||||||
|
@ -718,22 +733,22 @@ function DoJoined(){
|
||||||
ReqIndex=i
|
ReqIndex=i
|
||||||
i=i+1
|
i=i+1
|
||||||
}
|
}
|
||||||
#get the best index, whose Reqlable and Reqid are
|
#get the best index, whose ReqLabel and Reqid are
|
||||||
ReqLable=m_MessageReqLable[mapRequests[ReqIndex]]
|
ReqLabel=m_MessageReqLabel[mapRequests[ReqIndex]]
|
||||||
var ReqID=m_MessageReqID[mapRequests[ReqIndex]]
|
var ReqID=m_MessageReqID[mapRequests[ReqIndex]]
|
||||||
m_selfMessage.ReqLable=ReqLable
|
m_selfMessage.ReqLabel=ReqLabel
|
||||||
m_selfMessage.ReqID=ReqID
|
m_selfMessage.ReqID=ReqID
|
||||||
m_selfMessage.Response=r2i("REQ_GRANTED")
|
m_selfMessage.Response=r2i("REQ_GRANTED")
|
||||||
}
|
}
|
||||||
|
|
||||||
#lost pred, wait for some time and transit to free
|
#lost pred, wait for some time and transit to free
|
||||||
#if(seenPred==0){
|
if(seenPred==0){
|
||||||
#m_unWaitCount=m_unWaitCount-1
|
m_unWaitCount=m_unWaitCount-1
|
||||||
#if(m_unWaitCount==0){
|
if(m_unWaitCount==0){
|
||||||
#TransitionToFree()
|
TransitionToFree()
|
||||||
#return
|
return
|
||||||
#}
|
}
|
||||||
#}
|
}
|
||||||
|
|
||||||
|
|
||||||
m_navigation.x=0.0
|
m_navigation.x=0.0
|
||||||
|
@ -742,11 +757,12 @@ function DoJoined(){
|
||||||
|
|
||||||
|
|
||||||
#check if should to transists to lock
|
#check if should to transists to lock
|
||||||
|
#write statues
|
||||||
|
#v_tag.put(m_nLabel, 1)
|
||||||
# if(v_tag.size()==ROBOTS){
|
log(v_tag.size(), " of ", ROBOTS, " ready to lock")
|
||||||
# TransitionToLock()
|
if(v_tag.size()==ROBOTS){
|
||||||
# }
|
TransitionToLock()
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
#
|
#
|
||||||
|
@ -754,7 +770,7 @@ function DoJoined(){
|
||||||
#
|
#
|
||||||
function DoLock(){
|
function DoLock(){
|
||||||
m_selfMessage.State=s2i(m_eState)
|
m_selfMessage.State=s2i(m_eState)
|
||||||
m_selfMessage.Lable=m_nLabel
|
m_selfMessage.Label=m_nLabel
|
||||||
m_navigation.x=0.0
|
m_navigation.x=0.0
|
||||||
m_navigation.y=0.0
|
m_navigation.y=0.0
|
||||||
#calculate motion vection
|
#calculate motion vection
|
||||||
|
@ -784,15 +800,16 @@ function init() {
|
||||||
#
|
#
|
||||||
m_unResponseTimeThreshold=10
|
m_unResponseTimeThreshold=10
|
||||||
m_unLabelSearchWaitTime=10
|
m_unLabelSearchWaitTime=10
|
||||||
m_fTargetDistanceTolerance=10
|
m_fTargetDistanceTolerance=1
|
||||||
m_unJoiningLostPeriod=100
|
m_unJoiningLostPeriod=100
|
||||||
|
|
||||||
#
|
#
|
||||||
# Join Swarm
|
# Join Swarm
|
||||||
#
|
#
|
||||||
uav_initswarm()
|
uav_initswarm()
|
||||||
|
v_tag = stigmergy.create(m_lockstig)
|
||||||
Reset()
|
Reset()
|
||||||
Read_Graph()
|
TARGET_ALTITUDE = 2.5 + id * 1.5
|
||||||
}
|
}
|
||||||
|
|
||||||
#
|
#
|
||||||
|
@ -804,7 +821,7 @@ function step(){
|
||||||
#update the graph
|
#update the graph
|
||||||
UpdateNodeInfo()
|
UpdateNodeInfo()
|
||||||
#reset message package to be sent
|
#reset message package to be sent
|
||||||
m_selfMessage={.State=s2i("STATE_FREE"),.Lable=0,.ReqLable=0,.ReqID=0,.Response=r2i("REQ_NONE")}
|
m_selfMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
|
||||||
#
|
#
|
||||||
#act according to current state
|
#act according to current state
|
||||||
#
|
#
|
||||||
|
@ -837,13 +854,13 @@ function step(){
|
||||||
#
|
#
|
||||||
#clean message storage
|
#clean message storage
|
||||||
m_MessageState={}#store received neighbour message
|
m_MessageState={}#store received neighbour message
|
||||||
m_MessageLable={}#store received neighbour message
|
m_MessageLabel={}#store received neighbour message
|
||||||
m_MessageReqLable={}#store received neighbour message
|
m_MessageReqLabel={}#store received neighbour message
|
||||||
m_MessageReqID={}#store received neighbour message
|
m_MessageReqID={}#store received neighbour message
|
||||||
m_MessageResponse={}#store received neighbour message
|
m_MessageResponse={}#store received neighbour message
|
||||||
m_MessageRange={}#store received neighbour message
|
m_MessageRange={}#store received neighbour message
|
||||||
m_MessageBearing={}#store received neighbour message
|
m_MessageBearing={}#store received neighbour message
|
||||||
m_neighbourCunt=0
|
m_neighbourCount=0
|
||||||
|
|
||||||
|
|
||||||
#step cunt+1
|
#step cunt+1
|
||||||
|
@ -858,6 +875,7 @@ function Reset(){
|
||||||
# m_vecNodes = parse_graph("/home/dave/ROS_WS/src/rosbuzz/buzz_scripts/include/Graph_drone.graph")#change the .graph file when necessary
|
# m_vecNodes = parse_graph("/home/dave/ROS_WS/src/rosbuzz/buzz_scripts/include/Graph_drone.graph")#change the .graph file when necessary
|
||||||
# m_vecNodes_fixed={}
|
# m_vecNodes_fixed={}
|
||||||
# m_vecNodes_fixed=parse_graph_fixed("/home/dave/ROS_WS/src/rosbuzz/buzz_scripts/include/Graph_fixed.graph")
|
# m_vecNodes_fixed=parse_graph_fixed("/home/dave/ROS_WS/src/rosbuzz/buzz_scripts/include/Graph_fixed.graph")
|
||||||
|
Read_Graph()
|
||||||
m_nLabel=-1
|
m_nLabel=-1
|
||||||
|
|
||||||
#start listening
|
#start listening
|
||||||
|
@ -881,7 +899,9 @@ function Reset(){
|
||||||
#
|
#
|
||||||
function destroy() {
|
function destroy() {
|
||||||
#clear neighbour message
|
#clear neighbour message
|
||||||
uav_moveto(0.0,0.0)
|
m_navigation.x=0.0
|
||||||
|
m_navigation.y=0.0
|
||||||
|
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
|
||||||
m_vecNodes={}
|
m_vecNodes={}
|
||||||
#stop listening
|
#stop listening
|
||||||
neighbors.ignore("m")
|
neighbors.ignore("m")
|
||||||
|
|
|
@ -1,5 +0,0 @@
|
||||||
0 -1 -1 -1 3000.0
|
|
||||||
1 0 1000.0 0.0 5000.0
|
|
||||||
2 0 1000.0 1.57 7000.0
|
|
||||||
3 0 1000.0 3.14 9000.0
|
|
||||||
4 0 1000.0 4.71 11000.0
|
|
|
@ -1,5 +0,0 @@
|
||||||
0 -1 -1 -1 -1
|
|
||||||
1 0 10.0 -1 -1
|
|
||||||
2 0 10.0 1 1414.2
|
|
||||||
3 0 10.0 2 1414.2
|
|
||||||
4 0 10.0 1 1414.2
|
|
|
@ -2,9 +2,9 @@
|
||||||
m_vecNodes={}
|
m_vecNodes={}
|
||||||
|
|
||||||
function Read_Graph(){
|
function Read_Graph(){
|
||||||
m_vecNodes[0] = { # The .graph file is stored according the sequence of lable, predecessor, distance, bearing
|
m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing
|
||||||
.Lable = 0, # Lable of the point
|
.Label = 0, # Label of the point
|
||||||
.Pred = -1, # Lable of its predecessor
|
.Pred = -1, # Label of its predecessor
|
||||||
.distance = -1, # distance to the predecessor
|
.distance = -1, # distance to the predecessor
|
||||||
.bearing = -1, # bearing form the predecessor to this dot
|
.bearing = -1, # bearing form the predecessor to this dot
|
||||||
.height = 3000, # height of this dot
|
.height = 3000, # height of this dot
|
||||||
|
@ -12,7 +12,7 @@ m_vecNodes[0] = { # The .graph file is stored according the sequence of lab
|
||||||
.StateAge=0
|
.StateAge=0
|
||||||
}
|
}
|
||||||
m_vecNodes[1] = {
|
m_vecNodes[1] = {
|
||||||
.Lable = 1,
|
.Label = 1,
|
||||||
.Pred = 0,
|
.Pred = 0,
|
||||||
.distance = 2000,
|
.distance = 2000,
|
||||||
.bearing = 0.0,
|
.bearing = 0.0,
|
||||||
|
@ -21,7 +21,7 @@ m_vecNodes[1] = {
|
||||||
.StateAge=0
|
.StateAge=0
|
||||||
}
|
}
|
||||||
m_vecNodes[2] = {
|
m_vecNodes[2] = {
|
||||||
.Lable = 2,
|
.Label = 2,
|
||||||
.Pred = 0,
|
.Pred = 0,
|
||||||
.distance = 2000,
|
.distance = 2000,
|
||||||
.bearing = 1.57,
|
.bearing = 1.57,
|
||||||
|
@ -30,7 +30,7 @@ m_vecNodes[2] = {
|
||||||
.StateAge=0
|
.StateAge=0
|
||||||
}
|
}
|
||||||
m_vecNodes[3] = {
|
m_vecNodes[3] = {
|
||||||
.Lable = 3,
|
.Label = 3,
|
||||||
.Pred = 1,
|
.Pred = 1,
|
||||||
.distance = 2000,
|
.distance = 2000,
|
||||||
.bearing = 0.0,
|
.bearing = 0.0,
|
||||||
|
@ -39,7 +39,7 @@ m_vecNodes[3] = {
|
||||||
.StateAge=0
|
.StateAge=0
|
||||||
}
|
}
|
||||||
m_vecNodes[4] = {
|
m_vecNodes[4] = {
|
||||||
.Lable = 4,
|
.Label = 4,
|
||||||
.Pred = 2,
|
.Pred = 2,
|
||||||
.distance = 2000,
|
.distance = 2000,
|
||||||
.bearing = 1.57,
|
.bearing = 1.57,
|
||||||
|
@ -48,7 +48,7 @@ m_vecNodes[4] = {
|
||||||
.StateAge=0
|
.StateAge=0
|
||||||
}
|
}
|
||||||
m_vecNodes[5] = {
|
m_vecNodes[5] = {
|
||||||
.Lable = 5,
|
.Label = 5,
|
||||||
.Pred = 4,
|
.Pred = 4,
|
||||||
.distance = 2000,
|
.distance = 2000,
|
||||||
.bearing = 1.57,
|
.bearing = 1.57,
|
||||||
|
|
|
@ -2,9 +2,9 @@
|
||||||
m_vecNodes={}
|
m_vecNodes={}
|
||||||
m_vecNodes_fixed={}
|
m_vecNodes_fixed={}
|
||||||
function Read_Graph(){
|
function Read_Graph(){
|
||||||
m_vecNodes[0] = { # The .graph file is stored according the sequence of lable, predecessor, distance, bearing
|
m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing
|
||||||
.Lable = 0, # Lable of the point
|
.Label = 0, # Label of the point
|
||||||
.Pred = -1, # Lable of its predecessor
|
.Pred = -1, # Label of its predecessor
|
||||||
.distance = -1, # distance to the predecessor
|
.distance = -1, # distance to the predecessor
|
||||||
.bearing = -1, # bearing form the predecessor to this dot
|
.bearing = -1, # bearing form the predecessor to this dot
|
||||||
.height = 3000, # height of this dot
|
.height = 3000, # height of this dot
|
||||||
|
@ -12,7 +12,7 @@ m_vecNodes[0] = { # The .graph file is stored according the sequence of lab
|
||||||
.StateAge=0
|
.StateAge=0
|
||||||
}
|
}
|
||||||
m_vecNodes[1] = {
|
m_vecNodes[1] = {
|
||||||
.Lable = 1,
|
.Label = 1,
|
||||||
.Pred = 0,
|
.Pred = 0,
|
||||||
.distance = 2000,
|
.distance = 2000,
|
||||||
.bearing = 0.0,
|
.bearing = 0.0,
|
||||||
|
@ -21,7 +21,7 @@ m_vecNodes[1] = {
|
||||||
.StateAge=0
|
.StateAge=0
|
||||||
}
|
}
|
||||||
m_vecNodes[2] = {
|
m_vecNodes[2] = {
|
||||||
.Lable = 2,
|
.Label = 2,
|
||||||
.Pred = 0,
|
.Pred = 0,
|
||||||
.distance = 2000,
|
.distance = 2000,
|
||||||
.bearing = 1.57,
|
.bearing = 1.57,
|
||||||
|
@ -30,7 +30,7 @@ m_vecNodes[2] = {
|
||||||
.StateAge=0
|
.StateAge=0
|
||||||
}
|
}
|
||||||
m_vecNodes[3] = {
|
m_vecNodes[3] = {
|
||||||
.Lable = 3,
|
.Label = 3,
|
||||||
.Pred = 1,
|
.Pred = 1,
|
||||||
.distance = 2000,
|
.distance = 2000,
|
||||||
.bearing = 1.57,
|
.bearing = 1.57,
|
||||||
|
@ -39,7 +39,7 @@ m_vecNodes[3] = {
|
||||||
.StateAge=0
|
.StateAge=0
|
||||||
}
|
}
|
||||||
m_vecNodes[4] = {
|
m_vecNodes[4] = {
|
||||||
.Lable = 4,
|
.Label = 4,
|
||||||
.Pred = 1,
|
.Pred = 1,
|
||||||
.distance = 1414,
|
.distance = 1414,
|
||||||
.bearing = 3.93,
|
.bearing = 3.93,
|
||||||
|
@ -48,7 +48,7 @@ m_vecNodes[4] = {
|
||||||
.StateAge=0
|
.StateAge=0
|
||||||
}
|
}
|
||||||
m_vecNodes[5] = {
|
m_vecNodes[5] = {
|
||||||
.Lable = 5,
|
.Label = 5,
|
||||||
.Pred = 2,
|
.Pred = 2,
|
||||||
.distance = 1414,
|
.distance = 1414,
|
||||||
.bearing = 0.785,
|
.bearing = 0.785,
|
||||||
|
|
|
@ -1,9 +1,10 @@
|
||||||
#Table of the nodes in the graph
|
#Table of the nodes in the graph
|
||||||
m_vecNodes={}
|
m_vecNodes={}
|
||||||
|
m_vecNodes_fixed={}
|
||||||
function Read_Graph(){
|
function Read_Graph(){
|
||||||
m_vecNodes[0] = { # The .graph file is stored according the sequence of lable, predecessor, distance, bearing
|
m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing
|
||||||
.Lable = 0, # Lable of the point
|
.Label = 0, # Label of the point
|
||||||
.Pred = -1, # Lable of its predecessor
|
.Pred = -1, # Label of its predecessor
|
||||||
.distance = -1, # distance to the predecessor
|
.distance = -1, # distance to the predecessor
|
||||||
.bearing = -1, # bearing form the predecessor to this dot
|
.bearing = -1, # bearing form the predecessor to this dot
|
||||||
.height = 3000, # height of this dot
|
.height = 3000, # height of this dot
|
||||||
|
@ -11,7 +12,7 @@ m_vecNodes[0] = { # The .graph file is stored according the sequence of lab
|
||||||
.StateAge=0
|
.StateAge=0
|
||||||
}
|
}
|
||||||
m_vecNodes[1] = {
|
m_vecNodes[1] = {
|
||||||
.Lable = 1,
|
.Label = 1,
|
||||||
.Pred = 0,
|
.Pred = 0,
|
||||||
.distance = 2000,
|
.distance = 2000,
|
||||||
.bearing = 0.0,
|
.bearing = 0.0,
|
||||||
|
@ -20,7 +21,7 @@ m_vecNodes[1] = {
|
||||||
.StateAge=0
|
.StateAge=0
|
||||||
}
|
}
|
||||||
m_vecNodes[2] = {
|
m_vecNodes[2] = {
|
||||||
.Lable = 2,
|
.Label = 2,
|
||||||
.Pred = 0,
|
.Pred = 0,
|
||||||
.distance = 2000,
|
.distance = 2000,
|
||||||
.bearing = 1.57,
|
.bearing = 1.57,
|
||||||
|
@ -29,7 +30,7 @@ m_vecNodes[2] = {
|
||||||
.StateAge=0
|
.StateAge=0
|
||||||
}
|
}
|
||||||
m_vecNodes[3] = {
|
m_vecNodes[3] = {
|
||||||
.Lable = 3,
|
.Label = 3,
|
||||||
.Pred = 0,
|
.Pred = 0,
|
||||||
.distance = 2000,
|
.distance = 2000,
|
||||||
.bearing = 4.71,
|
.bearing = 4.71,
|
||||||
|
@ -38,7 +39,7 @@ m_vecNodes[3] = {
|
||||||
.StateAge=0
|
.StateAge=0
|
||||||
}
|
}
|
||||||
m_vecNodes[4] = {
|
m_vecNodes[4] = {
|
||||||
.Lable = 4,
|
.Label = 4,
|
||||||
.Pred = 1,
|
.Pred = 1,
|
||||||
.distance = 1414,
|
.distance = 1414,
|
||||||
.bearing = 0.79,
|
.bearing = 0.79,
|
||||||
|
@ -47,7 +48,7 @@ m_vecNodes[4] = {
|
||||||
.StateAge=0
|
.StateAge=0
|
||||||
}
|
}
|
||||||
m_vecNodes[5] = {
|
m_vecNodes[5] = {
|
||||||
.Lable = 5,
|
.Label = 5,
|
||||||
.Pred = 2,
|
.Pred = 2,
|
||||||
.distance = 2000,
|
.distance = 2000,
|
||||||
.bearing = 0.0,
|
.bearing = 0.0,
|
||||||
|
|
|
@ -1,9 +1,10 @@
|
||||||
#Table of the nodes in the graph
|
#Table of the nodes in the graph
|
||||||
m_vecNodes={}
|
m_vecNodes={}
|
||||||
|
m_vecNodes_fixed={}
|
||||||
function Read_Graph(){
|
function Read_Graph(){
|
||||||
m_vecNodes[0] = { # The .graph file is stored according the sequence of lable, predecessor, distance, bearing
|
m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing
|
||||||
.Lable = 0, # Lable of the point
|
.Label = 0, # Label of the point
|
||||||
.Pred = -1, # Lable of its predecessor
|
.Pred = -1, # Label of its predecessor
|
||||||
.distance = -1, # distance to the predecessor
|
.distance = -1, # distance to the predecessor
|
||||||
.bearing = -1, # bearing form the predecessor to this dot
|
.bearing = -1, # bearing form the predecessor to this dot
|
||||||
.height = 3000, # height of this dot
|
.height = 3000, # height of this dot
|
||||||
|
@ -11,7 +12,7 @@ m_vecNodes[0] = { # The .graph file is stored according the sequence of lab
|
||||||
.StateAge=0
|
.StateAge=0
|
||||||
}
|
}
|
||||||
m_vecNodes[1] = {
|
m_vecNodes[1] = {
|
||||||
.Lable = 1,
|
.Label = 1,
|
||||||
.Pred = 0,
|
.Pred = 0,
|
||||||
.distance = 2000,
|
.distance = 2000,
|
||||||
.bearing = 4.71,
|
.bearing = 4.71,
|
||||||
|
@ -20,7 +21,7 @@ m_vecNodes[1] = {
|
||||||
.StateAge=0
|
.StateAge=0
|
||||||
}
|
}
|
||||||
m_vecNodes[2] = {
|
m_vecNodes[2] = {
|
||||||
.Lable = 2,
|
.Label = 2,
|
||||||
.Pred = 0,
|
.Pred = 0,
|
||||||
.distance = 1414,
|
.distance = 1414,
|
||||||
.bearing = 2.36,
|
.bearing = 2.36,
|
||||||
|
@ -29,7 +30,7 @@ m_vecNodes[2] = {
|
||||||
.StateAge=0
|
.StateAge=0
|
||||||
}
|
}
|
||||||
m_vecNodes[3] = {
|
m_vecNodes[3] = {
|
||||||
.Lable = 3,
|
.Label = 3,
|
||||||
.Pred = 0,
|
.Pred = 0,
|
||||||
.distance = 1414,
|
.distance = 1414,
|
||||||
.bearing = 0.79,
|
.bearing = 0.79,
|
||||||
|
@ -38,7 +39,7 @@ m_vecNodes[3] = {
|
||||||
.StateAge=0
|
.StateAge=0
|
||||||
}
|
}
|
||||||
m_vecNodes[4] = {
|
m_vecNodes[4] = {
|
||||||
.Lable = 4,
|
.Label = 4,
|
||||||
.Pred = 2,
|
.Pred = 2,
|
||||||
.distance = 1414,
|
.distance = 1414,
|
||||||
.bearing = 2.36,
|
.bearing = 2.36,
|
||||||
|
@ -47,7 +48,7 @@ m_vecNodes[4] = {
|
||||||
.StateAge=0
|
.StateAge=0
|
||||||
}
|
}
|
||||||
m_vecNodes[5] = {
|
m_vecNodes[5] = {
|
||||||
.Lable = 5,
|
.Label = 5,
|
||||||
.Pred = 3,
|
.Pred = 3,
|
||||||
.distance = 1414,
|
.distance = 1414,
|
||||||
.bearing = 0.79,
|
.bearing = 0.79,
|
||||||
|
|
|
@ -1,11 +1,10 @@
|
||||||
#Table of the nodes in the graph
|
#Table of the nodes in the graph
|
||||||
m_vecNodes={}
|
m_vecNodes={}
|
||||||
|
m_vecNodes_fixed={}
|
||||||
function Read_Graph(){
|
function Read_Graph(){
|
||||||
|
m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing
|
||||||
m_vecNodes[0] = { # The .graph file is stored according the sequence of lable, predecessor, distance, bearing
|
.Label = 0, # Label of the point
|
||||||
.Lable = 0, # Lable of the point
|
.Pred = -1, # Label of its predecessor
|
||||||
.Pred = -1, # Lable of its predecessor
|
|
||||||
.distance = -1, # distance to the predecessor
|
.distance = -1, # distance to the predecessor
|
||||||
.bearing = -1, # bearing form the predecessor to this dot
|
.bearing = -1, # bearing form the predecessor to this dot
|
||||||
.height = 3000, # height of this dot
|
.height = 3000, # height of this dot
|
||||||
|
@ -13,7 +12,7 @@ m_vecNodes[0] = { # The .graph file is stored according the sequence of lab
|
||||||
.StateAge=0
|
.StateAge=0
|
||||||
}
|
}
|
||||||
m_vecNodes[1] = {
|
m_vecNodes[1] = {
|
||||||
.Lable = 1,
|
.Label = 1,
|
||||||
.Pred = 0,
|
.Pred = 0,
|
||||||
.distance = 1000,
|
.distance = 1000,
|
||||||
.bearing = 0.0,
|
.bearing = 0.0,
|
||||||
|
@ -22,7 +21,7 @@ m_vecNodes[1] = {
|
||||||
.StateAge=0
|
.StateAge=0
|
||||||
}
|
}
|
||||||
m_vecNodes[2] = {
|
m_vecNodes[2] = {
|
||||||
.Lable = 2,
|
.Label = 2,
|
||||||
.Pred = 0,
|
.Pred = 0,
|
||||||
.distance = 1000,
|
.distance = 1000,
|
||||||
.bearing = 1.57,
|
.bearing = 1.57,
|
||||||
|
@ -31,7 +30,7 @@ m_vecNodes[2] = {
|
||||||
.StateAge=0
|
.StateAge=0
|
||||||
}
|
}
|
||||||
m_vecNodes[3] = {
|
m_vecNodes[3] = {
|
||||||
.Lable = 3,
|
.Label = 3,
|
||||||
.Pred = 0,
|
.Pred = 0,
|
||||||
.distance = 1000,
|
.distance = 1000,
|
||||||
.bearing = 3.14,
|
.bearing = 3.14,
|
||||||
|
@ -40,7 +39,16 @@ m_vecNodes[3] = {
|
||||||
.StateAge=0
|
.StateAge=0
|
||||||
}
|
}
|
||||||
m_vecNodes[4] = {
|
m_vecNodes[4] = {
|
||||||
.Lable = 4,
|
.Label = 4,
|
||||||
|
.Pred = 0,
|
||||||
|
.distance = 707,
|
||||||
|
.bearing = 3.93,
|
||||||
|
.height = 11000,
|
||||||
|
.State="UNASSIGNED",
|
||||||
|
.StateAge=0
|
||||||
|
}
|
||||||
|
m_vecNodes[5] = {
|
||||||
|
.Label = 5,
|
||||||
.Pred = 0,
|
.Pred = 0,
|
||||||
.distance = 1000,
|
.distance = 1000,
|
||||||
.bearing = 4.71,
|
.bearing = 4.71,
|
||||||
|
|
|
@ -1,3 +1,25 @@
|
||||||
|
STATUS_VSTIG = 10
|
||||||
|
GROUND_VSTIG = 11
|
||||||
|
WAIT4STEP = 10
|
||||||
|
v_status = {}
|
||||||
|
v_ground = {}
|
||||||
|
|
||||||
|
function uav_initstig() {
|
||||||
|
v_status = stigmergy.create(STATUS_VSTIG)
|
||||||
|
v_ground = stigmergy.create(GROUND_VSTIG)
|
||||||
|
}
|
||||||
|
|
||||||
|
counter=WAIT4STEP
|
||||||
|
function uav_updatestig() {
|
||||||
|
if(counter<=0) {
|
||||||
|
var ls={.g=0,.b=battery.capacity,.x=xbee_status.rssi,.f=flight.status}
|
||||||
|
log("Pushing ", ls, "on vstig with id:", id)
|
||||||
|
v_status.put(id, ls)
|
||||||
|
counter=WAIT4STEP
|
||||||
|
} else
|
||||||
|
counter=counter-1
|
||||||
|
}
|
||||||
|
|
||||||
function checkusers() {
|
function checkusers() {
|
||||||
# Read a value from the structure
|
# Read a value from the structure
|
||||||
if(size(users)>0)
|
if(size(users)>0)
|
||||||
|
@ -24,10 +46,27 @@ function users_save(t) {
|
||||||
}
|
}
|
||||||
|
|
||||||
# printing the contents of a table: a custom function
|
# printing the contents of a table: a custom function
|
||||||
function table_print(t) {
|
function usertab_print(t) {
|
||||||
if(size(t)>0) {
|
if(size(t)>0) {
|
||||||
foreach(t, function(u, tab) {
|
foreach(t, function(u, tab) {
|
||||||
log("id: ",u," Range ", tab.r, "Bearing ", tab.b)
|
log("id: ",u," Range ", tab.r, "Bearing ", tab.b)
|
||||||
})
|
})
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
function stattab_print() {
|
||||||
|
if(v_status.size()>0) {
|
||||||
|
u=0
|
||||||
|
while(u<8){
|
||||||
|
tab = v_status.get(u)
|
||||||
|
if(tab!=nil) {
|
||||||
|
log("id: ", u)
|
||||||
|
log("- GPS ", tab.g)
|
||||||
|
log("- Battery ", tab.b)
|
||||||
|
log("- Xbee ", tab.x)
|
||||||
|
log("- Status ", tab.f)
|
||||||
|
}
|
||||||
|
u=u+1
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
|
@ -17,7 +17,7 @@ namespace buzz_utility{
|
||||||
static char* BO_FNAME = 0;
|
static char* BO_FNAME = 0;
|
||||||
static uint8_t* BO_BUF = 0;
|
static uint8_t* BO_BUF = 0;
|
||||||
static buzzdebug_t DBG_INFO = 0;
|
static buzzdebug_t DBG_INFO = 0;
|
||||||
static uint32_t MSG_SIZE = 600;//250; // Only 100 bytes of Buzz messages every step
|
static uint32_t MSG_SIZE = 500; // Only 250 bytes of Buzz messages every step (limited to Xbee frame size)
|
||||||
static uint32_t MAX_MSG_SIZE = 10000; // Maximum Msg size for sending update packets
|
static uint32_t MAX_MSG_SIZE = 10000; // Maximum Msg size for sending update packets
|
||||||
static uint8_t Robot_id = 0;
|
static uint8_t Robot_id = 0;
|
||||||
static std::vector<uint8_t*> IN_MSG;
|
static std::vector<uint8_t*> IN_MSG;
|
||||||
|
@ -177,16 +177,15 @@ namespace buzz_utility{
|
||||||
|
|
||||||
void in_message_process(){
|
void in_message_process(){
|
||||||
while(!IN_MSG.empty()){
|
while(!IN_MSG.empty()){
|
||||||
uint8_t* first_INmsg = (uint8_t*)IN_MSG.front();
|
|
||||||
/* Go through messages and append them to the FIFO */
|
/* Go through messages and append them to the FIFO */
|
||||||
uint16_t* data= u64_cvt_u16((uint64_t)first_INmsg[0]);
|
uint8_t* first_INmsg = (uint8_t*)IN_MSG.front();
|
||||||
|
size_t tot =0;
|
||||||
/*Size is at first 2 bytes*/
|
/*Size is at first 2 bytes*/
|
||||||
uint16_t size=data[0]*sizeof(uint64_t);
|
uint16_t size=(*(uint16_t*)first_INmsg)*sizeof(uint64_t);
|
||||||
uint16_t neigh_id = data[1];
|
tot += sizeof(uint16_t);
|
||||||
ROS_WARN("NEIG ID %i",neigh_id);
|
/*Decode neighbor Id*/
|
||||||
delete[] data;
|
uint16_t neigh_id =*(uint16_t*)(first_INmsg+tot);
|
||||||
/*size and robot id read*/
|
tot+=sizeof(uint16_t);
|
||||||
size_t tot = sizeof(uint32_t);
|
|
||||||
/* Go through the messages until there's nothing else to read */
|
/* Go through the messages until there's nothing else to read */
|
||||||
uint16_t unMsgSize=0;
|
uint16_t unMsgSize=0;
|
||||||
/*Obtain Buzz messages push it into queue*/
|
/*Obtain Buzz messages push it into queue*/
|
||||||
|
@ -202,8 +201,8 @@ namespace buzz_utility{
|
||||||
tot += unMsgSize;
|
tot += unMsgSize;
|
||||||
}
|
}
|
||||||
}while(size - tot > sizeof(uint16_t) && unMsgSize > 0);
|
}while(size - tot > sizeof(uint16_t) && unMsgSize > 0);
|
||||||
IN_MSG.erase(IN_MSG.begin());
|
|
||||||
free(first_INmsg);
|
free(first_INmsg);
|
||||||
|
IN_MSG.erase(IN_MSG.begin());
|
||||||
}
|
}
|
||||||
/* Process messages VM call*/
|
/* Process messages VM call*/
|
||||||
buzzvm_process_inmsgs(VM);
|
buzzvm_process_inmsgs(VM);
|
||||||
|
|
Loading…
Reference in New Issue