Spiri
/
ROSBuzz_MISTLab
12
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

re-fix msgOut (vivek), debug bandwidth-optimised graph and add vstig of uav status

This commit is contained in:
dave 2017-07-24 22:56:26 -04:00
parent efe1c762b0
commit b9c0dfb117
11 changed files with 228 additions and 183 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
buzz_scripts/flock.bzz
View File

@ -23,19 +23,6 @@ function lj_vector(rid, data) {
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() {
@ -45,9 +32,6 @@ function action() {
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))
}
@ -80,6 +64,7 @@ function action() {
# Executed once at init time.
function init() {
uav_initswarm()
uav_initstig()
}
# Executed at each time step.
@ -88,9 +73,11 @@ function step() {
uav_neicmd()
statef()
uav_updatestig()
log("Current state: ", UAVSTATE)
log("Swarm size: ",ROBOTS)
if(id==0)
stattab_print()
}
# Executed once when the robot (or the simulator) is reset.

214
buzz_scripts/graphform.bzz
View File

@ -7,35 +7,38 @@ 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 "graphs/shapes_square.bzz"
include "graphs/shapes_L.bzz"
ROBOT_RADIUS=50
ROBOT_DIAMETER=2.0*ROBOT_RADIUS
ROBOT_SAFETYDIST=2.0*ROBOT_DIAMETER
# max velocity in cm/step
ROBOT_MAXVEL = 500.0
#
# Global variables
#
#
#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
m_MessageState={}#store received neighbour message
m_MessageLable={}#store received neighbour message
m_MessageReqLable={}#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_neighbourCunt=0#used to cunt neighbours
m_neighbourCount=0#used to cunt neighbours
#Save message from one neighbour
#the indexes are as State(received state),Lable(received lable),ReqLable,ReqID,Response,Range,Bearing
m_receivedMessage={.State=s2i("STATE_FREE"),.Lable=0,.ReqLable=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0}
#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),Lable(current lable),ReqLable(requested lable),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")}
#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"
@ -84,7 +87,7 @@ m_fTargetDistanceTolerance=0
step_cunt=0
#virtual stigmergy
v_tag = stigmergy.create(1)
m_lockstig = 1
# Lennard-Jones parameters, may need change
EPSILON = 13.5 #the LJ parameter for other robots
@ -220,7 +223,7 @@ function pow(base,exponent){
#
function packmessage(send_table){
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
}
#
@ -253,10 +256,10 @@ function unpackmessage(recv_value){
var shi=(recv_value-recv_value%10)/10
recv_value=recv_value-shi*10
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.Lable=qian
return_table.ReqLable=bai
return_table.Label=qian
return_table.ReqLabel=bai
return_table.ReqID=shi
return_table.Response=ge
return return_table
@ -309,13 +312,13 @@ function LJ_vec(i){
function motion_vector(){
var i=0
var m_vector={.x=0.0,.y=0.0}
while(i<m_neighbourCunt){
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,1000.0)
m_vector=math.vec2.scale(m_vector,1.0/m_neighbourCount)
return m_vector
}
@ -328,15 +331,15 @@ neighbors.listen("m",
Get_DisAndAzi(temp_id)
#add the received message
#
m_MessageState[m_neighbourCunt]=i2s(recv_val.State)
m_MessageLable[m_neighbourCunt]=recv_val.Lable
m_MessageReqLable[m_neighbourCunt]=recv_val.ReqLable
m_MessageReqID[m_neighbourCunt]=recv_val.ReqID
m_MessageResponse[m_neighbourCunt]=i2r(recv_val.Response)
m_MessageRange[m_neighbourCunt]=m_receivedMessage.Range
m_MessageBearing[m_neighbourCunt]=m_receivedMessage.Bearing
m_messageID[m_neighbourCunt]=rid
m_neighbourCunt=m_neighbourCunt+1
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
})
}
#
@ -359,14 +362,14 @@ function UpdateNodeInfo(){
#Update information
var i=0
while(i<m_neighbourCunt){
while(i<m_neighbourCount){
if(m_MessageState[i]=="STATE_JOINED"){
m_vecNodes[m_MessageLable[i]].State="ASSIGNED"
m_vecNodes[m_MessageLable[i]].StateAge=m_unJoiningLostPeriod
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_MessageLable[i]].State="ASSIGNING"
m_vecNodes[m_MessageLable[i]].StateAge=m_unJoiningLostPeriod
m_vecNodes[m_MessageLabel[i]].State="ASSIGNING"
m_vecNodes[m_MessageLabel[i]].StateAge=m_unJoiningLostPeriod
}
i=i+1
}
@ -399,7 +402,7 @@ function TransitionToAsking(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_selfMessage.State=s2i(m_eState)
m_selfMessage.ReqLable=m_nLabel
m_selfMessage.ReqLabel=m_nLabel
m_selfMessage.ReqID=m_unRequestId
m_unWaitCount=m_unResponseTimeThreshold
@ -411,7 +414,7 @@ function TransitionToAsking(un_label){
function TransitionToJoining(){
m_eState="STATE_JOINING"
m_selfMessage.State=s2i(m_eState)
m_selfMessage.Lable=m_nLabel
m_selfMessage.Label=m_nLabel
m_unWaitCount=m_unJoiningLostPeriod
neighbors.listen("r",
@ -432,12 +435,12 @@ function TransitionToJoining(){
function TransitionToJoined(){
m_eState="STATE_JOINED"
m_selfMessage.State=s2i(m_eState)
m_selfMessage.Lable=m_nLabel
m_selfMessage.Label=m_nLabel
m_vecNodes[m_nLabel].State="ASSIGNED"
neighbors.ignore("r")
#write statues
v_tag.put(m_nLabel, 1)
v_tag.put(m_nLabel, m_lockstig)
m_navigation.x=0.0
m_navigation.y=0.0
@ -453,13 +456,13 @@ function TransitionToJoined(){
function TransitionToLock(){
m_eState="STATE_LOCK"
m_selfMessage.State=s2i(m_eState)
m_selfMessage.Lable=m_nLabel
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_neighbourCunt){
while(i<m_neighbourCount){
lock_neighbor_id[i]=m_messageID[i]
lock_neighbor_dis[i]=m_MessageRange[i]
i=i+1
@ -475,39 +478,39 @@ while(i<m_neighbourCunt){
function DoFree() {
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)
m_unWaitCount=m_unWaitCount-1
#find a set of joined robots
var setJoinedLables={}
var setJoinedLabels={}
var setJoinedIndexes={}
var i=0
var j=0
while(i<m_neighbourCunt){
while(i<m_neighbourCount){
if(m_MessageState[i]=="STATE_JOINED"){
setJoinedLables[j]=m_MessageLable[i]
setJoinedLabels[j]=m_MessageLabel[i]
setJoinedIndexes[j]=i
j=j+1
}
i=i+1
}
#go through the graph to look for a proper lable
var unFoundLable=0
var IDofPred=0
#go through the graph to look for a proper Label
var unFoundLabel=0
# var IDofPred=0
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(m_vecNodes[i].State=="UNASSIGNED" and count(setJoinedLables,m_vecNodes[i].Pred)==1){
unFoundLable=m_vecNodes[i].Lable
IDofPred=find(m_MessageLable,m_vecNodes[unFoundLable].Pred)
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(unFoundLable>0){
TransitionToAsking(unFoundLable)
if(unFoundLabel>0){
TransitionToAsking(unFoundLabel)
return
}
@ -527,12 +530,15 @@ function DoFree() {
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)
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)
}else{ #no joined robots in sight
i=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]))
i=i+1
}
@ -544,7 +550,9 @@ function DoFree() {
#jump the first step
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
m_selfMessage.State=s2i(m_eState)
@ -561,12 +569,12 @@ function DoAsking(){
#look for response from predecessor
var i=0
var psResponse=-1
while(i<m_neighbourCunt and psResponse==-1){
while(i<m_neighbourCount and psResponse==-1){
#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
if(m_MessageState[i]=="STATE_JOINED"){
if(m_MessageReqLable[i]==m_nLabel)
if(m_MessageReqLabel[i]==m_nLabel)
if(m_MessageResponse[i]!="REQ_NONE"){
psResponse=i
}}
@ -577,7 +585,7 @@ function DoAsking(){
#no response, wait
m_unWaitCount=m_unWaitCount-1
m_selfMessage.State=s2i(m_eState)
m_selfMessage.ReqLable=m_nLabel
m_selfMessage.ReqLabel=m_nLabel
m_selfMessage.ReqID=m_unRequestId
if(m_unWaitCount==0){
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
var i=0
var IDofPred=-1
while(i<m_neighbourCunt and IDofPred==-1){
if(m_MessageLable[i]==m_vecNodes[m_nLabel].Pred and m_MessageState[i]=="STATE_JOINED")
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
}
@ -640,7 +650,12 @@ function DoJoining(){
#change the vector to local coordinate of self
var S2Target_bearing=Angle(S2Target)
m_bias=m_cMeToPred.Bearing-S2PGlobalBearing
S2Target_bearing=S2Target_bearing+m_bias # 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)
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
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()
return
}
@ -664,7 +679,7 @@ function DoJoining(){
#pack the communication package
m_selfMessage.State=s2i(m_eState)
m_selfMessage.Lable=m_nLabel
m_selfMessage.Label=m_nLabel
}
@ -673,35 +688,35 @@ function DoJoining(){
#
function DoJoined(){
m_selfMessage.State=s2i(m_eState)
m_selfMessage.Lable=m_nLabel
m_selfMessage.Label=m_nLabel
#collect all requests
var mapRequests={}
var i=0
var j=0
var ReqLable
var JoiningLable
var ReqLabel
var JoiningLabel
var seenPred=0
while(i<m_neighbourCunt){
while(i<m_neighbourCount){
if(m_MessageState[i]=="STATE_ASKING"){
ReqLable=m_MessageReqLable[i]
if(m_vecNodes[ReqLable].State=="UNASSIGNED")
if(m_nLabel==m_vecNodes[ReqLable].Pred){
ReqLabel=m_MessageReqLabel[i]
if(m_vecNodes[ReqLabel].State=="UNASSIGNED")
if(m_nLabel==m_vecNodes[ReqLabel].Pred){
#is a request, store the index
mapRequests[j]=i
j=j+1
}
}
if(m_MessageState[i]=="STATE_JOINING"){
JoiningLable=m_MessageLable[i]
if(m_nLabel==m_vecNodes[JoiningLable].Pred){
JoiningLabel=m_MessageLabel[i]
if(m_nLabel==m_vecNodes[JoiningLabel].Pred){
##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))
}
}
#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
m_unWaitCount=m_unJoiningLostPeriod
}
@ -718,22 +733,22 @@ function DoJoined(){
ReqIndex=i
i=i+1
}
#get the best index, whose Reqlable and Reqid are
ReqLable=m_MessageReqLable[mapRequests[ReqIndex]]
#get the best index, whose ReqLabel and Reqid are
ReqLabel=m_MessageReqLabel[mapRequests[ReqIndex]]
var ReqID=m_MessageReqID[mapRequests[ReqIndex]]
m_selfMessage.ReqLable=ReqLable
m_selfMessage.ReqLabel=ReqLabel
m_selfMessage.ReqID=ReqID
m_selfMessage.Response=r2i("REQ_GRANTED")
}
#lost pred, wait for some time and transit to free
#if(seenPred==0){
#m_unWaitCount=m_unWaitCount-1
#if(m_unWaitCount==0){
#TransitionToFree()
#return
#}
#}
if(seenPred==0){
m_unWaitCount=m_unWaitCount-1
if(m_unWaitCount==0){
TransitionToFree()
return
}
}
m_navigation.x=0.0
@ -742,11 +757,12 @@ function DoJoined(){
#check if should to transists to lock
# if(v_tag.size()==ROBOTS){
# TransitionToLock()
# }
#write statues
#v_tag.put(m_nLabel, 1)
log(v_tag.size(), " of ", ROBOTS, " ready to lock")
if(v_tag.size()==ROBOTS){
TransitionToLock()
}
}
#
@ -754,7 +770,7 @@ function DoJoined(){
#
function DoLock(){
m_selfMessage.State=s2i(m_eState)
m_selfMessage.Lable=m_nLabel
m_selfMessage.Label=m_nLabel
m_navigation.x=0.0
m_navigation.y=0.0
#calculate motion vection
@ -784,15 +800,16 @@ function init() {
#
m_unResponseTimeThreshold=10
m_unLabelSearchWaitTime=10
m_fTargetDistanceTolerance=10
m_fTargetDistanceTolerance=1
m_unJoiningLostPeriod=100
#
# Join Swarm
#
uav_initswarm()
v_tag = stigmergy.create(m_lockstig)
Reset()
Read_Graph()
TARGET_ALTITUDE = 2.5 + id * 1.5
}
#
@ -804,7 +821,7 @@ function step(){
#update the graph
UpdateNodeInfo()
#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
#
@ -837,13 +854,13 @@ function step(){
#
#clean message storage
m_MessageState={}#store received neighbour message
m_MessageLable={}#store received neighbour message
m_MessageReqLable={}#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_neighbourCunt=0
m_neighbourCount=0
#step cunt+1
@ -854,10 +871,11 @@ function step(){
# Executed when reset
#
function Reset(){
#m_vecNodes={}
#m_vecNodes = parse_graph("/home/dave/ROS_WS/src/rosbuzz/buzz_scripts/include/Graph_drone.graph")#change the .graph file when necessary
# m_vecNodes={}
# m_vecNodes = parse_graph("/home/dave/ROS_WS/src/rosbuzz/buzz_scripts/include/Graph_drone.graph")#change the .graph file when necessary
# m_vecNodes_fixed={}
# m_vecNodes_fixed=parse_graph_fixed("/home/dave/ROS_WS/src/rosbuzz/buzz_scripts/include/Graph_fixed.graph")
Read_Graph()
m_nLabel=-1
#start listening
@ -881,7 +899,9 @@ function Reset(){
#
function destroy() {
#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={}
#stop listening
neighbors.ignore("m")

5
buzz_scripts/include/Graph_drone.graph
View File

@ -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

5
buzz_scripts/include/Graph_fixed.graph
View File

@ -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

16
buzz_scripts/include/graphs/shapes_L.bzz
View File

@ -2,9 +2,9 @@
m_vecNodes={}
function Read_Graph(){
m_vecNodes[0] = { # The .graph file is stored according the sequence of lable, predecessor, distance, bearing
.Lable = 0, # Lable of the point
.Pred = -1, # Lable of its predecessor
m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing
.Label = 0, # Label of the point
.Pred = -1, # Label of its predecessor
.distance = -1, # distance to the predecessor
.bearing = -1, # bearing form the predecessor to 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
}
m_vecNodes[1] = {
.Lable = 1,
.Label = 1,
.Pred = 0,
.distance = 2000,
.bearing = 0.0,
@ -21,7 +21,7 @@ m_vecNodes[1] = {
.StateAge=0
}
m_vecNodes[2] = {
.Lable = 2,
.Label = 2,
.Pred = 0,
.distance = 2000,
.bearing = 1.57,
@ -30,7 +30,7 @@ m_vecNodes[2] = {
.StateAge=0
}
m_vecNodes[3] = {
.Lable = 3,
.Label = 3,
.Pred = 1,
.distance = 2000,
.bearing = 0.0,
@ -39,7 +39,7 @@ m_vecNodes[3] = {
.StateAge=0
}
m_vecNodes[4] = {
.Lable = 4,
.Label = 4,
.Pred = 2,
.distance = 2000,
.bearing = 1.57,
@ -48,7 +48,7 @@ m_vecNodes[4] = {
.StateAge=0
}
m_vecNodes[5] = {
.Lable = 5,
.Label = 5,
.Pred = 4,
.distance = 2000,
.bearing = 1.57,

16
buzz_scripts/include/graphs/shapes_O.bzz
View File

@ -2,9 +2,9 @@
m_vecNodes={}
m_vecNodes_fixed={}
function Read_Graph(){
m_vecNodes[0] = { # The .graph file is stored according the sequence of lable, predecessor, distance, bearing
.Lable = 0, # Lable of the point
.Pred = -1, # Lable of its predecessor
m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing
.Label = 0, # Label of the point
.Pred = -1, # Label of its predecessor
.distance = -1, # distance to the predecessor
.bearing = -1, # bearing form the predecessor to 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
}
m_vecNodes[1] = {
.Lable = 1,
.Label = 1,
.Pred = 0,
.distance = 2000,
.bearing = 0.0,
@ -21,7 +21,7 @@ m_vecNodes[1] = {
.StateAge=0
}
m_vecNodes[2] = {
.Lable = 2,
.Label = 2,
.Pred = 0,
.distance = 2000,
.bearing = 1.57,
@ -30,7 +30,7 @@ m_vecNodes[2] = {
.StateAge=0
}
m_vecNodes[3] = {
.Lable = 3,
.Label = 3,
.Pred = 1,
.distance = 2000,
.bearing = 1.57,
@ -39,7 +39,7 @@ m_vecNodes[3] = {
.StateAge=0
}
m_vecNodes[4] = {
.Lable = 4,
.Label = 4,
.Pred = 1,
.distance = 1414,
.bearing = 3.93,
@ -48,7 +48,7 @@ m_vecNodes[4] = {
.StateAge=0
}
m_vecNodes[5] = {
.Lable = 5,
.Label = 5,
.Pred = 2,
.distance = 1414,
.bearing = 0.785,

17
buzz_scripts/include/graphs/shapes_P.bzz
View File

@ -1,9 +1,10 @@
#Table of the nodes in the graph
m_vecNodes={}
m_vecNodes_fixed={}
function Read_Graph(){
m_vecNodes[0] = { # The .graph file is stored according the sequence of lable, predecessor, distance, bearing
.Lable = 0, # Lable of the point
.Pred = -1, # Lable of its predecessor
m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing
.Label = 0, # Label of the point
.Pred = -1, # Label of its predecessor
.distance = -1, # distance to the predecessor
.bearing = -1, # bearing form the predecessor to 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
}
m_vecNodes[1] = {
.Lable = 1,
.Label = 1,
.Pred = 0,
.distance = 2000,
.bearing = 0.0,
@ -20,7 +21,7 @@ m_vecNodes[1] = {
.StateAge=0
}
m_vecNodes[2] = {
.Lable = 2,
.Label = 2,
.Pred = 0,
.distance = 2000,
.bearing = 1.57,
@ -29,7 +30,7 @@ m_vecNodes[2] = {
.StateAge=0
}
m_vecNodes[3] = {
.Lable = 3,
.Label = 3,
.Pred = 0,
.distance = 2000,
.bearing = 4.71,
@ -38,7 +39,7 @@ m_vecNodes[3] = {
.StateAge=0
}
m_vecNodes[4] = {
.Lable = 4,
.Label = 4,
.Pred = 1,
.distance = 1414,
.bearing = 0.79,
@ -47,7 +48,7 @@ m_vecNodes[4] = {
.StateAge=0
}
m_vecNodes[5] = {
.Lable = 5,
.Label = 5,
.Pred = 2,
.distance = 2000,
.bearing = 0.0,

17
buzz_scripts/include/graphs/shapes_Y.bzz
View File

@ -1,9 +1,10 @@
#Table of the nodes in the graph
m_vecNodes={}
m_vecNodes_fixed={}
function Read_Graph(){
m_vecNodes[0] = { # The .graph file is stored according the sequence of lable, predecessor, distance, bearing
.Lable = 0, # Lable of the point
.Pred = -1, # Lable of its predecessor
m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing
.Label = 0, # Label of the point
.Pred = -1, # Label of its predecessor
.distance = -1, # distance to the predecessor
.bearing = -1, # bearing form the predecessor to 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
}
m_vecNodes[1] = {
.Lable = 1,
.Label = 1,
.Pred = 0,
.distance = 2000,
.bearing = 4.71,
@ -20,7 +21,7 @@ m_vecNodes[1] = {
.StateAge=0
}
m_vecNodes[2] = {
.Lable = 2,
.Label = 2,
.Pred = 0,
.distance = 1414,
.bearing = 2.36,
@ -29,7 +30,7 @@ m_vecNodes[2] = {
.StateAge=0
}
m_vecNodes[3] = {
.Lable = 3,
.Label = 3,
.Pred = 0,
.distance = 1414,
.bearing = 0.79,
@ -38,7 +39,7 @@ m_vecNodes[3] = {
.StateAge=0
}
m_vecNodes[4] = {
.Lable = 4,
.Label = 4,
.Pred = 2,
.distance = 1414,
.bearing = 2.36,
@ -47,7 +48,7 @@ m_vecNodes[4] = {
.StateAge=0
}
m_vecNodes[5] = {
.Lable = 5,
.Label = 5,
.Pred = 3,
.distance = 1414,
.bearing = 0.79,

26
buzz_scripts/include/graphs/shapes_square.bzz
View File

@ -1,11 +1,10 @@
#Table of the nodes in the graph
m_vecNodes={}
m_vecNodes_fixed={}
function Read_Graph(){
m_vecNodes[0] = { # The .graph file is stored according the sequence of lable, predecessor, distance, bearing
.Lable = 0, # Lable of the point
.Pred = -1, # Lable of its predecessor
m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing
.Label = 0, # Label of the point
.Pred = -1, # Label of its predecessor
.distance = -1, # distance to the predecessor
.bearing = -1, # bearing form the predecessor to 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
}
m_vecNodes[1] = {
.Lable = 1,
.Label = 1,
.Pred = 0,
.distance = 1000,
.bearing = 0.0,
@ -22,7 +21,7 @@ m_vecNodes[1] = {
.StateAge=0
}
m_vecNodes[2] = {
.Lable = 2,
.Label = 2,
.Pred = 0,
.distance = 1000,
.bearing = 1.57,
@ -31,7 +30,7 @@ m_vecNodes[2] = {
.StateAge=0
}
m_vecNodes[3] = {
.Lable = 3,
.Label = 3,
.Pred = 0,
.distance = 1000,
.bearing = 3.14,
@ -40,7 +39,16 @@ m_vecNodes[3] = {
.StateAge=0
}
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,
.distance = 1000,
.bearing = 4.71,

49
buzz_scripts/include/vstigenv.bzz
View File

@ -1,12 +1,34 @@
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() {
# Read a value from the structure
if(size(users)>0)
log("Got a user!")
# log(users)
#users_print(users.dataG)
# if(size(users.dataG)>0)
# vt.put("p", users.dataG)
# 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")
@ -24,10 +46,27 @@ function users_save(t) {
}
# printing the contents of a table: a custom function
function table_print(t) {
function usertab_print(t) {
if(size(t)>0) {
foreach(t, function(u, tab) {
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
}
}
}

25
src/buzz_utility.cpp
View File

@ -17,7 +17,7 @@ namespace buzz_utility{
static char* BO_FNAME = 0;
static uint8_t* BO_BUF = 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 uint8_t Robot_id = 0;
static std::vector<uint8_t*> IN_MSG;
@ -175,18 +175,17 @@ namespace buzz_utility{
}
void in_message_process(){
void in_message_process(){
while(!IN_MSG.empty()){
uint8_t* first_INmsg = (uint8_t*)IN_MSG.front();
/* 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*/
uint16_t size=data[0]*sizeof(uint64_t);
uint16_t neigh_id = data[1];
ROS_WARN("NEIG ID %i",neigh_id);
delete[] data;
/*size and robot id read*/
size_t tot = sizeof(uint32_t);
uint16_t size=(*(uint16_t*)first_INmsg)*sizeof(uint64_t);
tot += sizeof(uint16_t);
/*Decode neighbor Id*/
uint16_t neigh_id =*(uint16_t*)(first_INmsg+tot);
tot+=sizeof(uint16_t);
/* Go through the messages until there's nothing else to read */
uint16_t unMsgSize=0;
/*Obtain Buzz messages push it into queue*/
@ -197,13 +196,13 @@ namespace buzz_utility{
/* Append message to the Buzz input message queue */
if(unMsgSize > 0 && unMsgSize <= size - tot ) {
buzzinmsg_queue_append(VM,
neigh_id,
neigh_id,
buzzmsg_payload_frombuffer(first_INmsg +tot, unMsgSize));
tot += unMsgSize;
}
}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*/
buzzvm_process_inmsgs(VM);