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

update rosbuzz repo

This commit is contained in:
David St-Onge 2017-11-28 13:22:29 -05:00
parent 25ed9328da 6018b681d7
commit 369006413f
41 changed files with 3383 additions and 1725 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@ -5,3 +5,4 @@ src/test*
*.bo
*.bdb
*.bdbg
buzz_scripts/log*

17
buzz_scripts/empty.bzz Normal file
View File

@ -0,0 +1,17 @@
include "update.bzz"
# Executed once at init time.
function init() {
}
# Executed at each time step.
function step() {
}
# Executed once when the robot (or the simulator) is reset.
function reset() {
}
# Executed once at the end of experiment.
function destroy() {
}

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)

203
buzz_scripts/graphform.bzz
View File

@ -4,18 +4,20 @@
include "string.bzz"
include "vec2.bzz"
include "update.bzz"
include "barrier.bzz" # don't use a stigmergy id=11 with this header.
include "vstigenv.bzz" # reserve stigmergy id=20 and 21 for this header.
include "barrier.bzz" # reserve stigmergy id=11 for this header.
include "uavstates.bzz" # require an 'action' function to be defined here.
include "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
ROBOT_MAXVEL = 150.0
#
# Global variables
@ -42,7 +44,7 @@ m_receivedMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Respo
m_selfMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
#Current robot state
m_eState="STATE_FREE"
# m_eState="STATE_FREE" # replace with default UAVSTATE
#navigation vector
m_navigation={.x=0,.y=0}
@ -87,11 +89,11 @@ m_fTargetDistanceTolerance=0
#step cunt
step_cunt=0
#virtual stigmergy
# virtual stigmergy for the LOCK barrier.
m_lockstig = 1
# Lennard-Jones parameters, may need change
EPSILON = 4000 #13.5 the LJ parameter for other robots
EPSILON = 1800 #13.5 the LJ parameter for other robots
# Lennard-Jones interaction magnitude
@ -100,24 +102,6 @@ function FlockInteraction(dist,target,epsilon){
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
#
@ -204,26 +188,12 @@ function find(table,value){
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
}
}
#
#pack message into 1 number
#
function packmessage(send_table){
var send_value
send_value=10000*send_table.State+1000*send_table.Label+100*send_table.ReqLabel+10*send_table.ReqID+send_table.Response
send_value=100000*send_table.State+10000*send_table.Label+1000*send_table.ReqLabel+10*send_table.ReqID+send_table.Response
return send_value
}
#
@ -247,12 +217,12 @@ function pack_guide_msg(send_table){
#unpack message
#
function unpackmessage(recv_value){
var wan=(recv_value-recv_value%10000)/10000
recv_value=recv_value-wan*10000
var qian=(recv_value-recv_value%1000)/1000
recv_value=recv_value-qian*1000
var bai=(recv_value-recv_value%100)/100
recv_value=recv_value-bai*100
var wan=(recv_value-recv_value%100000)/100000
recv_value=recv_value-wan*100000
var qian=(recv_value-recv_value%10000)/10000
recv_value=recv_value-qian*10000
var bai=(recv_value-recv_value%1000)/1000
recv_value=recv_value-bai*1000
var shi=(recv_value-recv_value%10)/10
recv_value=recv_value-shi*10
var ge=recv_value
@ -391,19 +361,19 @@ function UpdateNodeInfo(){
#Transistion to state free
#
function TransitionToFree(){
m_eState="STATE_FREE"
UAVSTATE="STATE_FREE"
m_unWaitCount=m_unLabelSearchWaitTime
m_selfMessage.State=s2i(m_eState)
m_selfMessage.State=s2i(UAVSTATE)
}
#
#Transistion to state asking
#
function TransitionToAsking(un_label){
m_eState="STATE_ASKING"
UAVSTATE="STATE_ASKING"
m_nLabel=un_label
m_unRequestId=id #don't know why the random numbers are the same, add id to make the ReqID different
m_selfMessage.State=s2i(m_eState)
m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.ReqLabel=m_nLabel
m_selfMessage.ReqID=m_unRequestId
@ -414,8 +384,8 @@ function TransitionToAsking(un_label){
#Transistion to state joining
#
function TransitionToJoining(){
m_eState="STATE_JOINING"
m_selfMessage.State=s2i(m_eState)
UAVSTATE="STATE_JOINING"
m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.Label=m_nLabel
m_unWaitCount=m_unJoiningLostPeriod
@ -435,8 +405,8 @@ function TransitionToJoining(){
#Transistion to state joined
#
function TransitionToJoined(){
m_eState="STATE_JOINED"
m_selfMessage.State=s2i(m_eState)
UAVSTATE="STATE_JOINED"
m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.Label=m_nLabel
m_vecNodes[m_nLabel].State="ASSIGNED"
neighbors.ignore("r")
@ -446,7 +416,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)
}
#
@ -456,8 +426,8 @@ function TransitionToJoined(){
#Transistion to state Lock, lock the current formation
#
function TransitionToLock(){
m_eState="STATE_LOCK"
m_selfMessage.State=s2i(m_eState)
UAVSTATE="STATE_LOCK"
m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.Label=m_nLabel
m_vecNodes[m_nLabel].State="ASSIGNED"
#record neighbor distance
@ -471,14 +441,14 @@ 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)
}
#
# Do free
#
function DoFree() {
m_selfMessage.State=s2i(m_eState)
m_selfMessage.State=s2i(UAVSTATE)
#wait for a while before looking for a Label
if(m_unWaitCount>0)
@ -535,7 +505,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 +515,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,10 +524,10 @@ 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)
m_selfMessage.State=s2i(UAVSTATE)
}
@ -576,8 +546,10 @@ function DoAsking(){
#the request Label be the same as requesed
#get a respond
if(m_MessageState[i]=="STATE_JOINED"){
log("received label = ",m_MessageReqLabel[i])
if(m_MessageReqLabel[i]==m_nLabel)
if(m_MessageResponse[i]!="REQ_NONE"){
log("get response")
psResponse=i
}}
i=i+1
@ -586,7 +558,7 @@ function DoAsking(){
if(psResponse==-1){
#no response, wait
m_unWaitCount=m_unWaitCount-1
m_selfMessage.State=s2i(m_eState)
m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.ReqLabel=m_nLabel
m_selfMessage.ReqID=m_unRequestId
if(m_unWaitCount==0){
@ -595,6 +567,7 @@ function DoAsking(){
}
}
else{
log("respond id=",m_MessageReqID[psResponse])
if(m_MessageReqID[psResponse]!=m_unRequestId){
m_vecNodes[m_nLabel].State="ASSIGNING"
m_vecNodes[m_nLabel].StateAge=m_unJoiningLostPeriod
@ -603,7 +576,6 @@ function DoAsking(){
if(m_MessageReqID[psResponse]==m_unRequestId){
if(m_MessageResponse[psResponse]=="REQ_GRANTED"){
TransitionToJoining()
#TransitionToJoined()
return
}
else{
@ -614,7 +586,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)
}
#
@ -653,7 +625,7 @@ function DoJoining(){
#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)
var S2Target_bearing=math.atan(S2Target.y, S2Target.x)
m_bias=m_cMeToPred.Bearing-S2PGlobalBearing
S2Target_bearing=S2Target_bearing+m_bias
@ -662,7 +634,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)
@ -683,7 +655,7 @@ function DoJoining(){
}
#pack the communication package
m_selfMessage.State=s2i(m_eState)
m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.Label=m_nLabel
}
@ -692,7 +664,7 @@ function DoJoining(){
#Do joined
#
function DoJoined(){
m_selfMessage.State=s2i(m_eState)
m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.Label=m_nLabel
#collect all requests
@ -705,10 +677,13 @@ function DoJoined(){
while(i<m_neighbourCount){
if(m_MessageState[i]=="STATE_ASKING"){
ReqLabel=m_MessageReqLabel[i]
log("ReqLabel var:",ReqLabel)
log("M_vec var",m_vecNodes[ReqLabel].State)
if(m_vecNodes[ReqLabel].State=="UNASSIGNED")
if(m_nLabel==m_vecNodes[ReqLabel].Pred){
#is a request, store the index
mapRequests[j]=i
log("Into if m_nLabel")
j=j+1
}
}
@ -745,6 +720,8 @@ function DoJoined(){
m_selfMessage.ReqID=ReqID
m_selfMessage.Response=r2i("REQ_GRANTED")
m_vecNodes[ReqLabel].State="ASSIGNING"
log("Label=",ReqLabel)
log("ID=",ReqID)
m_vecNodes[ReqLabel].StateAge=m_unJoiningLostPeriod
}
@ -760,7 +737,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
@ -776,25 +753,28 @@ function DoJoined(){
#Do Lock
#
function DoLock(){
m_selfMessage.State=s2i(m_eState)
m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.Label=m_nLabel
m_navigation.x=0.0
m_navigation.y=0.0
#calculate motion vection
if(m_nLabel==0){
m_navigation.x=0.25 #change value so that robot 0 will move
m_navigation.x=0.0 #change value so that robot 0 will move
m_navigation.y=0.0
}
if(m_nLabel!=0){
m_navigation=motion_vector()
}
#move
uav_moveto(m_navigation.x,m_navigation.y)
uav_moveto(m_navigation.x, m_navigation.y, 0.0)
}
function action(){
statef=action
UAVSTATE="GRAPH"
UAVSTATE="STATE_FREE"
# reset the graph
Reset()
}
#
@ -802,11 +782,11 @@ function action(){
#
function init() {
#
#Adjust parameters here
# Global parameters for graph formation
#
m_unResponseTimeThreshold=10
m_unLabelSearchWaitTime=10
m_fTargetDistanceTolerance=50
m_fTargetDistanceTolerance=100
m_fTargetAngleTolerance=0.1
m_unJoiningLostPeriod=100
@ -814,52 +794,53 @@ function init() {
# Join Swarm
#
uav_initswarm()
v_tag = stigmergy.create(m_lockstig)
uav_initstig()
Reset()
TARGET_ALTITUDE = 2.5 + id * 1.5
v_tag = stigmergy.create(m_lockstig)
uav_initstig()
# go to diff. height since no collision avoidance implemented yet
TARGET_ALTITUDE = 7.5 #2.5 + id * 1.5
statef=turnedoff
UAVSTATE = "TURNEDOFF"
}
#
# Executed every step
#
function step(){
function step() {
# listen to potential RC
uav_rccmd()
# get the swarm commands
uav_neicmd()
uav_updatestig()
# 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")
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()
}
if(UAVSTATE=="STATE_FREE")
statef=DoFree
else if(UAVSTATE=="STATE_ESCAPE")
statef=DoEscape
else if(UAVSTATE=="STATE_ASKING")
statef=DoAsking
else if(UAVSTATE=="STATE_JOINING")
statef=DoJoining
else if(UAVSTATE=="STATE_JOINED")
statef=DoJoined
else if(UAVSTATE=="STATE_LOCK")
statef=DoLock
# high level UAV state machine
statef()
debug(m_eState,m_nLabel)
log("Current state: ", UAVSTATE)
log("Current state: ", UAVSTATE, " and label: ", m_nLabel)
log("Swarm size: ", ROBOTS)
# if(id==0)
# stattab_print()
#navigation
#broadcast messag
#broadcast message
neighbors.broadcast("m",packmessage(m_selfMessage))
#
@ -882,12 +863,8 @@ function step(){
# Executed when reset
#
function Reset(){
# m_vecNodes={}
# m_vecNodes = parse_graph("/home/dave/ROS_WS/src/rosbuzz/buzz_scripts/include/Graph_drone.graph")#change the .graph file when necessary
# m_vecNodes_fixed={}
# m_vecNodes_fixed=parse_graph_fixed("/home/dave/ROS_WS/src/rosbuzz/buzz_scripts/include/Graph_fixed.graph")
Read_Graph()
m_nLabel=-1
m_nLabel=-1
#start listening
start_listen()
@ -895,7 +872,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 +889,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")
}

849
buzz_scripts/graphformGPS.bzz Normal file
View File

@ -0,0 +1,849 @@
#
# Include files
#
include "string.bzz"
include "vec2.bzz"
include "update.bzz"
#include "vstigenv.bzz" # reserve stigmergy id=20 and 21 for this header.
include "barrier.bzz" # reserve stigmergy id=80 (auto-increment up) for this header.
include "uavstates.bzz" # require an 'action' function to be defined here.
include "graphs/shapes_P.bzz"
include "graphs/shapes_O.bzz"
include "graphs/shapes_L.bzz"
include "graphs/shapes_Y.bzz"
ROBOT_RADIUS = 50
ROBOT_DIAMETER = 2.0*ROBOT_RADIUS
ROBOT_SAFETYDIST = 2.0*ROBOT_DIAMETER
ROOT_ID = 2
old_state = -1
# max velocity in cm/step
ROBOT_MAXVEL = 150.0
#
# Global variables
#
#
# Save message from all neighours
#the indexes are as 1,2,3..., while each value is a table that store the information of a neighbour robot
m_MessageState={}#store received neighbour message
m_MessageLabel={}#store received neighbour message
m_MessageReqLabel={}#store received neighbour message
m_MessageReqID={}#store received neighbour message
m_MessageResponse={}#store received neighbour message
m_MessageRange={}#store received neighbour message
m_MessageBearing={}#store received neighbour message
m_neighbourCount=0#used to cunt neighbours
#Save message from one neighbour
#the indexes are as State(received state),Label(received Label),ReqLabel,ReqID,Response,Range,Bearing
m_receivedMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0}
#
#Save the message to send
#The keys of the talbe is State(current state),Label(current Label),ReqLabel(requested Label),ReqID(request id),Response(reply message{REQ_NONE,REQ_GRANTED,REQ_RESEND})
m_selfMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
#navigation vector
m_navigation={.x=0,.y=0}
#Current label being requested or chosen (-1 when none)
m_nLabel=-1
m_messageID={}
repeat_assign=0
assign_label=-1
assign_id=-1
m_gotjoinedparent = 0
#neighbor distance to lock the current pattern
lock_neighbor_id={}
lock_neighbor_dis={}
#Label request id
m_unRequestId=0
#Global bias, used to map local coordinate to global coordinate
m_bias=0
#Vector to predecessor,range is the distance between robots, bearing is the angle of pred wrt self in local coordinate of self, globalbearing is the angle of self wrt pred in global coordinate
m_cMeToPred={.Range=0.0,.Bearing=0.0,.GlobalBearing=0.0}
#Counter to wait for something to happen
m_unWaitCount=0
#Number of steps to wait before looking for a free label
m_unLabelSearchWaitTime=0
#Number of steps to wait for an answer to be received
m_unResponseTimeThreshold=0
#Number of steps to wait until giving up joining
m_unJoiningLostPeriod=0
#Tolerance distance to a target location
m_fTargetDistanceTolerance=0
# virtual stigmergy for the LOCK barrier.
m_lockstig = 1
# Lennard-Jones parameters, may need change
EPSILON = 4000 #13.5 the LJ parameter for other robots
# Lennard-Jones interaction magnitude
function FlockInteraction(dist,target,epsilon){
var mag = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
return mag
}
#
#return the number of value in table
#
function count(table,value){
var number=0
var i=0
while(i<size(table)){
if(table[i]==value){
number=number+1
}
i=i+1
}
return number
}
#
#map from int to state
#
function i2s(value){
if(value==1){
return "STATE_FREE"
}
else if(value==2){
return "STATE_ASKING"
}
else if(value==3){
return "STATE_JOINING"
}
else if(value==4){
return "STATE_JOINED"
}
else if(value==5){
return "STATE_LOCK"
}
}
#
#map from state to int
#
function s2i(value){
if(value=="STATE_FREE"){
return 1
}
else if(value=="STATE_ASKING"){
return 2
}
else if(value=="STATE_JOINING"){
return 3
}
else if(value=="STATE_JOINED"){
return 4
}
else if(value=="STATE_LOCK"){
return 5
}
}
#
#map form int to response
#
function i2r(value){
if(value==1){
return "REQ_NONE"
}
else if(value==2){
return "REQ_GRANTED"
}
}
#
#map from response to int
#
function r2i(value){
if(value=="REQ_NONE"){
return 1
}
else if(value=="REQ_GRANTED"){
return 2
}
}
#
#return the index of value
#
function find(table,value){
var ind=nil
var i=0
while(i<size(table)){
if(table[i]==value)
ind=i
i=i+1
}
return ind
}
#
#pack message into 1 number
#
function packmessage(send_table){
var send_value
send_value=100000*send_table.State+10000*send_table.Label+1000*send_table.ReqLabel+10*send_table.ReqID+send_table.Response
return send_value
}
#
#pack guide message into 1 number
#
function pack_guide_msg(send_table){
var send_value
var r_id=send_table.Label#id of target robot
var pon#positive or negative ,0 postive, 1 negative
if(send_table.Bearing>=0){
pon=0
}
else{
pon=1
}
var b=math.abs(send_table.Bearing)
send_value=r_id*1000+pon*100+b
return send_value
}
#
#unpack message
#
function unpackmessage(recv_value){
var wan=(recv_value-recv_value%100000)/100000
recv_value=recv_value-wan*100000
var qian=(recv_value-recv_value%10000)/10000
recv_value=recv_value-qian*10000
var bai=(recv_value-recv_value%1000)/1000
recv_value=recv_value-bai*1000
var shi=(recv_value-recv_value%10)/10
recv_value=recv_value-shi*10
var ge=recv_value
var return_table={.State=0.0,.Label=0.0,.ReqLabel=0.0,.ReqID=0.0,.Response=0.0}
return_table.State=wan
return_table.Label=qian
return_table.ReqLabel=bai
return_table.ReqID=shi
return_table.Response=ge
return return_table
}
#
#unpack guide message
#
function unpack_guide_msg(recv_value){
#log(id,"I pass value=",recv_value)
var qian=(recv_value-recv_value%1000)/1000
recv_value=recv_value-qian*1000
var bai=(recv_value-recv_value%100)/100
recv_value=recv_value-bai*100
var b=recv_value
var return_table={.Label=0.0,.Bearing=0.0}
return_table.Label=qian
if(bai==1){
b=b*-1.0
}
return_table.Bearing=b
return return_table
}
#
#get the target distance to neighbr nei_id
#
function target4label(nei_id){
var return_val="miss"
var i=0
while(i<size(lock_neighbor_id)){
if(lock_neighbor_id[i]==nei_id){
return_val=lock_neighbor_dis[i]
}
i=i+1
}
return return_val
}
#
#calculate LJ vector for neibhor stored in i
#
function LJ_vec(i){
var dis=m_MessageRange[i]
var ljbearing=m_MessageBearing[i]
var nei_id=m_messageID[i]
var target=target4label(nei_id)
var cDir={.x=0.0,.y=0.0}
if(target!="miss"){
cDir=math.vec2.newp(FlockInteraction(dis,target,EPSILON),ljbearing)
}
#log(id,"dis=",dis,"target=",target,"label=",nei_id)
#log("x=",cDir.x,"y=",cDir.y)
return cDir
}
#
#calculate the motion vector
#
function motion_vector(){
var i=0
var m_vector={.x=0.0,.y=0.0}
while(i<m_neighbourCount){
#calculate and add the motion vector
m_vector=math.vec2.add(m_vector,LJ_vec(i))
#log(id,"x=",m_vector.x,"y=",m_vector.y)
i=i+1
}
m_vector=math.vec2.scale(m_vector,1.0/m_neighbourCount)
#log(id,"fnal=","x=",m_vector.x,"y=",m_vector.y)
return m_vector
}
#
# starts the neighbors listener
#
function start_listen(){
neighbors.listen("m",
function(vid,value,rid){
#store the received message
var temp_id=rid
var recv_val=unpackmessage(value)
Get_DisAndAzi(temp_id)
#add the received message
#
m_MessageState[m_neighbourCount]=i2s(recv_val.State)
m_MessageLabel[m_neighbourCount]=recv_val.Label
m_MessageReqLabel[m_neighbourCount]=recv_val.ReqLabel
m_MessageReqID[m_neighbourCount]=recv_val.ReqID
m_MessageResponse[m_neighbourCount]=i2r(recv_val.Response)
m_MessageRange[m_neighbourCount]=m_receivedMessage.Range
m_MessageBearing[m_neighbourCount]=m_receivedMessage.Bearing
m_messageID[m_neighbourCount]=rid
#log(rid, " is in ", m_MessageState[m_neighbourCount], " ", m_MessageLabel[m_neighbourCount])
m_neighbourCount=m_neighbourCount+1
})
}
#
#Function used to get the station info of the sender of the message
#
function Get_DisAndAzi(id){
neighbors.foreach(
function(rid, data) {
if(rid==id){
m_receivedMessage.Range=data.distance*100.0
m_receivedMessage.Bearing=data.azimuth
}
})
}
#
#Update node info according to neighbour robots
#
function UpdateNodeInfo(){
#Collect informaiton
#Update information
var i=0
while(i<m_neighbourCount){
if(m_MessageState[i]=="STATE_JOINED"){
m_vecNodes[m_MessageLabel[i]].State="ASSIGNED"
m_vecNodes[m_MessageLabel[i]].StateAge=m_unJoiningLostPeriod
}
else if(m_MessageState[i]=="STATE_JOINING"){
m_vecNodes[m_MessageLabel[i]].State="ASSIGNING"
m_vecNodes[m_MessageLabel[i]].StateAge=m_unJoiningLostPeriod
}
i=i+1
}
#Forget old information
i=0
while(i<size(m_vecNodes)){
if((m_vecNodes[i].StateAge>0) and (m_vecNodes[i].State=="ASSIGNING")){
m_vecNodes[i].StateAge=m_vecNodes[i].StateAge-1
if(m_vecNodes[i].StateAge==0)
m_vecNodes[i].State="UNASSIGNED"
}
i=i+1
}
}
#
#Transistion to state free
#
function TransitionToFree(){
UAVSTATE="STATE_FREE"
m_unWaitCount=m_unLabelSearchWaitTime
m_selfMessage.State=s2i(UAVSTATE)
}
#
#Transistion to state asking
#
function TransitionToAsking(un_label){
UAVSTATE="STATE_ASKING"
m_nLabel=un_label
m_unRequestId=id #don't know why the random numbers are the same, add id to make the ReqID different
m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.ReqLabel=m_nLabel
m_selfMessage.ReqID=m_unRequestId
m_unWaitCount=m_unResponseTimeThreshold
}
#
#Transistion to state joining
#
function TransitionToJoining(){
UAVSTATE="STATE_JOINING"
m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.Label=m_nLabel
m_unWaitCount=m_unJoiningLostPeriod
}
#
#Transistion to state joined
#
function TransitionToJoined(){
UAVSTATE="STATE_JOINED"
m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.Label=m_nLabel
m_vecNodes[m_nLabel].State="ASSIGNED"
#write statues
#v_tag.put(m_nLabel, m_lockstig)
barrier_create()
barrier_ready()
m_navigation.x=0.0
m_navigation.y=0.0
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
}
#
#Transistion to state Lock, lock the current formation
#
function TransitionToLock(){
UAVSTATE="STATE_LOCK"
m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.Label=m_nLabel
m_vecNodes[m_nLabel].State="ASSIGNED"
#record neighbor distance
lock_neighbor_id={}
lock_neighbor_dis={}
var i=0
while(i<m_neighbourCount){
lock_neighbor_id[i]=m_messageID[i]
lock_neighbor_dis[i]=m_MessageRange[i]
i=i+1
}
m_navigation.x=0.0
m_navigation.y=0.0
uav_moveto(m_navigation.x, m_navigation.y, 0.0)
# prepare to restart a new shape
old_state = rc_State
#stop listening
neighbors.ignore("m")
}
#
# Do free
#
function DoFree() {
m_selfMessage.State=s2i(UAVSTATE)
#wait for a while before looking for a Label
if(m_unWaitCount>0)
m_unWaitCount=m_unWaitCount-1
#find a set of joined robots
var setJoinedLabels={}
var setJoinedIndexes={}
var i=0
var j=0
while(i<m_neighbourCount){
if(m_MessageState[i]=="STATE_JOINED"){
setJoinedLabels[j]=m_MessageLabel[i]
setJoinedIndexes[j]=i
j=j+1
}
i=i+1
}
#go through the graph to look for a proper Label
var unFoundLabel=0
# var IDofPred=0
i=1
while(i<size(m_vecNodes) and (unFoundLabel==0)){
#if the node is unassigned and the predecessor is insight
if(m_vecNodes[i].State=="UNASSIGNED" and count(setJoinedLabels,m_vecNodes[i].Pred)==1){
unFoundLabel=m_vecNodes[i].Label
# IDofPred=find(m_MessageLabel,m_vecNodes[unFoundLabel].Pred)
}
i=i+1
}
if(unFoundLabel>0){
TransitionToAsking(unFoundLabel)
return
}
#set message
m_selfMessage.State=s2i(UAVSTATE)
}
#
#Do asking
#
function DoAsking(){
#look for response from predecessor
var i=0
var psResponse=-1
while(i<m_neighbourCount and psResponse==-1){
#the respond robot in joined state
#the request Label be the same as requesed
#get a respond
if(m_MessageState[i]=="STATE_JOINED"){
#log("received label = ",m_MessageReqLabel[i])
if(m_MessageReqLabel[i]==m_nLabel)
if(m_MessageResponse[i]!="REQ_NONE"){
psResponse=i
}}
if(m_MessageState[i]=="STATE_JOINING" and m_MessageLabel[i]==m_nLabel){
TransitionToFree()
return
}
i=i+1
}
#analyse response
if(psResponse==-1){
#no response, wait
m_unWaitCount=m_unWaitCount-1
m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.ReqLabel=m_nLabel
m_selfMessage.ReqID=m_unRequestId
#if(m_unWaitCount==0){
#TransitionToFree()
#return
#}
}
else{
log("respond id=",m_MessageReqID[psResponse])
if(m_MessageReqID[psResponse]!=m_unRequestId){
m_vecNodes[m_nLabel].State="ASSIGNING"
m_vecNodes[m_nLabel].StateAge=m_unJoiningLostPeriod
TransitionToFree()
}
if(m_MessageReqID[psResponse]==m_unRequestId){
if(m_MessageResponse[psResponse]=="REQ_GRANTED"){
TransitionToJoining()
return
}
else{
TransitionToAsking(m_nLabel)
return
}
}
}
m_selfMessage.Label=m_nLabel
m_navigation.x=0.0
m_navigation.y=0.0
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
}
#
#Do joining
#
function DoJoining(){
if(m_gotjoinedparent!=1)
set_rc_goto()
else
gotoWP(TransitionToJoined)
#pack the communication package
m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.Label=m_nLabel
}
function set_rc_goto() {
#get information of pred
var i=0
var IDofPred=-1
while(i<m_neighbourCount and IDofPred==-1){
if(m_MessageLabel[i]==m_vecNodes[m_nLabel].Pred and m_MessageState[i]=="STATE_JOINED")
IDofPred=i
i=i+1
}
#found pred
if(IDofPred!=-1){
m_unWaitCount=m_unJoiningLostPeriod#if see pred, reset the timer
var P2Target=math.vec2.newp(m_vecNodes[m_nLabel].distance,m_vecNodes[m_nLabel].bearing)
m_cMeToPred.Range=m_MessageRange[IDofPred]#the poition of self to pred in local coordinate
m_cMeToPred.Bearing=m_MessageBearing[IDofPred]
var S2Pred=math.vec2.newp(m_cMeToPred.Range,m_cMeToPred.Bearing)
var S2Target=math.vec2.add(S2Pred,P2Target)
goal = gps_from_vec(math.vec2.newp(math.vec2.length(S2Target)/100.0, math.atan(S2Target.y,S2Target.x)))
print("Saving GPS goal: ",goal.latitude, goal.longitude)
uav_storegoal(goal.latitude, goal.longitude, position.altitude)
m_gotjoinedparent = 1
}
}
#
#Do joined
#
function DoJoined(){
m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.Label=m_nLabel
#collect all requests
var mapRequests={}
var i=0
var j=0
var ReqLabel
var JoiningLabel
var seenPred=0
while(i<m_neighbourCount){
if(m_MessageState[i]=="STATE_ASKING"){
ReqLabel=m_MessageReqLabel[i]
#log("ReqLabel var:",ReqLabel)
#log("M_vec var",m_vecNodes[ReqLabel].State)
if(m_vecNodes[ReqLabel].State=="UNASSIGNED")
if(m_nLabel==m_vecNodes[ReqLabel].Pred){
#is a request, store the index
mapRequests[j]=i
j=j+1
}
}
if(m_MessageState[i]=="STATE_JOINING"){
JoiningLabel=m_MessageLabel[i]
if(m_nLabel==m_vecNodes[JoiningLabel].Pred){
##joining wrt this dot,send the global bearing
var m_messageForJoining={.Label=JoiningLabel,.Bearing=m_MessageBearing[i]-m_bias}
}
}
if(m_MessageState[i]=="STATE_JOINING" and repeat_assign==1 and m_MessageLabel[i]==assign_label){
repeat_assign=0
}
#if it is the pred
if((m_MessageState[i]=="STATE_JOINED" or m_MessageState[i]=="STATE_LOCK") and m_MessageLabel[i]==m_vecNodes[m_nLabel].Pred){
seenPred=1
m_unWaitCount=m_unJoiningLostPeriod
}
i=i+1
}
#get request
if(size(mapRequests)!=0){
i=1
var ReqIndex=0
while(i<size(mapRequests)){
#compare the distance
if(m_MessageRange[mapRequests[ReqIndex]]>m_MessageRange[mapRequests[i]])
ReqIndex=i
i=i+1
}
if(repeat_assign==0){
#get the best index, whose ReqLabel and Reqid are
ReqLabel=m_MessageReqLabel[mapRequests[ReqIndex]]
var ReqID=m_MessageReqID[mapRequests[ReqIndex]]
assign_label=ReqLabel
assign_id=ReqID
repeat_assign=1
}
m_selfMessage.ReqLabel=assign_label
m_selfMessage.ReqID=assign_id
m_selfMessage.Response=r2i("REQ_GRANTED")
#m_vecNodes[ReqLabel].State="ASSIGNING"
log("Label=",assign_label)
log("ID=",assign_id)
m_vecNodes[ReqLabel].StateAge=m_unJoiningLostPeriod
}
#lost pred, wait for some time and transit to free
if(seenPred==0){
m_unWaitCount=m_unWaitCount-1
if(m_unWaitCount==0){
TransitionToFree()
return
}
}
#check if should to transists to lock
#write statues
#v_tag.get(m_nLabel)
#log(v_tag.size(), " of ", ROBOTS, " ready to lock")
#if(v_tag.size()==ROBOTS){
# TransitionToLock()
#}
barrier_wait(ROBOTS, TransitionToLock, DoJoined, -1)
}
#
#Do Lock
#
function DoLock(){
m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.Label=m_nLabel
m_navigation.x=0.0
m_navigation.y=0.0
#calculate motion vection
if(m_nLabel==0){
m_navigation.x=0.0 #change value so that robot 0 will move
m_navigation.y=0.0
}
if(m_nLabel!=0){
m_navigation=motion_vector()
}
#move
uav_moveto(m_navigation.x, m_navigation.y, 0.0)
}
#
# Executed after takeoff
#
function action(){
statef=action
UAVSTATE="STATE_FREE"
# reset the graph
Reset()
}
#
# Executed at init
#
function init() {
#
# Global parameters for graph formation
#
m_unResponseTimeThreshold=10
m_unLabelSearchWaitTime=10
m_fTargetDistanceTolerance=100
m_fTargetAngleTolerance=0.1
m_unJoiningLostPeriod=100
#
# Join Swarm
#
uav_initswarm()
#v_tag = stigmergy.create(m_lockstig)
#uav_initstig()
# go to diff. height since no collision avoidance implemented yet
#TARGET_ALTITUDE = 20.0 + id * 2.5
statef=turnedoff
UAVSTATE = "TURNEDOFF"
}
#
# Executed every step (main loop)
#
function step() {
# listen to potential RC
uav_rccmd()
# get the swarm commands
uav_neicmd()
# update the vstig (status/net/batt)
#uav_updatestig()
#update the graph
UpdateNodeInfo()
#reset message package to be sent
m_selfMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
#
# graph state machine
#
if(UAVSTATE=="STATE_FREE")
statef=DoFree
else if(UAVSTATE=="STATE_ASKING")
statef=DoAsking
else if(UAVSTATE=="STATE_JOINING")
statef=DoJoining
else if(UAVSTATE=="STATE_JOINED")
statef=DoJoined
else if(UAVSTATE=="STATE_LOCK" and old_state!=rc_State)
statef=action
else if(UAVSTATE=="STATE_LOCK" and old_state==rc_State)
statef=DoLock
# high level UAV state machine
statef()
log("Current state: ", UAVSTATE, " and label: ", m_nLabel)
log("Swarm size: ", ROBOTS)
#navigation
#broadcast message
neighbors.broadcast("m",packmessage(m_selfMessage))
#
#clean message storage
m_MessageState={}#store received neighbour message
m_MessageLabel={}#store received neighbour message
m_MessageReqLabel={}#store received neighbour message
m_MessageReqID={}#store received neighbour message
m_MessageResponse={}#store received neighbour message
m_MessageRange={}#store received neighbour message
m_MessageBearing={}#store received neighbour message
m_neighbourCount=0
}
#
# Executed when reset
#
function Reset(){
m_receivedMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0}
m_selfMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
m_navigation={.x=0,.y=0}
m_nLabel=-1
m_messageID={}
lock_neighbor_id={}
lock_neighbor_dis={}
m_unRequestId=0
m_bias=0
m_cMeToPred={.Range=0.0,.Bearing=0.0,.GlobalBearing=0.0}
m_unWaitCount=0
repeat_assign=0
assign_label=-1
assign_id=-1
m_gotjoinedparent = 0
if(rc_State==0){
log("Loading P graph")
Read_GraphP()
} else if(rc_State==1) {
log("Loading O graph")
Read_GraphO()
} else if(rc_State==2) {
log("Loading L graph")
Read_GraphL()
} else if(rc_State==3) {
log("Loading Y graph")
Read_GraphY()
}
#start listening
start_listen()
#
#set initial state, only one robot choose [A], while the rest choose [B]
#
#[A]The robot used to triger the formation process is defined as joined,
if(id==ROOT_ID){
m_nLabel=0
TransitionToJoined()
}
#[B]Other robots are defined as free.
else{
TransitionToFree()
}
}
#
# Executed upon destroy
#
function destroy() {
#clear neighbour message
m_navigation.x=0.0
m_navigation.y=0.0
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
m_vecNodes={}
#stop listening
neighbors.ignore("m")
}

60
buzz_scripts/include/barrier.bzz
View File

@ -7,49 +7,73 @@
#
# Constants
#
BARRIER_VSTIG = 11
BARRIER_TIMEOUT = 1200 # in steps
BARRIER_VSTIG = 80
timeW = 0
barrier = nil
#
# Sets a barrier
#
function barrier_set(threshold, transf, resumef) {
statef = function() {
barrier_wait(threshold, transf, resumef);
function barrier_create() {
# reset
timeW = 0
# create barrier vstig
#log("---> Prev. br. ", barrier, " ", BARRIER_VSTIG)
if(barrier!=nil) {
barrier=nil
BARRIER_VSTIG = BARRIER_VSTIG +1
}
#log("---> New. br. ", barrier, " ", BARRIER_VSTIG)
barrier = stigmergy.create(BARRIER_VSTIG)
}
function barrier_set(threshold, transf, resumef, bdt) {
statef = function() {
barrier_wait(threshold, transf, resumef, bdt);
}
UAVSTATE = "BARRIERWAIT"
barrier_create()
}
#
# Make yourself ready
#
function barrier_ready() {
#log("BARRIER READY -------")
barrier.put(id, 1)
barrier.put("d", 0)
}
#
# Executes the barrier
#
BARRIER_TIMEOUT = 200
timeW=0
function barrier_wait(threshold, transf, resumef) {
barrier.get(id)
function barrier_wait(threshold, transf, resumef, bdt) {
barrier.put(id, 1)
UAVSTATE = "BARRIERWAIT"
if(barrier.size() >= threshold) {
# getlowest()
barrier.get(id)
#log("--->BS: ", barrier.size(), " (", BARRIER_VSTIG, ")")
if(barrier.size() - 1 >= threshold or barrier.get("d") == 1) {
barrier.put("d", 1)
timeW = 0
transf()
} else if(timeW>=BARRIER_TIMEOUT) {
barrier = nil
} else if(timeW >= BARRIER_TIMEOUT) {
log("------> Barrier Timeout !!!!")
barrier=nil
timeW = 0
resumef()
timeW=0
}
if(bdt!=-1)
neighbors.broadcast("cmd", bdt)
timeW = timeW+1
}
# get the lowest id of the fleet, but requires too much bandwidth
# get the lowest id of the fleet, but requires too much bandwidth...
function getlowest(){
Lid = 20;
u=20
Lid = 15;
u=15
while(u>=0){
tab = barrier.get(u)
if(tab!=nil){
@ -59,4 +83,4 @@ function getlowest(){
u=u-1
}
log("--> LOWEST ID:",Lid)
}
}

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

@ -1,7 +1,7 @@
#Table of the nodes in the graph
m_vecNodes={}
function Read_Graph(){
function Read_GraphL(){
m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing
.Label = 0, # Label of the point
.Pred = -1, # Label of its predecessor

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

@ -1,7 +1,7 @@
#Table of the nodes in the graph
m_vecNodes={}
m_vecNodes_fixed={}
function Read_Graph(){
function Read_GraphO(){
m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing
.Label = 0, # Label of the point
.Pred = -1, # Label of its predecessor

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

@ -1,7 +1,7 @@
#Table of the nodes in the graph
m_vecNodes={}
m_vecNodes_fixed={}
function Read_Graph(){
function Read_GraphP(){
m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing
.Label = 0, # Label of the point
.Pred = -1, # Label of its predecessor
@ -14,7 +14,7 @@ m_vecNodes[0] = { # The .graph file is stored according the sequence of Lab
m_vecNodes[1] = {
.Label = 1,
.Pred = 0,
.distance = 2000,
.distance = 1000,
.bearing = 0.0,
.height = 5000,
.State="UNASSIGNED",
@ -23,7 +23,7 @@ m_vecNodes[1] = {
m_vecNodes[2] = {
.Label = 2,
.Pred = 0,
.distance = 2000,
.distance = 1000,
.bearing = 1.57,
.height = 7000,
.State="UNASSIGNED",
@ -32,7 +32,7 @@ m_vecNodes[2] = {
m_vecNodes[3] = {
.Label = 3,
.Pred = 0,
.distance = 2000,
.distance = 1000,
.bearing = 4.71,
.height = 9000,
.State="UNASSIGNED",
@ -41,7 +41,7 @@ m_vecNodes[3] = {
m_vecNodes[4] = {
.Label = 4,
.Pred = 1,
.distance = 1414,
.distance = 707,
.bearing = 0.79,
.height = 11000,
.State="UNASSIGNED",
@ -50,7 +50,7 @@ m_vecNodes[4] = {
m_vecNodes[5] = {
.Label = 5,
.Pred = 2,
.distance = 2000,
.distance = 1000,
.bearing = 0.0,
.height = 14000,
.State="UNASSIGNED",

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

@ -1,7 +1,7 @@
#Table of the nodes in the graph
m_vecNodes={}
m_vecNodes_fixed={}
function Read_Graph(){
function Read_GraphY(){
m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing
.Label = 0, # Label of the point
.Pred = -1, # Label of its predecessor

89
buzz_scripts/include/graphs/waypointlist.csv Normal file
View File

@ -0,0 +1,89 @@
0,-73.1531935978886,45.5084960903092,15,15
1,-73.1530989420915,45.5085624255498,15,15
2,-73.1530042862771,45.5086287608025,15,15
3,-73.1529096304626,45.5086950960552,15,15
4,-73.1529458247399,45.5087204611798,15,15
5,-73.1530404805543,45.5086541259271,15,15
6,-73.1531351363515,45.5085877906865,15,15
7,-73.1532297921486,45.508521455446,15,15
8,-73.1533244479457,45.5084551202054,15,15
9,-73.1533606422273,45.508480485333,15,15
10,-73.1532659864302,45.5085468205736,15,15
11,-73.153171330633,45.5086131558142,15,15
12,-73.1530766748359,45.5086794910548,15,15
13,-73.1529820190215,45.5087458263075,15,15
14,-73.1530182132901,45.5087711914261,15,15
15,-73.1531128691045,45.5087048561733,15,15
16,-73.1532075249016,45.5086385209327,15,15
17,-73.1533021806988,45.5085721856922,15,15
18,-73.1533968364959,45.5085058504516,15,15
19,-73.1534330307645,45.5085312155701,15,15
20,-73.1533383749674,45.5085975508107,15,15
21,-73.1532437191702,45.5086638860513,15,15
22,-73.1531490633731,45.5087302212919,15,15
23,-73.1530544075587,45.5087965565446,15,15
24,-73.1530906018273,45.5088219216632,15,15
25,-73.1531852576417,45.5087555864105,15,15
26,-73.1532799134389,45.5086892511699,15,15
27,-73.153374569236,45.5086229159293,15,15
28,-73.1534692250331,45.5085565806887,15,15
29,-73.1535054193017,45.5085819458072,15,15
30,-73.1534107635046,45.5086482810478,15,15
31,-73.1533161077075,45.5087146162884,15,15
32,-73.1532214519103,45.508780951529,15,15
33,-73.1531267960959,45.5088472867817,15,15
34,-73.1531629903645,45.5088726519003,15,15
35,-73.1532576461789,45.5088063166476,15,15
36,-73.1533523019761,45.508739981407,15,15
37,-73.1534469577732,45.5086736461664,15,15
38,-73.1535416135703,45.5086073109258,15,15
39,-73.1535778078389,45.5086326760444,15,15
40,-73.1534831520418,45.5086990112849,15,15
41,-73.1533884962447,45.5087653465255,15,15
42,-73.1532938404476,45.5088316817661,15,15
43,-73.1531991846331,45.5088980170188,15,15
44,-73.1532353789017,45.5089233821374,15,15
45,-73.1533300347162,45.5088570468847,15,15
46,-73.1534246905133,45.5087907116441,15,15
47,-73.1535193463104,45.5087243764035,15,15
48,-73.1536140021075,45.5086580411629,15,15
49,-73.1536501963762,45.5086834062815,15,15
50,-73.153555540579,45.508749741522,15,15
51,-73.1534608847819,45.5088160767626,15,15
52,-73.1533662289848,45.5088824120032,15,15
53,-73.1532715731703,45.508948747256,15,15
54,-73.1533077674389,45.5089741123745,15,15
55,-73.1534024232534,45.5089077771218,15,15
56,-73.1534970790505,45.5088414418812,15,15
57,-73.1535917348476,45.5087751066406,15,15
58,-73.1536863906448,45.5087087714,15,15
59,-73.1537225849134,45.5087341365186,15,15
60,-73.1536279291163,45.5088004717592,15,15
61,-73.1535332733191,45.5088668069997,15,15
62,-73.153438617522,45.5089331422403,15,15
63,-73.1533439617076,45.5089994774931,15,15
64,-73.1533801559762,45.5090248426116,15,15
65,-73.1534748117906,45.5089585073589,15,15
66,-73.1535694675877,45.5088921721183,15,15
67,-73.1536641233849,45.5088258368777,15,15
68,-73.153758779182,45.5087595016371,15,15
69,-73.1537949734506,45.5087848667557,15,15
70,-73.1537003176535,45.5088512019963,15,15
71,-73.1536056618563,45.5089175372369,15,15
72,-73.1535110060592,45.5089838724775,15,15
73,-73.1534163502448,45.5090502077302,15,15
74,-73.1534525445134,45.5090755728487,15,15
75,-73.1535472003278,45.509009237596,15,15
76,-73.1536418561249,45.5089429023554,15,15
77,-73.1537365119221,45.5088765671148,15,15
78,-73.1538311677192,45.5088102318742,15,15
79,-73.1538673619878,45.5088355969928,15,15
80,-73.1537727061907,45.5089019322334,15,15
81,-73.1536780503936,45.508968267474,15,15
82,-73.1535833945964,45.5090346027146,15,15
83,-73.153488738782,45.5091009379673,15,15
84,-73.1535249330852,45.5091263031101,15,15
85,-73.1536195888996,45.5090599678574,15,15
86,-73.1537142446968,45.5089936326168,15,15
87,-73.1538089004939,45.5089272973762,15,15
88,-73.153903556291,45.5088609621356,15,15
1 0 -73.1531935978886 45.5084960903092 15 15
2 1 -73.1530989420915 45.5085624255498 15 15
3 2 -73.1530042862771 45.5086287608025 15 15
4 3 -73.1529096304626 45.5086950960552 15 15
5 4 -73.1529458247399 45.5087204611798 15 15
6 5 -73.1530404805543 45.5086541259271 15 15
7 6 -73.1531351363515 45.5085877906865 15 15
8 7 -73.1532297921486 45.508521455446 15 15
9 8 -73.1533244479457 45.5084551202054 15 15
10 9 -73.1533606422273 45.508480485333 15 15
11 10 -73.1532659864302 45.5085468205736 15 15
12 11 -73.153171330633 45.5086131558142 15 15
13 12 -73.1530766748359 45.5086794910548 15 15
14 13 -73.1529820190215 45.5087458263075 15 15
15 14 -73.1530182132901 45.5087711914261 15 15
16 15 -73.1531128691045 45.5087048561733 15 15
17 16 -73.1532075249016 45.5086385209327 15 15
18 17 -73.1533021806988 45.5085721856922 15 15
19 18 -73.1533968364959 45.5085058504516 15 15
20 19 -73.1534330307645 45.5085312155701 15 15
21 20 -73.1533383749674 45.5085975508107 15 15
22 21 -73.1532437191702 45.5086638860513 15 15
23 22 -73.1531490633731 45.5087302212919 15 15
24 23 -73.1530544075587 45.5087965565446 15 15
25 24 -73.1530906018273 45.5088219216632 15 15
26 25 -73.1531852576417 45.5087555864105 15 15
27 26 -73.1532799134389 45.5086892511699 15 15
28 27 -73.153374569236 45.5086229159293 15 15
29 28 -73.1534692250331 45.5085565806887 15 15
30 29 -73.1535054193017 45.5085819458072 15 15
31 30 -73.1534107635046 45.5086482810478 15 15
32 31 -73.1533161077075 45.5087146162884 15 15
33 32 -73.1532214519103 45.508780951529 15 15
34 33 -73.1531267960959 45.5088472867817 15 15
35 34 -73.1531629903645 45.5088726519003 15 15
36 35 -73.1532576461789 45.5088063166476 15 15
37 36 -73.1533523019761 45.508739981407 15 15
38 37 -73.1534469577732 45.5086736461664 15 15
39 38 -73.1535416135703 45.5086073109258 15 15
40 39 -73.1535778078389 45.5086326760444 15 15
41 40 -73.1534831520418 45.5086990112849 15 15
42 41 -73.1533884962447 45.5087653465255 15 15
43 42 -73.1532938404476 45.5088316817661 15 15
44 43 -73.1531991846331 45.5088980170188 15 15
45 44 -73.1532353789017 45.5089233821374 15 15
46 45 -73.1533300347162 45.5088570468847 15 15
47 46 -73.1534246905133 45.5087907116441 15 15
48 47 -73.1535193463104 45.5087243764035 15 15
49 48 -73.1536140021075 45.5086580411629 15 15
50 49 -73.1536501963762 45.5086834062815 15 15
51 50 -73.153555540579 45.508749741522 15 15
52 51 -73.1534608847819 45.5088160767626 15 15
53 52 -73.1533662289848 45.5088824120032 15 15
54 53 -73.1532715731703 45.508948747256 15 15
55 54 -73.1533077674389 45.5089741123745 15 15
56 55 -73.1534024232534 45.5089077771218 15 15
57 56 -73.1534970790505 45.5088414418812 15 15
58 57 -73.1535917348476 45.5087751066406 15 15
59 58 -73.1536863906448 45.5087087714 15 15
60 59 -73.1537225849134 45.5087341365186 15 15
61 60 -73.1536279291163 45.5088004717592 15 15
62 61 -73.1535332733191 45.5088668069997 15 15
63 62 -73.153438617522 45.5089331422403 15 15
64 63 -73.1533439617076 45.5089994774931 15 15
65 64 -73.1533801559762 45.5090248426116 15 15
66 65 -73.1534748117906 45.5089585073589 15 15
67 66 -73.1535694675877 45.5088921721183 15 15
68 67 -73.1536641233849 45.5088258368777 15 15
69 68 -73.153758779182 45.5087595016371 15 15
70 69 -73.1537949734506 45.5087848667557 15 15
71 70 -73.1537003176535 45.5088512019963 15 15
72 71 -73.1536056618563 45.5089175372369 15 15
73 72 -73.1535110060592 45.5089838724775 15 15
74 73 -73.1534163502448 45.5090502077302 15 15
75 74 -73.1534525445134 45.5090755728487 15 15
76 75 -73.1535472003278 45.509009237596 15 15
77 76 -73.1536418561249 45.5089429023554 15 15
78 77 -73.1537365119221 45.5088765671148 15 15
79 78 -73.1538311677192 45.5088102318742 15 15
80 79 -73.1538673619878 45.5088355969928 15 15
81 80 -73.1537727061907 45.5089019322334 15 15
82 81 -73.1536780503936 45.508968267474 15 15
83 82 -73.1535833945964 45.5090346027146 15 15
84 83 -73.153488738782 45.5091009379673 15 15
85 84 -73.1535249330852 45.5091263031101 15 15
86 85 -73.1536195888996 45.5090599678574 15 15
87 86 -73.1537142446968 45.5089936326168 15 15
88 87 -73.1538089004939 45.5089272973762 15 15
89 88 -73.153903556291 45.5088609621356 15 15

151
buzz_scripts/include/mapmatrix.bzz Normal file
View File

@ -0,0 +1,151 @@
# Write to matrix
robot_marker = "X"
function wmat(mat, row, col, val) {
var index = (row-1)*mat.nb_col + (col - 1)
if( row <= mat.nb_row) { # update val
mat.mat[index] = val
} else if(row == mat.nb_row + 1){ # add entry
mat.nb_row = mat.nb_row + 1
mat.mat[index] = val
}
}
# Read from matrix
function rmat(mat, row, col) {
#log("rmat ", mat, row, col)
index = (row-1)*mat.nb_col + (col - 1)
if (mat.mat[index] == nil) {
log("Wrong matrix read index: ", row, " ", col)
return -1
} else {
return mat.mat[index]
}
}
# copy a full matrix row
function mat_copyrow(out,ro,in,ri){
var indexI = (ri-1)*in.nb_col
var indexO = (ro-1)*out.nb_col
icr=0
while(icr<in.nb_col){
out.mat[indexO+icr]=in.mat[indexI+icr]
icr = icr + 1
}
}
function getvec(t,row){
return math.vec2.new(rmat(t,row,1),rmat(t,row,2))
}
function init_test_map(len){
map = {.nb_col=len, .nb_row=len, .mat={}}
index = 0
while(index<len*len){
map.mat[index]=1.0
index = index + 1
}
# puts an obstacle right in the middle
wmat(map,5,5,0.0)
wmat(map,6,5,0.0)
wmat(map,4,5,0.0)
log("Occupancy grid initialized (",len,"x",len,") with obstacles.")
}
function init_map(len){
map = {.nb_col=len, .nb_row=len, .mat={}}
index = 0
while(index<len*len){
map.mat[index]=1.0
index = index + 1
}
log("Occupancy grid initialized (",len,"x",len,").")
}
function add_obstacle(pos, off, inc_trust) {
xi = math.round(pos.x)
yi = math.round(pos.y)
if(xi < map.nb_col+1 and yi < map.nb_row+1 and xi > 0 and yi > 0) {
#log("Add obstacle in cell: ", xi, " ", yi)
old=rmat(map,xi,yi)
if(old-inc_trust > 0.0)
wmat(map,xi,yi,old-inc_trust)
else
wmat(map,xi,yi,0.0)
}
}
function remove_obstacle(pos, off, dec_trust) {
xi = math.round(pos.x)
yi = math.round(pos.y)
if(xi < map.nb_col+1 and yi < map.nb_row+1 and xi > 0 and yi > 0){
#log("Remove obstacle in cell: ", xi, " ", yi)
old=rmat(map, xi, yi)
if(old + dec_trust < 1.0) #x,y
wmat(map, xi, yi, old+dec_trust)
else
wmat(map, xi, yi, 1.0)
}
}
function table_print(t) {
foreach(t, function(key, value) {
log(key, " -> ", value)
})
}
function table_copy(t) {
var t2 = {}
foreach(t, function(key, value) {
t2[key] = value
})
return t2
}
function print_pos(t) {
ir=1
while(ir<=t.nb_row){
log(ir, ": ", rmat(t,ir,1), " ", rmat(t,ir,2))
ir = ir + 1
}
}
function print_map(t) {
ir=t.nb_row
log("Printing a ", t.nb_row, " by ", t.nb_col, " map")
while(ir>0){
logst=string.concat("\t", string.tostring(ir), "\t:")
ic=t.nb_col
while(ic>0){
if(ir==cur_cell.x and ic==cur_cell.y)
logst = string.concat(logst, " XXXXXXXX")
else
logst = string.concat(logst, " ", string.tostring(rmat(t,ir,ic)))
ic = ic - 1
}
log(logst)
ir = ir - 1
}
}
function print_map_argos(t){
ir=t.nb_row
msg = string.tostring(ir)
while(ir>0){
ic=t.nb_col
while(ic>0){
if(ir==cur_cell.x and ic==cur_cell.y){
msg = string.concat(msg, ":", robot_marker)
}
else {
msg = string.concat(msg, ":", string.tostring(rmat(t,ir,ic)))
}
ic = ic - 1
}
ir = ir - 1
}
set_argos_map(msg)
}

454
buzz_scripts/include/rrtstar.bzz Normal file
View File

@ -0,0 +1,454 @@
#####
# RRT* Path Planing
#
# map table-based matrix
#####
include "mapmatrix.bzz"
RRT_TIMEOUT = 500
map = nil
cur_cell = {}
nei_cell = {}
function UAVinit_map(m_navigation) {
# create a map big enough for the goal
init_map(2*math.round(math.vec2.length(m_navigation))+10)
# center the robot on the grid
cur_cell = math.vec2.new(math.round(map.nb_col/2.0),math.round(map.nb_row/2.0))
}
function UAVpathPlanner(m_navigation, m_pos) {
# place the robot on the grid
update_curcell(m_pos,0)
# create the goal in the map grid
mapgoal = math.vec2.add(m_navigation,cur_cell)
mapgoal = math.vec2.new(math.round(mapgoal.x),math.round(mapgoal.y))
# search for a path
return RRTSTAR(mapgoal,math.vec2.new(5,5)) #map.nb_col/20.0,map.nb_row/20.0))
}
function Kh4pathPlanner(m_goal, m_pos) {
# move 0,0 to a corner instead of the center
m_goal = math.vec2.sub(m_goal,math.vec2.new(GRIDCM.xmin, GRIDCM.ymin))
m_goal = math.vec2.new(math.round(m_goal.x*CM2KH4.x), math.round(m_goal.y*CM2KH4.y))
# place the robot on the grid
update_curcell(m_pos,1)
log("Starting from cell: ", cur_cell.x, " ", cur_cell.y)
log("Going to cell: ", m_goal.x, " ", m_goal.y)
# search for a path
print_map(map)
# print_map_argos(map)
return RRTSTAR(m_goal, math.vec2.new(10.0 * CM2KH4.x, 10.0 * CM2KH4.y))
}
function update_curcell(m_curpos, kh4) {
if(kh4) { # for khepera playground
cur_cell = math.vec2.sub(m_curpos, math.vec2.new(GRIDCM.xmin, GRIDCM.ymin))
cur_cell = math.vec2.new(math.round(cur_cell.x*CM2KH4.x), math.round(cur_cell.y*CM2KH4.y))
} else { # for uav map relative to home
cur_cell = math.vec2.add(cur_cell, m_curpos)
cur_cell = math.vec2.new(math.round(m_curpos.x), math.round(m_curpos.y))
}
if(cur_cell.x>map.nb_row)
cur_cell.x=map.nb_row
if(cur_cell.y>map.nb_col)
cur_cell.y=map.nb_col
if(cur_cell.x<1)
cur_cell.x=1
if(cur_cell.y<1)
cur_cell.y=1
}
function UAVgetneiobs (m_curpos) {
update_curcell(m_curpos,0)
# add all neighbors as obstacles in the grid
neighbors.foreach(function(rid, data) {
add_obstacle(math.vec2.add(math.vec2.newp(data.distance,data.azimuth),cur_cell), 0, 1.0)
})
}
function getneiobs (m_curpos) {
foundobstacle = 0
update_curcell(m_curpos,1)
old_nei = table_copy(nei_cell)
#log("NeiObs debug: ", nei_cell[9], " ", nei_cell[3])
neighbors.foreach(function(rid, data) {
#log("nei rel pos: ", data.distance, "m ", data.azimuth*180.0/math.pi, "deg")
Ncell = math.vec2.add(math.vec2.newp(data.distance*100,data.azimuth + absolute_position.theta), m_curpos)
#log("nei abs pos: ", Ncell.x, "cm ", Ncell.y, "cm")
Ncell = math.vec2.sub(Ncell, math.vec2.new(GRIDCM.xmin, GRIDCM.ymin))
Ncell = math.vec2.new(math.round(Ncell.x*CM2KH4.x), math.round(Ncell.y*CM2KH4.y))
nei_cell[rid] = {.x=Ncell.x, .y=Ncell.y}
#log("Neighbor in : ", Ncell.x, " ", Ncell.y)
if(old_nei[rid]!=nil) {
if(old_nei[rid].x!=nei_cell[rid].x or old_nei[rid].y!=nei_cell[rid].y) {
#log("Neighbor moved ! ", nei_cell[rid].x, " ", nei_cell[rid].y)
remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y), 0, 1)
remove_obstacle(math.vec2.new(old_nei[rid].x+1, old_nei[rid].y), 0, 1)
remove_obstacle(math.vec2.new(old_nei[rid].x-1, old_nei[rid].y), 0, 1)
remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y+1), 0, 1)
remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y-1), 0, 1)
add_obstacle(Ncell, 0, 1.0)
add_obstacle(math.vec2.new(Ncell.x+1, Ncell.y), 0, 1.0)
add_obstacle(math.vec2.new(Ncell.x-1, Ncell.y), 0, 1.0)
add_obstacle(math.vec2.new(Ncell.x, Ncell.y+1), 0, 1.0)
add_obstacle(math.vec2.new(Ncell.x, Ncell.y-1), 0, 1.0)
foundobstacle = 1
}
} else {
add_obstacle(Ncell, 0, 1.0)
add_obstacle(math.vec2.new(Ncell.x+1, Ncell.y), 0, 1.0)
add_obstacle(math.vec2.new(Ncell.x-1, Ncell.y), 0, 1.0)
add_obstacle(math.vec2.new(Ncell.x, Ncell.y+1), 0, 1.0)
add_obstacle(math.vec2.new(Ncell.x, Ncell.y-1), 0, 1.0)
foundobstacle = 1
}
})
#if(foundobstacle) {
#print_map(map)
#}
return foundobstacle
}
function getproxobs (m_curpos) {
foundobstacle = 0
update_curcell(m_curpos,1)
reduce(proximity,
function(key, value, acc) {
obs = math.vec2.add(math.vec2.newp(1.15, value.angle * math.pi / 180.0 + absolute_position.theta), cur_cell)
obs2 = math.vec2.add(math.vec2.newp(2.15, value.angle * math.pi / 180.0 + absolute_position.theta), cur_cell)
per = math.vec2.sub(obs,cur_cell)
obsr = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs)
obsr2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs2)
obsl = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs)
obsl2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs2)
if(value.value > IR_SENSOR_THRESHOLD) {
if(rmat(map,math.round(obs.x),math.round(obs.y))!=0) {
add_obstacle(obs, 0, 0.25)
add_obstacle(obs2, 0, 0.25)
add_obstacle(obsr, 0, 0.25)
add_obstacle(obsr2, 0, 0.25)
add_obstacle(obsl, 0, 0.25)
add_obstacle(obsl2, 0, 0.25)
foundobstacle = 1
}
} else if(rmat(map,math.round(obs.x),math.round(obs.y))!=1) {
remove_obstacle(obs, 0, 0.05)
foundobstacle = 1
}
return acc
}, math.vec2.new(0, 0))
#if(foundobstacle) {
# reduce(proximity,
# function(key, value, acc){
# log(value.value, ", ", value.angle)
# return acc
# }, math.vec2.new(0, 0))
# print_map(map)
#}
return foundobstacle
}
function check_path(m_path, goal_l, m_curpos, kh4) {
pathisblocked = 0
nb=goal_l
update_curcell(m_curpos,kh4)
Cvec = cur_cell
while(nb < m_path.nb_row and nb <= goal_l+1){
Nvec = getvec(m_path, nb)
if(kh4==0)
Nvec=vec_from_gps(Nvec.x,Nvec.y)
if(doesItIntersect(Cvec, Nvec)){
log("Obstacle in the way ! (", Cvec.x, "/", Cvec.y, "->", Nvec.x, "/", Nvec.y, ")")
pathisblocked = 1
}
Cvec=Nvec
nb = nb + 1
}
return pathisblocked
}
function RRTSTAR(GOAL,TOL) {
HEIGHT = map.nb_col-1
WIDTH = map.nb_row-1
RADIUS = 1.25 #TOL.x #map.nb_col/10.0 # to consider 2 points consecutive
goalBoundary = {.xmin=GOAL.x-TOL.x, .xmax=GOAL.x+TOL.x, .ymin=GOAL.y-TOL.y, .ymax=GOAL.y+TOL.y}
#table_print(goalBoundary)
arrayOfPoints = {.nb_col=2, .nb_row=1, .mat={.0=cur_cell.x,.1=cur_cell.y}}
Path = {.nb_col=2, .nb_row=1, .mat={.0=cur_cell.x,.1=cur_cell.y}}
numberOfPoints = 1
Q = {.nb_col=5,.nb_row=1,.mat={.0=cur_cell.x,.1=cur_cell.y,.2=0,.3=1,.4=0}}
goalReached = 0;
timeout = 0
##
# main search loop
##
while(goalReached == 0 and timeout < RRT_TIMEOUT) {
# Point generation only as grid cell centers
pt = math.vec2.new(math.round(HEIGHT * math.rng.uniform(1.0) + 1), math.round(WIDTH * math.rng.uniform(1.0) + 1))
#log("Point generated: ", pt.x, " ", pt.y)
pointList = findPointsInRadius(pt,Q,RADIUS);
# Find connection that provides the least cost to come
nbCount = 0;
minCounted = 999;
minCounter = 0;
if(pointList.nb_row!=0) {
#log("Found ", pointList.nb_row, " close point:", pointList.mat)
ipt=1
while(ipt<=pointList.nb_row){
pointNumber = {.nb_col=pointList.nb_col, .nb_row=1, .mat={}}
mat_copyrow(pointNumber,1,pointList,ipt)
# Follow the line to see if it intersects anything
intersects = doesItIntersect(pt,getvec(pointNumber,1));
#log("intersects1: ", intersects)
# If there is no intersection we need consider its connection
nbCount = nbCount + 1;
if(intersects != 1) {
#log(pointNumber, "do not intersect (",pointNumber.mat[3],")")
distance = math.vec2.length(math.vec2.sub(getvec(pointNumber,1),pt))+rmat(Q,pointNumber.mat[3],5)
if(distance < minCounted) {
minCounted = distance;
minCounter = nbCount;
}
}
ipt = ipt + 1
}
if(minCounter > 0) {
numberOfPoints = numberOfPoints + 1;
wmat(arrayOfPoints,numberOfPoints,1,pt.x)
wmat(arrayOfPoints,numberOfPoints,2,pt.y)
wmat(Q,numberOfPoints,1, pt.x)
wmat(Q,numberOfPoints,2, pt.y)
wmat(Q,numberOfPoints,3, rmat(pointList,minCounter,4));
wmat(Q,numberOfPoints,4, numberOfPoints)
wmat(Q,numberOfPoints,5, minCounted)
#log("added point to Q(", Q.nb_row, "): ", pt.x, " ", pt.y)
# Now check to see if any of the other points can be redirected
nbCount = 0;
ipt = 1
while(ipt<pointList.nb_row) {
pointNumber = {.nb_col=pointList.nb_col, .nb_row=1, .mat={}}
mat_copyrow(pointNumber,1,pointList,ipt)
# Follow the line to see if it intersects anything
intersects = doesItIntersect(pt,getvec(pointNumber,1));
#log("intersects2: ", intersects)
# If there is no intersection we need consider its connection
nbCount = nbCount + 1;
if(intersects != 1) {
# If the alternative path is shorter than change it
tmpdistance = rmat(Q,numberOfPoints,5)+math.vec2.length(math.vec2.sub(getvec(pointNumber,1),pt))
if(tmpdistance < rmat(Q,rmat(pointNumber,1,4),5)) {
wmat(Q,rmat(pointNumber,1,4),3, numberOfPoints)
wmat(Q,rmat(pointNumber,1,4),5, tmpdistance)
}
}
ipt = ipt + 1
}
# Check to see if this new point is within the goal
if(pt.x < goalBoundary.xmax and pt.x > goalBoundary.xmin and pt.y > goalBoundary.ymin and pt.y < goalBoundary.ymax)
goalReached = 1;
}
} else {
# Associate with the closest point
pointNum = findClosestPoint(pt,arrayOfPoints);
# Follow the line to see if it intersects anything
intersects = doesItIntersect(pt,getvec(arrayOfPoints,pointNum));
#log("intersects3 (", pointNum, "): ", intersects)
# If there is no intersection we need to add to the tree
if(intersects != 1) {
numberOfPoints = numberOfPoints + 1;
wmat(arrayOfPoints,numberOfPoints,1,pt.x)
wmat(arrayOfPoints,numberOfPoints,2,pt.y)
wmat(Q,numberOfPoints,1, pt.x)
wmat(Q,numberOfPoints,2, pt.y)
wmat(Q,numberOfPoints,3, pointNum);
wmat(Q,numberOfPoints,4, numberOfPoints)
wmat(Q,numberOfPoints,5, rmat(Q,pointNum,5)+math.vec2.length(math.vec2.sub(getvec(Q,pointNum),pt)))
#log("added point to Q(", Q.nb_row, "): ", pt.x, " ", pt.y)
# Check to see if this new point is within the goal
if(pt.x < goalBoundary.xmax and pt.x > goalBoundary.xmin and pt.y > goalBoundary.ymin and pt.y < goalBoundary.ymax)
goalReached = 1;
}
}
if(numberOfPoints % 100 == 0) {
log(numberOfPoints, " points processed. Still looking for goal.");
}
timeout = timeout + 1
}
if(goalReached){
log("Goal found(",numberOfPoints,")!")
Path = getPathGPS(Q,numberOfPoints)
print_pos(Path)
} else {
log("FAILED TO FIND A PATH!!!")
Path = nil
}
return Path
}
function findClosestPoint(point,aPt) {
# Go through each points and find the distances between them and the
# target point
distance = 999
pointNumber = -1
ifcp=1
while(ifcp<=aPt.nb_row) {
range = math.vec2.length(math.vec2.sub(point,getvec(aPt,ifcp)))
if(range < distance) {
distance = range;
pointNumber = ifcp;
}
ifcp = ifcp + 1
}
return pointNumber
}
# Find the closest point in the tree
function findPointsInRadius(point,q,r) {
counted = 0;
pointList = {.nb_col=q.nb_col, .nb_row=counted, .mat={}}
iir=1
while(iir <= q.nb_row) {
tmpvv = getvec(q,iir)
#log("FPiR :", point.x, " ", point.y," ", tmpvv.x," ", tmpvv.y)
distance = math.vec2.length(math.vec2.sub(getvec(q,iir),point))
if(distance < r) {
counted = counted+1;
pointList.nb_row=counted
mat_copyrow(pointList,counted,q,iir)
}
iir = iir + 1
}
return pointList
}
# check if the line between 2 point intersect an obstacle
function doesItIntersect(point,vector) {
#log("DiI :", point.x, " ", point.y," ", vector.x," ", vector.y)
dif = math.vec2.sub(point,vector)
distance = math.vec2.length(dif)
if(distance==0.0){
# Find what block we're in right now
xi = math.round(point.x) #+1
yi = math.round(point.y) #+1
if(xi!=cur_cell.x and yi!=cur_cell.y){
if(rmat(map,xi,yi) > 0.5)
return 1
else
return 0
} else
return 0
}
vec = math.vec2.scale(dif,1.0/distance)
pathstep = map.nb_col - 2
idii = 1.0
while(idii <= pathstep) {
range = distance*idii/pathstep
pos_chk = math.vec2.sub(point,math.vec2.scale(vec,range));
# Find what block we're in right now
xi = math.round(pos_chk.x) #+1
yi = math.round(pos_chk.y) #+1
#log("Grid :", pos_chk.x, " ", pos_chk.y," ", xi," ", yi, " R: ", range)
if(xi!=cur_cell.x and yi!=cur_cell.y){
if(xi < map.nb_col+1 and yi < map.nb_row+1 and xi > 0 and yi > 0) {
if(rmat(map,xi,yi) < 0.5) { # because obstacle use trust values
#log("Obstacle in the way(", xi, " ", yi, ": ", rmat(map,xi,yi), ")!")
return 1;
}
} else {
#log("Outside the map(", xi, " ", yi, ")! ", range, "/", distance, " : ", pos_chk.x, " ", pos_chk.y, " : ", vec.x, " ", vec.y)
return 1;
}
}
idii = idii + 1.0
}
#log("No intersect!")
return 0
}
function getPathGPS(Q,nb){
path={.nb_col=2, .nb_row=0, .mat={}}
npt=0
# get the path from the point list
while(nb!=1){
npt=npt+1
path.nb_row=npt
path.mat[(npt-1)*2]=rmat(Q,nb,1)
path.mat[(npt-1)*2+1]=rmat(Q,nb,2)
nb = rmat(Q,nb,3);
}
# re-order the list and make the path points absolute
pathR={.nb_col=2, .nb_row=path.nb_row, .mat={}}
while(npt>0){
tmpgoal = gps_from_vec(math.vec2.sub(getvec(path,npt),cur_cell))
pathR.mat[(path.nb_row-npt)*2]=tmpgoal.latitude
pathR.mat[(path.nb_row-npt)*2+1]=tmpgoal.longitude
npt = npt - 1
}
return pathR
}
# create a table with only the path's points in order
function getPath(Q,nb){
path={.nb_col=2, .nb_row=0, .mat={}}
npt=0
# log("get the path from the point list")
while(nb!=1){
npt=npt+1
path.nb_row=npt
path.mat[(npt-1)*2]=rmat(Q,nb,1)
path.mat[(npt-1)*2+1]=rmat(Q,nb,2)
nb = rmat(Q,nb,3);
}
# log("re-order the list")
# table_print(path.mat)
pathR={.nb_col=2, .nb_row=path.nb_row, .mat={}}
while(npt>0){
tmpgoal = getvec(path,npt)
pathR.mat[(path.nb_row-npt)*2]=tmpgoal.x
pathR.mat[(path.nb_row-npt)*2+1]=tmpgoal.y
npt = npt - 1
}
#log("Double-check path: ", check_path(pathR, 1, cur_cell, 1))
return pathR
}

250
buzz_scripts/include/uavstates.bzz
View File

@ -3,14 +3,22 @@
# FLIGHT-RELATED FUNCTIONS
#
########################################
TARGET_ALTITUDE = 5.0
UAVSTATE = "TURNEDOFF"
include "rrtstar.bzz"
function uav_initswarm(){
TARGET_ALTITUDE = 15.0 # m.
UAVSTATE = "TURNEDOFF"
PICTURE_WAIT = 20 # steps
GOTO_MAXVEL = 2 # m/steps
GOTO_MAXDIST = 150 # m.
GOTODIST_TOL = 0.5 # m.
GOTOANG_TOL = 0.1 # rad.
cur_goal_l = 0
rc_State = 0
homegps = {.lat=0, .long=0}
function uav_initswarm() {
s = swarm.create(1)
s.join()
statef=turnedoff
UAVSTATE = "TURNEDOFF"
}
function turnedoff() {
@ -26,38 +34,130 @@ 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, -1)
barrier_ready()
#statef=hexagon
}
else {
log("Altitude: ", TARGET_ALTITUDE)
} else {
log("Altitude: ", position.altitude)
neighbors.broadcast("cmd", 22)
uav_takeoff(TARGET_ALTITUDE)
}
}
}
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)
neighbors.broadcast("cmd", 21)
uav_land()
if(flight.status != 2 and flight.status != 3) {
barrier_set(ROBOTS,turnedoff,land, 21)
barrier_ready()
timeW=0
#barrier = nil
#statef=idle
}
}
function set_goto(transf) {
UAVSTATE = "GOTOGPS"
statef=function() {
gotoWP(transf)
}
if(rc_goto.id==id){
if(s!=nil){
if(s.in())
s.leave()
}
} else {
neighbors.broadcast("cmd", 16)
neighbors.broadcast("gt", rc_goto.id+rc_goto.longitude+rc_goto.latitude)
}
}
ptime=0
function picture() {
statef=picture
UAVSTATE="PICTURE"
uav_setgimbal(0.0, 0.0, -90.0, 20.0)
if(ptime==PICTURE_WAIT/2) { # wait for the drone to stabilize
uav_takepicture()
} else if(ptime>=PICTURE_WAIT) { # wait for the picture
statef=action
ptime=0
}
ptime=ptime+1
}
#
# still requires to be tuned, replaning takes too much time..
# DS 23/11/2017
function gotoWPRRT(transf) {
rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude)
homegps.lat = position.latitude
homegps.long = position.longitude
m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long),-1)
print(" has to move ", math.vec2.length(rc_goal), "from ", m_pos.x, " ", m_pos.y)
if(math.vec2.length(rc_goal)>GOTO_MAXDIST*5)
log("Sorry this is too far.")
else {
if(Path==nil){
# create the map
if(map==nil)
UAVinit_map(rc_goal)
# add proximity sensor and neighbor obstacles to the map
while(Path==nil) {
#getproxobs(m_pos)
UAVgetneiobs (m_pos)
Path = UAVpathPlanner(rc_goal, m_pos)
}
cur_goal_l = 1
} else if(cur_goal_l<=Path.nb_row) {
cur_gps=getvec(Path,cur_goal_l) #x=latitude, y=longitude
cur_pt=vec_from_gps(cur_gps.x,cur_gps.y)
print(" heading to ", cur_pt.x,cur_pt.y)
if(math.vec2.length(cur_pt)>GOTODIST_TOL) {
m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long),-1)
UAVgetneiobs(m_pos)
if(check_path(Path,cur_goal_l,m_pos,0)) {
uav_moveto(0.0, 0.0, rc_goto.altitude-position.altitude)
Path = nil
rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude)
while(Path == nil) {
#getproxobs(m_pos)
UAVgetneiobs (m_pos)
Path = UAVpathPlanner(rc_goal, m_pos)
}
cur_goal_l = 1
}
cur_pt = math.vec2.scale(cur_pt, GOTO_MAXVEL/math.vec2.length(cur_pt))
uav_moveto(cur_pt.x, cur_pt.y, rc_goto.altitude-position.altitude)
}
else
cur_goal_l = cur_goal_l + 1
} else {
Path = nil
transf()
}
}
}
function gotoWP(transf) {
m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude)
print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation))
if(math.vec2.length(m_navigation)>GOTO_MAXDIST)
log("Sorry this is too far.")
else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity
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) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination
transf()
else
uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude)
}
function follow() {
if(size(targets)>0) {
UAVSTATE = "FOLLOW"
@ -67,7 +167,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 +190,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 = "GOTOGPS"
statef = goto
} else if(flight.rc_cmd==400) {
flight.rc_cmd=0
uav_arm()
@ -106,21 +203,90 @@ function uav_rccmd() {
} else if (flight.rc_cmd==666){
flight.rc_cmd=0
stattab_send()
} else if (flight.rc_cmd==900){
flight.rc_cmd=0
rc_State = 0
neighbors.broadcast("cmd", 900)
} else if (flight.rc_cmd==901){
flight.rc_cmd=0
rc_State = 1
neighbors.broadcast("cmd", 901)
} else if (flight.rc_cmd==902){
flight.rc_cmd=0
rc_State = 2
neighbors.broadcast("cmd", 902)
} else if (flight.rc_cmd==903){
flight.rc_cmd=0
rc_State = 3
neighbors.broadcast("cmd", 903)
}
}
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, ") #", rid, "(", UAVSTATE, ")")
if(value==22 and UAVSTATE!="TAKEOFF" and UAVSTATE!="BARRIERWAIT") {
statef=takeoff
UAVSTATE = "TAKEOFF"
} else if(value==21 and UAVSTATE!="LAND" and UAVSTATE!="BARRIERWAIT") {
statef=land
UAVSTATE = "LAND"
} else if(value==400 and UAVSTATE=="TURNEDOFF") {
uav_arm()
} else if(value==401 and UAVSTATE=="TURNEDOFF"){
uav_disarm()
} else if(value==900){
rc_State = 0
} else if(value==901){
rc_State = 1
} else if(value==902){
rc_State = 2
} else if(value==903){
rc_State = 3
} else if(value==16 and UAVSTATE=="IDLE"){
# 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 vec_from_gps(lat, lon) {
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);
#Lgoal.range = math.sqrt(ned_x*ned_x+ned_y*ned_y);
#Lgoal.bearing = LimitAngle(math.atan(ned_y,ned_x));
return math.vec2.new(ned_x,ned_y)
}
function gps_from_vec(vec) {
Lgoal = {.latitude=0, .longitude=0}
Vrange = math.vec2.length(vec)
Vbearing = LimitAngle(math.atan(vec.y, vec.x))
# print("rb2gps: ",Vrange,Vbearing,position.latitude,position.longitude)
latR = position.latitude*math.pi/180.0;
lonR = position.longitude*math.pi/180.0;
target_lat = math.asin(math.sin(latR) * math.cos(Vrange/6371000.0) + math.cos(latR) * math.sin(Vrange/6371000.0) * math.cos(Vbearing));
target_lon = lonR + math.atan(math.sin(Vbearing) * math.sin(Vrange/6371000.0) * math.cos(latR), math.cos(Vrange/6371000.0) - math.sin(latR) * math.sin(target_lat));
Lgoal.latitude = target_lat*180.0/math.pi;
Lgoal.longitude = target_lon*180.0/math.pi;
#d_lat = (vec.x / 6371000.0)*180.0/math.pi;
#goal.latitude = d_lat + position.latitude;
#d_lon = (vec.y / (6371000.0 * math.cos(goal.latitude*math.pi/180.0)))*180.0/math.pi;
#goal.longitude = d_lon + position.longitude;
return Lgoal
}

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

2
buzz_scripts/include/vec2.bzz
View File

@ -41,7 +41,7 @@ math.vec2.length = function(v) {
# RETURN: The angle of the vector.
#
math.vec2.angle = function(v) {
return math.atan2(v.y, v.x)
return math.atan(v.y, v.x)
}
#

27
buzz_scripts/include/vstigenv.bzz
View File

@ -1,16 +1,29 @@
STATUS_VSTIG = 10
GROUND_VSTIG = 11
########################################
#
# FLEET V.STIGMERGY-RELATED FUNCTIONS
#
########################################
#
# Constants
#
STATUS_VSTIG = 20
GROUND_VSTIG = 21
HIGHEST_ROBOTID = 14
WAIT4STEP = 10
v_status = {}
v_ground = {}
#
# Init var
#
var v_status = {}
var v_ground = {}
b_updating = 0
counter=WAIT4STEP
function uav_initstig() {
v_status = stigmergy.create(STATUS_VSTIG)
v_ground = stigmergy.create(GROUND_VSTIG)
}
counter=WAIT4STEP
function uav_updatestig() {
# TODO: Push values on update only.
if(counter<=0) {
@ -78,7 +91,7 @@ function stattab_print() {
if(v_status.size()>0) {
if(b_updating==0) {
u=0
while(u<8){
while(u<HIGHEST_ROBOTID){
tab = v_status.get(u)
if(tab!=nil)
unpackstatus(tab)
@ -92,7 +105,7 @@ function stattab_send() {
if(v_status.size()>0) {
if(b_updating==0) {
u=0
while(u<8){
while(u<HIGHEST_ROBOTID){
tab = v_status.get(u)
if(tab!=nil){
recv_value=tab

35
buzz_scripts/startswarmsimER.launch Normal file
View File

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

13
buzz_scripts/testalone.bzz
View File

@ -3,21 +3,28 @@ include "update.bzz"
include "barrier.bzz" # don't use a stigmergy id=11 with this header.
include "uavstates.bzz" # require an 'action' function to be defined here.
include "vstigenv.bzz"
include "rrtstar.bzz"
function action() {
statef=action
# test moveto cmd dx, dy
# uav_moveto(0.5, 0.5)
uav_storegoal(45.5085,-73.1531935979886,5.0)
set_goto(picture)
}
# 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.

35
buzz_scripts/testaloneWP.bzz Normal file
View File

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

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/buzz_utility.h
View File

@ -23,8 +23,8 @@ struct pos_struct
typedef struct pos_struct Pos_struct;
struct rb_struct
{
double r,b,la,lo;
rb_struct(double la, double lo, double r,double b):la(la),lo(lo),r(r),b(b){};
double r,b,latitude,longitude,altitude;
rb_struct(double la, double lo, double al, double r,double b):latitude(la),longitude(lo),altitude(al),r(r),b(b){};
rb_struct(){}
};
typedef struct rb_struct RB_struct;
@ -77,4 +77,6 @@ int get_robotid();
buzzvm_t get_vm();
void set_robot_var(int ROBOTS);
int get_inmsg_size();
}

21
include/buzzuav_closures.h
View File

@ -9,8 +9,9 @@
#include "ros/ros.h"
#include "buzz_utility.h"
#define EARTH_RADIUS (double) 6371000.0
#define DEG2RAD(DEG) ((DEG)*((M_PI)/(180.0)))
#define EARTH_RADIUS (double) 6371000.0
#define DEG2RAD(DEG) (double) ((DEG)*((M_PI)/(180.0)))
#define RAD2DEG(RAD) (double) ((RAD)*((180.0)/(M_PI)))
namespace buzzuav_closures{
typedef enum {
@ -22,6 +23,8 @@ namespace buzzuav_closures{
COMMAND_DISARM,
COMMAND_GOTO,
COMMAND_MOVETO,
COMMAND_PICTURE,
COMMAND_GIMBAL,
} Custom_CommandCode;
/*
@ -30,19 +33,25 @@ namespace buzzuav_closures{
* The command to use in Buzz is buzzros_print takes any available datatype in Buzz
*/
int buzzros_print(buzzvm_t vm);
void setWPlist(std::string path);
/*
* buzzuav_goto(latitude,longitude,altitude) function in Buzz
* commands the UAV to go to a position supplied
*/
int buzz_floor(buzzvm_t vm);
int buzzuav_moveto(buzzvm_t vm);
int buzzuav_goto(buzzvm_t vm);
int buzzuav_storegoal(buzzvm_t vm);
int buzzuav_setgimbal(buzzvm_t vm);
void parse_gpslist();
int buzzuav_takepicture(buzzvm_t vm);
/* Returns the current command from local variable*/
int getcmd();
/*Sets goto position */
void set_goto(double pos[]);
/*Sets goto position from rc client*/
void rc_set_goto(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 roll, float pitch, float t);
/*sets rc requested command */
void rc_call(int rc_cmd);
/* sets the battery state */
@ -56,6 +65,8 @@ void set_api_rssi(float value);
void set_currentpos(double latitude, double longitude, double altitude);
/*retuns the current go to position */
double* getgoto();
std::string getuavstate();
float* getgimbal();
/* updates flight status*/
void flight_status_update(uint8_t state);
/* Update neighbors table */

23
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;
@ -54,6 +55,8 @@ public:
//void RosControllerInit();
void RosControllerRun();
static const string CAPTURE_SRV_DEFAULT_NAME;
private:
struct num_robot_count
{
@ -68,8 +71,8 @@ private:
double longitude=0.0;
double latitude=0.0;
float altitude=0.0;
}; typedef struct gps GPS ; // not useful in cpp
}; typedef struct gps GPS ;
GPS target, home, cur_pos;
double cur_rel_altitude;
@ -91,7 +94,19 @@ private:
/*tmp to be corrected*/
uint8_t no_cnt=0;
uint8_t old_val=0 ;
std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,obstacles_topic,stand_by,xbeesrv_name, setpoint_name;
std::string bzzfile_name;
std::string fcclient_name;
std::string armclient;
std::string modeclient;
std::string rcservice_name;
std::string bcfname,dbgfname;
std::string out_payload;
std::string in_payload;
std::string obstacles_topic;
std::string stand_by;
std::string xbeesrv_name;
std::string capture_srv_name;
std::string setpoint_name;
std::string stream_client_name;
std::string relative_altitude_sub_name;
std::string setpoint_nonraw;
@ -101,6 +116,7 @@ private:
Num_robot_count count_robots;
ros::ServiceClient mav_client;
ros::ServiceClient xbeestatus_srv;
ros::ServiceClient capture_srv;
ros::Publisher payload_pub;
ros::Publisher MPpayload_pub;
ros::Publisher neigh_pos_pub;
@ -110,6 +126,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>

7
misc/cmdlinectr.sh
View File

@ -31,5 +31,10 @@ function stoprobot {
}
function updaterobot {
# rosrun robot_upstart install --logdir ~/ROS_WS/log/ robot_upstart/launch/m100buzzynocam.launch
rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch/m100buzzy.launch
rosrun robot_upstart install --logdir /media/key/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100buzzy.launch
# rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100TXbuzzy.launch
}
function uavstate {
let "a = $1 + 900"
rosservice call robot0/buzzcmd 0 $a 0 0 0 0 0 0 0 0
}

52
src/buzz_utility.cpp
View File

@ -17,7 +17,6 @@ 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 MAX_MSG_SIZE = 10000; // Maximum Msg size for sending update packets
static uint8_t Robot_id = 0;
static std::vector<uint8_t*> IN_MSG;
@ -229,7 +228,7 @@ void in_message_process(){
buzzmsg_payload_t m = buzzoutmsg_queue_first(VM);
/* Make sure the next message makes the data buffer with buzz messages to be less than MAX SIZE Bytes */
//ROS_INFO("read size : %i", (int)(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)));
if((uint32_t)(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)) > MSG_SIZE) {
if((uint32_t)(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)) > MAX_MSG_SIZE) {
buzzmsg_payload_destroy(&m);
break;
}
@ -304,8 +303,14 @@ 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_setgimbal", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_setgimbal));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takepicture", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_takepicture));
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 +354,13 @@ 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_setgimbal", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takepicture", 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 +498,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 */
@ -498,6 +509,11 @@ int create_stig_tables() {
return 0;
}
// Initialize UAVSTATE variable
buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1));
buzzvm_pushs(VM, buzzvm_string_register(VM, "TURNEDOFF", 1));
buzzvm_gstore(VM);
/* Create vstig tables
if(create_stig_tables() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
@ -550,16 +566,22 @@ 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;
}
// Initialize UAVSTATE variable
buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1));
buzzvm_pushs(VM, buzzvm_string_register(VM, "TURNEDOFF", 1));
buzzvm_gstore(VM);
/* Create vstig tables
if(create_stig_tables() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
@ -606,16 +628,22 @@ 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;
}
// Initialize UAVSTATE variable
buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1));
buzzvm_pushs(VM, buzzvm_string_register(VM, "TURNEDOFF", 1));
buzzvm_gstore(VM);
/* Create vstig tables
if(create_stig_tables() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
@ -786,4 +814,8 @@ int create_stig_tables() {
buzzvm_pushi(VM, ROBOTS);
buzzvm_gstore(VM);
}
int get_inmsg_size(){
return IN_MSG.size();
}
}

377
src/buzzuav_closures.cpp
View File

@ -16,82 +16,93 @@ 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[4];
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 float raw_packet_loss = 0.0;
static int filtered_packet_loss = 0;
static float api_rssi = 0.0;
string WPlistname = "";
std::map< int, buzz_utility::RB_struct> targets_map;
std::map< int, buzz_utility::RB_struct> wplist_map;
std::map< int, buzz_utility::Pos_struct> neighbors_map;
std::map< int, buzz_utility::neighbors_status> neighbors_status_map;
/****************************************/
/****************************************/
int buzzros_print(buzzvm_t vm) {
int i;
char buffer [50] = "";
sprintf(buffer,"%s [%i]", buffer, (int)buzz_utility::get_robotid());
for(i = 1; i < buzzdarray_size(vm->lsyms->syms); ++i) {
buzzvm_lload(vm, i);
buzzobj_t o = buzzvm_stack_at(vm, 1);
buzzvm_pop(vm);
switch(o->o.type) {
case BUZZTYPE_NIL:
sprintf(buffer,"%s BUZZ - [nil]", buffer);
break;
case BUZZTYPE_INT:
sprintf(buffer,"%s %d", buffer, o->i.value);
//fprintf(stdout, "%d", o->i.value);
break;
case BUZZTYPE_FLOAT:
sprintf(buffer,"%s %f", buffer, o->f.value);
break;
case BUZZTYPE_TABLE:
sprintf(buffer,"%s [table with %d elems]", buffer, (buzzdict_size(o->t.value)));
break;
case BUZZTYPE_CLOSURE:
if(o->c.value.isnative)
sprintf(buffer,"%s [n-closure @%d]", buffer, o->c.value.ref);
else
sprintf(buffer,"%s [c-closure @%d]", buffer, o->c.value.ref);
break;
case BUZZTYPE_STRING:
sprintf(buffer,"%s %s", buffer, o->s.value.str);
break;
case BUZZTYPE_USERDATA:
sprintf(buffer,"%s [userdata @%p]", buffer, o->u.value);
break;
default:
break;
}
}
ROS_INFO(buffer);
//fprintf(stdout, "\n");
return buzzvm_ret0(vm);
}
int buzzros_print(buzzvm_t vm)
{
std::ostringstream buffer (std::ostringstream::ate);
buffer << buzz_utility::get_robotid();
for (uint32_t index = 1; index < buzzdarray_size(vm->lsyms->syms); ++index)
{
buzzvm_lload(vm, index);
buzzobj_t o = buzzvm_stack_at(vm, 1);
buzzvm_pop(vm);
switch (o->o.type)
{
case BUZZTYPE_NIL:
buffer << " BUZZ - [nil]";
break;
case BUZZTYPE_INT:
buffer << " " << o->i.value;
break;
case BUZZTYPE_FLOAT:
buffer << " " << o->f.value;
break;
case BUZZTYPE_TABLE:
buffer << " [table with " << buzzdict_size(o->t.value) << " elems]";
break;
case BUZZTYPE_CLOSURE:
if (o->c.value.isnative)
{
buffer << " [n-closure @" << o->c.value.ref << "]";
}
else
{
buffer << " [c-closure @" << o->c.value.ref << "]";
}
break;
case BUZZTYPE_STRING:
buffer << " " << o->s.value.str;
break;
case BUZZTYPE_USERDATA:
buffer << " [userdata @" << o->u.value << "]";
break;
default:
break;
}
}
ROS_INFO("%s",buffer.str().c_str());
return buzzvm_ret0(vm);
}
void setWPlist(string path){
WPlistname = path + "include/graphs/waypointlist.csv";
}
/*----------------------------------------/
/ Compute GPS destination from current position and desired Range and Bearing
/----------------------------------------*/
void gps_from_rb(double range, double bearing, double out[3]) {
double lat = cur_pos[0]*M_PI/180.0;
double lon = cur_pos[1]*M_PI/180.0;
double lat = RAD2DEG(cur_pos[0]);
double lon = RAD2DEG(cur_pos[1]);
out[0] = asin(sin(lat) * cos(range/EARTH_RADIUS) + cos(lat) * sin(range/EARTH_RADIUS) * cos(bearing));
out[1] = lon + atan2(sin(bearing) * sin(range/EARTH_RADIUS) * cos(lat), cos(range/EARTH_RADIUS) - sin(lat)*sin(out[0]));
out[0] = out[0]*180.0/M_PI;
out[1] = out[1]*180.0/M_PI;
out[0] = RAD2DEG(out[0]);
out[1] = RAD2DEG(out[1]);
out[2] = height; //constant height.
}
@ -103,10 +114,10 @@ namespace buzzuav_closures{
}
void rb_from_gps(double nei[], double out[], double cur[]){
double d_lon = nei[1] - cur[1];
double d_lat = nei[0] - cur[0];
double ned_x = DEG2RAD(d_lat) * EARTH_RADIUS;
double ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0]));
double d_lon = nei[1] - cur[1];
double d_lat = nei[0] - cur[0];
double ned_x = DEG2RAD(d_lat) * EARTH_RADIUS;
double ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0]));
out[0] = sqrt(ned_x*ned_x+ned_y*ned_y);
out[1] = constrainAngle(atan2(ned_y,ned_x));
out[2] = 0.0;
@ -117,31 +128,78 @@ namespace buzzuav_closures{
double hcpos2[4] = {45.564729, -73.562060, 45.564362, -73.562958};
double hcpos3[4] = {45.564969, -73.562838, 45.564636, -73.563677};
void parse_gpslist()
{
// Open file:
ROS_INFO("WP list file: %s", WPlistname.c_str());
std::ifstream fin(WPlistname.c_str()); // Open in text-mode.
// Opening may fail, always check.
if (!fin) {
ROS_ERROR("GPS list parser, could not open file.");
return;
}
// Prepare a C-string buffer to be used when reading lines.
const int MAX_LINE_LENGTH = 1024; // Choose this large enough for your need.
char buffer[MAX_LINE_LENGTH] = {};
const char * DELIMS = "\t ,"; // Tab, space or comma.
// Read the file and load the data:
double lat, lon;
int alt, tilt, tid;
buzz_utility::RB_struct RB_arr;
// Read one line at a time.
while ( fin.getline(buffer, MAX_LINE_LENGTH) ) {
// Extract the tokens:
tid = atoi(strtok( buffer, DELIMS ));
lon = atof(strtok( NULL, DELIMS ));
lat = atof(strtok( NULL, DELIMS ));
alt = atoi(strtok( NULL, DELIMS ));
tilt = atoi(strtok( NULL, DELIMS ));
//ROS_INFO("%.6f, %.6f, %i %i %i",lat, lon, alt, tilt, tid);
RB_arr.latitude=lat;
RB_arr.longitude=lon;
RB_arr.altitude=alt;
// Insert elements.
map< int, buzz_utility::RB_struct >::iterator it = wplist_map.find(tid);
if(it!=wplist_map.end())
wplist_map.erase(it);
wplist_map.insert(make_pair(tid, RB_arr));
}
ROS_INFO("----->Saved %i waypoints.", wplist_map.size());
// Close the file:
fin.close();
}
/*----------------------------------------/
/ 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) {
@ -156,27 +214,27 @@ namespace buzzuav_closures{
double rb[3], tmp[3];
map< int, buzz_utility::RB_struct >::iterator it;
for (it=targets_map.begin(); it!=targets_map.end(); ++it){
tmp[0]=(it->second).la;tmp[1]=(it->second).lo;tmp[2]=height;
rb_from_gps(tmp, rb, cur_pos);
ROS_WARN("----------Pushing target id %i (%f,%f)", rb[0], rb[1]);
tmp[0]=(it->second).latitude; tmp[1]=(it->second).longitude; tmp[2]=height;
rb_from_gps(tmp, rb, cur_pos);
//ROS_WARN("----------Pushing target id %i (%f,%f)", it->first, rb[0], rb[1]);
buzzvm_push(vm, targettbl);
/* When we get here, the "targets" table is on top of the stack */
// When we get here, the "targets" table is on top of the stack
//ROS_INFO("Buzz_utility will save user %i.", it->first);
/* Push user id */
// Push user id
buzzvm_pushi(vm, it->first);
/* Create entry table */
// Create entry table
buzzobj_t entry = buzzheap_newobj(vm->heap, BUZZTYPE_TABLE);
/* Insert range */
// Insert range
buzzvm_push(vm, entry);
buzzvm_pushs(vm, buzzvm_string_register(vm, "range", 1));
buzzvm_pushf(vm, rb[0]);
buzzvm_tput(vm);
/* Insert longitude */
// Insert longitude
buzzvm_push(vm, entry);
buzzvm_pushs(vm, buzzvm_string_register(vm, "bearing", 1));
buzzvm_pushf(vm, rb[1]);
buzzvm_tput(vm);
/* Save entry into data table */
// Save entry into data table
buzzvm_push(vm, entry);
buzzvm_tput(vm);
}
@ -204,8 +262,9 @@ namespace buzzuav_closures{
if(fabs(rb[0])<100.0) {
//printf("\tGot new user from bzz stig: %i - %f, %f\n", uid, rb[0], rb[1]);
buzz_utility::RB_struct RB_arr;
RB_arr.la=tmp[0];
RB_arr.lo=tmp[1];
RB_arr.latitude=tmp[0];
RB_arr.longitude=tmp[1];
RB_arr.altitude=tmp[2];
RB_arr.r=rb[0];
RB_arr.b=rb[1];
map< int, buzz_utility::RB_struct >::iterator it = targets_map.find(uid);
@ -215,30 +274,30 @@ namespace buzzuav_closures{
//ROS_INFO("Buzz_utility got updated/new user: %i (%f,%f,%f)", id, latitude, longitude, altitude);
return vm->state;
} else
printf(" ---------- Target too far %f\n",rb[0]);
ROS_WARN(" ---------- Target too far %f",rb[0]);
return 0;
}
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));
@ -247,7 +306,7 @@ namespace buzzuav_closures{
mavros_msgs::Mavlink get_status(){
mavros_msgs::Mavlink payload_out;
map< int, buzz_utility::neighbors_status >::iterator it;
map< int, buzz_utility::neighbors_status >::iterator it;
for (it= neighbors_status_map.begin(); it!= neighbors_status_map.end(); ++it){
payload_out.payload64.push_back(it->first);
payload_out.payload64.push_back(it->second.gps_strenght);
@ -263,15 +322,69 @@ namespace buzzuav_closures{
return payload_out;
}
/*----------------------------------------/
/ Buzz closure to go directly to a GPS destination from the Mission Planner
/ Buzz closure to take a picture here.
/----------------------------------------*/
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;
return buzzvm_ret0(vm);
int buzzuav_takepicture(buzzvm_t vm) {
//cur_cmd = mavros_msgs::CommandCode::CMD_DO_SET_CAM_TRIGG_DIST;
buzz_cmd = COMMAND_PICTURE;
return buzzvm_ret0(vm);
}
/*----------------------------------------/
/ Buzz closure to change locally the gimbal orientation
/----------------------------------------*/
int buzzuav_setgimbal(buzzvm_t vm) {
buzzvm_lnum_assert(vm, 4);
buzzvm_lload(vm, 1); // time
buzzvm_lload(vm, 2); // pitch
buzzvm_lload(vm, 3); // roll
buzzvm_lload(vm, 4); // yaw
buzzvm_type_assert(vm, 4, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
rc_gimbal[0] = buzzvm_stack_at(vm, 4)->f.value;
rc_gimbal[1] = buzzvm_stack_at(vm, 3)->f.value;
rc_gimbal[2] = buzzvm_stack_at(vm, 2)->f.value;
rc_gimbal[3] = buzzvm_stack_at(vm, 1)->f.value;
ROS_WARN("Set RC_GIMBAL ---- %f %f %f (%f)",rc_gimbal[0],rc_gimbal[1],rc_gimbal[2],rc_gimbal[3]);
buzz_cmd = COMMAND_GIMBAL;
return buzzvm_ret0(vm);
}
/*----------------------------------------/
/ Buzz closure to store locally a GPS destination from the fleet
/----------------------------------------*/
int buzzuav_storegoal(buzzvm_t vm) {
buzzvm_lnum_assert(vm, 3);
buzzvm_lload(vm, 1); // altitude
buzzvm_lload(vm, 2); // longitude
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);
double goal[3];
goal[0] = buzzvm_stack_at(vm, 3)->f.value;
goal[1] = buzzvm_stack_at(vm, 2)->f.value;
goal[2] = buzzvm_stack_at(vm, 1)->f.value;
if(goal[0]==-1 && goal[1]==-1 && goal[2]==-1){
if(wplist_map.size()<=0)
parse_gpslist();
goal[0] = wplist_map.begin()->second.latitude;
goal[1] = wplist_map.begin()->second.longitude;
goal[2] = wplist_map.begin()->second.altitude;
wplist_map.erase(wplist_map.begin()->first);
}
double rb[3];
rb_from_gps(goal, rb, cur_pos);
if(fabs(rb[0])<150.0)
rc_set_goto((int)buzz_utility::get_robotid(), goal[0], goal[1], goal[2]);
ROS_WARN("Set RC_GOTO ---- %f %f %f (%f %f, %f %f)",goal[0],goal[1],goal[2],cur_pos[0],cur_pos[1],rb[0],rb[1]);
return buzzvm_ret0(vm);
}
/*----------------------------------------/
@ -327,6 +440,20 @@ namespace buzzuav_closures{
return goto_pos;
}
float* getgimbal() {
return rc_gimbal;
}
string getuavstate() {
static buzzvm_t VM = buzz_utility::get_vm();
std::stringstream state_buff;
buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE",1));
buzzvm_gload(VM);
buzzobj_t uav_state = buzzvm_stack_at(VM, 1);
buzzvm_pop(VM);
return uav_state->s.value.str;
}
int getcmd() {
return cur_cmd;
}
@ -344,12 +471,24 @@ 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 roll, float pitch, float t) {
rc_id = id;
rc_gimbal[0] = yaw;
rc_gimbal[1] = roll;
rc_gimbal[2] = pitch;
rc_gimbal[3] = t;
}
void rc_call(int rc_cmd_in) {
rc_cmd = rc_cmd_in;
}
@ -517,6 +656,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);

511
src/roscontroller.cpp
View File

@ -2,6 +2,9 @@
#include <thread>
namespace rosbzz_node {
const string roscontroller::CAPTURE_SRV_DEFAULT_NAME = "/image_sender/capture_image";
const bool debug = false;
/*---------------
/Constructor
---------------*/
@ -17,6 +20,7 @@ roscontroller::roscontroller(ros::NodeHandle &n_c, ros::NodeHandle &n_c_priv)
bcfname = fname + ".bo";
dbgfname = fname + ".bdb";
set_bzz_file(bzzfile_name.c_str());
buzzuav_closures::setWPlist(bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/");
/*Initialize variables*/
// Solo things
SetMode("LOITER", 0);
@ -46,7 +50,14 @@ roscontroller::roscontroller(ros::NodeHandle &n_c, ros::NodeHandle &n_c_priv)
}
std::string path =
bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
path += "Update.log";
path = path.substr(0, bzzfile_name.find_last_of("\\/"))+"/log/";
std::string folder_check="mkdir -p "+path;
system(folder_check.c_str());
for(int i=5;i>0;i--){
rename((path +"logger_"+ std::to_string((uint8_t)robot_id)+"_"+std::to_string(i-1)+".log").c_str(),
(path +"logger_"+ std::to_string((uint8_t)robot_id)+"_"+std::to_string(i)+".log").c_str());
}
path += "logger_"+std::to_string(robot_id)+"_0.log";
log.open(path.c_str(), std::ios_base::trunc | std::ios_base::out);
}
@ -185,9 +196,13 @@ void roscontroller::RosControllerRun()
/* Set the Buzz bytecode */
if (buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(),
robot_id)) {
int packet_loss_tmp,time_step=0;
double cur_packet_loss=0;
ROS_INFO("[%i] Bytecode file found and set", robot_id);
std::string standby_bo = Compile_bzz(stand_by) + ".bo";
init_update_monitor(bcfname.c_str(), standby_bo.c_str());
//init_update_monitor(bcfname.c_str(), standby_bo.c_str());
/*loop rate of ros*/
ros::Rate loop_rate(BUZZRATE);
///////////////////////////////////////////////////////
// MAIN LOOP
//////////////////////////////////////////////////////
@ -195,11 +210,43 @@ void roscontroller::RosControllerRun()
while (ros::ok() && !buzz_utility::buzz_script_done()) {
/*Update neighbors position inside Buzz*/
// buzz_closure::neighbour_pos_callback(neighbours_pos_map);
/*log ROS Time stamp, pos (lat,long,alt), Swarm size, no. of neigh,
neigh pos, RSSI val, Packet loss, filtered packet loss*/
log<<ros::Time::now()<<",";
log<<cur_pos.latitude << "," << cur_pos.longitude << ","
<< cur_pos.altitude << ",";
log << (int)no_of_robots<<",";
map<int, buzz_utility::Pos_struct>::iterator it =
neighbours_pos_map.begin();
log << neighbours_pos_map.size()<< ",";
for (; it != neighbours_pos_map.end(); ++it)
{
log << (double)it->second.x << "," << (double)it->second.y
<< "," << (double)it->second.z << ",";
}
const uint8_t shrt_id= 0xFF;
float result;
/*Neighbours of the robot published with id in respective topic*/
neighbours_pos_publisher();
send_MPpayload();
/*Check updater state and step code*/
update_routine(bcfname.c_str(), dbgfname.c_str());
// update_routine(bcfname.c_str(), dbgfname.c_str());
if(time_step==BUZZRATE){
time_step=0;
cur_packet_loss= 1 -( (double)packet_loss_tmp/(BUZZRATE*( (int)no_of_robots-1 )) );
packet_loss_tmp=0;
if(cur_packet_loss<0) cur_packet_loss=0;
else if (cur_packet_loss>1) cur_packet_loss=1;
}
else{
packet_loss_tmp+=(int)buzz_utility::get_inmsg_size();
time_step++;
}
if(debug) ROS_WARN("CURRENT PACKET DROP : %f ",cur_packet_loss);
/*Log In Msg queue size*/
log<<(int)buzz_utility::get_inmsg_size()<<",";
/*Step buzz script */
buzz_utility::buzz_script_step();
/*Prepare messages and publish them in respective topic*/
@ -207,13 +254,13 @@ void roscontroller::RosControllerRun()
/*call flight controler service to set command long*/
flight_controller_service_call();
/*Set multi message available after update*/
if (get_update_status())
{
set_read_update_status();
//if (get_update_status())
//{
/* set_read_update_status();
multi_msg = true;
log << cur_pos.latitude << "," << cur_pos.longitude << ","
<< cur_pos.altitude << ",";
collect_data(log);
collect_data(log);
map<int, buzz_utility::Pos_struct>::iterator it =
neighbours_pos_map.begin();
log << "," << neighbours_pos_map.size();
@ -222,26 +269,29 @@ void roscontroller::RosControllerRun()
log << "," << (double)it->second.x << "," << (double)it->second.y
<< "," << (double)it->second.z;
}
log << std::endl;
}
log << std::endl;*/
//}
/*Set ROBOTS variable for barrier in .bzz from neighbours count*/
// no_of_robots=get_number_of_robots();
get_number_of_robots();
buzz_utility::set_robot_var(no_of_robots);
/*Retrive the state of the graph and uav and log*/
std::stringstream state_buff;
state_buff << buzzuav_closures::getuavstate();
log<<state_buff.str()<<std::endl;
// if(neighbours_pos_map.size() >0) no_of_robots
// =neighbours_pos_map.size()+1;
// buzz_utility::set_robot_var(no_of_robots);
/*Set no of robots for updates TODO only when not updating*/
// if(multi_msg)
updates_set_robots(no_of_robots);
// ROS_INFO("ROBOTS: %i , acutal :
// %i",(int)no_of_robots,(int)buzzdict_size(buzz_utility::get_vm()->swarmmembers)+1);
get_xbee_status();
//updates_set_robots(no_of_robots);
//get_xbee_status(); // commented out because it may slow down the node too much, to be tested
/*run once*/
ros::spinOnce();
/*loop rate of ros*/
ros::Rate loop_rate(BUZZRATE);
loop_rate.sleep();
// make sure to sleep for the remainder of our cycle time
if (loop_rate.cycleTime() > ros::Duration(1.0 / (float)BUZZRATE))
ROS_WARN("ROSBuzz loop missed its desired rate of %iHz... the loop actually took %.4f seconds", BUZZRATE, loop_rate.cycleTime().toSec());
if (fcu_timeout <= 0)
buzzuav_closures::rc_call(mavros_msgs::CommandCode::NAV_LAND);
else
@ -266,7 +316,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 +335,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,21 +349,26 @@ 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;
if(!n_c.getParam("capture_image_srv", capture_srv_name))
{
capture_srv_name = CAPTURE_SRV_DEFAULT_NAME;
}
GetSubscriptionParameters(n_c);
// initialize topics to null?
@ -325,70 +379,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 +460,29 @@ 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);
capture_srv = n_c.serviceClient<mavros_msgs::CommandBool>(capture_srv_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 +495,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 +515,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());
@ -589,12 +621,15 @@ void roscontroller::prepare_msg_and_publish()
payload_out.sysid = (uint8_t)robot_id;
payload_out.msgid = (uint32_t)message_number;
/*Log out message id and message size*/
log<<(int)message_number<<",";
log<<(int)out[0]<<",";
/*publish prepared messages in respective topic*/
payload_pub.publish(payload_out);
delete[] out;
delete[] payload_out_ptr;
/*Check for updater message if present send*/
if ((int)get_update_mode() != CODE_RUNNING && is_msg_present() == 1 &&
/* if ((int)get_update_mode() != CODE_RUNNING && is_msg_present() == 1 &&
multi_msg)
{
uint8_t *buff_send = 0;
@ -636,7 +671,7 @@ void roscontroller::prepare_msg_and_publish()
payload_pub.publish(update_packets);
multi_msg = false;
delete[] payload_64;
}
}*/
}
/*---------------------------------------------------------------------------------
/Flight controller service call every step if there is a command set from bzz
@ -646,19 +681,33 @@ script
void roscontroller::flight_controller_service_call()
{
/* flight controller client call if requested from Buzz*/
/*FC call for takeoff,land, gohome and arm/disarm*/
int tmp = buzzuav_closures::bzz_cmd();
double *goto_pos = buzzuav_closures::getgoto();
if (tmp == buzzuav_closures::COMMAND_TAKEOFF ||
tmp == buzzuav_closures::COMMAND_LAND ||
tmp == buzzuav_closures::COMMAND_GOHOME) {
cmd_srv.request.param7 = goto_pos[2];
cmd_srv.request.command = buzzuav_closures::getcmd();
// std::cout << " CMD: " << buzzuav_closures::getcmd() << std::endl;
// SOLO SPECIFIC: SITL DOES NOT USE 21 TO LAND -- workaround: set to LAND
// mode
switch (buzzuav_closures::getcmd()) {
case mavros_msgs::CommandCode::NAV_LAND:
double *goto_pos;
float *gimbal;
switch (buzzuav_closures::bzz_cmd()) {
case buzzuav_closures::COMMAND_TAKEOFF:
goto_pos = buzzuav_closures::getgoto();
cmd_srv.request.param7 = goto_pos[2];
cmd_srv.request.command = buzzuav_closures::getcmd();
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;
case buzzuav_closures::COMMAND_LAND:
cmd_srv.request.command = buzzuav_closures::getcmd();
if (current_mode != "LAND") {
SetMode("LAND", 0);
armstate = 0;
@ -671,60 +720,80 @@ void roscontroller::flight_controller_service_call()
}
armstate = 0;
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)
case buzzuav_closures::COMMAND_GOHOME: // NOT FULLY IMPLEMENTED/TESTED !!!
cmd_srv.request.param5 = home.latitude;
cmd_srv.request.param6 = home.longitude;
cmd_srv.request.param7 = home.altitude;
cmd_srv.request.command = buzzuav_closures::getcmd();
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;
case buzzuav_closures::COMMAND_GOTO: // NOT FULLY IMPLEMENTED/TESTED !!!
goto_pos = buzzuav_closures::getgoto();
cmd_srv.request.param5 = goto_pos[0];
cmd_srv.request.param6 = goto_pos[1];
cmd_srv.request.param7 = goto_pos[2];
cmd_srv.request.command = buzzuav_closures::getcmd();
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");
cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START;
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*/
/*prepare the goto publish message */
cmd_srv.request.param5 = goto_pos[0];
cmd_srv.request.param6 = goto_pos[1];
cmd_srv.request.param7 = goto_pos[2];
cmd_srv.request.command = buzzuav_closures::getcmd();
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");
cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START;
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");
} else if (tmp == buzzuav_closures::COMMAND_ARM) { /*FC call for arm*/
if (!armstate) {
SetMode("LOITER", 0);
armstate = 1;
Arm();
}
} else if (tmp == buzzuav_closures::COMMAND_DISARM) {
if (armstate) {
armstate = 0;
SetMode("LOITER", 0);
Arm();
}
} else if (tmp == buzzuav_closures::COMMAND_MOVETO) { /*Buzz call for moveto*/
roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], 0);
case buzzuav_closures::COMMAND_ARM:
if (!armstate) {
SetMode("LOITER", 0);
armstate = 1;
Arm();
}
break;
// roscontroller::SetLocalPositionNonRaw(goto_pos[1], goto_pos[0],
// goto_pos[2], 0);
case buzzuav_closures::COMMAND_DISARM:
if (armstate) {
armstate = 0;
SetMode("LOITER", 0);
Arm();
}
break;
case buzzuav_closures::COMMAND_MOVETO:
goto_pos = buzzuav_closures::getgoto();
roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], 0);
break;
case buzzuav_closures::COMMAND_GIMBAL:
gimbal = buzzuav_closures::getgimbal();
cmd_srv.request.param1 = gimbal[0];
cmd_srv.request.param2 = gimbal[1];
cmd_srv.request.param3 = gimbal[2];
cmd_srv.request.param4 = gimbal[3];
cmd_srv.request.command = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL;
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;
case buzzuav_closures::COMMAND_PICTURE:
ROS_INFO("TAKING A PICTURE HERE!! --------------");
mavros_msgs::CommandBool capture_command;
if (capture_srv.call(capture_command)) {
ROS_INFO("Reply: %ld", (long int)capture_command.response.success);
} else
ROS_ERROR("Failed to call service from camera streamer");
break;
}
}
/*----------------------------------------------
/Refresh neighbours Position for every ten step
/---------------------------------------------*/
@ -774,39 +843,6 @@ void roscontroller::set_cur_pos(double latitude, double longitude,
/ Compute Range and Bearing of a neighbor in a local reference frame
/ from GPS coordinates
----------------------------------------------------------- */
void ecef2ned_matrix(const double ref_ecef[3], double M[3][3]) {
double hyp_az, hyp_el;
double sin_el, cos_el, sin_az, cos_az;
/* Convert reference point to spherical earth centered coordinates. */
hyp_az = sqrt(ref_ecef[0] * ref_ecef[0] + ref_ecef[1] * ref_ecef[1]);
hyp_el = sqrt(hyp_az * hyp_az + ref_ecef[2] * ref_ecef[2]);
sin_el = ref_ecef[2] / hyp_el;
cos_el = hyp_az / hyp_el;
sin_az = ref_ecef[1] / hyp_az;
cos_az = ref_ecef[0] / hyp_az;
M[0][0] = -sin_el * cos_az;
M[0][1] = -sin_el * sin_az;
M[0][2] = cos_el;
M[1][0] = -sin_az;
M[1][1] = cos_az;
M[1][2] = 0.0;
M[2][0] = -cos_el * cos_az;
M[2][1] = -cos_el * sin_az;
M[2][2] = -sin_el;
}
void matrix_multiply(uint32_t n, uint32_t m, uint32_t p, const double *a,
const double *b, double *c) {
uint32_t i, j, k;
for (i = 0; i < n; i++)
for (j = 0; j < p; j++) {
c[p * i + j] = 0;
for (k = 0; k < m; k++)
c[p * i + j] += a[m * i + k] * b[p * k + j];
}
}
float roscontroller::constrainAngle(float x) {
x = fmod(x,2*M_PI);
if (x < 0.0)
@ -940,18 +976,10 @@ void roscontroller::SetLocalPosition(float x, float y, float z, float yaw) {
moveMsg.header.stamp = ros::Time::now();
moveMsg.header.seq = setpoint_counter++;
moveMsg.header.frame_id = 1;
// float ned_x, ned_y;
// gps_ned_home(ned_x, ned_y);
// ROS_INFO("[%i] ROSBuzz Home: %.7f, %.7f", robot_id, home[0],
// home[1]);
// ROS_INFO("[%i] ROSBuzz LocalPos: %.7f, %.7f", robot_id,
// local_pos[0], local_pos[1]);
/*prepare the goto publish message ATTENTION: ENU FRAME FOR MAVROS STANDARD
* (then converted to NED)*/
// target[0]+=y; target[1]+=x;
moveMsg.pose.position.x = local_pos_new[0] + y; // ned_y+y;
moveMsg.pose.position.y = local_pos_new[1] + x; // ned_x+x;
//ROS_INFO("Lp: %.3f, %.3f - Del: %.3f, %.3f", local_pos_new[0], local_pos_new[1], x, y);
moveMsg.pose.position.x = local_pos_new[0] + y;
moveMsg.pose.position.y = local_pos_new[1] + x;
moveMsg.pose.position.z = z;
moveMsg.pose.orientation.x = 0;
@ -977,7 +1005,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 +1021,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 +1087,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]);
if(debug) 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 +1114,62 @@ 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,req.param4,req.param5);
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:
buzzuav_closures::rc_call(req.command);
ROS_INFO("----> Received unregistered command: ", req.command);
res.success = true;
break;
}
return true;
}