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

fix topic naming and rosbuzz launch

This commit is contained in:
dave 2017-08-30 14:58:44 -04:00
parent 13267a9e97
commit 32c10dbc42
15 changed files with 87 additions and 947 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
buzz_scripts/flock.bzz
View File

@ -65,16 +65,19 @@ function action() {
function init() { function init() {
uav_initswarm() uav_initswarm()
uav_initstig() uav_initstig()
TARGET_ALTITUDE = 2.5 + id * 5 # TARGET_ALTITUDE = 2.5 + id * 5
statef=turnedoff
UAVSTATE = "TURNEDOFF"
} }
# Executed at each time step. # Executed at each time step.
function step() { function step() {
uav_rccmd() uav_rccmd()
uav_neicmd() uav_neicmd()
uav_updatestig()
statef() statef()
uav_updatestig()
log("Current state: ", UAVSTATE) log("Current state: ", UAVSTATE)
log("Swarm size: ",ROBOTS) log("Swarm size: ",ROBOTS)
if(id==0) if(id==0)

29
buzz_scripts/graphform.bzz
View File

@ -10,9 +10,10 @@ include "vstigenv.bzz"
include "graphs/shapes_Y.bzz" include "graphs/shapes_Y.bzz"
ROBOT_RADIUS=50 ROBOT_RADIUS = 50
ROBOT_DIAMETER=2.0*ROBOT_RADIUS ROBOT_DIAMETER = 2.0*ROBOT_RADIUS
ROBOT_SAFETYDIST=2.0*ROBOT_DIAMETER ROBOT_SAFETYDIST = 2.0*ROBOT_DIAMETER
ROOT_ID = 2
# max velocity in cm/step # max velocity in cm/step
ROBOT_MAXVEL = 250.0 ROBOT_MAXVEL = 250.0
@ -802,7 +803,7 @@ function action(){
# #
function init() { function init() {
# #
#Adjust parameters here # Global parameters for graph formation
# #
m_unResponseTimeThreshold=10 m_unResponseTimeThreshold=10
m_unLabelSearchWaitTime=10 m_unLabelSearchWaitTime=10
@ -816,23 +817,32 @@ function init() {
uav_initswarm() uav_initswarm()
v_tag = stigmergy.create(m_lockstig) v_tag = stigmergy.create(m_lockstig)
uav_initstig() uav_initstig()
Reset() # go to diff. height since no collision avoidance implemented yet
TARGET_ALTITUDE = 2.5 + id * 1.5 TARGET_ALTITUDE = 2.5 + id * 1.5
statef=turnedoff
UAVSTATE = "TURNEDOFF"
# reset the graph
Reset()
} }
# #
# Executed every step # Executed every step
# #
function step(){ function step(){
# listen to potential RC
uav_rccmd() uav_rccmd()
# get the swarm commands
uav_neicmd() uav_neicmd()
# update the vstig (status/net/batt)
uav_updatestig() uav_updatestig()
#update the graph #update the graph
UpdateNodeInfo() UpdateNodeInfo()
#reset message package to be sent #reset message package to be sent
m_selfMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=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 # graph state machine
# #
if(UAVSTATE=="GRAPH"){ if(UAVSTATE=="GRAPH"){
if(m_eState=="STATE_FREE") if(m_eState=="STATE_FREE")
@ -849,17 +859,16 @@ function step(){
DoLock() DoLock()
} }
# high level UAV state machine
statef() statef()
debug(m_eState,m_nLabel) debug(m_eState,m_nLabel)
log("Current state: ", UAVSTATE) log("Current state: ", UAVSTATE)
log("Swarm size: ", ROBOTS) log("Swarm size: ", ROBOTS)
# if(id==0)
# stattab_print()
#navigation #navigation
#broadcast messag #broadcast message
neighbors.broadcast("m",packmessage(m_selfMessage)) neighbors.broadcast("m",packmessage(m_selfMessage))
# #
@ -895,7 +904,7 @@ function Reset(){
#set initial state, only one robot choose [A], while the rest choose [B] #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, #[A]The robot used to triger the formation process is defined as joined,
if(id==0){ if(id==ROOT_ID){
m_nLabel=0 m_nLabel=0
TransitionToJoined() TransitionToJoined()
} }

826
buzz_scripts/graphform.bzz_old
View File

@ -1,826 +0,0 @@
#
# Include files
#
include "string.bzz"
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.
ROBOT_RADIUS=50
ROBOT_DIAMETER=2.0*ROBOT_RADIUS
ROBOT_SAFETYDIST=2.0*ROBOT_DIAMETER
#
# 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_MessageLable={}#store received neighbour message
m_MessageReqLable={}#store received neighbour message
m_MessageReqID={}#store received neighbour message
m_MessageSide={}#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
#Save message from one neighbour
#the indexes are as State(received state),Lable(received lable),ReqLable,ReqID,Side,Response,Range,Bearing
m_receivedMessage={.State="STATE_FREE",.Lable=0,.ReqLable=0,.ReqID=0,.Side=0,.Response="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),Side(side),Response(reply message{REQ_NONE,REQ_GRANTED,REQ_RESEND})
m_selfMessage={.State="STATE_FREE",.Lable=0,.ReqLable=0,.ReqID=0,.Side=0,.Response="REQ_NONE"}
#Current robot state
m_eState="STATE_FREE"
#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
#Table of the nodes in the graph
m_vecNodes={}
m_vecNodes_fixed={}
#Current label being requested or chosen (-1 when none)
m_nLabel=-1
#Label request id
m_unRequestId=0
#Side
m_unbSide=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
v_tag = stigmergy.create(1)
# Lennard-Jones parameters
EPSILON = 13.5 #3.5
# Lennard-Jones interaction magnitude
function FlockInteraction(dist,target,epsilon){
var mag = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
return mag
}
function LimitAngle(angle){
if(angle>2*math.pi)
return angle-2*math.pi
else if (angle<0)
return angle+2*math.pi
else
return angle
}
#
# Calculates the angle of the given vector2.
# PARAM v: The vector2.
# RETURN: The angle of the vector.
#
Angle = function(v) {
return math.atan(v.y, v.x)
}
#
#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
}
#
#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
}
function pow(base,exponent){
var i=0
var renturn_val=1
if(exponent==0)
return 1
else{
while(i<exponent){
renturn_val=renturn_val*base
i=i+1
}
return renturn_val
}
}
#
# Graph parsing
#
function parse_graph(fname) {
# Graph data
var gd = {}
# Open the file
var fd = io.fopen(fname, "r")
if(not fd) {
log("Can't open '", fname, "'")
return nil
}
# Parse the file line by line
var rrec # Record read from line
var grec # Record parsed into graph
io.fforeach(fd, function(line) {
# Parse file line
rrec = string.split(line, "\t ")
# Make record
gd[string.toint(rrec[0])] = { # The .graph file is stored according the sequence of lable, predecessor, distance, bearing
.Lable = string.toint(rrec[0]), # Lable of the point
.Pred = string.toint(rrec[1]), # Lable of its predecessor
.distance = string.tofloat(rrec[2]), # distance to the predecessor
.bearing = string.tofloat(rrec[3]), # bearing form the predecessor to this dot
.State="UNASSIGNED",
.StateAge=0
}})
# All done
io.fclose(fd)
return gd
}
function parse_graph_fixed(fname) {
# Graph data
var gd = {}
# Open the file
var fd = io.fopen(fname, "r")
if(not fd) {
log("Can't open '", fname, "'")
return nil
}
# Parse the file line by line
var rrec # Record read from line
var grec # Record parsed into graph
io.fforeach(fd, function(line) {
# Parse file line
rrec = string.split(line, "\t ")
# Make record
gd[string.toint(rrec[0])] = { # The .graph file is stored according the sequence of lable, pre1, dis2pr1, pre2, ids2pre2, side
.Pred1 = string.toint(rrec[1]), # Pred 1 lable
.Pred2 = string.toint(rrec[3]), # Pred 2 lable
.d1 = string.tofloat(rrec[2]), # Pred 1 distance
.d2 = string.tofloat(rrec[4]), # Pred 2 distance
#.S = string.toint(rrec[5]), # Side
.Lable=string.toint(rrec[0]),
.State="UNASSIGNED",
.StateAge=0
}})
# All done
io.fclose(fd)
return gd
}
function start_listen(){
neighbors.listen("msg",
function(vid,value,rid){
#store the received message
var temp_id=rid
m_receivedMessage.State=value.State
m_receivedMessage.Lable=value.Lable
m_receivedMessage.ReqLable=value.ReqLable
m_receivedMessage.ReqID=value.ReqID
m_receivedMessage.Side=value.Side
m_receivedMessage.Response=value.Response
Get_DisAndAzi(temp_id)
#add the received message
#
m_MessageState[m_neighbourCunt]=value.State
m_MessageLable[m_neighbourCunt]=value.Lable
m_MessageReqLable[m_neighbourCunt]=value.ReqLable
m_MessageReqID[m_neighbourCunt]=value.ReqID
m_MessageSide[m_neighbourCunt]=value.Side
m_MessageResponse[m_neighbourCunt]=value.Response
m_MessageRange[m_neighbourCunt]=m_receivedMessage.Range
m_MessageBearing[m_neighbourCunt]=m_receivedMessage.Bearing
m_neighbourCunt=m_neighbourCunt+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_neighbourCunt){
if(m_MessageState[i]=="STATE_JOINED"){
m_vecNodes[m_MessageLable[i]].State="ASSIGNED"
m_vecNodes[m_MessageLable[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
}
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(){
m_eState="STATE_FREE"
m_unWaitCount=m_unLabelSearchWaitTime
m_selfMessage.State=m_eState
}
#
#Transistion to state asking
#
function TransitionToAsking(un_label){
m_eState="STATE_ASKING"
m_nLabel=un_label
m_unRequestId=rng.uniform(0,65536)+id#don't know why the random numbers are the same, add id to make the ReqID different
m_selfMessage.State=m_eState
m_selfMessage.ReqLable=m_nLabel
m_selfMessage.ReqID=m_unRequestId
m_unWaitCount=m_unResponseTimeThreshold
}
#
#Transistion to state joining
#
function TransitionToJoining(){
m_eState="STATE_JOINING"
m_selfMessage.State=m_eState
m_selfMessage.Lable=m_nLabel
m_unWaitCount=m_unJoiningLostPeriod
neighbors.listen("reply",
function(vid,value,rid){
#store the received message
if(value.Lable==m_nLabel){
m_cMeToPred.GlobalBearing=value.GlobalBearing
}
})
}
#
#Transistion to state joined
#
function TransitionToJoined(){
m_eState="STATE_JOINED"
m_selfMessage.State=m_eState
m_selfMessage.Lable=m_nLabel
m_vecNodes[m_nLabel].State="ASSIGNED"
neighbors.ignore("reply")
#write statues
v_tag.put(m_nLabel, 1)
m_navigation.x=0.0
m_navigation.y=0.0
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
}
#
#Transistion to state Lock, lock the current formation
#
function TransitionToLock(){
m_eState="STATE_LOCK"
m_selfMessage.State=m_eState
m_selfMessage.Lable=m_nLabel
m_vecNodes[m_nLabel].State="ASSIGNED"
m_navigation.x=0.0
m_navigation.y=0.0
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
}
#
# Do free
#
function DoFree() {
m_selfMessage.State=m_eState
#wait for a while before looking for a lable
if(m_unWaitCount>0)
m_unWaitCount=m_unWaitCount-1
#find a set of joined robots
var setJoinedLables={}
var setJoinedIndexes={}
var i=0
var j=0
while(i<m_neighbourCunt){
if(m_MessageState[i]=="STATE_JOINED"){
setJoinedLables[j]=m_MessageLable[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
i=1
while(i<size(m_vecNodes) and (unFoundLable==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)
}
i=i+1
}
if(unFoundLable>0){
TransitionToAsking(unFoundLable)
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)
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){
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)
}
#jump the first step
if(step_cunt<=1){
uav_moveto(0.0,0.0)
}
#set message
m_selfMessage.State=m_eState
}
#
#Do asking
#
function DoAsking(){
#look for response from predecessor
var i=0
var psResponse=-1
while(i<m_neighbourCunt and psResponse==-1){
#the respond robot in joined state
#the request lable be the same as requesed
#get a respond
if(m_MessageState[i]=="STATE_JOINED"){
if(m_MessageReqLable[i]==m_nLabel)
if(m_MessageResponse[i]!="REQ_NONE"){
psResponse=i
}}
i=i+1
}
#analyse response
if(psResponse==-1){
#no response, wait
m_unWaitCount=m_unWaitCount-1
m_selfMessage.State=m_eState
m_selfMessage.ReqLable=m_nLabel
m_selfMessage.ReqID=m_unRequestId
if(m_unWaitCount==0){
TransitionToFree()
return
}
}
else{
if(m_MessageReqID[psResponse]!=m_unRequestId)
TransitionToFree()
if(m_MessageReqID[psResponse]==m_unRequestId){
if(m_MessageResponse[psResponse]=="REQ_GRANTED"){
TransitionToJoining()
#TransitionToJoined()
return
}
else{
TransitionToAsking(m_nLabel)
return
}
}
}
uav_moveto(0.0,0.0)
}
#
#Do joining
#
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")
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=Angle(S2Target)
m_bias=m_cMeToPred.Bearing-S2PGlobalBearing
#S2Target_bearing=S2Target_bearing+m_bias # commented out by DS'06/17
m_navigation=math.vec2.newp(math.vec2.length(S2Target),S2Target_bearing)
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
#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)
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=m_eState
m_selfMessage.Lable=m_nLabel
}
#
#Do joined
#
function DoJoined(){
m_selfMessage.State=m_eState
m_selfMessage.Lable=m_nLabel
#collect all requests
var mapRequests={}
var i=0
var j=0
var ReqLable
var JoiningLable
var seenPred=0
while(i<m_neighbourCunt){
if(m_MessageState[i]=="STATE_ASKING"){
ReqLable=m_MessageReqLable[i]
if(m_vecNodes[ReqLable].State=="UNASSIGNED")
if(m_nLabel==m_vecNodes[ReqLable].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){
##joining wrt this dot,send the global bearing
var m_messageForJoining={.Lable=JoiningLable,.GlobalBearing=m_MessageBearing[i]-m_bias}
neighbors.broadcast("reply",m_messageForJoining)
}
}
#if it is the pred
if(m_MessageState[i]=="STATE_JOINED" and m_MessageLable[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 Reqlable and Reqid are
ReqLable=m_MessageReqLable[mapRequests[ReqIndex]]
var ReqID=m_MessageReqID[mapRequests[ReqIndex]]
m_selfMessage.ReqLable=ReqLable
m_selfMessage.ReqID=ReqID
m_selfMessage.Response="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
#}
#}
m_navigation.x=0.0
m_navigation.y=0.0
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
#check if should to transists to lock
if(v_tag.size()==ROBOTS){
TransitionToLock()
}
}
#
#Do Lock
#
function DoLock(){
m_selfMessage.State=m_eState
m_selfMessage.Lable=m_nLabel
m_navigation.x=0.0
m_navigation.y=0.0
#collect preds information
var i=0
var mypred1={.range=0,.bearing=0}
var mypred2={.range=0,.bearing=0}
while(i<m_neighbourCunt){
#is the first predecessor
if(m_MessageLable[i]==m_vecNodes_fixed[m_nLabel].Pred1){
mypred1.range=m_MessageRange[i]
mypred1.bearing=m_MessageBearing[i]
}
#is the second predecessor
if(m_MessageLable[i]==m_vecNodes_fixed[m_nLabel].Pred2){
mypred2.range=m_MessageRange[i]
mypred2.bearing=m_MessageBearing[i]
}
i=i+1
}
#calculate motion vection
if(m_nLabel==0){
m_navigation.x=0.0
m_navigation.y=0.0
log(";",m_nLabel,";",0)
}
if(m_nLabel==1){
var tempvec={.Range=0.0,.Bearing=0.0}
tempvec.Range=FlockInteraction(mypred1.range,m_vecNodes_fixed[m_nLabel].d1,10)
#tempvec.Range=mypred1.range-m_vecNodes_fixed[m_nLabel].d1
tempvec.Bearing=mypred1.bearing
m_navigation=math.vec2.newp(tempvec.Range,tempvec.Bearing)
log(";",m_nLabel,";",mypred1.range-m_vecNodes_fixed[m_nLabel].d1)
}
if(m_nLabel>1){
var cDir={.x=0.0,.y=0.0}
var cDir1={.x=0.0,.y=0.0}
var cDir2={.x=0.0,.y=0.0}
cDir1=math.vec2.newp(FlockInteraction(mypred1.range,m_vecNodes_fixed[m_nLabel].d1,EPSILON),mypred1.bearing)
cDir2=math.vec2.newp(FlockInteraction(mypred2.range,m_vecNodes_fixed[m_nLabel].d2,EPSILON),mypred2.bearing)
#cDir1=math.vec2.newp((mypred1.range-m_vecNodes_fixed[m_nLabel].d1),mypred1.bearing)
#cDir2=math.vec2.newp((mypred2.range-m_vecNodes_fixed[m_nLabel].d2),mypred2.bearing)
cDir=math.vec2.add(cDir1,cDir2)
cDir=math.vec2.scale(cDir,100)
m_navigation.x=cDir.x
m_navigation.y=cDir.y
#log(m_nLabel,"error:",mypred1.range-m_vecNodes_fixed[m_nLabel].d1,"and",mypred2.range-m_vecNodes_fixed[m_nLabel].d2)
log(";",m_nLabel,";",mypred1.range-m_vecNodes_fixed[m_nLabel].d1)
}
#move
uav_moveto(m_navigation.x,m_navigation.y)
}
function action(){
statef=action
UAVSTATE="GRAPH"
}
#
# Executed at init
#
function init() {
#
#Adjust parameters here
#
m_unResponseTimeThreshold=10
m_unLabelSearchWaitTime=10
m_fTargetDistanceTolerance=10
m_unJoiningLostPeriod=100
#
# Join Swarm
#
uav_initswarm()
Reset();
}
#
# Executed every step
#
function step(){
uav_rccmd()
uav_neicmd()
#update the graph
UpdateNodeInfo()
#reset message package to be sent
m_selfMessage={.State="STATE_FREE",.Lable=0,.ReqLable=0,.ReqID=0,.Side=0,.Response="REQ_NONE"}
#
#act according to current state
#
if(UAVSTATE=="GRAPH"){
if(m_eState=="STATE_FREE")
DoFree()
else if(m_eState=="STATE_ESCAPE")
DoEscape()
else if(m_eState=="STATE_ASKING")
DoAsking()
else if(m_eState=="STATE_JOINING")
DoJoining()
else if(m_eState=="STATE_JOINED")
DoJoined()
else if(m_eState=="STATE_LOCK")
DoLock()
}
statef()
debug(m_eState,m_nLabel)
log("Current state: ", UAVSTATE)
log("Swarm size: ", ROBOTS)
#navigation
#broadcast messag
neighbors.broadcast("msg",m_selfMessage)
#
#clean message storage
m_MessageState={}#store received neighbour message
m_MessageLable={}#store received neighbour message
m_MessageReqLable={}#store received neighbour message
m_MessageReqID={}#store received neighbour message
m_MessageSide={}#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
#step cunt+1
step_cunt=step_cunt+1
}
#
# 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_fixed={}
m_vecNodes_fixed=parse_graph_fixed("/home/dave/ROS_WS/src/rosbuzz/buzz_scripts/include/Graph_fixed.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==0){
m_nLabel=0
TransitionToJoined()
}
#[B]Other robots are defined as free.
else{
TransitionToFree()
}
}
#
# Executed upon destroy
#
function destroy() {
#clear neighbour message
uav_moveto(0.0,0.0)
m_vecNodes={}
#stop listening
neighbors.ignore("msg")
}

12
buzz_scripts/include/barrier.bzz
View File

@ -12,9 +12,9 @@ BARRIER_VSTIG = 11
# #
# Sets a barrier # Sets a barrier
# #
function barrier_set(threshold, transf, resumef) { function barrier_set(threshold, transf, resumef, bdt) {
statef = function() { statef = function() {
barrier_wait(threshold, transf, resumef); barrier_wait(threshold, transf, resumef, bdt);
} }
barrier = stigmergy.create(BARRIER_VSTIG) barrier = stigmergy.create(BARRIER_VSTIG)
} }
@ -31,14 +31,16 @@ function barrier_ready() {
# #
BARRIER_TIMEOUT = 200 BARRIER_TIMEOUT = 200
timeW=0 timeW=0
function barrier_wait(threshold, transf, resumef) { function barrier_wait(threshold, transf, resumef, bdt) {
barrier.get(id) #barrier.get(id)
barrier.put(id, 1) barrier.put(id, 1)
UAVSTATE = "BARRIERWAIT" UAVSTATE = "BARRIERWAIT"
if(bdt!=-1)
neighbors.broadcast("cmd", brd)
if(barrier.size() >= threshold) { if(barrier.size() >= threshold) {
# getlowest() # getlowest()
transf() transf()
} else if(timeW>=BARRIER_TIMEOUT) { } else if(timeW >= BARRIER_TIMEOUT) {
barrier = nil barrier = nil
resumef() resumef()
timeW=0 timeW=0

15
buzz_scripts/include/uavstates.bzz
View File

@ -11,8 +11,6 @@ goal = {.range=0, .bearing=0}
function uav_initswarm() { function uav_initswarm() {
s = swarm.create(1) s = swarm.create(1)
s.join() s.join()
statef=turnedoff
UAVSTATE = "TURNEDOFF"
} }
function turnedoff() { function turnedoff() {
@ -30,7 +28,7 @@ function takeoff() {
statef=takeoff statef=takeoff
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
barrier_set(ROBOTS,action,land) barrier_set(ROBOTS,action,land,22)
barrier_ready() barrier_ready()
} }
else { else {
@ -49,7 +47,7 @@ function land() {
uav_land() uav_land()
} }
else { else {
barrier_set(ROBOTS,turnedoff,land) barrier_set(ROBOTS,turnedoff,land,21)
barrier_ready() barrier_ready()
timeW=0 timeW=0
#barrier = nil #barrier = nil
@ -140,11 +138,10 @@ function uav_neicmd() {
} else if(value==401 and UAVSTATE=="IDLE"){ } else if(value==401 and UAVSTATE=="IDLE"){
uav_disarm() uav_disarm()
} else if(value==16){ } else if(value==16){
neighbors.listen("gt",function(vid, value, rid) { # neighbors.listen("gt",function(vid, value, rid) {
print("Got (", vid, ",", value, ") from robot #", rid) # print("Got (", vid, ",", value, ") from robot #", rid)
# uav_storegoal(lat, lon, alt) # # if(gt.id == id) statef=goto
}) # })
statef=goto
} }
}) })
} }

3
buzz_scripts/testalone.bzz
View File

@ -12,7 +12,8 @@ function action() {
# Executed once at init time. # Executed once at init time.
function init() { function init() {
uav_initswarm() statef=turnedoff
UAVSTATE = "TURNEDOFF"
uav_initstig() uav_initstig()
} }

1
include/roscontroller.h
View File

@ -111,6 +111,7 @@ private:
ros::Subscriber users_sub; ros::Subscriber users_sub;
ros::Subscriber battery_sub; ros::Subscriber battery_sub;
ros::Subscriber payload_sub; ros::Subscriber payload_sub;
ros::Subscriber flight_estatus_sub;
ros::Subscriber flight_status_sub; ros::Subscriber flight_status_sub;
ros::Subscriber obstacle_sub; ros::Subscriber obstacle_sub;
ros::Subscriber Robot_id_sub; ros::Subscriber Robot_id_sub;

17
launch/launch_config/husky.yaml
View File

@ -1,17 +0,0 @@
topics:
gps : global_position
localpos : local_position
battery : power_status
status : flight_status
fcclient : dji_mavcmd
setpoint : setpoint_position/local
armclient: dji_mavarm
modeclient: dji_mavmode
altitude: rel_alt
stream: set_stream_rate
type:
gps : sensor_msgs/NavSatFix
battery : mavros_msgs/BatteryStatus
status : mavros_msgs/ExtendedState
altitude: std_msgs/Float64

17
launch/launch_config/m100.yaml
View File

@ -1,17 +0,0 @@
topics:
gps : global_position
localpos : local_position
battery : power_status
status : flight_status
fcclient : dji_mavcmd
setpoint : setpoint_position/local
armclient: dji_mavarm
modeclient: dji_mavmode
altitude: rel_alt
stream: set_stream_rate
type:
gps : sensor_msgs/NavSatFix
battery : mavros_msgs/BatteryStatus
status : mavros_msgs/ExtendedState
altitude: std_msgs/Float64

21
launch/launch_config/solo.yaml
View File

@ -1,21 +0,0 @@
topics:
gps : mavros/global_position/global
battery : mavros/battery
status : mavros/state
fcclient: mavros/cmd/command
setpoint: mavros/setpoint_position/local
armclient: mavros/cmd/arming
modeclient: mavros/set_mode
localpos: mavros/local_position/pose
stream: mavros/set_stream_rate
altitude: mavros/global_position/rel_alt
type:
gps : sensor_msgs/NavSatFix
# for SITL Solo
battery : mavros_msgs/BatteryState
# for solo
#battery : mavros_msgs/BatteryStatus
status : mavros_msgs/State
altitude: std_msgs/Float64
environment :
environment : solo-simulator

2
launch/rosbuzz.launch
View File

@ -7,7 +7,7 @@
<arg name="xbee_plugged" default="true"/> <arg name="xbee_plugged" default="true"/>
<arg name="script" default="testalone"/> <arg name="script" default="testalone"/>
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" launch-prefix="gdb -ex run --args"> <node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
<rosparam file="$(find rosbuzz)/launch/launch_config/topics.yaml"/> <rosparam file="$(find rosbuzz)/launch/launch_config/topics.yaml"/>
<param name="bzzfile_name" value="$(find rosbuzz)/buzz_scripts/$(arg script).bzz" /> <param name="bzzfile_name" value="$(find rosbuzz)/buzz_scripts/$(arg script).bzz" />
<param name="rcclient" value="true" /> <param name="rcclient" value="true" />

22
launch/rosbuzzd.launch Normal file
View File

@ -0,0 +1,22 @@
<?xml version="1.0"?>
<!-- Generic launch file for ROSBuzz -->
<!-- This file is included in all ROS workspace launch files -->
<!-- Modify with great care! -->
<launch>
<arg name="name" default="robot0"/>
<arg name="xbee_plugged" default="true"/>
<arg name="script" default="testalone"/>
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" launch-prefix="gdb -ex run --args">
<rosparam file="$(find rosbuzz)/launch/launch_config/topics.yaml"/>
<param name="bzzfile_name" value="$(find rosbuzz)/buzz_scripts/$(arg script).bzz" />
<param name="rcclient" value="true" />
<param name="rcservice_name" value="buzzcmd" />
<param name="in_payload" value="inMavlink"/>
<param name="out_payload" value="outMavlink"/>
<param name="xbee_plugged" value="$(arg xbee_plugged)"/>
<param name="name" value="$(arg name)"/>
<param name="xbee_status_srv" value="xbee_status"/>
<param name="stand_by" value="$(find rosbuzz)/buzz_scripts/stand_by.bzz"/>
</node>
</launch>

10
src/buzz_utility.cpp
View File

@ -487,7 +487,7 @@ int create_stig_tables() {
if(buzzvm_set_bcode(VM, BO_BUF, bcode_size) != BUZZVM_STATE_READY) { if(buzzvm_set_bcode(VM, BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM); buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO); buzzdebug_destroy(&DBG_INFO);
ROS_ERROR("%s: Error loading Buzz script", bo_filename); ROS_ERROR("[%i] %s: Error loading Buzz script", Robot_id, bo_filename);
return 0; return 0;
} }
/* Register hook functions */ /* Register hook functions */
@ -550,14 +550,14 @@ int create_stig_tables() {
if(buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) { if(buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM); buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO); buzzdebug_destroy(&DBG_INFO);
fprintf(stdout, "%s: Error loading Buzz script\n\n", BO_FNAME); ROS_ERROR("[%i] %s: Error loading Buzz bytecode (update)", Robot_id);
return 0; return 0;
} }
// Register hook functions // Register hook functions
if(buzz_register_hooks() != BUZZVM_STATE_READY) { if(buzz_register_hooks() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM); buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO); buzzdebug_destroy(&DBG_INFO);
fprintf(stdout, "%s: Error registering hooks\n\n", BO_FNAME); ROS_ERROR("[%i] Error registering hooks (update)", Robot_id);
return 0; return 0;
} }
/* Create vstig tables /* Create vstig tables
@ -606,14 +606,14 @@ int create_stig_tables() {
if(buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) { if(buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM); buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO); buzzdebug_destroy(&DBG_INFO);
fprintf(stdout, "%s: Error loading Buzz script\n\n", BO_FNAME); ROS_ERROR("[%i] %s: Error loading Buzz bytecode (update init)", Robot_id);
return 0; return 0;
} }
// Register hook functions // Register hook functions
if(testing_buzz_register_hooks() != BUZZVM_STATE_READY) { if(testing_buzz_register_hooks() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM); buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO); buzzdebug_destroy(&DBG_INFO);
fprintf(stdout, "%s: Error registering hooks\n\n", BO_FNAME); ROS_ERROR("[%i] Error registering hooks (update init)", Robot_id);
return 0; return 0;
} }
/* Create vstig tables /* Create vstig tables

26
src/roscontroller.cpp
View File

@ -337,7 +337,7 @@ void roscontroller::GetSubscriptionParameters(ros::NodeHandle &node_handle)
std::string battery_topic; std::string battery_topic;
node_handle.getParam("topics/battery", battery_topic); node_handle.getParam("topics/battery", battery_topic);
m_smTopic_infos.insert(pair<std::string, std::string>(battery_topic, "mavros_msgs/BatteryState")); m_smTopic_infos.insert(pair<std::string, std::string>(battery_topic, "mavros_msgs/BatteryStatus"));
std::string status_topic; std::string status_topic;
node_handle.getParam("topics/status", status_topic); node_handle.getParam("topics/status", status_topic);
@ -439,9 +439,9 @@ void roscontroller::Subscribe(ros::NodeHandle &n_c)
m_smTopic_infos.begin(); m_smTopic_infos.begin();
it != m_smTopic_infos.end(); ++it) { it != m_smTopic_infos.end(); ++it) {
if (it->second == "mavros_msgs/ExtendedState") { if (it->second == "mavros_msgs/ExtendedState") {
flight_status_sub = n_c.subscribe(it->first, 100, &roscontroller::flight_extended_status_update, this); flight_estatus_sub = n_c.subscribe(it->first, 5, &roscontroller::flight_extended_status_update, this);
} else if (it->second == "mavros_msgs/State") { } else if (it->second == "mavros_msgs/State") {
flight_status_sub = n_c.subscribe(it->first, 100, &roscontroller::flight_status_update, this); flight_status_sub = n_c.subscribe(it->first, 5, &roscontroller::flight_status_update, this);
} else if (it->second == "mavros_msgs/BatteryStatus") { } else if (it->second == "mavros_msgs/BatteryStatus") {
battery_sub = n_c.subscribe(it->first, 5, &roscontroller::battery, this); battery_sub = n_c.subscribe(it->first, 5, &roscontroller::battery, this);
} else if (it->second == "sensor_msgs/NavSatFix") { } else if (it->second == "sensor_msgs/NavSatFix") {
@ -459,31 +459,17 @@ void roscontroller::Subscribe(ros::NodeHandle &n_c)
/-------------------------------------------------------*/ /-------------------------------------------------------*/
std::string roscontroller::Compile_bzz(std::string bzzfile_name) std::string roscontroller::Compile_bzz(std::string bzzfile_name)
{ {
/*TODO: change to bzzc instead of bzzparse and also add -I for includes*/
/*Compile the buzz code .bzz to .bo*/ /*Compile the buzz code .bzz to .bo*/
stringstream bzzfile_in_compile; stringstream bzzfile_in_compile;
std::string path = std::string path =
bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
// bzzfile_in_compile << path << "/";
// path = bzzfile_in_compile.str();
// bzzfile_in_compile.str("");
std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
name = name.substr(0, name.find_last_of(".")); name = name.substr(0, name.find_last_of("."));
bzzfile_in_compile << "bzzc -I " << path bzzfile_in_compile << "bzzc -I " << path
<< "include/"; //<<" "<<path<< name<<".basm"; << "include/";
// bzzfile_in_compile.str("");
// bzzfile_in_compile <<"bzzasm "<<path<<name<<".basm "<<path<<name<<".bo
// "<<path<<name<<".bdbg";
// system(bzzfile_in_compile.str().c_str());
// bzzfile_in_compile.str("");
bzzfile_in_compile << " -b " << path << name << ".bo"; bzzfile_in_compile << " -b " << path << name << ".bo";
// bcfname = bzzfile_in_compile.str();
// std::string tmp_bcfname = path + name + ".bo";
// bzzfile_in_compile.str("");
bzzfile_in_compile << " -d " << path << name << ".bdb "; bzzfile_in_compile << " -d " << path << name << ".bdb ";
// bzzfile_in_compile << " -a " << path << name << ".asm ";
bzzfile_in_compile << bzzfile_name; bzzfile_in_compile << bzzfile_name;
// std::string tmp_dbgfname = path + name + ".bdb";
ROS_WARN("Launching buzz compilation: %s", bzzfile_in_compile.str().c_str()); ROS_WARN("Launching buzz compilation: %s", bzzfile_in_compile.str().c_str());
@ -967,7 +953,7 @@ void roscontroller::SetMode(std::string mode, int delay_miliseconds) {
set_mode_message.request.custom_mode = mode; set_mode_message.request.custom_mode = mode;
current_mode = mode; current_mode = mode;
if (mode_client.call(set_mode_message)) { if (mode_client.call(set_mode_message)) {
ROS_INFO("Set Mode Service call successful!"); ;//ROS_INFO("Set Mode Service call successful!");
} else { } else {
ROS_INFO("Set Mode Service call failed!"); ROS_INFO("Set Mode Service call failed!");
} }
@ -983,7 +969,7 @@ void roscontroller::SetStreamRate(int id, int rate, int on_off) {
ROS_INFO("Set stream rate call failed!, trying again..."); ROS_INFO("Set stream rate call failed!, trying again...");
ros::Duration(0.1).sleep(); ros::Duration(0.1).sleep();
} }
ROS_INFO("Set stream rate call successful"); //ROS_INFO("Set stream rate call successful");
} }
/*------------------------------------------------------------- /*-------------------------------------------------------------