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

Merge commit '32c10dbc42497d12f7b3dc57a2aab0f601d280b5' into ros_drones_ws

This commit is contained in:
David St-Onge 2017-08-30 15:19:27 -04:00
parent 25ed9328da 32c10dbc42
commit f22d54b016
25 changed files with 1076 additions and 1420 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
buzz_scripts/flock.bzz
View File

@ -38,7 +38,7 @@ function action() {
# Move according to vector
print("Robot ", id, "must push ", math.vec2.length(accum) )#, "; ", math.vec2.angle(accum))
uav_moveto(accum.x, accum.y)
uav_moveto(accum.x, accum.y, 0.0)
UAVSTATE = "LENNARDJONES"
# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS
@ -47,11 +47,11 @@ function action() {
# } else if(timeW>=WAIT_TIMEOUT/2) {
# UAVSTATE ="GOEAST"
# timeW = timeW+1
# uav_moveto(0.0,5.0)
# uav_moveto(0.0, 5.0, 0.0)
# } else {
# UAVSTATE ="GONORTH"
# timeW = timeW+1
# uav_moveto(5.0,0.0)
# uav_moveto(5.0, 0.0, 0.0)
# }
}
@ -65,15 +65,19 @@ function action() {
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()
uav_updatestig()
log("Current state: ", UAVSTATE)
log("Swarm size: ",ROBOTS)
if(id==0)

49
buzz_scripts/graphform.bzz
View File

@ -10,9 +10,10 @@ include "vstigenv.bzz"
include "graphs/shapes_Y.bzz"
ROBOT_RADIUS=50
ROBOT_DIAMETER=2.0*ROBOT_RADIUS
ROBOT_SAFETYDIST=2.0*ROBOT_DIAMETER
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 = 250.0
@ -446,7 +447,7 @@ function TransitionToJoined(){
m_navigation.x=0.0
m_navigation.y=0.0
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
}
#
@ -471,7 +472,7 @@ while(i<m_neighbourCount){
}
m_navigation.x=0.0
m_navigation.y=0.0
uav_moveto(m_navigation.x,m_navigation.y)
uav_moveto(m_navigation.x, m_navigation.y, 0.0)
}
#
@ -535,7 +536,7 @@ function DoFree() {
# 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)
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}
@ -545,7 +546,7 @@ function DoFree() {
i=i+1
}
m_navigation=math.vec2.scale(tempvec,1.0/i)
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
}
@ -554,7 +555,7 @@ function DoFree() {
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)
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
}
#set message
m_selfMessage.State=s2i(m_eState)
@ -614,7 +615,7 @@ function DoAsking(){
}
m_navigation.x=0.0
m_navigation.y=0.0
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
}
#
@ -662,7 +663,7 @@ function DoJoining(){
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)
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
@ -760,7 +761,7 @@ function DoJoined(){
m_navigation.x=0.0
m_navigation.y=0.0
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
#check if should to transists to lock
@ -789,7 +790,7 @@ if(m_nLabel!=0){
m_navigation=motion_vector()
}
#move
uav_moveto(m_navigation.x,m_navigation.y)
uav_moveto(m_navigation.x, m_navigation.y, 0.0)
}
function action(){
@ -802,7 +803,7 @@ function action(){
#
function init() {
#
#Adjust parameters here
# Global parameters for graph formation
#
m_unResponseTimeThreshold=10
m_unLabelSearchWaitTime=10
@ -816,23 +817,32 @@ function init() {
uav_initswarm()
v_tag = stigmergy.create(m_lockstig)
uav_initstig()
Reset()
# go to diff. height since no collision avoidance implemented yet
TARGET_ALTITUDE = 2.5 + id * 1.5
statef=turnedoff
UAVSTATE = "TURNEDOFF"
# reset the graph
Reset()
}
#
# 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")}
#
#act according to current state
# graph state machine
#
if(UAVSTATE=="GRAPH"){
if(m_eState=="STATE_FREE")
@ -849,17 +859,16 @@ function step(){
DoLock()
}
# high level UAV state machine
statef()
debug(m_eState,m_nLabel)
log("Current state: ", UAVSTATE)
log("Swarm size: ", ROBOTS)
# if(id==0)
# stattab_print()
#navigation
#broadcast messag
#broadcast message
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]
#
#[A]The robot used to triger the formation process is defined as joined,
if(id==0){
if(id==ROOT_ID){
m_nLabel=0
TransitionToJoined()
}
@ -912,7 +921,7 @@ 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)
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
m_vecNodes={}
#stop listening
neighbors.ignore("m")

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

92
buzz_scripts/include/uavstates.bzz
View File

@ -5,12 +5,12 @@
########################################
TARGET_ALTITUDE = 5.0
UAVSTATE = "TURNEDOFF"
GOTO_MAXVEL = 2
goal = {.range=0, .bearing=0}
function uav_initswarm(){
function uav_initswarm() {
s = swarm.create(1)
s.join()
statef=turnedoff
UAVSTATE = "TURNEDOFF"
}
function turnedoff() {
@ -26,13 +26,10 @@ function idle() {
function takeoff() {
UAVSTATE = "TAKEOFF"
statef=takeoff
#log("TakeOff: ", flight.status)
#log("Relative position: ", position.altitude)
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()
#statef=hexagon
}
else {
log("Altitude: ", TARGET_ALTITUDE)
@ -44,13 +41,13 @@ function takeoff() {
function land() {
UAVSTATE = "LAND"
statef=land
#log("Land: ", flight.status)
if(flight.status == 2 or flight.status == 3){
neighbors.broadcast("cmd", 21)
uav_land()
}
else {
barrier_set(ROBOTS,turnedoff,land)
barrier_set(ROBOTS,turnedoff,land,21)
barrier_ready()
timeW=0
#barrier = nil
@ -58,6 +55,28 @@ function land() {
}
}
function goto() {
UAVSTATE = "GOTO"
statef=goto
# print(rc_goto.id, " is going alone to lat=", rc_goto.latitude)
if(rc_goto.id==id){
s.leave()
rb_from_gps(rc_goto.latitude, rc_goto.longitude)
print(id, " has to move ", goal.range)
m_navigation = math.vec2.newp(goal.range, goal.bearing)
if(math.vec2.length(m_navigation)>GOTO_MAXVEL) {
m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation))
uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude)
} else if(math.vec2.length(m_navigation)>0.25)
uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude)
else
statef = idle
} else {
neighbors.broadcast("cmd", 16)
neighbors.broadcast("gt", rc_goto.id+rc_goto.longitude+rc_goto.latitude)
}
}
function follow() {
if(size(targets)>0) {
UAVSTATE = "FOLLOW"
@ -67,7 +86,7 @@ function follow() {
force=(0.05)*(tab.range)^4
attractor=math.vec2.add(attractor, math.vec2.newp(force, tab.bearing))
})
uav_moveto(attractor.x, attractor.y)
uav_moveto(attractor.x, attractor.y, 0.0)
} else {
log("No target in local table!")
#statef=idle
@ -90,11 +109,8 @@ function uav_rccmd() {
neighbors.broadcast("cmd", 21)
} else if(flight.rc_cmd==16) {
flight.rc_cmd=0
UAVSTATE = "FOLLOW"
log(rc_goto.latitude, " ", rc_goto.longitude)
add_targetrb(0,rc_goto.latitude,rc_goto.longitude)
statef = follow
#uav_goto()
UAVSTATE = "GOTO"
statef = goto
} else if(flight.rc_cmd==400) {
flight.rc_cmd=0
uav_arm()
@ -111,16 +127,40 @@ function uav_rccmd() {
function uav_neicmd() {
neighbors.listen("cmd",
function(vid, value, rid) {
print("Got (", vid, ",", value, ") from robot #", rid)
if(value==22 and UAVSTATE!="TAKEOFF") {
statef=takeoff
} else if(value==21) {
statef=land
} else if(value==400 and UAVSTATE=="IDLE") {
uav_arm()
} else if(value==401 and UAVSTATE=="IDLE"){
uav_disarm()
}
function(vid, value, rid) {
print("Got (", vid, ",", value, ") from robot #", rid)
if(value==22 and UAVSTATE!="TAKEOFF") {
statef=takeoff
} else if(value==21) {
statef=land
} else if(value==400 and UAVSTATE=="IDLE") {
uav_arm()
} else if(value==401 and UAVSTATE=="IDLE"){
uav_disarm()
} else if(value==16){
# neighbors.listen("gt",function(vid, value, rid) {
# print("Got (", vid, ",", value, ") from robot #", rid)
# # if(gt.id == id) statef=goto
# })
}
})
}
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
}
function rb_from_gps(lat, lon) {
print("rb dbg: ",lat,lon,position.latitude,position.longitude)
d_lon = lon - position.longitude
d_lat = lat - position.latitude
ned_x = d_lat/180*math.pi * 6371000.0;
ned_y = d_lon/180*math.pi * 6371000.0 * math.cos(lat/180*math.pi);
goal.range = math.sqrt(ned_x*ned_x+ned_y*ned_y);
goal.bearing = LimitAngle(math.atan(ned_y,ned_x));
}

152
buzz_scripts/include/utilities.bzz Normal file
View File

@ -0,0 +1,152 @@
# Utilities
# Rads to degrees
function rtod(r) {
return (r*(180.0/math.pi))
}
# Copy a table
function table_deep_copy(new_t, old_t, depth) {
depth = depth - 1
if (old_t != nil) {
foreach(old_t, function(key, value) {
new_t[key] = value
if(depth != 0) {
new_t[key] = {}
table_deep_copy(new_t[key], value, depth)
}
})
}
}
function table_copy(old_t, depth) {
var t = {};
table_deep_copy(t, old_t, depth);
return t;
}
# Print the contents of a table
function table_print(t, depth) {
depth = depth - 1
if (t != nil) {
foreach(t, function(key, value) {
log(key, " -> ", value)
if(depth != 0) {
table_print(t[key], depth)
}
})
}
}
# Write a table as if it was a vector
function write_vector(k, index, val) {
var key = string.tostring(index)
k[key] = val
}
# Read a table as if it was a vector
function read_vector(k, index) {
var key = string.tostring(index)
if (k[key] == nil) {
return -1
} else {
return k[key]
}
}
# Write a table as if it was a matrix
function write_matrix(k, row, col, val) {
var key = string.concat(string.tostring(row),"-",string.tostring(col))
k[key] = val
}
# Read a table as if it was a matrix
function read_matrix(k, row, col) {
var key = string.concat(string.tostring(row),"-",string.tostring(col))
if (k[key] == nil) {
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
}
# 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
}
############################################
#base = {}
#base.create = function() {
# return {
# .method = function(a,b) {
# return a + b
# }
# }
# }
#x = base.create()
#x.method(3,4) # returns 7
#derived = {}
#derived.create = function() {
# b = base.create()
# b.method = function(a,b) {
# return a * b
# }
#}
#x = derived.create()
#x.method(3,4) # returns 12

10
buzz_scripts/testalone.bzz
View File

@ -7,17 +7,23 @@ include "vstigenv.bzz"
function action() {
statef=action
# test moveto cmd dx, dy
# uav_moveto(0.5, 0.5)
# uav_moveto(0.5, 0.5, 0.0)
}
# Executed once at init time.
function init() {
statef=turnedoff
UAVSTATE = "TURNEDOFF"
uav_initstig()
}
# Executed at each time step.
function step() {
log("Altitude: ", position.altitude)
uav_rccmd()
statef()
log("Current state: ", UAVSTATE)
}
# Executed once when the robot (or the simulator) is reset.

537
buzz_scripts/tree_centroid.bzz Normal file
View File

@ -0,0 +1,537 @@
# 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
}
################

6
include/buzzuav_closures.h
View File

@ -36,13 +36,15 @@ int buzzros_print(buzzvm_t vm);
* commands the UAV to go to a position supplied
*/
int buzzuav_moveto(buzzvm_t vm);
int buzzuav_goto(buzzvm_t vm);
int buzzuav_storegoal(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(double pos[]);
void rc_set_goto(int id, double latitude, double longitude, double altitude);
/*Sets gimbal orientation from rc client*/
void rc_set_gimbal(int id, float yaw, float pitch);
/*sets rc requested command */
void rc_call(int rc_cmd);
/* sets the battery state */

2
include/roscontroller.h
View File

@ -39,6 +39,7 @@
#define XBEE_STOP_TRANSMISSION 4355356352
#define TIMEOUT 60
#define BUZZRATE 10
#define CMD_REQUEST_UPDATE 666
using namespace std;
@ -110,6 +111,7 @@ private:
ros::Subscriber users_sub;
ros::Subscriber battery_sub;
ros::Subscriber payload_sub;
ros::Subscriber flight_estatus_sub;
ros::Subscriber flight_status_sub;
ros::Subscriber obstacle_sub;
ros::Subscriber Robot_id_sub;

59
launch/husky.launch
View File

@ -1,59 +0,0 @@
<launch>
<group ns="gps">
<!-- NavSat Serial -->
<node pkg="nmea_comms" type="serial_node" name="nmea_serial_node" output="screen">
<param name="port" value="$(optenv HUSKY_NAVSAT_PORT /dev/clearpath/gps)" />
<param name="baud" value="$(optenv HUSKY_NAVSAT_BAUD 19200)" />
</node>
<!-- NavSat Processing -->
<node pkg="nmea_navsat_driver" type="nmea_topic_driver" name="nmea_topic_driver">
</node>
</group>
<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform" >
<rosparam>
magnetic_declination_radians: 0
roll_offset: 0
pitch_offset: 0
yaw_offset: 0
zero_altitude: false
broadcast_utm_transform: false
</rosparam>
</node>
<!-- run xbee>
<node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" />
<param name="Xbee_In_From_Buzz" type="str" value="outMavlink" />
<param name="Xbee_Out_To_Buzz" type="str" value="inMavlink" />
<param name="Xbee_In_From_Controller" type="str" value="xbee_cmd" / -->
<!-- xmee_mav Drone type and Commununication Mode -->
<!-- node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="master solo" output="screen" -->
<node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" />
<!-- node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave solo" output="screen" -->
<!--node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" -->
<!-- xmee_mav Topics and Services Names -->
<param name="Xbee_In_From_Buzz" type="str" value="outMavlink" />
<param name="Xbee_Out_To_Buzz" type="str" value="inMavlink" />
<param name="Xbee_In_From_Controller" type="str" value="xbee_cmd" />
<param name="Xbee_Out_To_Controller" type="str" value="mav_dji_cmd" />
<node pkg="dji_sdk_mistlab" type="dji_sdk_mav" name="dji_sdk_mav" output="screen"/>
<!-- run rosbuzz -->
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
<rosparam file="$(find rosbuzz)/launch/launch_config/husky.yaml"/>
<param name="bzzfile_name" value="$(find rosbuzz)/script/testflockfev.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_status_srv" value="xbee_status"/>
<param name="xbee_plugged" value="true"/>
<param name="name" value="husky1"/>
<param name="stand_by" value="$(find rosbuzz)/script/stand_by.bzz"/>
</node>
</launch>

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

12
launch/launch_config/topics.yaml Normal file
View File

@ -0,0 +1,12 @@
topics:
gps : global_position/global
battery : battery
status : state
estatus : extended_state
fcclient: cmd/command
setpoint: setpoint_position/local
armclient: cmd/arming
modeclient: set_mode
localpos: local_position/pose
stream: set_stream_rate
altitude: global_position/rel_alt

57
launch/rosbuzz-solo.launch
View File

@ -1,57 +0,0 @@
<launch>
<!-- RUN mavros -->
<arg name="fcu_url" default="/dev/ttyS0:115200" />
<arg name="gcs_url" default="" />
<arg name="tgt_system" default="1" />
<arg name="tgt_component" default="1" />
<arg name="log_output" default="screen" />
<include file="$(find mavros)/launch/node.launch">
<arg name="pluginlists_yaml" value="$(find mavros)/launch/apm_pluginlists.yaml" />
<arg name="config_yaml" value="$(find mavros)/launch/apm_config.yaml" />
<arg name="fcu_url" value="$(arg fcu_url)" />
<arg name="gcs_url" value="$(arg gcs_url)" />
<arg name="tgt_system" value="$(arg tgt_system)" />
<arg name="tgt_component" value="$(arg tgt_component)" />
<arg name="log_output" value="$(arg log_output)" />
</include>
<!-- set streaming rate -->
<!-- node pkg="rosservice" type="rosservice" name="freq" args="call /mavros/set_stream_rate 0 10 1" output="screen" / -->
<!-- run xbee>
<node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" />
<param name="Xbee_In_From_Buzz" type="str" value="outMavlink" />
<param name="Xbee_Out_To_Buzz" type="str" value="inMavlink" />
<param name="Xbee_In_From_Controller" type="str" value="xbee_cmd" / -->
<!-- xmee_mav Drone type and Commununication Mode -->
<!-- node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="master solo" output="screen" -->
<node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" />
<!-- node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave solo" output="screen" -->
<!--node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" -->
<!-- xmee_mav Topics and Services Names -->
<param name="Xbee_In_From_Buzz" type="str" value="outMavlink" />
<param name="Xbee_Out_To_Buzz" type="str" value="inMavlink" />
<param name="Xbee_In_From_Controller" type="str" value="xbee_cmd" />
<param name="Xbee_Out_To_Controller" type="str" value="mav_dji_cmd" />
<param name="USB_port" type="str" value="/dev/ttyUSB0" />
<param name="Baud_rate" type="double" value="230400" />
<!-- run rosbuzz -->
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
<rosparam file="$(find rosbuzz)/launch/launch_config/solo.yaml"/>
<param name="bzzfile_name" value="$(find rosbuzz)/buzz_scripts/flock.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_status_srv" value="xbee_status"/>
<param name="xbee_plugged" value="true"/>
<param name="name" value="solos1"/>
<param name="stand_by" value="$(find rosbuzz)/buzz_scripts/stand_by.bzz"/>
</node>
</launch>

51
launch/rosbuzz-testing-sitl.launch
View File

@ -1,51 +0,0 @@
<launch>
<!-- RUN mavros
<arg name="fcu_url" default="tcp://127.0.0.1:5760" />
<arg name="gcs_url" default="" />
<arg name="tgt_system" default="1" />
<arg name="tgt_component" default="1" />
<arg name="log_output" default="screen" />
<include file="$(find mavros)/launch/node.launch">
<arg name="pluginlists_yaml" value="$(find mavros)/launch/apm_pluginlists.yaml" />
<arg name="config_yaml" value="$(find mavros)/launch/apm_config.yaml" />
<arg name="fcu_url" value="$(arg fcu_url)" />
<arg name="gcs_url" value="$(arg gcs_url)" />
<arg name="tgt_system" value="$(arg tgt_system)" />
<arg name="tgt_component" value="$(arg tgt_component)" />
<arg name="log_output" value="$(arg log_output)" />
</include>
-->
<!-- run xbee -->
<node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" />
<param name="Xbee_In_From_Buzz" type="str" value="outMavlink" />
<param name="Xbee_Out_To_Buzz" type="str" value="inMavlink" />
<param name="Xbee_In_From_Controller" type="str" value="xbee_cmd" />
<param name="Xbee_Out_To_Controller" type="str" value="mav_dji_cmd" />
<!-- run rosbuzz -->
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
<rosparam file="/home/ivan/catkin_ws/src/rosbuzz/launch/launch_config/solo.yaml"/>
<param name="bzzfile_name" value="/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.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_status_srv" value="xbee_status"/>
<param name="xbee_plugged" value="false"/>
<param name="name" value="solos1"/>
<param name="stand_by" value="/home/ivan/catkin_ws/src/rosbuzz/script/stand_by.bzz"/>
</node>
<!--
<node pkg="rosloader" type="rosloader" name="rosloader" output="screen"/>
-->
<!-- set streaming rate
/mavros/cmd/arming
<node pkg="rosservice" type="rosservice" name="stream_rate" args="call /mavros/set_stream_rate 0 10 1" />
-->
</launch>

43
launch/rosbuzz-testing.launch
View File

@ -1,43 +0,0 @@
<launch>
<!-- RUN mavros -->
<arg name="fcu_url" default="/dev/ttyAMA0:115200" />
<arg name="gcs_url" default="" />
<arg name="tgt_system" default="1" />
<arg name="tgt_component" default="1" />
<arg name="log_output" default="screen" />
<include file="$(find mavros)/launch/node.launch">
<arg name="pluginlists_yaml" value="$(find mavros)/launch/apm_pluginlists.yaml" />
<arg name="config_yaml" value="$(find mavros)/launch/apm_config.yaml" />
<arg name="fcu_url" value="$(arg fcu_url)" />
<arg name="gcs_url" value="$(arg gcs_url)" />
<arg name="tgt_system" value="$(arg tgt_system)" />
<arg name="tgt_component" value="$(arg tgt_component)" />
<arg name="log_output" value="$(arg log_output)" />
</include>
<!-- set streaming rate -->
<!-- node pkg="rosservice" type="rosservice" name="freq" args="call /mavros/set_stream_rate 0 10 1" output="screen" / -->
<!-- run xbee -->
<node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" />
<param name="Xbee_In_From_Buzz" type="str" value="outMavlink" />
<param name="Xbee_Out_To_Buzz" type="str" value="inMavlink" />
<param name="Xbee_In_From_Controller" type="str" value="xbee_cmd" />
<param name="No_of_dev" type="int" value="3" />
<!-- run rosbuzz -->
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
<rosparam file="/home/pi/ros_catkin_ws/src/ROSBuzz/launch/launch_config/solo.yaml"/>
<param name="bzzfile_name" value="/home/pi/ros_catkin_ws/src/ROSBuzz/src/testflockfev.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="robot_id" value="3"/>
<param name="No_of_Robots" value="3"/>
<param name="stand_by" value="/home/pi/ros_catkin_ws/src/ROSBuzz/src/stand_by.bo"/>
</node>
</launch>

32
launch/rosbuzz.launch
View File

@ -1,14 +1,22 @@
<!-- Launch file for ROSBuzz -->
<?xml version="1.0"?>
<!-- Generic launch file for ROSBuzz -->
<!-- This file is included in all ROS workspace launch files -->
<!-- Modify with great care! -->
<launch>
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
<rosparam file="/home/ivan/catkin_ws/src/rosbuzz/launch/launch_config/solo.yaml"/>
<param name="bzzfile_name" value="/home/ivan/catkin_ws/src/rosbuzz/src/testmoveto.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_status_srv" value="/xbee_status"/>
<param name="stand_by" value="$(env ROS_WS)/src/ROSBuzz/src/stand_by.bo"/>
</node>
<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" >
<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>

27
launch/rosbuzz_2_parallel.launch
View File

@ -1,27 +0,0 @@
<!-- Launch file for ROSBuzz -->
<launch>
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
<param name="bzzfile_name" value="/home/vivek/catkin_ws/src/rosbuzz/src/testflockfev_2parallel.bzz" />
<rosparam file="/home/vivek/catkin_ws/src/rosbuzz/launch/launch_config/m100.yaml"/>
<param name="rcclient" value="true" />
<param name="rcservice_name" value="rc_cmd" />
<param name="fcclient_name" value="/djicmd" />
<param name="in_payload" value="/inMavlink"/>
<!-- param name="in_payload" value="/rosbuzz_node1/outMavlink"-->
<param name="out_payload" value="/outMavlink"/>
<param name="robot_id" value="1"/>
<param name="No_of_Robots" value="1"/>
<param name="stand_by" value="/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bo"/>
</node>
<!-- node name="rosbuzz_node1" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" -->
<!-- param name="bzzfile_name" value="/home/vivek/catkin_ws/src/rosbuzz/src/test1.bzz" -->
<!-- param name="rcclient" value="true" -->
<!-- param name="rcservice_name" value="rc_cmd" -->
<!-- param name="fcclient_name" value="/djicmd" -->
<!-- param name="in_payload" value="/rosbuzz_node/outMavlink"-->
<!-- param name="out_payload" value="outMavlink"-->
<!-- param name="robot_id" value="2"-->
<!-- node-->
</launch>

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>

16
launch/rosbuzzm100.launch
View File

@ -1,16 +0,0 @@
<!-- Launch file for ROSBuzz -->
<launch>
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen">
<rosparam file="$(find rosbuzz)/launch/launch_config/m100.yaml"/>
<param name="bzzfile_name" value="$(find rosbuzz)/script/testflockfev.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="true"/>
<param name="name" value="m1001"/>
<param name="xbee_status_srv" value="xbee_status"/>
<param name="stand_by" value="$(find rosbuzz)/script/stand_by.bzz"/>
</node>
</launch>

18
src/buzz_utility.cpp
View File

@ -17,7 +17,7 @@ namespace buzz_utility{
static char* BO_FNAME = 0;
static uint8_t* BO_BUF = 0;
static buzzdebug_t DBG_INFO = 0;
static uint32_t MSG_SIZE = 500; // Only 250 bytes of Buzz messages every step (limited to Xbee frame size)
static uint32_t MSG_SIZE = 250; // Only 250 bytes of Buzz messages every step (limited to Xbee frame size)
static uint32_t MAX_MSG_SIZE = 10000; // Maximum Msg size for sending update packets
static uint8_t Robot_id = 0;
static std::vector<uint8_t*> IN_MSG;
@ -304,8 +304,8 @@ void in_message_process(){
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_moveto));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_goto", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_goto));
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_storegoal));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_arm));
@ -349,7 +349,7 @@ void in_message_process(){
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_goto", 1));
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1));
@ -487,7 +487,7 @@ int create_stig_tables() {
if(buzzvm_set_bcode(VM, BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
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;
}
/* Register hook functions */
@ -550,14 +550,14 @@ int create_stig_tables() {
if(buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
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;
}
// Register hook functions
if(buzz_register_hooks() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
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;
}
/* Create vstig tables
@ -606,14 +606,14 @@ int create_stig_tables() {
if(buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
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;
}
// Register hook functions
if(testing_buzz_register_hooks() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
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;
}
/* Create vstig tables

125
src/buzzuav_closures.cpp
View File

@ -16,20 +16,22 @@ namespace buzzuav_closures{
//void set_ros_controller_ptr(const rosbzz_node::roscontroller* roscontroller_ptrin);
static double goto_pos[3];
static double rc_goto_pos[3];
static float rc_gimbal[2];
static float batt[3];
static float obst[5]={0,0,0,0,0};
static double cur_pos[3];
static uint8_t status;
static int cur_cmd = 0;
static int rc_cmd=0;
static int rc_id=-1;
static int buzz_cmd=0;
static float height=0;
static bool deque_full = false;
static int rssi = 0;
static int message_number = 0;
static float raw_packet_loss = 0.0;
static int filtered_packet_loss = 0;
static float api_rssi = 0.0;
static bool deque_full = false;
static int rssi = 0;
static int message_number = 0;
static float raw_packet_loss = 0.0;
static int filtered_packet_loss = 0;
static float api_rssi = 0.0;
std::map< int, buzz_utility::RB_struct> targets_map;
std::map< int, buzz_utility::Pos_struct> neighbors_map;
@ -121,27 +123,27 @@ namespace buzzuav_closures{
/ Buzz closure to move following a 2D vector
/----------------------------------------*/
int buzzuav_moveto(buzzvm_t vm) {
buzzvm_lnum_assert(vm, 2);
buzzvm_lload(vm, 1); /* dx */
buzzvm_lload(vm, 2); /* dy */
//buzzvm_lload(vm, 3); /* Latitude */
//buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
float dy = buzzvm_stack_at(vm, 1)->f.value;
float dx = buzzvm_stack_at(vm, 2)->f.value;
double d = sqrt(dx*dx+dy*dy); //range
goto_pos[0]=dx;
goto_pos[1]=dy;
goto_pos[2]=height;
/*double b = atan2(dy,dx); //bearing
printf(" Vector for Goto: %.7f,%.7f\n",dx,dy);
gps_from_rb(d, b, goto_pos);
cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;*/
//printf(" Vector for Goto: %.7f,%.7f\n",dx,dy);
//ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0], goto_pos[1], goto_pos[2]);
buzz_cmd= COMMAND_MOVETO; // TO DO what should we use
return buzzvm_ret0(vm);
buzzvm_lnum_assert(vm, 3);
buzzvm_lload(vm, 1); /* dx */
buzzvm_lload(vm, 2); /* dy */
buzzvm_lload(vm, 3); /* dheight */
buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
float dh = buzzvm_stack_at(vm, 1)->f.value;
float dy = buzzvm_stack_at(vm, 2)->f.value;
float dx = buzzvm_stack_at(vm, 3)->f.value;
goto_pos[0]=dx;
goto_pos[1]=dy;
goto_pos[2]=height+dh;
/*double b = atan2(dy,dx); //bearing
printf(" Vector for Goto: %.7f,%.7f\n",dx,dy);
gps_from_rb(d, b, goto_pos);
cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;*/
//printf(" Vector for Goto: %.7f,%.7f\n",dx,dy);
//ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0], goto_pos[1], goto_pos[2]);
buzz_cmd= COMMAND_MOVETO; // TO DO what should we use
return buzzvm_ret0(vm);
}
int buzzuav_update_targets(buzzvm_t vm) {
@ -222,23 +224,23 @@ namespace buzzuav_closures{
int buzzuav_addNeiStatus(buzzvm_t vm){
buzzvm_lnum_assert(vm, 5);
buzzvm_lload(vm, 1); // fc
buzzvm_lload(vm, 2); // xbee
buzzvm_lload(vm, 3); // batt
buzzvm_lload(vm, 4); // gps
buzzvm_lload(vm, 5); // id
buzzvm_type_assert(vm, 5, BUZZTYPE_INT);
buzzvm_type_assert(vm, 4, BUZZTYPE_INT);
buzzvm_type_assert(vm, 3, BUZZTYPE_INT);
buzzvm_type_assert(vm, 2, BUZZTYPE_INT);
buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
buzzvm_lload(vm, 1); // fc
buzzvm_lload(vm, 2); // xbee
buzzvm_lload(vm, 3); // batt
buzzvm_lload(vm, 4); // gps
buzzvm_lload(vm, 5); // id
buzzvm_type_assert(vm, 5, BUZZTYPE_INT);
buzzvm_type_assert(vm, 4, BUZZTYPE_INT);
buzzvm_type_assert(vm, 3, BUZZTYPE_INT);
buzzvm_type_assert(vm, 2, BUZZTYPE_INT);
buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
buzz_utility::neighbors_status newRS;
uint8_t id = buzzvm_stack_at(vm, 5)->i.value;
newRS.gps_strenght= buzzvm_stack_at(vm, 4)->i.value;
newRS.batt_lvl= buzzvm_stack_at(vm, 3)->i.value;
newRS.xbee= buzzvm_stack_at(vm, 2)->i.value;
newRS.flight_status= buzzvm_stack_at(vm, 1)->i.value;
map< int, buzz_utility::neighbors_status >::iterator it = neighbors_status_map.find(id);
newRS.gps_strenght= buzzvm_stack_at(vm, 4)->i.value;
newRS.batt_lvl= buzzvm_stack_at(vm, 3)->i.value;
newRS.xbee= buzzvm_stack_at(vm, 2)->i.value;
newRS.flight_status= buzzvm_stack_at(vm, 1)->i.value;
map< int, buzz_utility::neighbors_status >::iterator it = neighbors_status_map.find(id);
if(it!=neighbors_status_map.end())
neighbors_status_map.erase(it);
neighbors_status_map.insert(make_pair(id, newRS));
@ -263,14 +265,17 @@ namespace buzzuav_closures{
return payload_out;
}
/*----------------------------------------/
/ Buzz closure to go directly to a GPS destination from the Mission Planner
/ Buzz closure to store locally a GPS destination from the fleet
/----------------------------------------*/
int buzzuav_goto(buzzvm_t vm) {
rc_goto_pos[2]=height;
set_goto(rc_goto_pos);
cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
printf(" Buzz requested Go To, to Latitude: %.7f , Longitude: %.7f, Altitude: %.7f \n",goto_pos[0],goto_pos[1],goto_pos[2]);
buzz_cmd=COMMAND_GOTO;
int buzzuav_storegoal(buzzvm_t vm) {
buzzvm_lnum_assert(vm, 3);
buzzvm_lload(vm, 1); // latitude
buzzvm_lload(vm, 2); // longitude
buzzvm_lload(vm, 3); // altitude
buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
rc_set_goto(buzzvm_stack_at(vm, 1)->f.value, buzzvm_stack_at(vm, 2)->f.value, buzzvm_stack_at(vm, 3)->f.value, (int)buzz_utility::get_robotid());
return buzzvm_ret0(vm);
}
@ -344,12 +349,22 @@ namespace buzzuav_closures{
return cmd;
}
void rc_set_goto(double pos[]) {
rc_goto_pos[0] = pos[0];
rc_goto_pos[1] = pos[1];
rc_goto_pos[2] = pos[2];
void rc_set_goto(int id, double latitude, double longitude, double altitude) {
rc_id = id;
rc_goto_pos[0] = latitude;
rc_goto_pos[1] = longitude;
rc_goto_pos[2] = altitude;
}
void rc_set_gimbal(int id, float yaw, float pitch) {
rc_id = id;
rc_gimbal[0] = yaw;
rc_gimbal[1] = pitch;
}
void rc_call(int rc_cmd_in) {
rc_cmd = rc_cmd_in;
}
@ -517,6 +532,10 @@ namespace buzzuav_closures{
buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_goto", 1));
buzzvm_pusht(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "id", 1));
buzzvm_pushi(vm, rc_id);
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1));
buzzvm_pushf(vm, rc_goto_pos[0]);
buzzvm_tput(vm);

281
src/roscontroller.cpp
View File

@ -266,7 +266,8 @@ void roscontroller::Rosparameters_get(ros::NodeHandle &n_c)
/*Obtain .bzz file name from parameter server*/
if (n_c.getParam("bzzfile_name", bzzfile_name))
;
else {
else
{
ROS_ERROR("Provide a .bzz file to run in Launch file");
system("rosnode kill rosbuzz_node");
}
@ -284,13 +285,11 @@ void roscontroller::Rosparameters_get(ros::NodeHandle &n_c)
{
ROS_INFO("RC service is disabled");
}
} else {
} else
{
ROS_ERROR("Provide a rc client option: yes or no in Launch file");
system("rosnode kill rosbuzz_node");
}
/*Obtain robot_id from parameter server*/
// n_c.getParam("robot_id", robot_id);
// robot_id=(int)buzz_utility::get_robotid();
/*Obtain out payload name*/
n_c.getParam("out_payload", out_payload);
/*Obtain in payload name*/
@ -300,22 +299,22 @@ void roscontroller::Rosparameters_get(ros::NodeHandle &n_c)
if (n_c.getParam("xbee_plugged", xbeeplugged))
;
else {
else
{
ROS_ERROR("Provide the xbee plugged boolean in Launch file");
system("rosnode kill rosbuzz_node");
}
if (!xbeeplugged) {
if (n_c.getParam("name", robot_name))
;
else {
else
{
ROS_ERROR("Provide the xbee plugged boolean in Launch file");
system("rosnode kill rosbuzz_node");
}
} else
n_c.getParam("xbee_status_srv", xbeesrv_name);
std::cout << "////////////////// " << xbeesrv_name;
GetSubscriptionParameters(n_c);
// initialize topics to null?
}
@ -325,70 +324,71 @@ used
/-----------------------------------------------------------------------------------*/
void roscontroller::GetSubscriptionParameters(ros::NodeHandle &node_handle)
{
// todo: make it as an array in yaml?
m_sMySubscriptions.clear();
std::string gps_topic, gps_type;
if (node_handle.getParam("topics/gps", gps_topic))
;
else {
else
{
ROS_ERROR("Provide a gps topic in Launch file");
system("rosnode kill rosbuzz_node");
}
node_handle.getParam("type/gps", gps_type);
m_smTopic_infos.insert(pair<std::string, std::string>(gps_topic, gps_type));
m_smTopic_infos.insert(pair<std::string, std::string>(gps_topic, "sensor_msgs/NavSatFix"));
std::string battery_topic, battery_type;
std::string battery_topic;
node_handle.getParam("topics/battery", battery_topic);
node_handle.getParam("type/battery", battery_type);
m_smTopic_infos.insert(
pair<std::string, std::string>(battery_topic, battery_type));
m_smTopic_infos.insert(pair<std::string, std::string>(battery_topic, "mavros_msgs/BatteryStatus"));
std::string status_topic, status_type;
std::string status_topic;
node_handle.getParam("topics/status", status_topic);
node_handle.getParam("type/status", status_type);
m_smTopic_infos.insert(
pair<std::string, std::string>(status_topic, status_type));
m_smTopic_infos.insert(pair<std::string, std::string>(status_topic, "mavros_msgs/State"));
node_handle.getParam("topics/estatus", status_topic);
m_smTopic_infos.insert(pair<std::string, std::string>(status_topic, "mavros_msgs/ExtendedState"));
std::string altitude_topic, altitude_type;
std::string altitude_topic;
node_handle.getParam("topics/altitude", altitude_topic);
node_handle.getParam("type/altitude", altitude_type);
m_smTopic_infos.insert(
pair<std::string, std::string>(altitude_topic, altitude_type));
m_smTopic_infos.insert(pair<std::string, std::string>(altitude_topic, "std_msgs/Float64"));
/*Obtain fc client name from parameter server*/
// Obtain required topic and service names from the parameter server
if (node_handle.getParam("topics/fcclient", fcclient_name))
;
else {
else
{
ROS_ERROR("Provide a fc client name in Launch file");
system("rosnode kill rosbuzz_node");
}
if (node_handle.getParam("topics/setpoint", setpoint_name))
;
else {
else
{
ROS_ERROR("Provide a Set Point name in Launch file");
system("rosnode kill rosbuzz_node");
}
if (node_handle.getParam("topics/armclient", armclient))
;
else {
else
{
ROS_ERROR("Provide an arm client name in Launch file");
system("rosnode kill rosbuzz_node");
}
if (node_handle.getParam("topics/modeclient", modeclient))
;
else {
else
{
ROS_ERROR("Provide a mode client name in Launch file");
system("rosnode kill rosbuzz_node");
}
if (node_handle.getParam("topics/stream", stream_client_name))
;
else {
else
{
ROS_ERROR("Provide a mode client name in Launch file");
system("rosnode kill rosbuzz_node");
}
if (node_handle.getParam("topics/localpos", local_pos_sub_name))
;
else {
else
{
ROS_ERROR("Provide a localpos name in YAML file");
system("rosnode kill rosbuzz_node");
}
@ -405,33 +405,28 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle &n_c)
Subscribe(n_c);
payload_sub =
n_c.subscribe(in_payload, 5, &roscontroller::payload_obt, this);
payload_sub = n_c.subscribe(in_payload, 5, &roscontroller::payload_obt, this);
obstacle_sub = n_c.subscribe(obstacles_topic, 5,
&roscontroller::obstacle_dist, this);
obstacle_sub = n_c.subscribe(obstacles_topic, 5, &roscontroller::obstacle_dist, this);
/*publishers*/
payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 5);
MPpayload_pub = n_c.advertise<mavros_msgs::Mavlink>("fleet_status", 5);
neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("neighbours_pos", 5);
localsetpoint_nonraw_pub =
n_c.advertise<geometry_msgs::PoseStamped>(setpoint_name, 5);
localsetpoint_nonraw_pub = n_c.advertise<geometry_msgs::PoseStamped>(setpoint_name, 5);
/* Service Clients*/
arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient);
mode_client = n_c.serviceClient<mavros_msgs::SetMode>(modeclient);
mav_client = n_c.serviceClient<mavros_msgs::CommandLong>(fcclient_name);
if (rcclient == true)
service =
n_c.advertiseService(rcservice_name, &roscontroller::rc_callback, this);
service = n_c.advertiseService(rcservice_name, &roscontroller::rc_callback, this);
ROS_INFO("Ready to receive Mav Commands from RC client");
xbeestatus_srv = n_c.serviceClient<mavros_msgs::ParamGet>(xbeesrv_name);
stream_client =
n_c.serviceClient<mavros_msgs::StreamRate>(stream_client_name);
stream_client = n_c.serviceClient<mavros_msgs::StreamRate>(stream_client_name);
users_sub = n_c.subscribe("users_pos", 5, &roscontroller::users_pos, this);
local_pos_sub = n_c.subscribe(local_pos_sub_name, 5,
&roscontroller::local_pos_callback, this);
local_pos_sub = n_c.subscribe(local_pos_sub_name, 5, &roscontroller::local_pos_callback, this);
multi_msg = true;
}
@ -444,20 +439,15 @@ void roscontroller::Subscribe(ros::NodeHandle &n_c)
m_smTopic_infos.begin();
it != m_smTopic_infos.end(); ++it) {
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") {
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") {
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") {
current_position_sub =
n_c.subscribe(it->first, 5, &roscontroller::current_pos, this);
current_position_sub = n_c.subscribe(it->first, 5, &roscontroller::current_pos, this);
} else if (it->second == "std_msgs/Float64") {
relative_altitude_sub =
n_c.subscribe(it->first, 5, &roscontroller::current_rel_alt, this);
relative_altitude_sub = n_c.subscribe(it->first, 5, &roscontroller::current_rel_alt, this);
}
std::cout << "Subscribed to: " << it->first << endl;
@ -469,31 +459,17 @@ void roscontroller::Subscribe(ros::NodeHandle &n_c)
/-------------------------------------------------------*/
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*/
stringstream bzzfile_in_compile;
std::string path =
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);
name = name.substr(0, name.find_last_of("."));
bzzfile_in_compile << "bzzc -I " << path
<< "include/"; //<<" "<<path<< name<<".basm";
// 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("");
<< "include/";
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 << " -a " << path << name << ".asm ";
bzzfile_in_compile << bzzfile_name;
// std::string tmp_dbgfname = path + name + ".bdb";
ROS_WARN("Launching buzz compilation: %s", bzzfile_in_compile.str().c_str());
@ -658,37 +634,37 @@ void roscontroller::flight_controller_service_call()
// SOLO SPECIFIC: SITL DOES NOT USE 21 TO LAND -- workaround: set to LAND
// mode
switch (buzzuav_closures::getcmd()) {
case mavros_msgs::CommandCode::NAV_LAND:
if (current_mode != "LAND") {
SetMode("LAND", 0);
case mavros_msgs::CommandCode::NAV_LAND:
if (current_mode != "LAND") {
SetMode("LAND", 0);
armstate = 0;
Arm();
}
if (mav_client.call(cmd_srv)) {
ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
} else {
ROS_ERROR("Failed to call service from flight controller");
}
armstate = 0;
Arm();
}
if (mav_client.call(cmd_srv)) {
ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
} else {
ROS_ERROR("Failed to call service from flight controller");
}
armstate = 0;
break;
case mavros_msgs::CommandCode::NAV_TAKEOFF:
if (!armstate) {
break;
case mavros_msgs::CommandCode::NAV_TAKEOFF:
if (!armstate) {
SetMode("LOITER", 0);
armstate = 1;
Arm();
ros::Duration(0.5).sleep();
// Registering HOME POINT.
home = cur_pos;
}
if (current_mode != "GUIDED")
SetMode("GUIDED", 2000); // for real solo, just add 2000ms delay (it
// should always be in loiter after arm/disarm)
if (mav_client.call(cmd_srv)) {
ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
} else
ROS_ERROR("Failed to call service from flight controller");
break;
SetMode("LOITER", 0);
armstate = 1;
Arm();
ros::Duration(0.5).sleep();
// Registering HOME POINT.
home = cur_pos;
}
if (current_mode != "GUIDED")
SetMode("GUIDED", 2000); // for real solo, just add 2000ms delay (it
// should always be in loiter after arm/disarm)
if (mav_client.call(cmd_srv)) {
ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
} else
ROS_ERROR("Failed to call service from flight controller");
break;
}
} else if (tmp == buzzuav_closures::COMMAND_GOTO) { /*FC call for goto*/
@ -977,7 +953,7 @@ void roscontroller::SetMode(std::string mode, int delay_miliseconds) {
set_mode_message.request.custom_mode = mode;
current_mode = mode;
if (mode_client.call(set_mode_message)) {
ROS_INFO("Set Mode Service call successful!");
;//ROS_INFO("Set Mode Service call successful!");
} else {
ROS_INFO("Set Mode Service call failed!");
}
@ -993,7 +969,7 @@ void roscontroller::SetStreamRate(int id, int rate, int on_off) {
ROS_INFO("Set stream rate call failed!, trying again...");
ros::Duration(0.1).sleep();
}
ROS_INFO("Set stream rate call successful");
//ROS_INFO("Set stream rate call successful");
}
/*-------------------------------------------------------------
@ -1059,13 +1035,10 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr &msg) {
nei_pos.longitude = neighbours_pos_payload[1];
nei_pos.altitude = neighbours_pos_payload[2];
double cvt_neighbours_pos_payload[3];
// cout<<"Got" << neighbours_pos_payload[0] <<", " <<
//neighbours_pos_payload[1] << ", " << neighbours_pos_payload[2] << endl;
gps_rb(nei_pos, cvt_neighbours_pos_payload);
/*Extract robot id of the neighbour*/
uint16_t *out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + 3));
ROS_WARN("RAB of %i: %f, %f", (int)out[1], cvt_neighbours_pos_payload[0],
cvt_neighbours_pos_payload[1]);
//ROS_WARN("RAB of %i: %f, %f", (int)out[1], cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1]);
/*pass neighbour position to local maintaner*/
buzz_utility::Pos_struct n_pos(cvt_neighbours_pos_payload[0],
cvt_neighbours_pos_payload[1],
@ -1089,58 +1062,60 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request &req,
mavros_msgs::CommandLong::Response &res) {
int rc_cmd;
switch (req.command) {
case mavros_msgs::CommandCode::NAV_TAKEOFF:
ROS_INFO("RC_call: TAKE OFF!!!!");
rc_cmd = mavros_msgs::CommandCode::NAV_TAKEOFF;
buzzuav_closures::rc_call(rc_cmd);
res.success = true;
break;
case mavros_msgs::CommandCode::NAV_LAND:
ROS_INFO("RC_Call: LAND!!!! sending land");
rc_cmd = mavros_msgs::CommandCode::NAV_LAND;
buzzuav_closures::rc_call(rc_cmd);
res.success = true;
break;
case mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM:
rc_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM;
armstate = req.param1;
if (armstate) {
ROS_INFO("RC_Call: ARM!!!!");
case mavros_msgs::CommandCode::NAV_TAKEOFF:
ROS_INFO("RC_call: TAKE OFF!!!!");
rc_cmd = mavros_msgs::CommandCode::NAV_TAKEOFF;
buzzuav_closures::rc_call(rc_cmd);
res.success = true;
} else {
ROS_INFO("RC_Call: DISARM!!!!");
buzzuav_closures::rc_call(rc_cmd + 1);
break;
case mavros_msgs::CommandCode::NAV_LAND:
ROS_INFO("RC_Call: LAND!!!!");
rc_cmd = mavros_msgs::CommandCode::NAV_LAND;
buzzuav_closures::rc_call(rc_cmd);
res.success = true;
}
break;
case mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH:
ROS_INFO("RC_Call: GO HOME!!!!");
rc_cmd = mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
buzzuav_closures::rc_call(rc_cmd);
res.success = true;
break;
case mavros_msgs::CommandCode::NAV_WAYPOINT:
ROS_INFO("RC_Call: GO TO!!!! --- Doing this! ");
double rc_goto[3];
// testing PositionTarget
rc_goto[0] = req.param5;
rc_goto[1] = req.param6;
rc_goto[2] = req.param7;
buzzuav_closures::rc_set_goto(rc_goto);
rc_cmd = mavros_msgs::CommandCode::NAV_WAYPOINT;
buzzuav_closures::rc_call(rc_cmd);
res.success = true;
break;
case 666:
ROS_INFO("RC_Call: Update Fleet Status!!!!");
rc_cmd = 666;
buzzuav_closures::rc_call(rc_cmd);
res.success = true;
break;
default:
res.success = false;
break;
break;
case mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM:
rc_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM;
armstate = req.param1;
if (armstate) {
ROS_INFO("RC_Call: ARM!!!!");
buzzuav_closures::rc_call(rc_cmd);
res.success = true;
} else {
ROS_INFO("RC_Call: DISARM!!!!");
buzzuav_closures::rc_call(rc_cmd + 1);
res.success = true;
}
break;
case mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH:
ROS_INFO("RC_Call: GO HOME!!!!");
rc_cmd = mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
buzzuav_closures::rc_call(rc_cmd);
res.success = true;
break;
case mavros_msgs::CommandCode::NAV_WAYPOINT:
ROS_INFO("RC_Call: GO TO!!!! ");
buzzuav_closures::rc_set_goto(req.param1,req.param5,req.param6,req.param7);
rc_cmd = mavros_msgs::CommandCode::NAV_WAYPOINT;
buzzuav_closures::rc_call(rc_cmd);
res.success = true;
break;
case mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL:
ROS_INFO("RC_Call: Gimbal!!!! ");
buzzuav_closures::rc_set_gimbal(req.param1,req.param2,req.param3);
rc_cmd = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL;
buzzuav_closures::rc_call(rc_cmd);
res.success = true;
break;
case CMD_REQUEST_UPDATE:
//ROS_INFO("RC_Call: Update Fleet Status!!!!");
rc_cmd = CMD_REQUEST_UPDATE;
buzzuav_closures::rc_call(rc_cmd);
res.success = true;
break;
default:
res.success = false;
break;
}
return true;
}