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

merged with master

This commit is contained in:
dave 2017-12-18 19:10:30 -05:00
parent c77c8044cf d42d0a0cdb
commit 269f22a23b
20 changed files with 2430 additions and 4415 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
CMakeLists.txt
View File

@ -5,6 +5,11 @@ if(UNIX)
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=gnu++11")
endif()
if(SIM)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=1")
else()
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=0")
endif()
## Find catkin macros and libraries
find_package(catkin REQUIRED COMPONENTS
roscpp
@ -54,7 +59,6 @@ add_executable(rosbuzz_node src/rosbuzz.cpp
src/roscontroller.cpp
src/buzz_utility.cpp
src/buzzuav_closures.cpp
src/uav_utility.cpp
src/buzz_update.cpp)
target_link_libraries(rosbuzz_node ${catkin_LIBRARIES} buzz buzzdbg pthread)
add_dependencies(rosbuzz_node rosbuzz_generate_messages_cpp)

93
buzz_scripts/flock.bzz
View File

@ -1,93 +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"
# Lennard-Jones parameters
TARGET = 12.0
EPSILON = 18.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)
}
# 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())
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, 0.0)
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, 0.0)
# } else {
# UAVSTATE ="GONORTH"
# timeW = timeW+1
# uav_moveto(5.0, 0.0, 0.0)
# }
}
########################################
#
# MAIN FUNCTIONS
#
########################################
# Executed once at init time.
function init() {
uav_initswarm()
uav_initstig()
# TARGET_ALTITUDE = 2.5 + id * 5
statef=turnedoff
UAVSTATE = "TURNEDOFF"
}
# Executed at each time step.
function step() {
uav_rccmd()
uav_neicmd()
uav_updatestig()
statef()
log("Current state: ", UAVSTATE)
log("Swarm size: ",ROBOTS)
if(id==0)
stattab_print()
}
# Executed once when the robot (or the simulator) is reset.
function reset() {
}
# Executed once at the end of experiment.
function destroy() {
}

896
buzz_scripts/graphform.bzz
View File

@ -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")
}

35
buzz_scripts/startswarmsimER.launch
View File

@ -1,35 +0,0 @@
include "vec2.bzz"
include "update.bzz"
include "barrier.bzz" # don't use a stigmergy id=80 (auto-increment up) with this header.
include "uavstates.bzz" # require an 'action' function to be defined here.
include "vstigenv.bzz"
function action() {
uav_storegoal(-1.0,-1.0,-1.0)
set_goto(picture)
}
# Executed once at init time.
function init() {
statef=turnedoff
UAVSTATE = "TURNEDOFF"
uav_initstig()
TARGET_ALTITUDE = 15.0
}
# Executed at each time step.
function step() {
uav_rccmd()
statef()
log("Current state: ", UAVSTATE)
}
# Executed once when the robot (or the simulator) is reset.
function reset() {
}
# Executed once at the end of experiment.
function destroy() {
}

269
buzz_scripts/tentacle.bzz
View File

@ -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() {
}

119
buzz_scripts/testflocksim.bzz
View File

@ -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() {
}

537
buzz_scripts/tree_centroid.bzz
View File

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

110
include/buzz_update.h
View File

@ -1,5 +1,8 @@
#ifndef BUZZ_UPDATE_H
#define BUZZ_UPDATE_H
/*Simulation or robot check*/
//#define SIMULATION 1 // set in CMAKELIST
#include <stdlib.h>
#include <stdio.h>
#include <buzz/buzztype.h>
@ -7,80 +10,100 @@
#include <buzz/buzzdarray.h>
#include <buzz/buzzvstig.h>
#include <fstream>
#define delete_p(p) do { free(p); p = NULL; } while(0)
#define delete_p(p) \
do \
{ \
free(p); \
p = NULL; \
} while (0)
static const uint16_t CODE_REQUEST_PADDING = 250;
static const uint16_t MIN_UPDATE_PACKET = 251;
static const uint16_t UPDATE_CODE_HEADER_SIZE = 5;
static const uint16_t TIMEOUT_FOR_ROLLBACK = 50;
/*********************/
/* Updater states */
/********************/
typedef enum {
CODE_RUNNING = 0, // Code executing
CODE_STANDBY, // Standing by for others to update
} code_states_e;
CODE_RUNNING = 0, // Code executing
CODE_STANDBY, // Standing by for others to update
} code_states_e;
/*********************/
/*Message types */
/********************/
typedef enum {
SEND_CODE = 0, // Broadcast code with state
STATE_MSG, // Broadcast state
} code_message_e;
SENT_CODE = 0, // Broadcast code
RESEND_CODE, // ReBroadcast request
} code_message_e;
/*************************/
/*Updater message queue */
/*************************/
struct updater_msgqueue_s {
uint8_t* queue;
uint8_t* size;
} ;
typedef struct updater_msgqueue_s* updater_msgqueue_t;
struct updater_msgqueue_s
{
uint8_t* queue;
uint8_t* size;
};
typedef struct updater_msgqueue_s* updater_msgqueue_t;
struct updater_code_s
{
uint8_t* bcode;
uint8_t* bcode_size;
};
typedef struct updater_code_s* updater_code_t;
/**************************/
/*Updater data*/
/**************************/
struct buzz_updater_elem_s {
/* robot id */
//uint16_t robotid;
/*current Bytecode content */
uint8_t* bcode;
/*current bcode size*/
size_t* bcode_size;
/*current Bytecode content */
uint8_t* standby_bcode;
/*current bcode size*/
size_t* standby_bcode_size;
/*updater out msg queue */
updater_msgqueue_t outmsg_queue;
/*updater in msg queue*/
updater_msgqueue_t inmsg_queue;
/*Current state of the updater one in code_states_e ENUM*/
int* mode;
uint8_t* update_no;
} ;
typedef struct buzz_updater_elem_s* buzz_updater_elem_t;
struct buzz_updater_elem_s
{
/* robot id */
// uint16_t robotid;
/*current Bytecode content */
uint8_t* bcode;
/*old Bytecode name */
const char* old_bcode;
/*current bcode size*/
size_t* bcode_size;
/*Update patch*/
uint8_t* patch;
/* Update patch size*/
size_t* patch_size;
/*current Bytecode content */
uint8_t* standby_bcode;
/*current bcode size*/
size_t* standby_bcode_size;
/*updater out msg queue */
updater_msgqueue_t outmsg_queue;
/*updater in msg queue*/
updater_msgqueue_t inmsg_queue;
/*Current state of the updater one in code_states_e ENUM*/
int* mode;
uint8_t* update_no;
};
typedef struct buzz_updater_elem_s* buzz_updater_elem_t;
/**************************************************************************/
/*Updater routine from msg processing to file checks to be called from main*/
/**************************************************************************/
void update_routine(const char* bcfname,
const char* dbgfname);
void update_routine();
/************************************************/
/*Initalizes the updater */
/************************************************/
void init_update_monitor(const char* bo_filename,const char* stand_by_script);
void init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id);
/*********************************************************/
/*Appends buffer of given size to in msg queue of updater*/
/*********************************************************/
void code_message_inqueue_append(uint8_t* msg,uint16_t size);
void code_message_inqueue_append(uint8_t* msg, uint16_t size);
/*********************************************************/
/*Processes messages inside the queue of the updater*/
@ -108,14 +131,13 @@ void destroy_out_msg_queue();
/***************************************************/
int get_update_mode();
buzz_updater_elem_t get_updater();
/***************************************************/
/*sets bzz file name*/
/***************************************************/
void set_bzz_file(const char* in_bzz_file);
int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_size);
int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size);
/****************************************************/
/*Destroys the updater*/
@ -129,9 +151,11 @@ int get_update_status();
void set_read_update_status();
int compile_bzz();
int compile_bzz(std::string bzz_file);
void updates_set_robots(int robots);
void collect_data(std::ofstream &logger);
void set_packet_id(int packet_id);
void collect_data(std::ofstream& logger);
#endif

42
include/buzz_utility.h
View File

@ -12,20 +12,25 @@
#include <map>
using namespace std;
namespace buzz_utility{
namespace buzz_utility
{
struct pos_struct
{
double x,y,z;
pos_struct(double x,double y,double z):x(x),y(y),z(z){};
pos_struct(){}
double x, y, z;
pos_struct(double x, double y, double z) : x(x), y(y), z(z){};
pos_struct()
{
}
};
typedef struct pos_struct Pos_struct;
struct rb_struct
{
double r,b,latitude,longitude,altitude;
rb_struct(double la, double lo, double al, double r,double b):latitude(la),longitude(lo),altitude(al),r(r),b(b){};
rb_struct(){}
double r, b, latitude, longitude, altitude;
rb_struct(double la, double lo, double al, double r, double b)
: latitude(la), longitude(lo), altitude(al), r(r), b(b){};
rb_struct()
{
}
};
typedef struct rb_struct RB_struct;
@ -35,21 +40,15 @@ struct neiStatus
uint batt_lvl = 0;
uint xbee = 0;
uint flight_status = 0;
}; typedef struct neiStatus neighbors_status ;
};
typedef struct neiStatus neighbors_status;
uint16_t* u64_cvt_u16(uint64_t u64);
int buzz_listen(const char* type,
int msg_size);
void add_user(int id, double latitude, double longitude, float altitude);
void update_users();
int buzz_listen(const char* type, int msg_size);
int make_table(buzzobj_t* t);
int buzzusers_add(int id, double latitude, double longitude, double altitude);
int buzzusers_reset();
int compute_users_rb();
int create_stig_tables();
int create_stig_tables();
void in_msg_append(uint64_t* payload);
@ -57,12 +56,11 @@ uint64_t* obt_out_msg();
void update_sensors();
int buzz_script_set(const char* bo_filename,
const char* bdbg_filename, int robot_id);
int buzz_script_set(const char* bo_filename, const char* bdbg_filename, int robot_id);
int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size);
int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_size);
int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size);
int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_size);
void buzz_script_step();

44
include/buzzuav_closures.h
View File

@ -1,31 +1,30 @@
#pragma once
//#ifndef BUZZUAV_CLOSURES_H
//#define BUZZUAV_CLOSURES_H
#include <buzz/buzzvm.h>
#include <stdio.h>
#include "uav_utility.h"
#include "mavros_msgs/CommandCode.h"
#include "mavros_msgs/Mavlink.h"
#include "ros/ros.h"
#include "buzz_utility.h"
#define EARTH_RADIUS (double) 6371000.0
#define DEG2RAD(DEG) (double) ((DEG)*((M_PI)/(180.0)))
#define RAD2DEG(RAD) (double) ((RAD)*((180.0)/(M_PI)))
#define EARTH_RADIUS (double)6371000.0
#define DEG2RAD(DEG) (double)((DEG) * ((M_PI) / (180.0)))
#define RAD2DEG(RAD) (double)((RAD) * ((180.0) / (M_PI)))
namespace buzzuav_closures{
typedef enum {
COMMAND_NIL = 0, // Dummy command
COMMAND_TAKEOFF, // Take off
COMMAND_LAND,
COMMAND_GOHOME,
COMMAND_ARM,
COMMAND_DISARM,
COMMAND_GOTO,
COMMAND_MOVETO,
COMMAND_PICTURE,
COMMAND_GIMBAL,
} Custom_CommandCode;
namespace buzzuav_closures
{
typedef enum {
COMMAND_NIL = 0, // Dummy command
COMMAND_TAKEOFF, // Take off
COMMAND_LAND,
COMMAND_GOHOME,
COMMAND_ARM,
COMMAND_DISARM,
COMMAND_GOTO,
COMMAND_MOVETO,
COMMAND_PICTURE,
COMMAND_GIMBAL,
} Custom_CommandCode;
/*
* prextern int() function in Buzz
@ -46,8 +45,6 @@ void parse_gpslist();
int buzzuav_takepicture(buzzvm_t vm);
/* Returns the current command from local variable*/
int getcmd();
/*Sets goto position */
void set_goto(double pos[]);
/*Sets goto position from rc client*/
void rc_set_goto(int id, double latitude, double longitude, double altitude);
/*Sets gimbal orientation from rc client*/
@ -55,7 +52,7 @@ void rc_set_gimbal(int id, float yaw, float roll, float pitch, float t);
/*sets rc requested command */
void rc_call(int rc_cmd);
/* sets the battery state */
void set_battery(float voltage,float current,float remaining);
void set_battery(float voltage, float current, float remaining);
void set_deque_full(bool state);
void set_rssi(float value);
void set_raw_packet_loss(float value);
@ -89,7 +86,7 @@ int buzzuav_arm(buzzvm_t vm);
/*
* Disarm from buzz
*/
int buzzuav_disarm(buzzvm_t vm) ;
int buzzuav_disarm(buzzvm_t vm);
/* Commands the UAV to land
*/
int buzzuav_land(buzzvm_t vm);
@ -127,7 +124,6 @@ int buzzuav_update_prox(buzzvm_t vm);
int bzz_cmd();
int dummy_closure(buzzvm_t vm);
//#endif

97
include/roscontroller.h
View File

@ -20,12 +20,12 @@
#include "mavros_msgs/ParamGet.h"
#include "geometry_msgs/PoseStamped.h"
#include "std_msgs/Float64.h"
#include "std_msgs/String.h"
#include <sensor_msgs/LaserScan.h>
#include <rosbuzz/neigh_pos.h>
#include <sstream>
#include <buzz/buzzasm.h>
#include "buzz_utility.h"
#include "uav_utility.h"
#include <stdlib.h>
#include <string.h>
#include <math.h>
@ -37,7 +37,7 @@
#define UPDATER_MESSAGE_CONSTANT 987654321
#define XBEE_MESSAGE_CONSTANT 586782343
#define XBEE_STOP_TRANSMISSION 4355356352
#define TIMEOUT 60
#define TIMEOUT 60
#define BUZZRATE 10
#define CMD_REQUEST_UPDATE 666
@ -45,14 +45,12 @@ using namespace std;
namespace rosbzz_node
{
class roscontroller
{
public:
roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv);
~roscontroller();
//void RosControllerInit();
// void RosControllerInit();
void RosControllerRun();
static const string CAPTURE_SRV_DEFAULT_NAME;
@ -63,43 +61,49 @@ private:
uint8_t history[10];
uint8_t index = 0;
uint8_t current = 0;
num_robot_count(){}
}; typedef struct num_robot_count Num_robot_count ; // not useful in cpp
num_robot_count()
{
}
};
typedef struct num_robot_count Num_robot_count;
Num_robot_count count_robots;
struct gps
{
double longitude=0.0;
double latitude=0.0;
float altitude=0.0;
}; typedef struct gps GPS ;
double longitude = 0.0;
double latitude = 0.0;
float altitude = 0.0;
};
typedef struct gps GPS;
GPS target, home, cur_pos;
double cur_rel_altitude;
uint64_t payload;
std::map< int, buzz_utility::Pos_struct> neighbours_pos_map;
std::map< int, buzz_utility::Pos_struct> raw_neighbours_pos_map;
//std::map< int, buzz_utility::Pos_struct> pub_neigh_pos;
int timer_step=0;
int robot_id=0;
std::map<int, buzz_utility::Pos_struct> neighbours_pos_map;
std::map<int, buzz_utility::Pos_struct> raw_neighbours_pos_map;
// std::map< int, buzz_utility::Pos_struct> pub_neigh_pos;
int timer_step = 0;
int robot_id = 0;
std::string robot_name = "";
//int oldcmdID=0;
// int oldcmdID=0;
int rc_cmd;
float fcu_timeout;
int armstate;
int barrier;
int message_number=0;
uint8_t no_of_robots=0;
int message_number = 0;
uint8_t no_of_robots = 0;
/*tmp to be corrected*/
uint8_t no_cnt=0;
uint8_t old_val=0 ;
uint8_t no_cnt = 0;
uint8_t old_val = 0;
std::string bzzfile_name;
std::string fcclient_name;
std::string armclient;
std::string modeclient;
std::string rcservice_name;
std::string bcfname,dbgfname;
std::string bcfname, dbgfname;
std::string out_payload;
std::string in_payload;
std::string obstacles_topic;
@ -113,13 +117,13 @@ private:
bool rcclient;
bool xbeeplugged = false;
bool multi_msg;
Num_robot_count count_robots;
ros::ServiceClient mav_client;
ros::ServiceClient xbeestatus_srv;
ros::ServiceClient capture_srv;
ros::Publisher payload_pub;
ros::Publisher MPpayload_pub;
ros::Publisher neigh_pos_pub;
ros::Publisher uavstate_pub;
ros::Publisher localsetpoint_nonraw_pub;
ros::ServiceServer service;
ros::Subscriber current_position_sub;
@ -136,7 +140,6 @@ private:
ros::Subscriber local_pos_sub;
double local_pos_new[3];
ros::ServiceClient stream_client;
int setpoint_counter;
@ -145,7 +148,7 @@ private:
std::ofstream log;
/*Commands for flight controller*/
//mavros_msgs::CommandInt cmd_srv;
// mavros_msgs::CommandInt cmd_srv;
mavros_msgs::CommandLong cmd_srv;
std::vector<std::string> m_sMySubscriptions;
std::map<std::string, std::string> m_smTopic_infos;
@ -159,7 +162,7 @@ private:
/*Initialize publisher and subscriber, done in the constructor*/
void Initialize_pub_sub(ros::NodeHandle& n_c);
std::string current_mode; // SOLO SPECIFIC: just so you don't call the switch to same mode
std::string current_mode;
/*Obtain data from ros parameter server*/
void Rosparameters_get(ros::NodeHandle& n_c_priv);
@ -173,33 +176,33 @@ private:
/*Neighbours pos publisher*/
void neighbours_pos_publisher();
/*UAVState publisher*/
void uavstate_publisher();
/*BVM message payload publisher*/
void send_MPpayload();
/*Prepare messages and publish*/
void prepare_msg_and_publish();
/*Refresh neighbours Position for every ten step*/
void maintain_pos(int tim_step);
/*Puts neighbours position inside neigbours_pos_map*/
void neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr );
void neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr);
/*Puts raw neighbours position in lat.,long.,alt. inside raw_neigbours_pos_map*/
void raw_neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr );
void raw_neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr);
/*Set the current position of the robot callback*/
void set_cur_pos(double latitude,
double longitude,
double altitude);
void set_cur_pos(double latitude, double longitude, double altitude);
/*convert from spherical to cartesian coordinate system callback */
float constrainAngle(float x);
float constrainAngle(float x);
void gps_rb(GPS nei_pos, double out[]);
void gps_ned_cur(float &ned_x, float &ned_y, GPS t);
void gps_ned_home(float &ned_x, float &ned_y);
void gps_convert_ned(float &ned_x, float &ned_y,
double gps_t_lon, double gps_t_lat,
double gps_r_lon, double gps_r_lat);
void gps_ned_cur(float& ned_x, float& ned_y, GPS t);
void gps_convert_ned(float& ned_x, float& ned_y, double gps_t_lon, double gps_t_lat, double gps_r_lon,
double gps_r_lat);
/*battery status callback */
void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg);
@ -212,8 +215,6 @@ private:
/*current position callback*/
void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg);
void users_pos(const rosbuzz::neigh_pos msg);
/*current relative altitude callback*/
void current_rel_alt(const std_msgs::Float64::ConstPtr& msg);
@ -222,7 +223,7 @@ private:
void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg);
/* RC commands service */
bool rc_callback(mavros_msgs::CommandLong::Request &req, mavros_msgs::CommandLong::Response &res);
bool rc_callback(mavros_msgs::CommandLong::Request& req, mavros_msgs::CommandLong::Response& res);
/*robot id sub callback*/
void set_robot_id(const std_msgs::UInt8::ConstPtr& msg);
@ -244,7 +245,7 @@ private:
void local_pos_callback(const geometry_msgs::PoseStamped::ConstPtr& pose);
//void WaypointMissionSetup(float lat, float lng, float alt);
// void WaypointMissionSetup(float lat, float lng, float alt);
void fc_command_setup();
@ -257,15 +258,13 @@ private:
void get_number_of_robots();
void GetRobotId();
bool GetDequeFull(bool &result);
bool GetRssi(float &result);
bool GetDequeFull(bool& result);
bool GetRssi(float& result);
bool TriggerAPIRssi(const uint8_t short_id);
bool GetAPIRssi(const uint8_t short_id, float &result);
bool GetRawPacketLoss(const uint8_t short_id, float &result);
bool GetFilteredPacketLoss(const uint8_t short_id, float &result);
bool GetAPIRssi(const uint8_t short_id, float& result);
bool GetRawPacketLoss(const uint8_t short_id, float& result);
bool GetFilteredPacketLoss(const uint8_t short_id, float& result);
void get_xbee_status();
};
}

9
include/uav_utility.h
View File

@ -1,9 +0,0 @@
#ifndef UAV_UTILITY_H
#define UAV_UTILITY_H
extern void uav_setup();
extern void uav_done();
#endif

3
log.txt
View File

@ -1,3 +0,0 @@
2017-01-08-14-24-24 enter TranslateFunc
2017-01-08-14-24-24 start to read frames!

45
readme.md
View File

@ -21,20 +21,7 @@ More information is available at http://the.swarming.buzz/wiki/doku.php?id=start
Description:
============
Rosbuzz package is the ROS version of Buzz. The package contains a node called “rosbuzz_node”, which implements buzz as a node in ROS. The rosbuzz_node requires certain parameters to run. These parameters are supplied to the node from the ros parameter server. The package also contains a launch file called “rosbuzzm100.launch”, which initializes the required parameters. The required parameters are the .bzz file location, presence of Remote Controller client, Remote Controller service name, Flight Controller client name, Robot ID and Number of Robots.
* The .bzz file location parameter is a string with name “bzzfile_name”, the node compiles and parses the specified .bzz file into .basm, .bo and .bdbg files. The node runs the parsed .bo byte file.
* The presence of remote controller client is a bool parameter with name “rcclient”, this specifies whether there is a remote controller present to issue bypassing commands like takeoff, land, goto location and go home. If there is no remote controller present then this parameter could be set to “false”. If a remote controller exists, this parameter could be set “True” and the service topic offered by the remote controller should be specified to the parameter named “rcservice_name”.
* The flight controller client present in the node, that is used to send commands to the flight controller. The commands that could be sent are takeoff, land, goto location and go home. The topic name used to communicate with the flight controller should be set to the parameter named “fcclient_name”.
* The next parameter that the rosbuzz_node depends on is the robot ID of type “int”, this parameter should be set to the parameter named “robot_id” in the parameter server. This parameter is by default set to ‘0’, if not specified.
* Hot swaps of code can be integrated into the ROSBuzz node directly from a text editor. The ROSBuzz node monitors the file supplied i.e. the name specified in "bzzfile_name". When this file is modified and saved, the new script is tested and once test passes. The new script is set to the current robot and sent to all the robots in the network. To ensure convergence, i.e. all the robots recived the code, number of robots present in the network is needed. The number of robots has to be specified under the parameter name "No_of_Robots".
An example template of parameter setting could be found in the launch file “launch/rosbuzzm100.launch”.
The launch file named "launch/rosbuzz_2_parallel.launch" can be used to launch two nodes in parellel for testing ROSBuzz node on a single Machine.
Rosbuzz package is the ROS version of Buzz. The package contains a node called “rosbuzz_node”, which implements buzz virtual machine (BVM) as a node in ROS.
Downloading ROS Package
@ -75,37 +62,41 @@ Run
===
To run the ROSBuzz package using the launch file, execute the following:
$ roslaunch rosbuzz rosbuzzm100.launch
$ roslaunch rosbuzz rosbuzz.launch
Note : Before launching the ROSBuzz node, verify all the parameter in the launch file.
Note : Before launching the ROSBuzz node, verify all the parameters in the launch file. A launch file using gdb is available also (rosbuzzd.launch).
* Buzz scripts: Several behavioral scripts are included in the "buzz_Scripts" folder, such as "graphformGPS.bzz" uses in the ICRA publication below and the "testaloneWP.bzz" to control a single drone with a ".csv" list of waypoints. The script "empty.bzz" is a template script.
Publisher
=========
* Messages from Buzz (BVM):
The package publishes mavros_msgs/Mavlink message with a topic name of "outMavlink".
The package publishes mavros_msgs/Mavlink message "outMavlink".
* Command to the flight controller:
The package publishes geometry_msgs/PoseStamped message "setpoint_position/local".
Subscribers
-----------
* Current position of the Robot:
The package subscribes' to sensor_msgs/NavSatFix message with a topic name of "current_pos".
The package subscribes to sensor_msgs/NavSatFix message "global_position/global", to a std_msgs/Float64 message "global_position/rel_alt" and to a geometry_msgs/PoseStamped message "local_position/pose".
* Messages to Buzz (BVM):
The package subscribes' to mavros_msgs/Mavlink message with a topic name of "inMavlink".
The package subscribes to mavros_msgs/Mavlink message with a topic name of "inMavlink".
* Battery status:
The package subscribes' to mavros_msgs/BatteryStatus message with a topic name of "battery_state".
* Status:
The package subscribes to mavros_msgs/BatteryStatus message "battery" and to either a mavros_msgs/ExtendedState message "extended_state" or a mavros_msgs/State message "state".
Service
-------
* Remote Controller command:
The package offers service using mavros_msgs/CommandInt service with name "rc_cmd".
* Remote Controller:
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.
Client
References
------
* ROS and Buzz : consensus-based behaviors for heterogeneous teams. Submitted to the Internaional Conference on Robotics and Automation (September 2017). 6pgs. St-Onge, D., Shankar Varadharajan, V., Li, G., Svogor, I. and Beltrame, G. arXiv : https://arxiv.org/abs/1710.08843
* Flight controller client:
This package is a client of mavros_msgs/CommandInt service with name "fc_cmd".
* Over-The-Air Updates for Robotic Swarms. Accepted by IEEE Software (September 2017 - pending minor revision). 8pgs. Shankar Varadharajan, V., St-Onge, D., Guß, C. and Beltrame, G.

1062
src/buzz_update.cpp
View File

File diff suppressed because it is too large Load Diff

1344
src/buzz_utility.cpp
View File

File diff suppressed because it is too large Load Diff

1421
src/buzzuav_closures.cpp
View File

File diff suppressed because it is too large Load Diff

18
src/rosbuzz.cpp
View File

@ -1,8 +1,8 @@
/** @file rosbuzz.cpp
* @version 1.0
* @version 1.0
* @date 27.09.2016
* @brief Buzz Implementation as a node in ROS for Dji M100 Drone.
* @author Vivek Shankar Varadharajan
* @brief Buzz Implementation as a node in ROS.
* @author Vivek Shankar Varadharajan and David St-Onge
* @copyright 2016 MistLab. All rights reserved.
*/
@ -11,18 +11,16 @@
/**
* This program implements Buzz node in ros using mavros_msgs.
*/
int main(int argc, char **argv)
int main(int argc, char** argv)
{
/*Initialize rosBuzz node*/
ros::init(argc, argv, "rosBuzz");
ros::init(argc, argv, "rosBuzz");
ros::NodeHandle nh_priv("~");
ros::NodeHandle nh;
rosbzz_node::roscontroller RosController(nh, nh_priv);
/*Register ros controller object inside buzz*/
//buzzuav_closures::set_ros_controller_ptr(&RosController);
RosController.RosControllerRun();
// buzzuav_closures::set_ros_controller_ptr(&RosController);
RosController.RosControllerRun();
return 0;
}

670
src/roscontroller.cpp
View File

File diff suppressed because it is too large Load Diff

25
src/uav_utility.cpp
View File

@ -1,25 +0,0 @@
#include "uav_utility.h"
#include "stdio.h"
/****************************************/
/****************************************/
/****************************************/
/*To do !*/
/****************************************/
void uav_setup() {
}
/****************************************/
/*To do !*/
/****************************************/
void uav_done() {
fprintf(stdout, "Robot stopped.\n");
}
/****************************************/
/****************************************/