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

merged with master

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

6
CMakeLists.txt
View File

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

93
buzz_scripts/flock.bzz
View File

@ -1,93 +0,0 @@
include "vec2.bzz"
include "update.bzz"
include "barrier.bzz" # don't use a stigmergy id=11 with this header.
include "uavstates.bzz" # require an 'action' function to be defined here.
include "vstigenv.bzz"
# Lennard-Jones parameters
TARGET = 12.0
EPSILON = 18.0
# Lennard-Jones interaction magnitude
function lj_magnitude(dist, target, epsilon) {
return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
#return -(4 * epsilon) * ((target / dist)^12 - (target / dist)^6)
}
# Neighbor data to LJ interaction vector
function lj_vector(rid, data) {
return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth)
}
# Accumulator of neighbor LJ interactions
function lj_sum(rid, data, accum) {
return math.vec2.add(data, accum)
}
# Calculates and actuates the flocking interaction
function action() {
statef=action
# Calculate accumulator
var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
if(neighbors.count() > 0)
accum = math.vec2.scale(accum, 1.0 / neighbors.count())
if(math.vec2.length(accum) > 1.0) {
accum = math.vec2.scale(accum, 1.0 / math.vec2.length(accum))
}
# Move according to vector
print("Robot ", id, "must push ", math.vec2.length(accum) )#, "; ", math.vec2.angle(accum))
uav_moveto(accum.x, accum.y, 0.0)
UAVSTATE = "LENNARDJONES"
# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS
# timeW =0
# statef=land
# } else if(timeW>=WAIT_TIMEOUT/2) {
# UAVSTATE ="GOEAST"
# timeW = timeW+1
# uav_moveto(0.0, 5.0, 0.0)
# } else {
# UAVSTATE ="GONORTH"
# timeW = timeW+1
# uav_moveto(5.0, 0.0, 0.0)
# }
}
########################################
#
# MAIN FUNCTIONS
#
########################################
# Executed once at init time.
function init() {
uav_initswarm()
uav_initstig()
# TARGET_ALTITUDE = 2.5 + id * 5
statef=turnedoff
UAVSTATE = "TURNEDOFF"
}
# Executed at each time step.
function step() {
uav_rccmd()
uav_neicmd()
uav_updatestig()
statef()
log("Current state: ", UAVSTATE)
log("Swarm size: ",ROBOTS)
if(id==0)
stattab_print()
}
# Executed once when the robot (or the simulator) is reset.
function reset() {
}
# Executed once at the end of experiment.
function destroy() {
}

896
buzz_scripts/graphform.bzz
View File

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

35
buzz_scripts/startswarmsimER.launch
View File

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

269
buzz_scripts/tentacle.bzz
View File

@ -1,269 +0,0 @@
include "vec2.bzz"
include "update.bzz"
include "barrier.bzz" # don't use a stigmergy id=11 with this header.
include "uavstates.bzz" # require an 'action' function to be defined here.
# Lennard-Jones parameters
TARGET = 12.0
EPSILON = 10.0
#################################################
### UTILITY FUNCTIONS ###########################
#################################################
# Write a table as if it was a matrix
function write_knowledge(k, row, col, val) {
var key = string.concat(string.tostring(row),"-",string.tostring(col))
k[key] = val
log("Writing knowledge:", val, " to ", row, " ", col)
}
# Read a table as if it was a matrix
function read_knowledge(k, row, col) {
var key = string.concat(string.tostring(row),"-",string.tostring(col))
if (k[key] == nil) {
log("Warning: reading 'nil' value from the knowledge table at", row, " ", col, ", returning -1")
return -1
} else {
return k[key]
}
}
# Int to String
function itos(i) {
log("Use 'string.tostring(OJB)' instead")
if (i==0) { return "0" }
if (i==1) { return "1" }
if (i==2) { return "2" }
if (i==3) { return "3" }
if (i==4) { return "4" }
if (i==5) { return "5" }
if (i==6) { return "6" }
if (i==7) { return "7" }
if (i==8) { return "8" }
if (i==9) { return "9" }
log("Function 'itos' out of bounds, returning the answer (42)")
return "42"
}
# String to Int
function stoi(s) {
if (s=='0') { return 0 }
if (s=='1') { return 1 }
if (s=='2') { return 2 }
if (s=='3') { return 3 }
if (s=='4') { return 4 }
if (s=='5') { return 5 }
if (s=='6') { return 6 }
if (s=='7') { return 7 }
if (s=='8') { return 8 }
if (s=='9') { return 9 }
log("Function 'stoi' out of bounds, returning the answer (42)")
return 42
}
# Rads to degrees
function rtod(r) {
return (r*(180.0/math.pi))
}
# Degrees to rads
function dtor(d) {
return (math.pi*(d/180.0))
}
# Force angles in the (-180,180) interval
function degrees_interval(a) {
var temp = a
while ((temp>360.0) or (temp<0.0)) {
if (temp > 360.0) {
temp = temp - 360.0
} else if (temp < 0.0){
temp = temp + 360.0
}
}
if (temp > 180.0) {
temp = temp - 360.0
}
return temp
}
# Force angles in the (-pi,pi) interval
function radians_interval(a) {
var temp = a
while ((temp>2.0*math.pi) or (temp<0.0)) {
if (temp > 2.0*math.pi) {
temp = temp - 2.0*math.pi
} else if (temp < 0.0){
temp = temp + 2.0*math.pi
}
}
if (temp > math.pi) {
temp = temp - 2.0*math.pi
}
return temp
}
#################################################
### MOVEMENT/COMMUNICATION PRIMITIVES ###########
#################################################
# Lennard-Jones interaction magnitude
function lj_magnitude(dist, target, epsilon) {
return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
}
# Neighbor data to LJ interaction vector
function lj_vector(rid, data) {
return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth)
}
# Accumulator of neighbor LJ interactions
function lj_sum(rid, data, accum) {
return math.vec2.add(data, accum)
}
# Calculates and actuates the flocking interaction
function hexagon() {
# Calculate accumulator
var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
if(neighbors.count() > 0)
math.vec2.scale(accum, 1.0 / neighbors.count())
# Move according to vector
moveto(accum.x, accum.y)
}
function inform_your_neighborhood() {
# Reset to 0 the visibility of all neighbors
foreach(knowledge, function(key, value) {
column = string.sub(key, string.length(key)-1,string.length(key))
if (column=='3') {
knowledge[key] = 0
}
})
neighbors.foreach( function(rid, data) {
# For each neighbor, send a message with its azimuth, as seen by the broadcasting robot
message_id = string.tostring(rid)
neighbors.broadcast(message_id, rtod(data.azimuth))
# Record the neighbor azimuth in my own knowledge table
write_knowledge(knowledge, rid, 0, rtod(data.azimuth))
# Record the neighbor distance in my own knowledge table
write_knowledge(knowledge, rid, 2, data.distance)
# Set neighbor as visible
write_knowledge(knowledge, rid, 3, 1)
})
# Send a message with the desired direction, as seen by the broadcasting robot
neighbors.broadcast("direction", local_dir)
}
function listen_to_your_neighborhood() {
# For all "senders" in my neighborhood, record my azimuth, as seen by them
message_id = string.tostring(id)
neighbors.listen(message_id, function(vid, value, rid) {
write_knowledge(knowledge, rid, 1, value)
})
}
#################################################
### ACTUAL CONTROLLERS ##########################
#################################################
function zero() {
# Do not move
moveto(0.0,0.0)
# Tell the neighbors of the center where to go
inform_your_neighborhood()
}
function onetwo() {
if (id == 1) {
angle = 45
} else {
angle = -135
}
# Broadcast information
inform_your_neighborhood()
# If the center 0 is in sight
if (read_knowledge(knowledge, 0, 3) == 1) {
arm_offset = degrees_interval(read_knowledge(knowledge, 0, 1) - angle)
if (arm_offset<3 and arm_offset>(-3)) {
hexagon() # Underlying Lennard-Jones potential behavior
} else {
local_rotation = degrees_interval( read_knowledge(knowledge, 0, 1) + (180.0 - read_knowledge(knowledge, 0, 0)) )
local_arm = degrees_interval(angle - local_rotation)
if (read_knowledge(knowledge, 0, 2) > 250.0) {
x_mov = math.cos(dtor(read_knowledge(knowledge, 0, 0)))
y_mov = math.sin(dtor(read_knowledge(knowledge, 0, 0)))
} else if (read_knowledge(knowledge, 0, 2) < 30.0) {
x_mov = -math.cos(dtor(read_knowledge(knowledge, 0, 0)))
y_mov = -math.sin(dtor(read_knowledge(knowledge, 0, 0)))
} else {
spiraling = 2.0+(id/10.0) # Fun stuff but be careful with this, it affects how a robots turns around a central node, use random number generation, eventually
if (arm_offset > 0) { # Clockwise
x_mov = -math.sin(dtor(read_knowledge(knowledge, 0, 0)))
y_mov = math.cos(dtor(read_knowledge(knowledge, 0, 0))) * spiraling
} else { # Counterclockwise
x_mov = math.sin(dtor(read_knowledge(knowledge, 0, 0)))
y_mov = -math.cos(dtor(read_knowledge(knowledge, 0, 0))) * spiraling
}
}
speed = 100
moveto(speed * x_mov,speed * y_mov)
}
} else {
hexagon()
}
}
#################################################
### BUZZ FUNCTIONS ##############################
#################################################
function action(){
if (id == 0)
statef=zero
else
statef=onetwo
UAVSTATE="TENTACLES"
}
# Executed at init time
function init() {
uav_initswarm()
# Local knowledge table
knowledge = {}
# Update local knowledge with information from the neighbors
listen_to_your_neighborhood()
# Variables initialization
iteration = 0
}
# Executed every time step
function step() {
uav_rccmd()
uav_neicmd()
statef()
log("Current state: ", CURSTATE)
log("Swarm size: ",ROBOTS)
# Count the number of steps
iteration = iteration + 1
}
# Executed once when the robot (or the simulator) is reset.
function reset() {
}
# Execute at exit
function destroy() {
}

119
buzz_scripts/testflocksim.bzz
View File

@ -1,119 +0,0 @@
include "vec2.bzz"
include "update.bzz"
include "barrier.bzz" # don't use a stigmergy id=11 with this header.
include "uavstates.bzz" # require an 'action' function to be defined here.
include "vstigenv.bzz"
TARGET_ALTITUDE = 10.0
# Lennard-Jones parameters
TARGET = 12.0
EPSILON = 14.0
# Lennard-Jones interaction magnitude
function lj_magnitude(dist, target, epsilon) {
return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
#return -(4 * epsilon) * ((target / dist)^12 - (target / dist)^6)
}
# Neighbor data to LJ interaction vector
function lj_vector(rid, data) {
return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth)
}
# Accumulator of neighbor LJ interactions
function lj_sum(rid, data, accum) {
return math.vec2.add(data, accum)
}
function user_attract(t) {
fus = math.vec2.new(0.0, 0.0)
if(size(t)>0) {
foreach(t, function(u, tab) {
#log("id: ",u," Range ", tab.r, "Bearing ", tab.b)
fus = math.vec2.add(fus, math.vec2.newp(lj_magnitude(tab.r, 3 * TARGET / 4.0, EPSILON * 2.0), tab.b))
})
math.vec2.scale(fus, 1.0 / size(t))
}
#print("User attract:", fus.x," ", fus.y, " [", size(t), "]")
return fus
}
# Calculates and actuates the flocking interaction
function action() {
statef=action
# Calculate accumulator
var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
if(neighbors.count() > 0)
accum = math.vec2.scale(accum, 1.0 / neighbors.count())
accum = math.vec2.add(accum, user_attract(users.dataL))
accum = math.vec2.scale(accum, 1.0 / 2.0)
if(math.vec2.length(accum) > 1.0) {
accum = math.vec2.scale(accum, 1.0 / math.vec2.length(accum))
}
# Move according to vector
print("Robot ", id, "must push ", math.vec2.length(accum) )#, "; ", math.vec2.angle(accum))
uav_moveto(accum.x, accum.y)
UAVSTATE = "LENNARDJONES"
# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS
# timeW =0
# statef=land
# } else if(timeW>=WAIT_TIMEOUT/2) {
# UAVSTATE ="GOEAST"
# timeW = timeW+1
# uav_moveto(0.0,5.0)
# } else {
# UAVSTATE ="GONORTH"
# timeW = timeW+1
# uav_moveto(5.0,0.0)
# }
}
########################################
#
# MAIN FUNCTIONS
#
########################################
# Executed once at init time.
function init() {
uav_initswarm()
vt = stigmergy.create(5)
t = {}
vt.put("p",t)
}
# Executed at each time step.
function step() {
uav_rccmd()
uav_neicmd()
statef()
log("Current state: ", UAVSTATE)
log("Swarm size: ",ROBOTS)
# Read a value from the structure
# log(users)
#users_print(users.dataG)
if(size(users.dataG)>0)
vt.put("p", users.dataG)
# Get the number of keys in the structure
#log("The vstig has ", vt.size(), " elements")
users_save(vt.get("p"))
table_print(users.dataL)
}
# Executed once when the robot (or the simulator) is reset.
function reset() {
}
# Executed once at the end of experiment.
function destroy() {
}

537
buzz_scripts/tree_centroid.bzz
View File

@ -1,537 +0,0 @@
# Include files
include "string.bzz"
include "vec2.bzz"
include "utilities.bzz"
include "barrier.bzz"
############################################
# Global variables
RANGE = 200 # rab range in cm, get from argos file
N_SONS = 10 # Maximum number of sons
TRIGGER_VSTIG = 101 # ID of trigger virtual stigmergy
BARRIER_VSTIG = 102 # ID of barrier virtual stigmergy
################################################################
################################################################
# Tree utility functions
function parent_select() {
# Selects potential parents
var candidates = {}
candidates = neighbors.filter(function(rid, data) {
return ((knowledge.level[rid] > 0) and (data.distance < (RANGE - 10.0)) and (knowledge.free[rid] == 1))})
# and (data.distance > 10.0)
# Selects closest parent candidate as parent
var temp = {}
temp = candidates.reduce(function(rid, data, accum) {
accum.min = math.min(accum.min, data.distance)
return accum
}, {.min = RANGE})
var min = temp.min
var flag = 0
foreach(knowledge.distance, function(key, value) {
if ((flag == 0) and (candidates.data[key] != nil)) {
if (value == min) {
tree.parent.id = key
tree.parent.distance = value
tree.parent.azimuth = knowledge.azimuth[key]
flag = 1
}
}
#break (when implemented)
})
}
####################################
function count() {
if (nb_sons == 0) {
eq_level = level
}
else if (nb_sons >= 1) {
var temp = {}
temp = sons.reduce(function(rid, data, accum) {
accum.sum = accum.sum + tree.sons[rid].eq_level
return accum
}, {.sum = 0})
eq_level = temp.sum - (nb_sons - 1) * level
}
}
####################################
function change_frame(p01, p1, theta) {
var p0 = {
.x = math.cos(theta) * p1.x + math.sin(theta) * p1.y + p01.x,
.y = -math.sin(theta) * p1.x + math.cos(theta) * p1.y + p01.y
}
return p0
}
####################################
transform_accum = function(rid, data) {
# Son contribution in frame F1
var p1 = {
.x = tree.sons[rid].accum_x,
.y = tree.sons[rid].accum_y
}
# Rotation angle between F0 and F1
var theta = tree.sons[rid].angle_parent - data.azimuth - math.pi
# Translation vector from F0 to F1
var p01 = {
.x = 0,
.y = 0
}
var p0 = {}
if (tree.sons[rid].angle_parent != nil) {
var rot_angle = radians_interval(theta)
p0 = change_frame(p01, p1, rot_angle)
}
else {
p0.x = p1.x
p0.y = p1.y
}
return p0
}
####################################
function centroid() {
# If the robot has a parent
if ((tree.parent != nil) or (root == 1)) {
var sum_F1 = { .x = 0, .y = 0}
# If the robot has at least one son
if (nb_sons > 0) {
var temp = {}
# Expresses son contrib (in F1) in its own reference frame (F0)
temp = sons.map(transform_accum)
# Sums son contributions expressed in F0 frame
sum_F1 = temp.reduce(function(rid, data, accum) {
accum.x = accum.x + data.x
accum.y = accum.y + data.y
#log("id ", rid, " sum_x ", accum.x, " sum_y ", accum.y)
return accum
}, {.x = 0, .y = 0})
}
# If the robot is not the root
if ((root == 0) and (tree.parent.id != nil)) {
#var nb_descendants = eq_level - level
var p0 = knowledge.distance[tree.parent.id]#tree.parent.distance
var theta = knowledge.azimuth[tree.parent.id]#tree.parent.azimuth
# Adds current robot contribution to centroid sum
accum_x = sum_F1.x - (nb_descendants + 1) * p0 * math.cos(theta)
accum_y = sum_F1.y - (nb_descendants + 1) * p0 * math.sin(theta)
}
# If the robot is the root
else if ((root == 1) and (robot_count != 0)) {
# Centroid coordinates in root ref frame
accum_x = sum_F1.x / robot_count
accum_y = sum_F1.y / robot_count
}
}
}
################################################################
################################################################
# Tree reconfiguration functions
function tree_config() {
statef()
}
function end_fun() {
if (root == 1) {
log("centroid X: ", accum_x, " Y ", accum_y)
}
}
####################################
function root_select() {
log(id," root_select")
if (tree.parent.id != nil) {
if(neighbors.data[tree.parent.id] != nil) {
angle_parent = neighbors.data[tree.parent.id].azimuth
}
}
if (root == 1) {
# Listens for new root acknowledgment
foreach(knowledge.ackn, function(key, value) {
if (value == better_root) {
#log(id, " got it")
root = 0
level = 0
setleds(0,0,0)
}
})
if (ack == 1) {
# Triggers transition to new state
trigger.put("a", 1)
}
else if ((root == 1) and (better_root != id) and (trigger.get("a") != 1)) {
setleds(255,0,0)
better_root = id
# Centroid position in root reference frame (F0)
var c0 = math.vec2.new(accum_x, accum_y)
# Distance from current root to centroid
var dist_rc = math.vec2.length(c0)
#log("root ", id, " dist_centr ", dist_rc)
# Distances from neighbors to centroid
var dist_centroid = {}
dist_centroid = neighbors.map(function(rid, data) {
# Neighbour position in frame F0
var p0 = math.vec2.newp(data.distance, data.azimuth)
# Difference vector between p0 and c0
var v = math.vec2.sub(p0, c0)
# Distance between robot and centroid
return math.vec2.length(v)
})
# Minimal distance to centroid
var temp = {}
temp = dist_centroid.reduce(function(rid, data, accum) {
accum.min = math.min(accum.min, data)
return accum
}, {.min = dist_rc})
var min = temp.min
#log("min ", min)
# If the current root is the closest to the centroid
if(dist_rc == min) {
ack = 1
}
# Otherwise
else {
var flag = 0
# Selects closest robot to centroid as better root
foreach(dist_centroid.data, function(key, value) {
if(flag == 0) {
if(value == min) {
better_root = key
flag = 1
}
# break (when implemented)
}
})
#log(id, " better root : ", better_root)
#log("X : ", accum_x, "Y : ", accum_y)
var angle = neighbors.data[better_root].azimuth
# Broadcasts
var message = {
.better_root = better_root,
.centroid_x = accum_x,
.centroid_y = accum_y,
.angle_old_root = angle
}
neighbors.broadcast("msg1", message)
}
}
}
else if (better_root == nil) {
# Listen for old root message
foreach(knowledge.better_root, function(rid, value) {
if(value == id) {
var theta = neighbors.data[rid].azimuth
var p1 = {
.x = knowledge.centroid_x[rid],
.y = knowledge.centroid_y[rid]
}
var p01 = {
.x = neighbors.data[rid].distance * math.cos(theta),
.y = neighbors.data[rid].distance * math.sin(theta)
}
var p0 = {}
if (knowledge.angle_old_root[rid] != nil) {
var rot_angle = radians_interval(knowledge.angle_old_root[rid] - theta - math.pi)
p0 = change_frame(p01, p1, rot_angle)
}
else {
p0.x = p1.x
p0.y = p1.y
}
accum_x = p0.x
accum_y = p0.y
centroid_x = accum_x
centroid_y = accum_y
root = 1
neighbors.broadcast("got_it", id)
}
})
}
# Transitions to new state when all robots are ready
if (trigger.get("a") == 1) {
barrier_set(ROBOTS, end_fun)
barrier_ready()
}
}
####################################
function tree_select() {
log(id, " tree_select")
neighbors.map(function(rid, data) {
knowledge.distance[rid] = data.distance
knowledge.azimuth[rid] = data.azimuth
return 1
})
if (level == 0) {
# Finds a parent
parent_select()
# Updates robot level
if (tree.parent.id != nil) {
#log(" ")
#log("selected")
#log("son ", id)
#log("parent ", tree.parent.id)
#log(" ")
level = knowledge.level[tree.parent.id] + 1
angle_parent = neighbors.data[tree.parent.id].azimuth
}
}
else {
# Updates list of sons
foreach(knowledge.parent, function(key, value) {
if(value == id) {
#log(value)
if(tree.sons[key] == nil) {
# Updates robot nb_sons
nb_sons = nb_sons + 1
# Updates robot free status
if (nb_sons >= N_SONS) {
free = 0
}
}
tree.sons[key] = {
.distance = knowledge.distance[key],
.azimuth = knowledge.azimuth[key],
.eq_level = knowledge.eq_level[key],
.accum_x = knowledge.accum_x[key],
.accum_y = knowledge.accum_y[key],
.angle_parent = knowledge.angle_parent[key]
}
}
})
}
# Creates a subset of neighbors to get the sons
# and parent
sons = {}
sons = neighbors.filter(function(rid, data) {
return (tree.sons[rid] != nil)
})
parent = {}
parent = neighbors.filter(function(rid, data) {
return (tree.parent.id == rid)
})
# Counts robots (updates eq_level)
count()
# Updates count of robots in the tree
if (root == 1) {
robot_count = eq_level
}
nb_descendants = eq_level - level
# Computes centroid (updates accum_x, accum_y)
centroid()
# Broadcast information to other robots
var message = {
.level = level,
.parent = tree.parent.id,
.eq_level = eq_level,
.free = free,
.accum_x = accum_x,
.accum_y = accum_y,
.angle_parent = angle_parent
}
neighbors.broadcast("msg2", message)
# Triggers transition to new state if root count = ROBOTS
if (root == 1) {
if(robot_count == ROBOTS) {
log("centroid X: ", accum_x, " Y ", accum_y)
trigger.put("b", 1)
}
}
# Transitions to new state when all robots are ready
if (trigger.get("b") == 1) {
barrier_set(ROBOTS, root_select)
barrier_ready()
}
}
################################################################
################################################################
# This function is executed every time you press the 'execute' button
function init() {
trigger = stigmergy.create(TRIGGER_VSTIG)
barrier = stigmergy.create(BARRIER_VSTIG)
# Trees
old_tree = {}
tree = old_tree
old_tree.parent = {}
old_tree.sons = {}
# Boolean variables
root = 0 # Root status
free = 1 # Node status (true if accepts sons)
ack = 0
# Tree variables
level = 0
eq_level = 0
nb_sons = 0
nb_descendants = 0
# Update root status
if (id == 0) {
root = 1 # True if robot is the ro ot
level = 1
robot_count = 0
}
statef = tree_select
knowledge = {
.level = {},
.parent = {},
.eq_level = {},
.free = {},
.accum_x = {},
.accum_y = {},
.angle_parent = {},
.distance = {},
.azimuth = {},
.better_root = {},
.centroid_x = {},
.centroid_y = {},
.angle_old_root = {},
.ackn = {}
}
# Broadcast information to other robots
var message = {
.level = level,
.parent = old_tree.parent.id,
.eq_level = eq_level,
.free = free,
.accum_x = accum_x,
.accum_y = accum_y,
.angle_parent = 0.0
}
neighbors.broadcast("msg2", message)
# Listen to information from other robots for root_select
neighbors.listen("msg1", function(vid, value, rid) {
knowledge.better_root[rid] = value.better_root
knowledge.centroid_x[rid] = value.centroid_x
knowledge.centroid_y[rid] = value.centroid_y
knowledge.angle_old_root[rid] = value.angle_old_root
knowledge.angle_parent[rid] = value.angle_parent
})
# Listen to information from other robots for tree_select
neighbors.listen("msg2", function(vid, value, rid) {
knowledge.level[rid] = value.level
knowledge.parent[rid] = value.parent
knowledge.eq_level[rid] = value.eq_level
knowledge.free[rid] = value.free
knowledge.accum_x[rid] = value.accum_x
knowledge.accum_y[rid] = value.accum_y
knowledge.angle_parent[rid] = value.angle_parent
})
neighbors.listen("got_it", function(vid, value, rid) {
knowledge.ackn[rid] = value
})
}
############################################
# This function is executed at each time step
function step() {
tree_config()
goal = math.vec2.new(0.0, 0.0)
}
############################################
# This function is executed every time you press the 'reset'
# button in the GUI.
function reset() {
# put your code here
}
############################################
# This function is executed only once, when the robot is removed
# from the simulation
function destroy() {
# put your code here
}
################

70
include/buzz_update.h
View File

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

42
include/buzz_utility.h
View File

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

24
include/buzzuav_closures.h
View File

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

93
include/roscontroller.h
View File

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

9
include/uav_utility.h
View File

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

3
log.txt
View File

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

45
readme.md
View File

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

722
src/buzz_update.cpp
View File

@ -9,293 +9,580 @@
#include <buzz/buzzdebug.h>
#include <sys/time.h>
/*Temp for data collection*/
//static int neigh=-1;
// static int neigh=-1;
static struct timeval t1, t2;
static int timer_steps=0;
//static clock_t end;
static int timer_steps = 0;
// static clock_t end;
/*Temp end */
static int fd,wd =0;
static int old_update =0;
static int fd, wd = 0;
static int old_update = 0;
static buzz_updater_elem_t updater;
static int no_of_robot;
static char* dbgf_name;
static const char* dbgf_name;
static const char* bcfname;
static const char* old_bcfname = "old_bcode.bo";
static const char* bzz_file;
static int neigh=-1;
static int updater_msg_ready ;
static int updated=0;
static int Robot_id = 0;
static int neigh = -1;
static int updater_msg_ready;
static uint16_t update_try_counter = 0;
static int updated = 0;
static const uint16_t MAX_UPDATE_TRY = 5;
static int packet_id_ = 0;
static size_t old_byte_code_size = 0;
/*Initialize updater*/
void init_update_monitor(const char* bo_filename, const char* stand_by_script){
void init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id)
{
Robot_id = robot_id;
dbgf_name = dbgfname;
bcfname = bo_filename;
ROS_INFO("Initializing file monitor...");
fd=inotify_init1(IN_NONBLOCK);
if ( fd < 0 ) {
perror( "inotify_init error" );
fd = inotify_init1(IN_NONBLOCK);
if (fd < 0)
{
perror("inotify_init error");
}
/*If simulation set the file monitor only for robot 1*/
if (SIMULATION == 1 && robot_id == 0)
{
/* watch /.bzz file for any activity and report it back to update */
wd=inotify_add_watch(fd, bzz_file,IN_ALL_EVENTS );
wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS);
}
else if (SIMULATION == 0)
{
/* watch /.bzz file for any activity and report it back to update */
wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS);
}
/*load the .bo under execution into the updater*/
uint8_t* BO_BUF = 0;
FILE* fp = fopen(bo_filename, "rb");
if(!fp) {
if (!fp)
{
perror(bo_filename);
}
fseek(fp, 0, SEEK_END);
size_t bcode_size = ftell(fp);
rewind(fp);
BO_BUF = (uint8_t*)malloc(bcode_size);
if(fread(BO_BUF, 1, bcode_size, fp) < bcode_size) {
if (fread(BO_BUF, 1, bcode_size, fp) < bcode_size)
{
perror(bo_filename);
//fclose(fp);
//return 0;
// fclose(fp);
// return 0;
}
fclose(fp);
/*Load stand_by .bo file into the updater*/
uint8_t* STD_BO_BUF = 0;
fp = fopen(stand_by_script, "rb");
if(!fp) {
if (!fp)
{
perror(stand_by_script);
}
fseek(fp, 0, SEEK_END);
size_t stdby_bcode_size = ftell(fp);
rewind(fp);
STD_BO_BUF = (uint8_t*)malloc(stdby_bcode_size);
if(fread(STD_BO_BUF, 1, stdby_bcode_size, fp) < stdby_bcode_size) {
if (fread(STD_BO_BUF, 1, stdby_bcode_size, fp) < stdby_bcode_size)
{
perror(stand_by_script);
//fclose(fp);
//return 0;
// fclose(fp);
// return 0;
}
fclose(fp);
/*Create the updater*/
updater = (buzz_updater_elem_t)malloc(sizeof(struct buzz_updater_elem_s));
/*Intialize the updater with the required data*/
updater->bcode = BO_BUF;
/*Store a copy of the Bcode for rollback*/
updater->outmsg_queue = NULL;
updater->inmsg_queue = NULL;
updater->bcode_size = (size_t*) malloc(sizeof(size_t));
updater->update_no = (uint8_t*) malloc(sizeof(uint16_t));
*(uint16_t*)(updater->update_no) =0;
*(size_t*)(updater->bcode_size)=bcode_size;
updater->patch = NULL;
updater->patch_size = (size_t*)malloc(sizeof(size_t));
updater->bcode_size = (size_t*)malloc(sizeof(size_t));
updater->update_no = (uint8_t*)malloc(sizeof(uint16_t));
*(uint16_t*)(updater->update_no) = 0;
*(size_t*)(updater->bcode_size) = bcode_size;
updater->standby_bcode = STD_BO_BUF;
updater->standby_bcode_size = (size_t*)malloc(sizeof(size_t));
*(size_t*)(updater->standby_bcode_size)=stdby_bcode_size;
updater->mode=(int*)malloc(sizeof(int));
*(int*)(updater->mode)=CODE_RUNNING;
//no_of_robot=barrier;
updater_msg_ready=0;
//neigh = 0;
//updater->outmsg_queue=
// update_table->barrier=nvs;
// open logger
*(size_t*)(updater->standby_bcode_size) = stdby_bcode_size;
updater->mode = (int*)malloc(sizeof(int));
*(int*)(updater->mode) = CODE_RUNNING;
// no_of_robot=barrier;
updater_msg_ready = 0;
/*Write the bcode to a file for rollback*/
fp = fopen("old_bcode.bo", "wb");
fwrite((updater->bcode), *(size_t*)updater->bcode_size, 1, fp);
fclose(fp);
}
/*Check for .bzz file chages*/
int check_update(){
struct inotify_event *event;
int check_update()
{
struct inotify_event* event;
char buf[1024];
int check =0;
int i=0;
int len=read(fd,buf,1024);
while(i<len){
event=(struct inotify_event *) &buf[i];
int check = 0;
int i = 0;
int len = read(fd, buf, 1024);
while (i < len)
{
event = (struct inotify_event*)&buf[i];
/* file was modified this flag is true in nano and self delet in gedit and other editors */
//fprintf(stdout,"inside file monitor.\n");
if(event->mask & (IN_MODIFY| IN_DELETE_SELF)){
// fprintf(stdout,"inside file monitor.\n");
if (event->mask & (IN_MODIFY | IN_DELETE_SELF))
{
/*respawn watch if the file is self deleted */
inotify_rm_watch(fd,wd);
inotify_rm_watch(fd, wd);
close(fd);
fd=inotify_init1(IN_NONBLOCK);
wd=inotify_add_watch(fd,bzz_file,IN_ALL_EVENTS);
//fprintf(stdout,"event.\n");
fd = inotify_init1(IN_NONBLOCK);
wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS);
// fprintf(stdout,"event.\n");
/* To mask multiple writes from editors*/
if(!old_update){
check=1;
old_update =1;
if (!old_update)
{
check = 1;
old_update = 1;
}
}
/* update index to start of next event */
i+=sizeof(struct inotify_event)+event->len;
i += sizeof(struct inotify_event) + event->len;
}
if (!check) old_update=0;
return check;
if (!check)
old_update = 0;
return check;
}
int test_patch(std::string path, std::string name1, size_t update_patch_size, uint8_t* patch)
{
if (SIMULATION == 1)
{
return 1;
}
else
{
/*Patch the old bo with new patch*/
std::stringstream patch_writefile;
patch_writefile << path << name1 << "tmp_patch.bo";
/*Write the patch to a file*/
FILE* fp = fopen(patch_writefile.str().c_str(), "wb");
fwrite(patch, update_patch_size, 1, fp);
fclose(fp);
std::stringstream patch_exsisting;
patch_exsisting << "bspatch " << path << name1 << ".bo " << path << name1 << "-patched.bo " << path << name1
<< "tmp_patch.bo";
fprintf(stdout, "Launching bspatch command: %s \n", patch_exsisting.str().c_str());
if (system(patch_exsisting.str().c_str()))
return 0;
else
return 1;
}
}
void code_message_outqueue_append(){
updater->outmsg_queue=(updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s));
uint16_t size =0;
updater->outmsg_queue->queue = (uint8_t*)malloc(2*sizeof(uint16_t)+ *(size_t*)(updater->bcode_size));
updater_code_t obtain_patched_bo(std::string path, std::string name1)
{
if (SIMULATION == 1)
{
/*Read the exsisting file to simulate the patching*/
std::stringstream read_patched;
read_patched << path << name1 << ".bo";
fprintf(stdout, "read file name %s \n", read_patched.str().c_str());
uint8_t* patched_BO_Buf = 0;
FILE* fp = fopen(read_patched.str().c_str(), "rb");
if (!fp)
{
perror(read_patched.str().c_str());
}
fseek(fp, 0, SEEK_END);
size_t patched_size = ftell(fp);
rewind(fp);
patched_BO_Buf = (uint8_t*)malloc(patched_size);
if (fread(patched_BO_Buf, 1, patched_size, fp) < patched_size)
{
perror(read_patched.str().c_str());
fclose(fp);
}
fclose(fp);
/*Write the patched to a code struct and return*/
updater_code_t update_code = (updater_code_t)malloc(sizeof(struct updater_code_s));
update_code->bcode = patched_BO_Buf;
update_code->bcode_size = (uint8_t*)malloc(sizeof(uint16_t));
*(uint16_t*)(update_code->bcode_size) = patched_size;
return update_code;
}
else
{
/*Read the new patched file*/
std::stringstream read_patched;
read_patched << path << name1 << "-patched.bo";
fprintf(stdout, "read file name %s \n", read_patched.str().c_str());
uint8_t* patched_BO_Buf = 0;
FILE* fp = fopen(read_patched.str().c_str(), "rb");
if (!fp)
{
perror(read_patched.str().c_str());
}
fseek(fp, 0, SEEK_END);
size_t patched_size = ftell(fp);
rewind(fp);
patched_BO_Buf = (uint8_t*)malloc(patched_size);
if (fread(patched_BO_Buf, 1, patched_size, fp) < patched_size)
{
perror(read_patched.str().c_str());
fclose(fp);
}
fclose(fp);
/* delete old bo file & rename new bo file */
remove((path + name1 + ".bo").c_str());
rename((path + name1 + "-patched.bo").c_str(), (path + name1 + ".bo").c_str());
/*Write the patched to a code struct and return*/
updater_code_t update_code = (updater_code_t)malloc(sizeof(struct updater_code_s));
update_code->bcode = patched_BO_Buf;
update_code->bcode_size = (uint8_t*)malloc(sizeof(uint16_t));
*(uint16_t*)(update_code->bcode_size) = patched_size;
return update_code;
}
}
void code_message_outqueue_append()
{
updater->outmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s));
/* if size less than 250 Pad with zeors to assure transmission*/
uint16_t size = UPDATE_CODE_HEADER_SIZE + *(size_t*)(updater->patch_size);
uint16_t padding_size = (size > MIN_UPDATE_PACKET) ? 0 : MIN_UPDATE_PACKET - size;
size += padding_size;
updater->outmsg_queue->queue = (uint8_t*)malloc(size);
memset(updater->outmsg_queue->queue, 0, size);
updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t));
/*append the update no, code size and code to out msg*/
*(uint16_t*)(updater->outmsg_queue->queue+size) = *(uint16_t*) (updater->update_no);
size+=sizeof(uint16_t);
*(uint16_t*)(updater->outmsg_queue->queue+size) = *(size_t*) (updater->bcode_size);
size+=sizeof(uint16_t);
memcpy(updater->outmsg_queue->queue+size, updater->bcode, *(size_t*)(updater->bcode_size));
size+=(uint16_t)*(size_t*)(updater->bcode_size);
/*FILE *fp;
fp=fopen("update.bo", "wb");
fwrite((updater->bcode), updater->bcode_size, 1, fp);
fclose(fp);*/
updater_msg_ready=1;
*(uint16_t*)(updater->outmsg_queue->size)=size;
//fprintf(stdout,"out msg append transfer code size %d\n", (int)*(size_t*) updater->bcode_size);
*(uint16_t*)(updater->outmsg_queue->size) = size;
size = 0;
/*Append message type*/
*(uint8_t*)(updater->outmsg_queue->queue + size) = SENT_CODE;
size += sizeof(uint8_t);
/*Append the update no, code size and code to out msg*/
*(uint16_t*)(updater->outmsg_queue->queue + size) = *(uint16_t*)(updater->update_no);
size += sizeof(uint16_t);
*(uint16_t*)(updater->outmsg_queue->queue + size) = (uint16_t) * (size_t*)(updater->patch_size);
size += sizeof(uint16_t);
memcpy(updater->outmsg_queue->queue + size, updater->patch, *(size_t*)(updater->patch_size));
size += (uint16_t) * (size_t*)(updater->patch_size);
updater_msg_ready = 1;
}
void code_message_inqueue_append(uint8_t* msg,uint16_t size){
updater->inmsg_queue=(updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s));
//ROS_INFO("[DEBUG] Updater append code of size %d\n", (int) size);
void outqueue_append_code_request(uint16_t update_no)
{
updater->outmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s));
uint16_t size = 0;
updater->outmsg_queue->queue = (uint8_t*)malloc(2 * sizeof(uint16_t) + sizeof(uint8_t) + CODE_REQUEST_PADDING);
updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t));
memset(updater->outmsg_queue->queue, 0, sizeof(uint16_t) + sizeof(uint8_t) + CODE_REQUEST_PADDING);
/*Append message type*/
*(uint8_t*)(updater->outmsg_queue->queue + size) = RESEND_CODE;
size += sizeof(uint8_t);
/*Append the update no, code size and code to out msg*/
*(uint16_t*)(updater->outmsg_queue->queue + size) = update_no;
size += sizeof(uint16_t);
*(uint16_t*)(updater->outmsg_queue->queue + size) = update_try_counter;
size += sizeof(uint16_t);
*(uint16_t*)(updater->outmsg_queue->size) = size + CODE_REQUEST_PADDING;
updater_msg_ready = 1;
ROS_INFO("[Debug] Requested update no. %u with try: %u \n", update_no, update_try_counter);
}
void code_message_inqueue_append(uint8_t* msg, uint16_t size)
{
updater->inmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s));
// ROS_INFO("[DEBUG] Updater append code of size %d\n", (int) size);
updater->inmsg_queue->queue = (uint8_t*)malloc(size);
updater->inmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t));
memcpy(updater->inmsg_queue->queue, msg, size);
*(uint16_t*)(updater->inmsg_queue->size) = size;
}
void code_message_inqueue_process(){
int size=0;
ROS_INFO("[Debug] Updater processing in msg with mode %d \n", *(int*)(updater->mode) );
ROS_INFO("[Debug] %u : Current update number, %u : Received update no \n",( *(uint16_t*) (updater->update_no) ), (*(uint16_t*)(updater->inmsg_queue->queue)) );
ROS_INFO("[Debug] Updater received code of size %u \n",(*(uint16_t*)(updater->inmsg_queue->queue+sizeof(uint16_t)) ) );
void set_packet_id(int packet_id)
{
/*Used for data logging*/
packet_id_ = packet_id;
}
void code_message_inqueue_process()
{
int size = 0;
updater_code_t out_code = NULL;
if( *(int*) (updater->mode) == CODE_RUNNING){
//fprintf(stdout,"[debug]Inside inmsg code running");
if( *(uint16_t*)(updater->inmsg_queue->queue) > *(uint16_t*) (updater->update_no) ){
//fprintf(stdout,"[debug]Inside update number comparision");
uint16_t update_no=*(uint16_t*)(updater->inmsg_queue->queue);
size +=sizeof(uint16_t);
uint16_t update_bcode_size =*(uint16_t*)(updater->inmsg_queue->queue+size);
size +=sizeof(uint16_t);
//fprintf(stdout,"in queue process Update no %d\n", (int) update_no);
//fprintf(stdout,"in queue process bcode size %d\n", (int) update_bcode_size);
//FILE *fp;
//fp=fopen("update.bo", "wb");
//fwrite((updater->inmsg_queue->queue+size), update_bcode_size, 1, fp);
//fclose(fp);
if( test_set_code((uint8_t*)(updater->inmsg_queue->queue+size),
(char*) dbgf_name,(size_t)update_bcode_size) ) {
*(uint16_t*)(updater->update_no)=update_no;
neigh=1;
//gettimeofday(&t1, NULL);
}
}
}
ROS_INFO("[Debug] Updater processing in msg with mode %d \n", *(int*)(updater->mode));
ROS_INFO("[Debug] %u : Current update number, %u : Received update no \n", (*(uint16_t*)(updater->update_no)),
(*(uint16_t*)(updater->inmsg_queue->queue + sizeof(uint8_t))));
ROS_INFO("[Debug] Updater received patch of size %u \n",
(*(uint16_t*)(updater->inmsg_queue->queue + sizeof(uint16_t) + sizeof(uint8_t))));
//fprintf(stdout,"in queue freed\n");
if (*(int*)(updater->mode) == CODE_RUNNING)
{
// fprintf(stdout,"[debug]Inside inmsg code running");
if (*(uint8_t*)(updater->inmsg_queue->queue) == SENT_CODE)
{
size += sizeof(uint8_t);
if (*(uint16_t*)(updater->inmsg_queue->queue + size) > *(uint16_t*)(updater->update_no))
{
// fprintf(stdout,"[debug]Inside update number comparision");
uint16_t update_no = *(uint16_t*)(updater->inmsg_queue->queue + size);
size += sizeof(uint16_t);
uint16_t update_patch_size = *(uint16_t*)(updater->inmsg_queue->queue + size);
size += sizeof(uint16_t);
/*Generate patch*/
std::string bzzfile_name(bzz_file);
std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
std::string name1 = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
name1 = name1.substr(0, name1.find_last_of("."));
if (test_patch(path, name1, update_patch_size, (updater->inmsg_queue->queue + size)))
{
out_code = obtain_patched_bo(path, name1);
// fprintf(stdout,"in queue process Update no %d\n", (int) update_no);
// fprintf(stdout,"in queue process bcode size %d\n", (int) update_bcode_size);
// FILE *fp;
// fp=fopen("update.bo", "wb");
// fwrite((updater->inmsg_queue->queue+size), update_bcode_size, 1, fp);
// fclose(fp);
if (test_set_code((out_code->bcode), (char*)dbgf_name, (size_t)(*(uint16_t*)out_code->bcode_size)))
{
printf("TEST PASSED!\n");
*(uint16_t*)updater->update_no = update_no;
/*Clear exisiting patch if any*/
delete_p(updater->patch);
/*copy the patch into the updater*/
updater->patch = (uint8_t*)malloc(update_patch_size);
memcpy(updater->patch, (updater->inmsg_queue->queue + size), update_patch_size);
*(size_t*)(updater->patch_size) = update_patch_size;
// code_message_outqueue_append();
neigh = 1;
}
/*clear the temp code buff*/
delete_p(out_code->bcode);
delete_p(out_code->bcode_size);
delete_p(out_code);
}
else
{
ROS_ERROR("Patching the .bo file failed, could be corrupt patch\n");
update_try_counter++;
outqueue_append_code_request(update_no);
}
}
}
}
size = 0;
if (*(uint8_t*)(updater->inmsg_queue->queue) == RESEND_CODE)
{
size += sizeof(uint8_t);
if (*(uint16_t*)(updater->inmsg_queue->queue + size) == *(uint16_t*)(updater->update_no))
{
size += sizeof(uint16_t);
if (*(uint16_t*)(updater->inmsg_queue->queue + size) > update_try_counter)
{
update_try_counter = *(uint16_t*)(updater->inmsg_queue->queue + size);
ROS_ERROR("Code appended! update try : %u \n", update_try_counter);
code_message_outqueue_append();
}
if (update_try_counter > MAX_UPDATE_TRY)
ROS_ERROR("TODO: ROLL BACK !!");
}
}
// fprintf(stdout,"in queue freed\n");
delete_p(updater->inmsg_queue->queue);
delete_p(updater->inmsg_queue->size);
delete_p(updater->inmsg_queue);
}
void update_routine(const char* bcfname,
const char* dbgfname){
dbgf_name=(char*)dbgfname;
void create_update_patch()
{
std::stringstream genpatch;
std::stringstream usepatch;
std::string bzzfile_name(bzz_file);
std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
std::string name1 = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
name1 = name1.substr(0, name1.find_last_of("."));
std::string name2 = name1 + "-update";
// CALL BSDIFF CMD
genpatch << "bsdiff " << path << name1 << ".bo " << path << name2 << ".bo " << path << "diff.bo";
fprintf(stdout, "Launching bsdiff command: %s \n", genpatch.str().c_str());
system(genpatch.str().c_str());
// BETTER: CALL THE FUNCTION IN BSDIFF.CPP
// bsdiff_do(path + name1 + ".bo", path + name2 + ".bo", path + "diff.bo");
/* delete old files & rename new files */
remove((path + name1 + ".bo").c_str());
remove((path + name1 + ".bdb").c_str());
rename((path + name2 + ".bo").c_str(), (path + name1 + ".bo").c_str());
rename((path + name2 + ".bdb").c_str(), (path + name1 + ".bdb").c_str());
/*Read the diff file */
std::stringstream patchfileName;
patchfileName << path << "diff.bo";
uint8_t* patch_buff = 0;
FILE* fp = fopen(patchfileName.str().c_str(), "rb");
if (!fp)
{
perror(patchfileName.str().c_str());
}
fseek(fp, 0, SEEK_END);
size_t patch_size = ftell(fp);
rewind(fp);
patch_buff = (uint8_t*)malloc(patch_size);
if (fread(patch_buff, 1, patch_size, fp) < patch_size)
{
perror(patchfileName.str().c_str());
fclose(fp);
}
fclose(fp);
delete_p(updater->patch);
updater->patch = patch_buff;
*(size_t*)(updater->patch_size) = patch_size;
/* Delete the diff file */
remove(patchfileName.str().c_str());
}
void update_routine()
{
buzzvm_t VM = buzz_utility::get_vm();
buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1));
buzzvm_pushi(VM, *(uint16_t*)(updater->update_no));
buzzvm_gstore(VM);
//fprintf(stdout,"[Debug : ]updater value = %i \n",updater->mode);
if(*(int*)updater->mode==CODE_RUNNING){
// fprintf(stdout,"[Debug : ]updater value = %i \n",updater->mode);
if (*(int*)updater->mode == CODE_RUNNING)
{
buzzvm_function_call(VM, "updated_neigh", 0);
if(check_update()){
// Check for update
if (check_update())
{
ROS_INFO("Update found \nUpdating script ...\n");
if(compile_bzz()){
if (compile_bzz(bzz_file))
{
ROS_WARN("Errors in comipilg script so staying on old script\n");
}
else {
else
{
std::string bzzfile_name(bzz_file);
stringstream bzzfile_in_compile;
std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
name = name.substr(0, name.find_last_of("."));
bzzfile_in_compile << path << name << "-update.bo";
uint8_t* BO_BUF = 0;
FILE* fp = fopen(bcfname, "rb"); // to change file edit
if(!fp) {
perror(bcfname);
FILE* fp = fopen(bzzfile_in_compile.str().c_str(), "rb"); // to change file edit
if (!fp)
{
perror(bzzfile_in_compile.str().c_str());
}
fseek(fp, 0, SEEK_END);
size_t bcode_size = ftell(fp);
rewind(fp);
BO_BUF = (uint8_t*)malloc(bcode_size);
if(fread(BO_BUF, 1, bcode_size, fp) < bcode_size) {
if (fread(BO_BUF, 1, bcode_size, fp) < bcode_size)
{
perror(bcfname);
fclose(fp);
}
fclose(fp);
if(test_set_code(BO_BUF, dbgfname,bcode_size)){
uint16_t update_no =*(uint16_t*)(updater->update_no);
*(uint16_t*)(updater->update_no) =update_no +1;
code_message_outqueue_append();
if (test_set_code(BO_BUF, dbgf_name, bcode_size))
{
uint16_t update_no = *(uint16_t*)(updater->update_no);
*(uint16_t*)(updater->update_no) = update_no + 1;
create_update_patch();
VM = buzz_utility::get_vm();
ROS_INFO("Current Update no %d\n", *(uint16_t*)(updater->update_no));
buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1));
buzzvm_pushi(VM, *(uint16_t*)(updater->update_no));
buzzvm_gstore(VM);
neigh=-1;
neigh = -1;
ROS_INFO("Sending code... \n");
code_message_outqueue_append();
}
delete_p(BO_BUF);
}
}
}
}
else{
if(neigh==0 && (!is_msg_present())){
ROS_INFO("Sending code... \n");
code_message_outqueue_append();
}
else
{
timer_steps++;
buzzvm_pushs(VM, buzzvm_string_register(VM, "barrier_val",1));
buzzvm_pushs(VM, buzzvm_string_register(VM, "barrier_val", 1));
buzzvm_gload(VM);
buzzobj_t tObj = buzzvm_stack_at(VM, 1);
buzzvm_pop(VM);
ROS_INFO("Barrier ..................... %i \n",tObj->i.value);
if(tObj->i.value==no_of_robot) {
ROS_INFO("Barrier ..................... %i \n", tObj->i.value);
if (tObj->i.value == no_of_robot)
{
*(int*)(updater->mode) = CODE_RUNNING;
gettimeofday(&t2, NULL);
//collect_data();
buzz_utility::buzz_update_set((updater)->bcode, (char*)dbgfname, *(size_t*)(updater->bcode_size));
//buzzvm_function_call(m_tBuzzVM, "updated", 0);
updated=1;
// collect_data();
buzz_utility::buzz_update_set((updater)->bcode, (char*)dbgf_name, *(size_t*)(updater->bcode_size));
// buzzvm_function_call(m_tBuzzVM, "updated", 0);
updated = 1;
update_try_counter = 0;
timer_steps = 0;
}
else if (timer_steps > TIMEOUT_FOR_ROLLBACK)
{
ROS_ERROR("TIME OUT Reached, decided to roll back");
/*Time out hit deceided to roll back*/
*(int*)(updater->mode) = CODE_RUNNING;
buzz_utility::buzz_script_set(old_bcfname, dbgf_name, (int)VM->robot);
updated = 1;
update_try_counter = 0;
timer_steps = 0;
}
}
}
uint8_t* getupdater_out_msg(){
return (uint8_t*)updater->outmsg_queue->queue;
uint8_t* getupdater_out_msg()
{
return (uint8_t*)updater->outmsg_queue->queue;
}
uint8_t* getupdate_out_msg_size(){
//fprintf(stdout,"[Debug from get out size in util: ]size = %i \n",*(uint16_t*)updater->outmsg_queue->size);
return (uint8_t*)updater->outmsg_queue->size;
uint8_t* getupdate_out_msg_size()
{
// fprintf(stdout,"[Debug from get out size in util: ]size = %i \n",*(uint16_t*)updater->outmsg_queue->size);
return (uint8_t*)updater->outmsg_queue->size;
}
int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_size ){
if(buzz_utility::buzz_update_init_test(BO_BUF, dbgfname,bcode_size)){
int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size)
{
if (buzz_utility::buzz_update_init_test(BO_BUF, dbgfname, bcode_size))
{
ROS_WARN("Initializtion of script test passed\n");
if(buzz_utility::update_step_test()){
if (buzz_utility::update_step_test())
{
/*data logging*/
//start =1;
old_byte_code_size = *(size_t*)updater->bcode_size;
/*data logging*/
ROS_WARN("Step test passed\n");
*(int*) (updater->mode) = CODE_STANDBY;
//fprintf(stdout,"updater value = %i\n",updater->mode);
*(int*)(updater->mode) = CODE_STANDBY;
// fprintf(stdout,"updater value = %i\n",updater->mode);
delete_p(updater->bcode);
updater->bcode = (uint8_t*)malloc(bcode_size);
memcpy(updater->bcode, BO_BUF, bcode_size);
*(size_t*)updater->bcode_size = bcode_size;
buzz_utility::buzz_update_init_test((updater)->standby_bcode,
(char*)dbgfname,(size_t) *(size_t*)(updater->standby_bcode_size));
buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname,
(size_t) * (size_t*)(updater->standby_bcode_size));
buzzvm_t VM = buzz_utility::get_vm();
gettimeofday(&t1, NULL);
buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1));
@ -304,36 +591,40 @@ int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_size ){
return 1;
}
/*Unable to step something wrong*/
else{
if(*(int*) (updater->mode) == CODE_RUNNING){
else
{
if (*(int*)(updater->mode) == CODE_RUNNING)
{
ROS_ERROR("step test failed, stick to old script\n");
buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (size_t)*(size_t*)(updater->bcode_size));
buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (size_t) * (size_t*)(updater->bcode_size));
}
else{
else
{
/*You will never reach here*/
ROS_ERROR("step test failed, Return to stand by\n");
buzz_utility::buzz_update_init_test((updater)->standby_bcode,
(char*)dbgfname,(size_t) *(size_t*)(updater->standby_bcode_size));
buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname,
(size_t) * (size_t*)(updater->standby_bcode_size));
buzzvm_t VM = buzz_utility::get_vm();
buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1));
buzzvm_pushi(VM, no_of_robot);
buzzvm_gstore(VM);
}
return 0;
}
}
else {
if(*(int*) (updater->mode) == CODE_RUNNING){
else
{
if (*(int*)(updater->mode) == CODE_RUNNING)
{
ROS_ERROR("Initialization test failed, stick to old script\n");
buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname,(int)*(size_t*) (updater->bcode_size));
buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (int)*(size_t*)(updater->bcode_size));
}
else{
else
{
/*You will never reach here*/
ROS_ERROR("Initialization test failed, Return to stand by\n");
buzz_utility::buzz_update_init_test((updater)->standby_bcode,
(char*)dbgfname,(size_t) *(size_t*)(updater->standby_bcode_size));
buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname,
(size_t) * (size_t*)(updater->standby_bcode_size));
buzzvm_t VM = buzz_utility::get_vm();
buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1));
buzzvm_pushi(VM, no_of_robot);
@ -343,88 +634,95 @@ int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_size ){
}
}
void destroy_out_msg_queue(){
//fprintf(stdout,"out queue freed\n");
void destroy_out_msg_queue()
{
// fprintf(stdout,"out queue freed\n");
delete_p(updater->outmsg_queue->queue);
delete_p(updater->outmsg_queue->size);
delete_p(updater->outmsg_queue);
updater_msg_ready=0;
updater_msg_ready = 0;
}
int get_update_status(){
return updated;
int get_update_status()
{
return updated;
}
void set_read_update_status(){
updated=0;
void set_read_update_status()
{
updated = 0;
}
int get_update_mode(){
return (int)*(int*)(updater->mode);
int get_update_mode()
{
return (int)*(int*)(updater->mode);
}
int is_msg_present(){
return updater_msg_ready;
int is_msg_present()
{
return updater_msg_ready;
}
buzz_updater_elem_t get_updater(){
return updater;
buzz_updater_elem_t get_updater()
{
return updater;
}
void destroy_updater(){
void destroy_updater()
{
delete_p(updater->bcode);
delete_p(updater->bcode_size);
delete_p(updater->standby_bcode);
delete_p(updater->standby_bcode_size);
delete_p(updater->mode);
delete_p(updater->update_no);
if(updater->outmsg_queue){
if (updater->outmsg_queue)
{
delete_p(updater->outmsg_queue->queue);
delete_p(updater->outmsg_queue->size);
delete_p(updater->outmsg_queue);
}
if(updater->inmsg_queue){
if (updater->inmsg_queue)
{
delete_p(updater->inmsg_queue->queue);
delete_p(updater->inmsg_queue->size);
delete_p(updater->inmsg_queue);
}
//
inotify_rm_watch(fd,wd);
inotify_rm_watch(fd, wd);
close(fd);
}
void set_bzz_file(const char* in_bzz_file){
bzz_file=in_bzz_file;
void set_bzz_file(const char* in_bzz_file)
{
bzz_file = in_bzz_file;
}
void updates_set_robots(int robots){
no_of_robot=robots;
void updates_set_robots(int robots)
{
no_of_robot = robots;
}
/*--------------------------------------------------------
/ Create Buzz bytecode from the bzz script inputed
/-------------------------------------------------------*/
int compile_bzz(){
int compile_bzz(std::string bzz_file)
{
/*Compile the buzz code .bzz to .bo*/
std::string bzzfile_name(bzz_file);
stringstream bzzfile_in_compile;
std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
name = name.substr(0,name.find_last_of("."));
name = name.substr(0, name.find_last_of("."));
bzzfile_in_compile << "bzzc -I " << path << "include/"; //<<" "<<path<< name<<".basm";
bzzfile_in_compile << " -b " << path << name << ".bo";
bzzfile_in_compile << " -d " << path << name << ".bdb ";
bzzfile_in_compile << " -b " << path << name << "-update.bo";
bzzfile_in_compile << " -d " << path << name << "-update.bdb ";
bzzfile_in_compile << bzzfile_name;
ROS_WARN("Launching buzz compilation for update: %s", bzzfile_in_compile.str().c_str());
return system(bzzfile_in_compile.str().c_str());
}
void collect_data(std::ofstream &logger){
//fprintf(stdout,"start and end time in data collection Info : %f,%f",(double)begin,(double)end);
void collect_data(std::ofstream& logger)
{
double time_spent = (t2.tv_sec - t1.tv_sec) * 1000.0; //(double)(end - begin) / CLOCKS_PER_SEC;
time_spent += (t2.tv_usec - t1.tv_usec) / 1000.0;
//int bytecodesize=(int);
logger<<(int)no_of_robot<<","<<neigh<<","<<(double)time_spent<<","<<(int)timer_steps<<","<<*(size_t*)updater->bcode_size<<","<<(int)*(uint8_t*)updater->update_no;
timer_steps=0;
neigh=0;
//fprintf(stdout,"Data logger Info : %i , %i , %f , %ld , %i , %d \n",(int)no_of_robot,neigh,time_spent,*(size_t*)updater->bcode_size,(int)no_of_robot,*(uint8_t*)updater->update_no);
//FILE *Fileptr;
//Fileptr=fopen("/home/ubuntu/ROS_WS/update_logger.csv", "a");
//fprintf(Fileptr,"%i,%i,%i,%i,%i,%u\n",(int)buzz_utility::get_robotid(),neigh,timer_steps,(int)*(size_t*)updater->bcode_size,(int)no_of_robot, *(uint8_t*)updater->update_no);
//fclose(Fileptr);
// RID,update trigger,time steps taken,old byte code size, new bytecode size, patch size,update number,
// Patch_packets_received_counter,Patch_request_packets_received,Patch_packets_sent_counter,Patch_request_packets_sent_counter
logger << (int)no_of_robot << "," << neigh << "," << (double)time_spent << "," << (int)timer_steps << ","
<< old_byte_code_size << "," << *(size_t*)updater->bcode_size << "," << *(size_t*)updater->patch_size << ","
<< (int)*(uint8_t*)updater->update_no << "," << (int)packet_id_;
}

700
src/buzz_utility.cpp
View File

@ -1,234 +1,135 @@
/** @file buzz_utility.cpp
* @version 1.0
* @date 27.09.2016
* @brief Buzz Implementation as a node in ROS for Dji M100 Drone.
* @author Vivek Shankar Varadharajan
* @brief Buzz Implementation as a node in ROS.
* @author Vivek Shankar Varadharajan and David St-Onge
* @copyright 2016 MistLab. All rights reserved.
*/
#include "buzz_utility.h"
namespace buzz_utility{
namespace buzz_utility
{
/****************************************/
/****************************************/
/****************************************/
/****************************************/
static buzzvm_t VM = 0;
static char* BO_FNAME = 0;
static uint8_t* BO_BUF = 0;
static buzzdebug_t DBG_INFO = 0;
static uint32_t MAX_MSG_SIZE = 250; // Maximum Msg size for sending update packets
static uint8_t Robot_id = 0;
static std::vector<uint8_t*> IN_MSG;
std::map<int, Pos_struct> users_map;
static buzzvm_t VM = 0;
static char* BO_FNAME = 0;
static uint8_t* BO_BUF = 0;
static buzzdebug_t DBG_INFO = 0;
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;
std::map< int, Pos_struct> users_map;
/****************************************/
/****************************************/
void add_user(int id, double latitude, double longitude, float altitude)
{
Pos_struct pos_arr;
pos_arr.x=latitude;
pos_arr.y=longitude;
pos_arr.z=altitude;
map< int, buzz_utility::Pos_struct >::iterator it = users_map.find(id);
if(it!=users_map.end())
users_map.erase(it);
users_map.insert(make_pair(id, pos_arr));
//ROS_INFO("Buzz_utility got updated/new user: %i (%f,%f,%f)", id, latitude, longitude, altitude);
}
void update_users(){
if(users_map.size()>0) {
// Reset users information
// buzzusers_reset();
// create_stig_tables();
// Get user id and update user information
map< int, Pos_struct >::iterator it;
for (it=users_map.begin(); it!=users_map.end(); ++it){
//ROS_INFO("Buzz_utility will save user %i.", it->first);
buzzusers_add(it->first,
(it->second).x,
(it->second).y,
(it->second).z);
}
}
}
/*int buzzusers_reset() {
if(VM->state != BUZZVM_STATE_READY) return VM->state;
//Make new table
buzzobj_t t = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE);
//make_table(&t);
if(VM->state != BUZZVM_STATE_READY) return VM->state;
//Register table as global symbol
//buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
buzzvm_gload(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1));
buzzvm_tget(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
buzzvm_gload(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "dataG", 1));
buzzvm_push(VM, t);
buzzvm_gstore(VM);
//buzzvm_pushi(VM, 2);
//buzzvm_callc(VM);
return VM->state;
}*/
int buzzusers_add(int id, double latitude, double longitude, double altitude) {
if(VM->state != BUZZVM_STATE_READY) return VM->state;
// Get users "p" table
/*buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
buzzvm_gload(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "get", 1));
buzzvm_tget(VM);*/
buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
buzzvm_tget(VM);
if(buzzvm_stack_at(VM, 1)->o.type == BUZZTYPE_NIL) {
//ROS_INFO("Empty data, create a new table");
buzzvm_pop(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
buzzvm_pusht(VM);
buzzobj_t data = buzzvm_stack_at(VM, 1);
buzzvm_tput(VM);
buzzvm_push(VM, data);
}
// When we get here, the "data" table is on top of the stack
// Push user id
buzzvm_pushi(VM, id);
// Create entry table
buzzobj_t entry = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE);
// Insert latitude
buzzvm_push(VM, entry);
buzzvm_pushs(VM, buzzvm_string_register(VM, "la", 1));
buzzvm_pushf(VM, latitude);
buzzvm_tput(VM);
// Insert longitude
buzzvm_push(VM, entry);
buzzvm_pushs(VM, buzzvm_string_register(VM, "lo", 1));
buzzvm_pushf(VM, longitude);
buzzvm_tput(VM);
// Insert altitude
buzzvm_push(VM, entry);
buzzvm_pushs(VM, buzzvm_string_register(VM, "al", 1));
buzzvm_pushf(VM, altitude);
buzzvm_tput(VM);
// Save entry into data table
buzzvm_push(VM, entry);
buzzvm_tput(VM);
//ROS_INFO("Buzz_utility saved new user: %i (%f,%f,%f)", id, latitude, longitude, altitude);
// forcing the new table into the stigmergy....
/*buzzobj_t newt = buzzvm_stack_at(VM, 0);
buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
buzzvm_gload(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1));
buzzvm_tget(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "p", 1));
buzzvm_push(VM, nbr);
buzzvm_pushi(VM, 2);
buzzvm_callc(VM);*/
//buzzvm_gstore(VM);
return VM->state;
}
/**************************************************************************/
/*Deserializes uint64_t into 4 uint16_t, freeing out is left to the user */
/**************************************************************************/
uint16_t* u64_cvt_u16(uint64_t u64){
/**************************************************************************/
/*Deserializes uint64_t into 4 uint16_t, freeing out is left to the user */
/**************************************************************************/
uint16_t* u64_cvt_u16(uint64_t u64)
{
uint16_t* out = new uint16_t[4];
uint32_t int32_1 = u64 & 0xFFFFFFFF;
uint32_t int32_2 = (u64 & 0xFFFFFFFF00000000 ) >> 32;
uint32_t int32_2 = (u64 & 0xFFFFFFFF00000000) >> 32;
out[0] = int32_1 & 0xFFFF;
out[1] = (int32_1 & (0xFFFF0000) ) >> 16;
out[1] = (int32_1 & (0xFFFF0000)) >> 16;
out[2] = int32_2 & 0xFFFF;
out[3] = (int32_2 & (0xFFFF0000) ) >> 16;
//cout << " values " <<out[0] <<" "<<out[1] <<" "<<out[2] <<" "<<out[3] <<" ";
out[3] = (int32_2 & (0xFFFF0000)) >> 16;
// cout << " values " <<out[0] <<" "<<out[1] <<" "<<out[2] <<" "<<out[3] <<" ";
return out;
}
}
int get_robotid() {
int get_robotid()
{
return Robot_id;
}
/***************************************************/
/*Appends obtained messages to buzz in message Queue*/
/***************************************************/
}
/***************************************************/
/*Appends obtained messages to buzz in message Queue*/
/***************************************************/
/*******************************************************************************************************************/
/* Message format of payload (Each slot is uint64_t) */
/* _______________________________________________________________________________________________________________ */
/*| | |*/
/*|Size in Uint64_t(but size is Uint16_t)|robot_id|Update msg size|Update msg|Update msgs+Buzz_msgs with size.....|*/
/*|__________________________________________________________________________|____________________________________|*/
/*******************************************************************************************************************/
void in_msg_append(uint64_t* payload){
/*******************************************************************************************************************/
/* Message format of payload (Each slot is uint64_t) */
/* _______________________________________________________________________________________________________________ */
/*| | |*/
/*|Size in Uint64_t(but size is Uint16_t)|robot_id|Update msg size|Update msg|Update msgs+Buzz_msgs with size.....|*/
/*|__________________________________________________________________________|____________________________________|*/
/*******************************************************************************************************************/
void in_msg_append(uint64_t* payload)
{
/* Go through messages and append them to the vector */
uint16_t* data= u64_cvt_u16((uint64_t)payload[0]);
uint16_t* data = u64_cvt_u16((uint64_t)payload[0]);
/*Size is at first 2 bytes*/
uint16_t size=data[0]*sizeof(uint64_t);
uint16_t size = data[0] * sizeof(uint64_t);
delete[] data;
uint8_t* pl =(uint8_t*)malloc(size);
uint8_t* pl = (uint8_t*)malloc(size);
/* Copy packet into temporary buffer */
memcpy(pl, payload ,size);
memcpy(pl, payload, size);
IN_MSG.push_back(pl);
}
}
void in_message_process(){
while(!IN_MSG.empty()){
void in_message_process()
{
while (!IN_MSG.empty())
{
/* Go through messages and append them to the FIFO */
uint8_t* first_INmsg = (uint8_t*)IN_MSG.front();
size_t tot =0;
size_t tot = 0;
/*Size is at first 2 bytes*/
uint16_t size=(*(uint16_t*)first_INmsg)*sizeof(uint64_t);
uint16_t size = (*(uint16_t*)first_INmsg) * sizeof(uint64_t);
tot += sizeof(uint16_t);
/*Decode neighbor Id*/
uint16_t neigh_id =*(uint16_t*)(first_INmsg+tot);
tot+=sizeof(uint16_t);
uint16_t neigh_id = *(uint16_t*)(first_INmsg + tot);
tot += sizeof(uint16_t);
/* Go through the messages until there's nothing else to read */
uint16_t unMsgSize=0;
uint16_t unMsgSize = 0;
/*Obtain Buzz messages push it into queue*/
do {
do
{
/* Get payload size */
unMsgSize = *(uint16_t*)(first_INmsg + tot);
tot += sizeof(uint16_t);
/* Append message to the Buzz input message queue */
if(unMsgSize > 0 && unMsgSize <= size - tot ) {
buzzinmsg_queue_append(VM,
neigh_id,
buzzmsg_payload_frombuffer(first_INmsg +tot, unMsgSize));
if (unMsgSize > 0 && unMsgSize <= size - tot)
{
buzzinmsg_queue_append(VM, neigh_id, buzzmsg_payload_frombuffer(first_INmsg + tot, unMsgSize));
tot += unMsgSize;
}
}while(size - tot > sizeof(uint16_t) && unMsgSize > 0);
} while (size - tot > sizeof(uint16_t) && unMsgSize > 0);
free(first_INmsg);
IN_MSG.erase(IN_MSG.begin());
}
/* Process messages VM call*/
buzzvm_process_inmsgs(VM);
}
}
/***************************************************/
/*Obtains messages from buzz out message Queue*/
/***************************************************/
uint64_t* obt_out_msg(){
/***************************************************/
/*Obtains messages from buzz out message Queue*/
/***************************************************/
uint64_t* obt_out_msg()
{
/* Process out messages */
buzzvm_process_outmsgs(VM);
uint8_t* buff_send =(uint8_t*)malloc(MAX_MSG_SIZE);
uint8_t* buff_send = (uint8_t*)malloc(MAX_MSG_SIZE);
memset(buff_send, 0, MAX_MSG_SIZE);
/*Taking into consideration the sizes included at the end*/
ssize_t tot = sizeof(uint16_t);
/* Send robot id */
*(uint16_t*)(buff_send+tot) = (uint16_t) VM->robot;
*(uint16_t*)(buff_send + tot) = (uint16_t)VM->robot;
tot += sizeof(uint16_t);
/* Send messages from FIFO */
do {
do
{
/* Are there more messages? */
if(buzzoutmsg_queue_isempty(VM)) break;
if (buzzoutmsg_queue_isempty(VM))
break;
/* Get first message */
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)) > MAX_MSG_SIZE) {
// 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)) > MAX_MSG_SIZE)
{
buzzmsg_payload_destroy(&m);
break;
}
@ -244,53 +145,48 @@ void in_message_process(){
/* Get rid of message */
buzzoutmsg_queue_next(VM);
buzzmsg_payload_destroy(&m);
} while(1);
} while (1);
uint16_t total_size =(ceil((float)tot/(float)sizeof(uint64_t)));
*(uint16_t*)buff_send = (uint16_t) total_size;
uint16_t total_size = (ceil((float)tot / (float)sizeof(uint64_t)));
*(uint16_t*)buff_send = (uint16_t)total_size;
uint64_t* payload_64 = new uint64_t[total_size];
memcpy((void*)payload_64, (void*)buff_send, total_size*sizeof(uint64_t));
memcpy((void*)payload_64, (void*)buff_send, total_size * sizeof(uint64_t));
free(buff_send);
/*for(int i=0;i<total_size;i++){
cout<<" payload from out msg "<<*(payload_64+i)<<endl;
}*/
/* Send message */
return payload_64;
}
}
/****************************************/
/*Buzz script not able to load*/
/****************************************/
/****************************************/
/*Buzz script not able to load*/
/****************************************/
static const char* buzz_error_info() {
static const char* buzz_error_info()
{
buzzdebug_entry_t dbg = *buzzdebug_info_get_fromoffset(DBG_INFO, &VM->pc);
char* msg;
if(dbg != NULL) {
asprintf(&msg,
"%s: execution terminated abnormally at %s:%" PRIu64 ":%" PRIu64 " : %s\n\n",
BO_FNAME,
dbg->fname,
dbg->line,
dbg->col,
VM->errormsg);
if (dbg != NULL)
{
asprintf(&msg, "%s: execution terminated abnormally at %s:%" PRIu64 ":%" PRIu64 " : %s\n\n", BO_FNAME, dbg->fname,
dbg->line, dbg->col, VM->errormsg);
}
else {
asprintf(&msg,
"%s: execution terminated abnormally at bytecode offset %d: %s\n\n",
BO_FNAME,
VM->pc,
VM->errormsg);
else
{
asprintf(&msg, "%s: execution terminated abnormally at bytecode offset %d: %s\n\n", BO_FNAME, VM->pc, VM->errormsg);
}
return msg;
}
}
/****************************************/
/*Buzz hooks that can be used inside .bzz file*/
/****************************************/
/****************************************/
/*Buzz hooks that can be used inside .bzz file*/
/****************************************/
static int buzz_register_hooks() {
static int buzz_register_hooks()
{
buzzvm_pushs(VM, buzzvm_string_register(VM, "print", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
buzzvm_gstore(VM);
@ -335,13 +231,14 @@ void in_message_process(){
buzzvm_gstore(VM);
return VM->state;
}
}
/**************************************************/
/*Register dummy Buzz hooks for test during update*/
/**************************************************/
/**************************************************/
/*Register dummy Buzz hooks for test during update*/
/**************************************************/
static int testing_buzz_register_hooks() {
static int testing_buzz_register_hooks()
{
buzzvm_pushs(VM, buzzvm_string_register(VM, "print", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
buzzvm_gstore(VM);
@ -386,92 +283,19 @@ void in_message_process(){
buzzvm_gstore(VM);
return VM->state;
}
int create_stig_tables() {
/*
// usersvstig = stigmergy.create(123)
buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
// get the stigmergy table from the global scope
buzzvm_pushs(VM, buzzvm_string_register(VM, "stigmergy", 1));
buzzvm_gload(VM);
// get the create method from the stigmergy table
buzzvm_pushs(VM, buzzvm_string_register(VM, "create", 1));
buzzvm_tget(VM);
// value of the stigmergy id
buzzvm_pushi(VM, 5);
// call the stigmergy.create() method
buzzvm_dump(VM);
// buzzvm_closure_call(VM, 1);
buzzvm_pushi(VM, 1);
buzzvm_callc(VM);
buzzvm_gstore(VM);
buzzvm_dump(VM);
//buzzusers_reset();
buzzobj_t t = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE);
buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
buzzvm_gload(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1));
buzzvm_tget(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "p", 1));
buzzvm_push(VM, t);
buzzvm_dump(VM);
// buzzvm_closure_call(VM, 2);
buzzvm_pushi(VM, 2);
buzzvm_callc(VM);
//buzzvm_gstore(VM);
//buzzvm_dump(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
buzzvm_gload(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1));
buzzvm_tget(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "u", 1));
buzzvm_pusht(VM);
buzzvm_pushi(VM, 2);
buzzvm_call(VM, 0);
buzzvm_gstore(VM);*/
buzzobj_t t = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE);
buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
buzzvm_push(VM,t);
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
buzzvm_gload(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "dataG", 1));
buzzvm_pusht(VM);
buzzobj_t data = buzzvm_stack_at(VM, 1);
buzzvm_tput(VM);
buzzvm_push(VM, data);
buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
buzzvm_gload(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "dataL", 1));
buzzvm_pusht(VM);
data = buzzvm_stack_at(VM, 1);
buzzvm_tput(VM);
buzzvm_push(VM, data);
return VM->state;
}
/****************************************/
/*Sets the .bzz and .bdbg file*/
/****************************************/
int buzz_script_set(const char* bo_filename,
const char* bdbg_filename, int robot_id) {
ROS_INFO(" Robot ID: %i" , robot_id);
/* Reset the Buzz VM */
if(VM) buzzvm_destroy(&VM);
/****************************************/
/*Sets the .bzz and .bdbg file*/
/****************************************/
int buzz_script_set(const char* bo_filename, const char* bdbg_filename, int robot_id)
{
ROS_INFO(" Robot ID: %i", robot_id);
Robot_id = robot_id;
VM = buzzvm_new((int)robot_id);
/* Get rid of debug info */
if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
DBG_INFO = buzzdebug_new();
/* Read bytecode and fill in data structure */
/* Read bytecode from file and fill the bo buffer*/
FILE* fd = fopen(bo_filename, "rb");
if(!fd) {
if (!fd)
{
perror(bo_filename);
return 0;
}
@ -479,7 +303,8 @@ int create_stig_tables() {
size_t bcode_size = ftell(fd);
rewind(fd);
BO_BUF = (uint8_t*)malloc(bcode_size);
if(fread(BO_BUF, 1, bcode_size, fd) < bcode_size) {
if (fread(BO_BUF, 1, bcode_size, fd) < bcode_size)
{
perror(bo_filename);
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
@ -487,90 +312,46 @@ int create_stig_tables() {
return 0;
}
fclose(fd);
/* Read debug information */
if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
perror(bdbg_filename);
return 0;
}
/* Set byte code */
if(buzzvm_set_bcode(VM, BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
ROS_ERROR("[%i] %s: Error loading Buzz script", Robot_id, bo_filename);
return 0;
}
/* Register hook functions */
if(buzz_register_hooks() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
ROS_ERROR("[%i] Error registering hooks", 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);
buzzdebug_destroy(&DBG_INFO);
ROS_ERROR("[%i] Error creating stigmergy tables", Robot_id);
//cout << "ERROR!!!! ---------- " << VM->errormsg << endl;
//cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl;
return 0;
}*/
/* Save bytecode file name */
BO_FNAME = strdup(bo_filename);
// Execute the global part of the script
if(buzzvm_execute_script(VM)!= BUZZVM_STATE_DONE){
ROS_ERROR("Error executing global part, VM state : %i",VM->state);
return 0;
}
// Call the Init() function
if(buzzvm_function_call(VM, "init", 0) != BUZZVM_STATE_READY){
ROS_ERROR("Error in calling init, VM state : %i", VM->state);
return 0;
}
/* All OK */
return buzz_update_set(BO_BUF, bdbg_filename, bcode_size);
}
ROS_INFO("[%i] INIT DONE!!!", Robot_id);
return 1;//buzz_update_set(BO_BUF, bdbg_filename, bcode_size);
}
/****************************************/
/*Sets a new update */
/****************************************/
int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){
/****************************************/
/*Sets a new update */
/****************************************/
int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_size)
{
// Reset the Buzz VM
if(VM) buzzvm_destroy(&VM);
if (VM)
buzzvm_destroy(&VM);
VM = buzzvm_new(Robot_id);
// Get rid of debug info
if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
if (DBG_INFO)
buzzdebug_destroy(&DBG_INFO);
DBG_INFO = buzzdebug_new();
// Read debug information
if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) {
if (!buzzdebug_fromfile(DBG_INFO, bdbg_filename))
{
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
perror(bdbg_filename);
return 0;
}
// Set byte code
if(buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
if (buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY)
{
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
ROS_ERROR("[%i] %s: Error loading Buzz bytecode (update)", Robot_id);
return 0;
}
// Register hook functions
if(buzz_register_hooks() != BUZZVM_STATE_READY) {
if (buzz_register_hooks() != BUZZVM_STATE_READY)
{
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
ROS_ERROR("[%i] Error registering hooks (update)", Robot_id);
@ -582,57 +363,55 @@ int create_stig_tables() {
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);
buzzdebug_destroy(&DBG_INFO);
ROS_ERROR("[%i] Error creating stigmergy tables", Robot_id);
//cout << "ERROR!!!! ---------- " << VM->errormsg << endl;
//cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl;
return 0;
}*/
// Execute the global part of the script
if(buzzvm_execute_script(VM)!= BUZZVM_STATE_DONE){
ROS_ERROR("Error executing global part, VM state : %i",VM->state);
if (buzzvm_execute_script(VM) != BUZZVM_STATE_DONE)
{
ROS_ERROR("Error executing global part, VM state : %i", VM->state);
return 0;
}
// Call the Init() function
if(buzzvm_function_call(VM, "init", 0) != BUZZVM_STATE_READY){
if (buzzvm_function_call(VM, "init", 0) != BUZZVM_STATE_READY)
{
ROS_ERROR("Error in calling init, VM state : %i", VM->state);
return 0;
}
// All OK
return 1;
}
}
/****************************************/
/*Performs a initialization test */
/****************************************/
int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){
/****************************************/
/*Performs a initialization test */
/****************************************/
int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_size)
{
// Reset the Buzz VM
if(VM) buzzvm_destroy(&VM);
if (VM)
buzzvm_destroy(&VM);
VM = buzzvm_new(Robot_id);
// Get rid of debug info
if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
if (DBG_INFO)
buzzdebug_destroy(&DBG_INFO);
DBG_INFO = buzzdebug_new();
// Read debug information
if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) {
if (!buzzdebug_fromfile(DBG_INFO, bdbg_filename))
{
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
perror(bdbg_filename);
return 0;
}
// Set byte code
if(buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
if (buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY)
{
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
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) {
if (testing_buzz_register_hooks() != BUZZVM_STATE_READY)
{
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
ROS_ERROR("[%i] Error registering hooks (update init)", Robot_id);
@ -644,77 +423,36 @@ int create_stig_tables() {
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);
buzzdebug_destroy(&DBG_INFO);
ROS_ERROR("[%i] Error creating stigmergy tables", Robot_id);
//cout << "ERROR!!!! ---------- " << VM->errormsg << endl;
//cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl;
return 0;
}*/
// Execute the global part of the script
if(buzzvm_execute_script(VM)!= BUZZVM_STATE_DONE){
ROS_ERROR("Error executing global part, VM state : %i",VM->state);
if (buzzvm_execute_script(VM) != BUZZVM_STATE_DONE)
{
ROS_ERROR("Error executing global part, VM state : %i", VM->state);
return 0;
}
// Call the Init() function
if(buzzvm_function_call(VM, "init", 0) != BUZZVM_STATE_READY){
if (buzzvm_function_call(VM, "init", 0) != BUZZVM_STATE_READY)
{
ROS_ERROR("Error in calling init, VM state : %i", VM->state);
return 0;
}
// All OK
return 1;
}
}
/****************************************/
/*Swarm struct*/
/****************************************/
/****************************************/
/*Swarm struct*/
/****************************************/
struct buzzswarm_elem_s {
struct buzzswarm_elem_s
{
buzzdarray_t swarms;
uint16_t age;
};
typedef struct buzzswarm_elem_s* buzzswarm_elem_t;
};
typedef struct buzzswarm_elem_s* buzzswarm_elem_t;
void check_swarm_members(const void* key, void* data, void* params) {
buzzswarm_elem_t e = *(buzzswarm_elem_t*)data;
int* status = (int*)params;
if(*status == 3) return;
fprintf(stderr, "CHECKING SWARM :%i, member: %i, age: %i \n",
buzzdarray_get(e->swarms, 0, uint16_t), *(uint16_t*)key, e->age);
if(buzzdarray_size(e->swarms) != 1) {
fprintf(stderr, "Swarm list size is not 1\n");
*status = 3;
}
else {
int sid = 1;
if(!buzzdict_isempty(VM->swarms)) {
if(*buzzdict_get(VM->swarms, &sid, uint8_t) &&
buzzdarray_get(e->swarms, 0, uint16_t) != sid) {
fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n",
sid,
buzzdarray_get(e->swarms, 0, uint16_t));
*status = 3;
return;
}
}
if(buzzdict_size(VM->swarms)>1) {
sid = 2;
if(*buzzdict_get(VM->swarms, &sid, uint8_t) &&
buzzdarray_get(e->swarms, 0, uint16_t) != sid) {
fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n",
sid,
buzzdarray_get(e->swarms, 0, uint16_t));
*status = 3;
return;
}
}
}
}
/*Step through the buzz script*/
void update_sensors(){
/*Step through the buzz script*/
void update_sensors()
{
/* Update sensors*/
buzzuav_closures::buzzuav_update_battery(VM);
buzzuav_closures::buzzuav_update_xbee_status(VM);
@ -722,44 +460,35 @@ int create_stig_tables() {
buzzuav_closures::buzzuav_update_currentpos(VM);
buzzuav_closures::update_neighbors(VM);
buzzuav_closures::buzzuav_update_targets(VM);
//update_users();
// update_users();
buzzuav_closures::buzzuav_update_flight_status(VM);
}
}
void buzz_script_step() {
void buzz_script_step()
{
/*Process available messages*/
in_message_process();
/*Update sensors*/
update_sensors();
/* Call Buzz step() function */
if(buzzvm_function_call(VM, "step", 0) != BUZZVM_STATE_READY) {
ROS_ERROR("%s: execution terminated abnormally: %s",
BO_FNAME,
buzz_error_info());
if (buzzvm_function_call(VM, "step", 0) != BUZZVM_STATE_READY)
{
ROS_ERROR("%s: execution terminated abnormally: %s", BO_FNAME, buzz_error_info());
buzzvm_dump(VM);
}
}
/*Print swarm*/
//buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
//int SwarmSize = buzzdict_size(VM->swarmmembers)+1;
//fprintf(stderr, "Real Swarm Size: %i\n",SwarmSize);
/****************************************/
/*Destroy the bvm and other resorces*/
/****************************************/
/* Check swarm state -- Not crashing thanks to test added in check_swarm_members */
//int status = 1;
//buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status);
}
/****************************************/
/*Destroy the bvm and other resorces*/
/****************************************/
void buzz_script_destroy() {
if(VM) {
if(VM->state != BUZZVM_STATE_READY) {
ROS_ERROR("%s: execution terminated abnormally: %s",
BO_FNAME,
buzz_error_info());
void buzz_script_destroy()
{
if (VM)
{
if (VM->state != BUZZVM_STATE_READY)
{
ROS_ERROR("%s: execution terminated abnormally: %s", BO_FNAME, buzz_error_info());
buzzvm_dump(VM);
}
buzzvm_function_call(VM, "destroy", 0);
@ -768,54 +497,55 @@ int create_stig_tables() {
buzzdebug_destroy(&DBG_INFO);
}
ROS_INFO("Script execution stopped.");
}
}
/****************************************/
/****************************************/
/****************************************/
/****************************************/
/****************************************/
/*Execution completed*/
/****************************************/
/****************************************/
/*Execution completed*/
/****************************************/
int buzz_script_done() {
int buzz_script_done()
{
return VM->state != BUZZVM_STATE_READY;
}
}
int update_step_test() {
int update_step_test()
{
/*Process available messages*/
in_message_process();
buzzuav_closures::buzzuav_update_battery(VM);
buzzuav_closures::buzzuav_update_prox(VM);
buzzuav_closures::buzzuav_update_currentpos(VM);
buzzuav_closures::update_neighbors(VM);
//update_users();
buzzuav_closures::buzzuav_update_flight_status(VM);
//set_robot_var(buzzdict_size(VM->swarmmembers)+1);
int a = buzzvm_function_call(VM, "step", 0);
if(a!= BUZZVM_STATE_READY) {
ROS_ERROR("%s: execution terminated abnormally: %s\n\n",
BO_FNAME,
buzz_error_info());
fprintf(stdout, "step test VM state %i\n",a);
if (a != BUZZVM_STATE_READY)
{
ROS_ERROR("%s: execution terminated abnormally: %s\n\n", BO_FNAME, buzz_error_info());
fprintf(stdout, "step test VM state %i\n", a);
}
return a == BUZZVM_STATE_READY;
}
}
buzzvm_t get_vm() {
buzzvm_t get_vm()
{
return VM;
}
}
void set_robot_var(int ROBOTS){
void set_robot_var(int ROBOTS)
{
buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1));
buzzvm_pushi(VM, ROBOTS);
buzzvm_gstore(VM);
}
int get_inmsg_size(){
return IN_MSG.size();
}
}
int get_inmsg_size()
{
return IN_MSG.size();
}
}

637
src/buzzuav_closures.cpp
View File

@ -1,49 +1,50 @@
/** @file buzzuav_closures.cpp
* @version 1.0
* @date 27.09.2016
* @brief Buzz Implementation as a node in ROS for Dji M100 Drone.
* @author Vivek Shankar Varadharajan
* @brief Buzz Implementation as a node in ROS.
* @author Vivek Shankar Varadharajan and David St-Onge
* @copyright 2016 MistLab. All rights reserved.
*/
//#define _GNU_SOURCE
#include "buzzuav_closures.h"
#include "math.h"
namespace buzzuav_closures{
// TODO: Minimize the required global variables and put them in the header
//static const rosbzz_node::roscontroller* roscontroller_ptr;
/*forward declaration of ros controller ptr storing function*/
//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]={0,0,0};
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 float raw_packet_loss = 0.0;
static int filtered_packet_loss = 0;
static float api_rssi = 0.0;
string WPlistname = "";
namespace buzzuav_closures
{
// TODO: Minimize the required global variables and put them in the header
// static const rosbzz_node::roscontroller* roscontroller_ptr;
/*forward declaration of ros controller ptr storing function*/
// 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 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;
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)
{
std::ostringstream buffer (std::ostringstream::ate);
std::ostringstream buffer(std::ostringstream::ate);
buffer << buzz_utility::get_robotid();
for (uint32_t index = 1; index < buzzdarray_size(vm->lsyms->syms); ++index)
{
@ -84,58 +85,46 @@ int buzzros_print(buzzvm_t vm)
break;
}
}
ROS_INFO("%s",buffer.str().c_str());
ROS_INFO("%s", buffer.str().c_str());
return buzzvm_ret0(vm);
}
void setWPlist(string path){
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 = 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] = RAD2DEG(out[0]);
out[1] = RAD2DEG(out[1]);
out[2] = height; //constant height.
}
float constrainAngle(float x){
x = fmod(x,2*M_PI);
float constrainAngle(float x)
{
x = fmod(x, 2 * M_PI);
if (x < 0.0)
x += 2*M_PI;
x += 2 * M_PI;
return x;
}
}
void rb_from_gps(double nei[], double out[], double cur[]){
/*----------------------------------------/
/ Compute Range and Bearing from 2 GPS set of coordinates
/----------------------------------------*/
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]));
out[0] = sqrt(ned_x*ned_x+ned_y*ned_y);
out[1] = constrainAngle(atan2(ned_y,ned_x));
out[0] = sqrt(ned_x * ned_x + ned_y * ned_y);
out[1] = constrainAngle(atan2(ned_y, ned_x));
out[2] = 0.0;
}
}
// Hard coded GPS position in Park Maisonneuve, Montreal, Canada for simulation tests
double hcpos1[4] = {45.564489, -73.562537, 45.564140, -73.562336};
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()
{
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) {
if (!fin)
{
ROS_ERROR("GPS list parser, could not open file.");
return;
}
@ -143,27 +132,28 @@ int buzzros_print(buzzvm_t vm)
// 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.
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) ) {
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;
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())
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));
}
@ -172,13 +162,13 @@ int buzzros_print(buzzvm_t vm)
// Close the file:
fin.close();
}
}
/*----------------------------------------/
/ Buzz closure to move following a 2D vector
/----------------------------------------*/
int buzzuav_moveto(buzzvm_t vm) {
/*----------------------------------------/
/ Buzz closure to move following a 2D vector
/----------------------------------------*/
int buzzuav_moveto(buzzvm_t vm)
{
buzzvm_lnum_assert(vm, 3);
buzzvm_lload(vm, 1); /* dx */
buzzvm_lload(vm, 2); /* dy */
@ -189,37 +179,34 @@ int buzzros_print(buzzvm_t vm)
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]);
goto_pos[0] = dx;
goto_pos[1] = dy;
goto_pos[2] = height + dh;
// 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) {
if(vm->state != BUZZVM_STATE_READY) return vm->state;
int buzzuav_update_targets(buzzvm_t vm)
{
if (vm->state != BUZZVM_STATE_READY)
return vm->state;
buzzvm_pushs(vm, buzzvm_string_register(vm, "targets", 1));
//buzzobj_t t = buzzheap_newobj(vm->heap, BUZZTYPE_TABLE);
//buzzvm_push(vm, t);
buzzvm_pusht(vm);
buzzobj_t targettbl = buzzvm_stack_at(vm, 1);
//buzzvm_tput(vm);
//buzzvm_dup(vm);
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).latitude; tmp[1]=(it->second).longitude; tmp[2]=height;
map<int, buzz_utility::RB_struct>::iterator it;
for (it = targets_map.begin(); it != targets_map.end(); ++it)
{
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]);
// 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
//ROS_INFO("Buzz_utility will save user %i.", it->first);
// ROS_INFO("Buzz_utility will save user %i.", it->first);
// Push user id
buzzvm_pushi(vm, it->first);
// Create entry table
@ -241,9 +228,10 @@ int buzzros_print(buzzvm_t vm)
buzzvm_gstore(vm);
return vm->state;
}
}
int buzzuav_addtargetRB(buzzvm_t vm) {
int buzzuav_addtargetRB(buzzvm_t vm)
{
buzzvm_lnum_assert(vm, 3);
buzzvm_lload(vm, 1); // longitude
buzzvm_lload(vm, 2); // latitude
@ -259,27 +247,30 @@ int buzzros_print(buzzvm_t vm)
double rb[3];
rb_from_gps(tmp, rb, cur_pos);
if(fabs(rb[0])<100.0) {
//printf("\tGot new user from bzz stig: %i - %f, %f\n", uid, rb[0], rb[1]);
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.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);
if(it!=targets_map.end())
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);
if (it != targets_map.end())
targets_map.erase(it);
targets_map.insert(make_pair(uid, RB_arr));
//ROS_INFO("Buzz_utility got updated/new user: %i (%f,%f,%f)", id, latitude, longitude, altitude);
// ROS_INFO("Buzz_utility got updated/new user: %i (%f,%f,%f)", id, latitude, longitude, altitude);
return vm->state;
} else
ROS_WARN(" ---------- Target too far %f",rb[0]);
}
else
ROS_WARN(" ---------- Target too far %f", rb[0]);
return 0;
}
}
int buzzuav_addNeiStatus(buzzvm_t vm){
int buzzuav_addNeiStatus(buzzvm_t vm)
{
buzzvm_lnum_assert(vm, 5);
buzzvm_lload(vm, 1); // fc
buzzvm_lload(vm, 2); // xbee
@ -293,21 +284,23 @@ int buzzros_print(buzzvm_t vm)
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);
if(it!=neighbors_status_map.end())
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));
return vm->state;
}
}
mavros_msgs::Mavlink get_status(){
mavros_msgs::Mavlink get_status()
{
mavros_msgs::Mavlink payload_out;
map< int, buzz_utility::neighbors_status >::iterator it;
for (it= neighbors_status_map.begin(); it!= neighbors_status_map.end(); ++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);
payload_out.payload64.push_back(it->second.batt_lvl);
@ -320,20 +313,22 @@ int buzzros_print(buzzvm_t vm)
message_number++;*/
return payload_out;
}
/*----------------------------------------/
/ Buzz closure to take a picture here.
/----------------------------------------*/
int buzzuav_takepicture(buzzvm_t vm) {
//cur_cmd = mavros_msgs::CommandCode::CMD_DO_SET_CAM_TRIGG_DIST;
}
/*----------------------------------------/
/ Buzz closure to take a picture here.
/----------------------------------------*/
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) {
/*----------------------------------------/
/ 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
@ -348,15 +343,16 @@ int buzzros_print(buzzvm_t vm)
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]);
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) {
/*----------------------------------------/
/ 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
@ -368,8 +364,9 @@ int buzzros_print(buzzvm_t vm)
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)
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;
@ -380,131 +377,135 @@ int buzzros_print(buzzvm_t vm)
double rb[3];
rb_from_gps(goal, rb, cur_pos);
if(fabs(rb[0])<150.0)
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]);
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);
}
}
/*----------------------------------------/
/ Buzz closure to arm/disarm the drone, useful for field tests to ensure all systems are up and running
/----------------------------------------*/
int buzzuav_arm(buzzvm_t vm) {
cur_cmd=mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM;
/*----------------------------------------/
/ Buzz closure to arm/disarm the drone, useful for field tests to ensure all systems are up and running
/----------------------------------------*/
int buzzuav_arm(buzzvm_t vm)
{
cur_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM;
printf(" Buzz requested Arm \n");
buzz_cmd=COMMAND_ARM;
buzz_cmd = COMMAND_ARM;
return buzzvm_ret0(vm);
}
int buzzuav_disarm(buzzvm_t vm) {
cur_cmd=mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM + 1;
}
int buzzuav_disarm(buzzvm_t vm)
{
cur_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM + 1;
printf(" Buzz requested Disarm \n");
buzz_cmd=COMMAND_DISARM;
buzz_cmd = COMMAND_DISARM;
return buzzvm_ret0(vm);
}
}
/*---------------------------------------/
/ Buzz closure for basic UAV commands
/---------------------------------------*/
int buzzuav_takeoff(buzzvm_t vm) {
/*---------------------------------------/
/ Buzz closure for basic UAV commands
/---------------------------------------*/
int buzzuav_takeoff(buzzvm_t vm)
{
buzzvm_lnum_assert(vm, 1);
buzzvm_lload(vm, 1); /* Altitude */
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
goto_pos[2] = buzzvm_stack_at(vm, 1) -> f.value;
goto_pos[2] = buzzvm_stack_at(vm, 1)->f.value;
height = goto_pos[2];
cur_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF;
cur_cmd = mavros_msgs::CommandCode::NAV_TAKEOFF;
printf(" Buzz requested Take off !!! \n");
buzz_cmd = COMMAND_TAKEOFF;
return buzzvm_ret0(vm);
}
}
int buzzuav_land(buzzvm_t vm) {
cur_cmd=mavros_msgs::CommandCode::NAV_LAND;
int buzzuav_land(buzzvm_t vm)
{
cur_cmd = mavros_msgs::CommandCode::NAV_LAND;
printf(" Buzz requested Land !!! \n");
buzz_cmd = COMMAND_LAND;
return buzzvm_ret0(vm);
}
}
int buzzuav_gohome(buzzvm_t vm) {
cur_cmd=mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
int buzzuav_gohome(buzzvm_t vm)
{
cur_cmd = mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
printf(" Buzz requested gohome !!! \n");
buzz_cmd = COMMAND_GOHOME;
return buzzvm_ret0(vm);
}
}
/*-------------------------------
/ Get/Set to transfer variable from Roscontroller to buzzuav
/------------------------------------*/
double* getgoto() {
/*-------------------------------
/ Get/Set to transfer variable from Roscontroller to buzzuav
/------------------------------------*/
double* getgoto()
{
return goto_pos;
}
}
float* getgimbal() {
float* getgimbal()
{
return rc_gimbal;
}
}
string getuavstate() {
string getuavstate()
{
static buzzvm_t VM = buzz_utility::get_vm();
std::stringstream state_buff;
buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE",1));
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() {
int getcmd()
{
return cur_cmd;
}
}
void set_goto(double pos[]) {
goto_pos[0] = pos[0];
goto_pos[1] = pos[1];
goto_pos[2] = pos[2];
}
int bzz_cmd() {
int bzz_cmd()
{
int cmd = buzz_cmd;
buzz_cmd = 0;
return cmd;
}
}
void rc_set_goto(int id, double latitude, double longitude, double altitude) {
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) {
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) {
void rc_call(int rc_cmd_in)
{
rc_cmd = rc_cmd_in;
}
}
void set_obstacle_dist(float dist[]) {
void set_obstacle_dist(float dist[])
{
for (int i = 0; i < 5; i++)
obst[i] = dist[i];
}
}
void set_battery(float voltage,float current,float remaining){
batt[0]=voltage;
batt[1]=current;
batt[2]=remaining;
}
void set_battery(float voltage, float current, float remaining)
{
batt[0] = voltage;
batt[1] = current;
batt[2] = remaining;
}
int buzzuav_update_battery(buzzvm_t vm) {
int buzzuav_update_battery(buzzvm_t vm)
{
buzzvm_pushs(vm, buzzvm_string_register(vm, "battery", 1));
buzzvm_pusht(vm);
buzzvm_dup(vm);
@ -521,34 +522,35 @@ int buzzros_print(buzzvm_t vm)
buzzvm_tput(vm);
buzzvm_gstore(vm);
return vm->state;
}
}
void set_deque_full(bool state)
{
void set_deque_full(bool state)
{
deque_full = state;
}
}
void set_rssi(float value)
{
void set_rssi(float value)
{
rssi = round(value);
}
}
void set_raw_packet_loss(float value)
{
void set_raw_packet_loss(float value)
{
raw_packet_loss = value;
}
}
void set_filtered_packet_loss(float value)
{
filtered_packet_loss = round(100*value);
}
void set_filtered_packet_loss(float value)
{
filtered_packet_loss = round(100 * value);
}
void set_api_rssi(float value)
{
void set_api_rssi(float value)
{
api_rssi = value;
}
}
int buzzuav_update_xbee_status(buzzvm_t vm) {
int buzzuav_update_xbee_status(buzzvm_t vm)
{
buzzvm_pushs(vm, buzzvm_string_register(vm, "xbee_status", 1));
buzzvm_pusht(vm);
buzzvm_dup(vm);
@ -573,45 +575,46 @@ int buzzros_print(buzzvm_t vm)
buzzvm_tput(vm);
buzzvm_gstore(vm);
return vm->state;
}
}
/***************************************/
/*current pos update*/
/***************************************/
void set_currentpos(double latitude, double longitude, double altitude){
cur_pos[0]=latitude;
cur_pos[1]=longitude;
cur_pos[2]=altitude;
}
/*adds neighbours position*/
void neighbour_pos_callback(int id, float range, float bearing, float elevation){
/***************************************/
/*current pos update*/
/***************************************/
void set_currentpos(double latitude, double longitude, double altitude)
{
cur_pos[0] = latitude;
cur_pos[1] = longitude;
cur_pos[2] = altitude;
}
/*adds neighbours position*/
void neighbour_pos_callback(int id, float range, float bearing, float elevation)
{
buzz_utility::Pos_struct pos_arr;
pos_arr.x=range;
pos_arr.y=bearing;
pos_arr.z=elevation;
map< int, buzz_utility::Pos_struct >::iterator it = neighbors_map.find(id);
if(it!=neighbors_map.end())
pos_arr.x = range;
pos_arr.y = bearing;
pos_arr.z = elevation;
map<int, buzz_utility::Pos_struct>::iterator it = neighbors_map.find(id);
if (it != neighbors_map.end())
neighbors_map.erase(it);
neighbors_map.insert(make_pair(id, pos_arr));
}
}
/* update at each step the VM table */
void update_neighbors(buzzvm_t vm){
/* update at each step the VM table */
void update_neighbors(buzzvm_t vm)
{
/* Reset neighbor information */
buzzneighbors_reset(vm);
/* Get robot id and update neighbor information */
map< int, buzz_utility::Pos_struct >::iterator it;
for (it=neighbors_map.begin(); it!=neighbors_map.end(); ++it){
buzzneighbors_add(vm,
it->first,
(it->second).x,
(it->second).y,
(it->second).z);
}
map<int, buzz_utility::Pos_struct>::iterator it;
for (it = neighbors_map.begin(); it != neighbors_map.end(); ++it)
{
buzzneighbors_add(vm, it->first, (it->second).x, (it->second).y, (it->second).z);
}
}
/****************************************/
int buzzuav_update_currentpos(buzzvm_t vm) {
/****************************************/
int buzzuav_update_currentpos(buzzvm_t vm)
{
buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 1));
buzzvm_pusht(vm);
buzzvm_dup(vm);
@ -628,18 +631,20 @@ int buzzros_print(buzzvm_t vm)
buzzvm_tput(vm);
buzzvm_gstore(vm);
return vm->state;
}
}
void flight_status_update(uint8_t state){
status=state;
}
void flight_status_update(uint8_t state)
{
status = state;
}
/*----------------------------------------------------
/ Create the generic robot table with status, remote controller current comand and destination
/ and current position of the robot
/ TODO: change global name for robot
/------------------------------------------------------*/
int buzzuav_update_flight_status(buzzvm_t vm) {
/*----------------------------------------------------
/ Create the generic robot table with status, remote controller current comand and destination
/ and current position of the robot
/ TODO: change global name for robot
/------------------------------------------------------*/
int buzzuav_update_flight_status(buzzvm_t vm)
{
buzzvm_pushs(vm, buzzvm_string_register(vm, "flight", 1));
buzzvm_pusht(vm);
buzzvm_dup(vm);
@ -647,12 +652,12 @@ int buzzros_print(buzzvm_t vm)
buzzvm_pushi(vm, rc_cmd);
buzzvm_tput(vm);
buzzvm_dup(vm);
rc_cmd=0;
rc_cmd = 0;
buzzvm_pushs(vm, buzzvm_string_register(vm, "status", 1));
buzzvm_pushi(vm, status);
buzzvm_tput(vm);
buzzvm_gstore(vm);
//also set rc_controllers goto
// also set rc_controllers goto
buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_goto", 1));
buzzvm_pusht(vm);
buzzvm_dup(vm);
@ -673,21 +678,19 @@ int buzzros_print(buzzvm_t vm)
buzzvm_tput(vm);
buzzvm_gstore(vm);
return vm->state;
}
}
/******************************************************/
/*Create an obstacle Buzz table from proximity sensors*/
/* Acessing proximity in buzz script
/******************************************************/
/*Create an obstacle Buzz table from proximity sensors*/
/* Acessing proximity in buzz script
proximity[0].angle and proximity[0].value - front
"" "" "" - right and back
proximity[3].angle and proximity[3].value - left
proximity[4].angle = -1 and proximity[4].value -bottom */
/******************************************************/
int buzzuav_update_prox(buzzvm_t vm) {
/******************************************************/
int buzzuav_update_prox(buzzvm_t vm)
{
buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity", 1));
buzzvm_pusht(vm);
buzzobj_t tProxTable = buzzvm_stack_at(vm, 1);
@ -695,8 +698,9 @@ int buzzros_print(buzzvm_t vm)
/* Fill into the proximity table */
buzzobj_t tProxRead;
float angle =0;
for(size_t i = 0; i < 4; ++i) {
float angle = 0;
for (size_t i = 0; i < 4; ++i)
{
/* Create table for i-th read */
buzzvm_pusht(vm);
tProxRead = buzzvm_stack_at(vm, 1);
@ -704,7 +708,7 @@ int buzzros_print(buzzvm_t vm)
/* Fill in the read */
buzzvm_push(vm, tProxRead);
buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0));
buzzvm_pushf(vm, obst[i+1]);
buzzvm_pushf(vm, obst[i + 1]);
buzzvm_tput(vm);
buzzvm_push(vm, tProxRead);
buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0));
@ -715,10 +719,10 @@ int buzzros_print(buzzvm_t vm)
buzzvm_pushi(vm, i);
buzzvm_push(vm, tProxRead);
buzzvm_tput(vm);
angle+=1.5708;
angle += 1.5708;
}
/* Create table for bottom read */
angle =-1;
angle = -1;
buzzvm_pusht(vm);
tProxRead = buzzvm_stack_at(vm, 1);
buzzvm_pop(vm);
@ -736,46 +740,15 @@ int buzzros_print(buzzvm_t vm)
buzzvm_pushi(vm, 4);
buzzvm_push(vm, tProxRead);
buzzvm_tput(vm);
/*
buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity", 1));
buzzvm_pusht(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "bottom", 1));
buzzvm_pushf(vm, obst[0]);
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "front", 1));
buzzvm_pushf(vm, obst[1]);
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "right", 1));
buzzvm_pushf(vm, obst[2]);
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "back", 1));
buzzvm_pushf(vm, obst[3]);
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "left", 1));
buzzvm_pushf(vm, obst[4]);
buzzvm_tput(vm);
buzzvm_gstore(vm);*/
return vm->state;
}
/**********************************************/
/*Dummy closure for use during update testing */
/**********************************************/
int dummy_closure(buzzvm_t vm){ return buzzvm_ret0(vm);}
/***********************************************/
/* Store Ros controller object pointer */
/***********************************************/
//void set_ros_controller_ptr(const rosbzz_node::roscontroller* roscontroller_ptrin){
//roscontroller_ptr = roscontroller_ptrin;
//}
}
/**********************************************/
/*Dummy closure for use during update testing */
/**********************************************/
int dummy_closure(buzzvm_t vm)
{
return buzzvm_ret0(vm);
}
}

10
src/rosbuzz.cpp
View File

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

634
src/roscontroller.cpp
View File

File diff suppressed because it is too large Load Diff

25
src/uav_utility.cpp
View File

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