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

Merge commit 'f24fb637230800de47d5b978bbebca1d4338cec9' into sim

This commit is contained in:
vivek-shankar 2017-07-17 19:56:24 -04:00
parent 78edc3c9f0 f24fb63723
commit 3d73e2e01c
27 changed files with 2724 additions and 1434 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

0
script/Update.log → buzz_scripts/Update.log
View File

2
script/flock.bzz → buzz_scripts/flock.bzz
View File

@ -6,7 +6,7 @@ include "vstigenv.bzz"
# Lennard-Jones parameters
TARGET = 12.0
EPSILON = 14.0
EPSILON = 18.0
# Lennard-Jones interaction magnitude
function lj_magnitude(dist, target, epsilon) {

816
buzz_scripts/graphform.bzz Normal file
View File

@ -0,0 +1,816 @@
#
# Include files
#
include "string.bzz"
include "vec2.bzz"
include "update.bzz"
include "barrier.bzz" # don't use a stigmergy id=11 with this header.
include "uavstates.bzz" # require an 'action' function to be defined here.
include "shapes.bzz"
ROBOT_RADIUS=50
ROBOT_DIAMETER=2.0*ROBOT_RADIUS
ROBOT_SAFETYDIST=2.0*ROBOT_DIAMETER
#
# Global variables
#
#
#Save message from all neighours
#the indexes are as 1,2,3..., while each value is a table that store the information of a neighbour robot
m_MessageState={}#store received neighbour message
m_MessageLable={}#store received neighbour message
m_MessageReqLable={}#store received neighbour message
m_MessageReqID={}#store received neighbour message
m_MessageResponse={}#store received neighbour message
m_MessageRange={}#store received neighbour message
m_MessageBearing={}#store received neighbour message
m_neighbourCunt=0#used to cunt neighbours
#Save message from one neighbour
#the indexes are as State(received state),Lable(received lable),ReqLable,ReqID,Response,Range,Bearing
m_receivedMessage={.State=s2i("STATE_FREE"),.Lable=0,.ReqLable=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),Lable(current lable),ReqLable(requested lable),ReqID(request id),Response(reply message{REQ_NONE,REQ_GRANTED,REQ_RESEND})
m_selfMessage={.State=s2i("STATE_FREE"),.Lable=0,.ReqLable=0,.ReqID=0,.Response=r2i("REQ_NONE")}
#Current robot state
m_eState="STATE_FREE"
#navigation vector
m_navigation={.x=0,.y=0}
#Debug message to be displayed in qt-opengl
#m_ossDebugMsg
#Debug vector to draw
#CVector2 m_cDebugVector
#Current label being requested or chosen (-1 when none)
m_nLabel=-1
#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
v_tag = stigmergy.create(1)
# Lennard-Jones parameters, may need change
EPSILON = 13.5 #the LJ parameter for other robots
EPSILON_FOR1 = 10.0 #the LJ parameter for the robot labeled 1
# Lennard-Jones interaction magnitude
function FlockInteraction(dist,target,epsilon){
var mag = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
return mag
}
function LimitAngle(angle){
if(angle>2*math.pi)
return angle-2*math.pi
else if (angle<0)
return angle+2*math.pi
else
return angle
}
#
# Calculates the angle of the given vector2.
# PARAM v: The vector2.
# RETURN: The angle of the vector.
#
Angle = function(v) {
return math.atan(v.y, v.x)
}
#
#return the number of value in table
#
function count(table,value){
var number=0
var i=0
while(i<size(table)){
if(table[i]==value){
number=number+1
}
i=i+1
}
return number
}
#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
}
function pow(base,exponent){
var i=0
var renturn_val=1
if(exponent==0)
return 1
else{
while(i<exponent){
renturn_val=renturn_val*base
i=i+1
}
return renturn_val
}
}
function start_listen(){
neighbors.listen("m",
function(vid,value,rid){
#store the received message
var temp_id=rid
m_receivedMessage.State=i2s(value.State)
m_receivedMessage.Lable=value.Lable
m_receivedMessage.ReqLable=value.ReqLable
m_receivedMessage.ReqID=value.ReqID
m_receivedMessage.Response=i2r(value.Response)
Get_DisAndAzi(temp_id)
#add the received message
#
m_MessageState[m_neighbourCunt]=i2s(value.State)
m_MessageLable[m_neighbourCunt]=value.Lable
m_MessageReqLable[m_neighbourCunt]=value.ReqLable
m_MessageReqID[m_neighbourCunt]=value.ReqID
m_MessageResponse[m_neighbourCunt]=i2r(value.Response)
m_MessageRange[m_neighbourCunt]=m_receivedMessage.Range
m_MessageBearing[m_neighbourCunt]=m_receivedMessage.Bearing
m_neighbourCunt=m_neighbourCunt+1
})
}
#
#Function used to get the station info of the sender of the message
function Get_DisAndAzi(id){
neighbors.foreach(
function(rid, data) {
if(rid==id){
m_receivedMessage.Range=data.distance*100.0
m_receivedMessage.Bearing=data.azimuth
}
})
}
#
#Update node info according to neighbour robots
#
function UpdateNodeInfo(){
#Collect informaiton
#Update information
var i=0
while(i<m_neighbourCunt){
if(m_MessageState[i]=="STATE_JOINED"){
m_vecNodes[m_MessageLable[i]].State="ASSIGNED"
m_vecNodes[m_MessageLable[i]].StateAge=m_unJoiningLostPeriod
}
else if(m_MessageState[i]=="STATE_JOINING"){
m_vecNodes[m_MessageLable[i]].State="ASSIGNING"
m_vecNodes[m_MessageLable[i]].StateAge=m_unJoiningLostPeriod
}
i=i+1
}
#Forget old information
i=0
while(i<size(m_vecNodes)){
if((m_vecNodes[i].StateAge>0) and (m_vecNodes[i].State=="ASSIGNING")){
m_vecNodes[i].StateAge=m_vecNodes[i].StateAge-1
if(m_vecNodes[i].StateAge==0)
m_vecNodes[i].State="UNASSIGNED"
}
i=i+1
}
}
#
#Transistion to state free
#
function TransitionToFree(){
m_eState="STATE_FREE"
m_unWaitCount=m_unLabelSearchWaitTime
m_selfMessage.State=s2i(m_eState)
}
#
#Transistion to state asking
#
function TransitionToAsking(un_label){
m_eState="STATE_ASKING"
m_nLabel=un_label
m_unRequestId=rng.uniform(0,65536)+id#don't know why the random numbers are the same, add id to make the ReqID different
m_selfMessage.State=s2i(m_eState)
m_selfMessage.ReqLable=m_nLabel
m_selfMessage.ReqID=m_unRequestId
m_unWaitCount=m_unResponseTimeThreshold
}
#
#Transistion to state joining
#
function TransitionToJoining(){
m_eState="STATE_JOINING"
m_selfMessage.State=s2i(m_eState)
m_selfMessage.Lable=m_nLabel
m_unWaitCount=m_unJoiningLostPeriod
neighbors.listen("r",
function(vid,value,rid){
#store the received message
if(value.Label==m_nLabel){
m_cMeToPred.GlobalBearing=value.Bearing
}
})
}
#
#Transistion to state joined
#
function TransitionToJoined(){
m_eState="STATE_JOINED"
m_selfMessage.State=s2i(m_eState)
m_selfMessage.Lable=m_nLabel
m_vecNodes[m_nLabel].State="ASSIGNED"
neighbors.ignore("r")
#write statues
v_tag.put(m_nLabel, 1)
m_navigation.x=0.0
m_navigation.y=0.0
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
}
#
#Transistion to state Lock, lock the current formation
#
function TransitionToLock(){
m_eState="STATE_LOCK"
m_selfMessage.State=s2i(m_eState)
m_selfMessage.Lable=m_nLabel
m_vecNodes[m_nLabel].State="ASSIGNED"
m_navigation.x=0.0
m_navigation.y=0.0
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
}
#
# Do free
#
function DoFree() {
m_selfMessage.State=s2i(m_eState)
#wait for a while before looking for a lable
if(m_unWaitCount>0)
m_unWaitCount=m_unWaitCount-1
#find a set of joined robots
var setJoinedLables={}
var setJoinedIndexes={}
var i=0
var j=0
while(i<m_neighbourCunt){
if(m_MessageState[i]=="STATE_JOINED"){
setJoinedLables[j]=m_MessageLable[i]
setJoinedIndexes[j]=i
j=j+1
}
i=i+1
}
#go through the graph to look for a proper lable
var unFoundLable=0
var IDofPred=0
i=1
while(i<size(m_vecNodes) and (unFoundLable==0)){
#if the node is unassigned and the predecessor is insight
if(m_vecNodes[i].State=="UNASSIGNED" and count(setJoinedLables,m_vecNodes[i].Pred)==1){
unFoundLable=m_vecNodes[i].Lable
IDofPred=find(m_MessageLable,m_vecNodes[unFoundLable].Pred)
}
i=i+1
}
if(unFoundLable>0){
TransitionToAsking(unFoundLable)
return
}
#navigation
#if there is a joined robot within sight, move around joined robots
#else, gather with other free robots
if(size(setJoinedIndexes)>0){
var tempvec_P={.x=0.0,.y=0.0}
var tempvec_N={.x=0.0,.y=0.0}
i=0
while(i<size(setJoinedIndexes)){
var index=setJoinedIndexes[i]
tempvec_P=math.vec2.add(tempvec_P,math.vec2.newp(m_MessageRange[index],m_MessageBearing[index]+0.5*math.pi))
tempvec_N=math.vec2.add(tempvec_N,math.vec2.newp(m_MessageRange[index]-5.0*ROBOT_SAFETYDIST,m_MessageBearing[index]))
i=i+1
}
tempvec_P=math.vec2.scale(tempvec_P,size(setJoinedIndexes))
tempvec_N=math.vec2.scale(tempvec_N,size(setJoinedIndexes))
m_navigation=math.vec2.add(tempvec_P,tempvec_N)
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
}else{ #no joined robots in sight
i=0
var tempvec={.x=0.0,.y=0.0}
while(i<m_neighbourCunt){
tempvec=math.vec2.add(tempvec,math.vec2.newp(m_MessageRange[i]-2.0*ROBOT_SAFETYDIST,m_MessageBearing[i]))
i=i+1
}
m_navigation=math.vec2.scale(tempvec,1.0/i)
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
}
#jump the first step
if(step_cunt<=1){
uav_moveto(0.0,0.0)
}
#set message
m_selfMessage.State=s2i(m_eState)
}
#
#Do asking
#
function DoAsking(){
#look for response from predecessor
var i=0
var psResponse=-1
while(i<m_neighbourCunt and psResponse==-1){
#the respond robot in joined state
#the request lable be the same as requesed
#get a respond
if(m_MessageState[i]=="STATE_JOINED"){
if(m_MessageReqLable[i]==m_nLabel)
if(m_MessageResponse[i]!="REQ_NONE"){
psResponse=i
}}
i=i+1
}
#analyse response
if(psResponse==-1){
#no response, wait
m_unWaitCount=m_unWaitCount-1
m_selfMessage.State=s2i(m_eState)
m_selfMessage.ReqLable=m_nLabel
m_selfMessage.ReqID=m_unRequestId
if(m_unWaitCount==0){
TransitionToFree()
return
}
}
else{
if(m_MessageReqID[psResponse]!=m_unRequestId)
TransitionToFree()
if(m_MessageReqID[psResponse]==m_unRequestId){
if(m_MessageResponse[psResponse]=="REQ_GRANTED"){
TransitionToJoining()
#TransitionToJoined()
return
}
else{
TransitionToAsking(m_nLabel)
return
}
}
}
uav_moveto(0.0,0.0)
}
#
#Do joining
#
function DoJoining(){
#get information of pred
var i=0
var IDofPred=-1
while(i<m_neighbourCunt and IDofPred==-1){
if(m_MessageLable[i]==m_vecNodes[m_nLabel].Pred and m_MessageState[i]=="STATE_JOINED")
IDofPred=i
i=i+1
}
#found pred
if(IDofPred!=-1){
m_unWaitCount=m_unJoiningLostPeriod#if see pred, reset the timer
var P2Target=math.vec2.newp(m_vecNodes[m_nLabel].distance,m_vecNodes[m_nLabel].bearing)
m_cMeToPred.Range=m_MessageRange[IDofPred]#the poition of self to pred in local coordinate
m_cMeToPred.Bearing=m_MessageBearing[IDofPred]
#attention, m_cMeToPred.GlobalBearing is the bearing of self wrt pred in global coordinate
var S2PGlobalBearing=0
m_cMeToPred.GlobalBearing=LimitAngle(m_cMeToPred.GlobalBearing)
if(m_cMeToPred.GlobalBearing>math.pi)
S2PGlobalBearing=m_cMeToPred.GlobalBearing-math.pi
else
S2PGlobalBearing=m_cMeToPred.GlobalBearing+math.pi
var S2Pred=math.vec2.newp(m_cMeToPred.Range,S2PGlobalBearing)
#the vector from self to target in global coordinate
var S2Target=math.vec2.add(S2Pred,P2Target)
#change the vector to local coordinate of self
var S2Target_bearing=Angle(S2Target)
m_bias=m_cMeToPred.Bearing-S2PGlobalBearing
S2Target_bearing=S2Target_bearing+m_bias # commented out by DS'06/17
m_navigation=math.vec2.newp(math.vec2.length(S2Target),S2Target_bearing)
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
#test if is already in desired position
if(math.abs(S2Target.x)<m_fTargetDistanceTolerance and math.abs(S2Target.y)<m_fTargetDistanceTolerance){
#log(S2Target_dis,S2Target_bearing)
TransitionToJoined()
return
}
} else{ #miss pred, there is a change the another robot block the sight, keep moving as before for sometime
m_unWaitCount=m_unWaitCount-1
}
if(m_unWaitCount==0){
TransitionToFree()
return
}
#pack the communication package
m_selfMessage.State=s2i(m_eState)
m_selfMessage.Lable=m_nLabel
}
#
#Do joined
#
function DoJoined(){
m_selfMessage.State=s2i(m_eState)
m_selfMessage.Lable=m_nLabel
#collect all requests
var mapRequests={}
var i=0
var j=0
var ReqLable
var JoiningLable
var seenPred=0
while(i<m_neighbourCunt){
if(m_MessageState[i]=="STATE_ASKING"){
ReqLable=m_MessageReqLable[i]
if(m_vecNodes[ReqLable].State=="UNASSIGNED")
if(m_nLabel==m_vecNodes[ReqLable].Pred){
#is a request, store the index
mapRequests[j]=i
j=j+1
}
}
if(m_MessageState[i]=="STATE_JOINING"){
JoiningLable=m_MessageLable[i]
if(m_nLabel==m_vecNodes[JoiningLable].Pred){
##joining wrt this dot,send the global bearing
var m_messageForJoining={.Label=JoiningLable,.Bearing=m_MessageBearing[i]-m_bias}
neighbors.broadcast("r",m_messageForJoining)
}
}
#if it is the pred
if(m_MessageState[i]=="STATE_JOINED" and m_MessageLable[i]==m_vecNodes[m_nLabel].Pred){
seenPred=1
m_unWaitCount=m_unJoiningLostPeriod
}
i=i+1
}
#get request
if(size(mapRequests)!=0){
i=1
var ReqIndex=0
while(i<size(mapRequests)){
#compare the distance
if(m_MessageRange[mapRequests[ReqIndex]]>m_MessageRange[mapRequests[i]])
ReqIndex=i
i=i+1
}
#get the best index, whose Reqlable and Reqid are
ReqLable=m_MessageReqLable[mapRequests[ReqIndex]]
var ReqID=m_MessageReqID[mapRequests[ReqIndex]]
m_selfMessage.ReqLable=ReqLable
m_selfMessage.ReqID=ReqID
m_selfMessage.Response=r2i("REQ_GRANTED")
}
#lost pred, wait for some time and transit to free
#if(seenPred==0){
#m_unWaitCount=m_unWaitCount-1
#if(m_unWaitCount==0){
#TransitionToFree()
#return
#}
#}
m_navigation.x=0.0
m_navigation.y=0.0
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
#check if should to transists to lock
# if(v_tag.size()==ROBOTS){
# TransitionToLock()
# }
}
#
#Do Lock
#
function DoLock(){
m_selfMessage.State=s2i(m_eState)
m_selfMessage.Lable=m_nLabel
m_navigation.x=0.0
m_navigation.y=0.0
#collect preds information
var i=0
var mypred1={.range=0,.bearing=0}
var mypred2={.range=0,.bearing=0}
while(i<m_neighbourCunt){
#is the first predecessor
if(m_MessageLable[i]==m_vecNodes_fixed[m_nLabel].Pred1){
mypred1.range=m_MessageRange[i]
mypred1.bearing=m_MessageBearing[i]
}
#is the second predecessor
if(m_MessageLable[i]==m_vecNodes_fixed[m_nLabel].Pred2){
mypred2.range=m_MessageRange[i]
mypred2.bearing=m_MessageBearing[i]
}
i=i+1
}
#calculate motion vection
if(m_nLabel==0){
m_navigation.x=0.0#change value so that robot 0 will move
m_navigation.y=0.0
log(";",m_nLabel,";",0)
}
if(m_nLabel==1){
var tempvec={.Range=0.0,.Bearing=0.0}
tempvec.Range=FlockInteraction(mypred1.range,m_vecNodes_fixed[m_nLabel].d1,EPSILON_FOR1)
#tempvec.Range=mypred1.range-m_vecNodes_fixed[m_nLabel].d1
tempvec.Bearing=mypred1.bearing
m_navigation=math.vec2.newp(tempvec.Range,tempvec.Bearing)
log(";",m_nLabel,";",mypred1.range-m_vecNodes_fixed[m_nLabel].d1)
}
if(m_nLabel>1){
var cDir={.x=0.0,.y=0.0}
var cDir1={.x=0.0,.y=0.0}
var cDir2={.x=0.0,.y=0.0}
cDir1=math.vec2.newp(FlockInteraction(mypred1.range,m_vecNodes_fixed[m_nLabel].d1,EPSILON),mypred1.bearing)
cDir2=math.vec2.newp(FlockInteraction(mypred2.range,m_vecNodes_fixed[m_nLabel].d2,EPSILON),mypred2.bearing)
#cDir1=math.vec2.newp((mypred1.range-m_vecNodes_fixed[m_nLabel].d1),mypred1.bearing)
#cDir2=math.vec2.newp((mypred2.range-m_vecNodes_fixed[m_nLabel].d2),mypred2.bearing)
cDir=math.vec2.add(cDir1,cDir2)
cDir=math.vec2.scale(cDir,5)
m_navigation.x=cDir.x
m_navigation.y=cDir.y
#log(m_nLabel,"error:",mypred1.range-m_vecNodes_fixed[m_nLabel].d1,"and",mypred2.range-m_vecNodes_fixed[m_nLabel].d2)
log(";",m_nLabel,";",mypred1.range-m_vecNodes_fixed[m_nLabel].d1)
}
#move
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
}
function action(){
statef=action
UAVSTATE="GRAPH"
}
#
# Executed at init
#
function init() {
#
#Adjust parameters here
#
m_unResponseTimeThreshold=10
m_unLabelSearchWaitTime=10
m_fTargetDistanceTolerance=10
m_unJoiningLostPeriod=100
#
# Join Swarm
#
uav_initswarm()
Reset();
}
#
# Executed every step
#
function step(){
uav_rccmd()
uav_neicmd()
#update the graph
UpdateNodeInfo()
#reset message package to be sent
m_selfMessage={.State=s2i("STATE_FREE"),.Lable=0,.ReqLable=0,.ReqID=0,.Response=r2i("REQ_NONE")}
#
#act according to current state
#
if(UAVSTATE=="GRAPH"){
if(m_eState=="STATE_FREE")
DoFree()
else if(m_eState=="STATE_ESCAPE")
DoEscape()
else if(m_eState=="STATE_ASKING")
DoAsking()
else if(m_eState=="STATE_JOINING")
DoJoining()
else if(m_eState=="STATE_JOINED")
DoJoined()
else if(m_eState=="STATE_LOCK")
DoLock()
}
statef()
debug(m_eState,m_nLabel)
log("Current state: ", UAVSTATE)
log("Swarm size: ", ROBOTS)
#navigation
#broadcast messag
neighbors.broadcast("m",m_selfMessage)
#
#clean message storage
m_MessageState={}#store received neighbour message
m_MessageLable={}#store received neighbour message
m_MessageReqLable={}#store received neighbour message
m_MessageReqID={}#store received neighbour message
m_MessageResponse={}#store received neighbour message
m_MessageRange={}#store received neighbour message
m_MessageBearing={}#store received neighbour message
m_neighbourCunt=0
#step cunt+1
step_cunt=step_cunt+1
}
#
# Executed when reset
#
function Reset(){
#m_vecNodes={}
#m_vecNodes = parse_graph("/home/dave/ROS_WS/src/rosbuzz/buzz_scripts/include/Graph_drone.graph")#change the .graph file when necessary
m_vecNodes_fixed={}
m_vecNodes_fixed=parse_graph_fixed("/home/dave/ROS_WS/src/rosbuzz/buzz_scripts/include/Graph_fixed.graph")
m_nLabel=-1
#start listening
start_listen()
#
#set initial state, only one robot choose [A], while the rest choose [B]
#
#[A]The robot used to triger the formation process is defined as joined,
if(id==0){
m_nLabel=0
TransitionToJoined()
}
#[B]Other robots are defined as free.
else{
TransitionToFree()
}
}
#
# Executed upon destroy
#
function destroy() {
#clear neighbour message
uav_moveto(0.0,0.0)
m_vecNodes={}
#stop listening
neighbors.ignore("m")
}

256
script/graphform.bzz → buzz_scripts/graphform.bzz_old
View File

@ -7,12 +7,7 @@ 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.
#
#Constant parameters, need to be adjust
#parameters for set_wheels
m_sWheelTurningParams={.MaxSpeed=1.0}
ROBOT_RADIUS=0.5
ROBOT_RADIUS=50
ROBOT_DIAMETER=2.0*ROBOT_RADIUS
ROBOT_SAFETYDIST=2.0*ROBOT_DIAMETER
@ -93,39 +88,22 @@ step_cunt=0
v_tag = stigmergy.create(1)
# Lennard-Jones parameters
EPSILON = 3.5 #3.5
EPSILON = 13.5 #3.5
# Lennard-Jones interaction magnitude
function FlockInteraction(dist,target,epsilon){
var mag = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
return mag
}
function Norm(vector) {
var l = math.vec2.length(vector)
return {
.x = vector.x / l,
.y = vector.y / l
}
var mag = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
return mag
}
function LimitAngle(angle){
if(angle>2*math.pi)
return angle-2*math.pi
else if (angle<0)
return angle+2*math.pi
else
return angle
}
#
# Calculates the length of the given vector2.
# PARAM v: The vector2.
# RETURN: The length of the vector.
#
Length = function(v) {
return math.sqrt(v.x * v.x + v.y * v.y)
if(angle>2*math.pi)
return angle-2*math.pi
else if (angle<0)
return angle+2*math.pi
else
return angle
}
#
@ -141,43 +119,43 @@ Angle = function(v) {
#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
var number=0
var i=0
while(i<size(table)){
if(table[i]==value){
number=number+1
}
i=i+1
}
return number
}
#
#return the index of value
#
function find(table,value){
var index=nil
var i=0
while(i<size(table)){
if(table[i]==value)
index=i
i=i+1
}
return index
var index=nil
var i=0
while(i<size(table)){
if(table[i]==value)
index=i
i=i+1
}
return index
}
function pow(base,exponent){
var i=0
var renturn_val=1
if(exponent==0)
return 1
else{
while(i<exponent){
renturn_val=renturn_val*base
i=i+1
}
return renturn_val
}
var i=0
var renturn_val=1
if(exponent==0)
return 1
else{
while(i<exponent){
renturn_val=renturn_val*base
i=i+1
}
return renturn_val
}
}
#
@ -272,86 +250,86 @@ neighbors.listen("msg",
#
#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
}
})
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
#Collect informaiton
#Update information
var i=0
while(i<m_neighbourCunt){
if(m_MessageState[i]=="STATE_JOINED"){
m_vecNodes[m_MessageLable[i]].State="ASSIGNED"
m_vecNodes[m_MessageLable[i]].StateAge=m_unJoiningLostPeriod
}
else if(m_MessageState[i]=="STATE_JOINING"){
m_vecNodes[m_MessageLable[i]].State="ASSIGNING"
m_vecNodes[m_MessageLable[i]].StateAge=m_unJoiningLostPeriod
}
i=i+1
}
#Forget old information
i=0
while(i<size(m_vecNodes)){
if((m_vecNodes[i].StateAge>0) and (m_vecNodes[i].State=="ASSIGNING")){
m_vecNodes[i].StateAge=m_vecNodes[i].StateAge-1
if(m_vecNodes[i].StateAge==0)
m_vecNodes[i].State="UNASSIGNED"
while(i<m_neighbourCunt){
if(m_MessageState[i]=="STATE_JOINED"){
m_vecNodes[m_MessageLable[i]].State="ASSIGNED"
m_vecNodes[m_MessageLable[i]].StateAge=m_unJoiningLostPeriod
}
else if(m_MessageState[i]=="STATE_JOINING"){
m_vecNodes[m_MessageLable[i]].State="ASSIGNING"
m_vecNodes[m_MessageLable[i]].StateAge=m_unJoiningLostPeriod
}
i=i+1
}
#Forget old information
i=0
while(i<size(m_vecNodes)){
if((m_vecNodes[i].StateAge>0) and (m_vecNodes[i].State=="ASSIGNING")){
m_vecNodes[i].StateAge=m_vecNodes[i].StateAge-1
if(m_vecNodes[i].StateAge==0)
m_vecNodes[i].State="UNASSIGNED"
}
i=i+1
}
i=i+1
}
}
#
#Transistion to state free
#
function TransitionToFree(){
m_eState="STATE_FREE"
m_unWaitCount=m_unLabelSearchWaitTime
m_selfMessage.State=m_eState
m_eState="STATE_FREE"
m_unWaitCount=m_unLabelSearchWaitTime
m_selfMessage.State=m_eState
}
#
#Transistion to state asking
#
function TransitionToAsking(un_label){
m_eState="STATE_ASKING"
m_nLabel=un_label
m_unRequestId=rng.uniform(0,65536)+id#don't know why the random numbers are the same, add id to make the ReqID different
m_selfMessage.State=m_eState
m_selfMessage.ReqLable=m_nLabel
m_selfMessage.ReqID=m_unRequestId
m_eState="STATE_ASKING"
m_nLabel=un_label
m_unRequestId=rng.uniform(0,65536)+id#don't know why the random numbers are the same, add id to make the ReqID different
m_selfMessage.State=m_eState
m_selfMessage.ReqLable=m_nLabel
m_selfMessage.ReqID=m_unRequestId
m_unWaitCount=m_unResponseTimeThreshold
m_unWaitCount=m_unResponseTimeThreshold
}
#
#Transistion to state joining
#
function TransitionToJoining(){
m_eState="STATE_JOINING"
m_selfMessage.State=m_eState
m_selfMessage.Lable=m_nLabel
m_unWaitCount=m_unJoiningLostPeriod
m_eState="STATE_JOINING"
m_selfMessage.State=m_eState
m_selfMessage.Lable=m_nLabel
m_unWaitCount=m_unJoiningLostPeriod
neighbors.listen("reply",
function(vid,value,rid){
#store the received message
if(value.Lable==m_nLabel){
m_cMeToPred.GlobalBearing=value.GlobalBearing
neighbors.listen("reply",
function(vid,value,rid){
#store the received message
if(value.Lable==m_nLabel){
m_cMeToPred.GlobalBearing=value.GlobalBearing
}
})
}
})
}
@ -359,32 +337,32 @@ neighbors.listen("reply",
#Transistion to state joined
#
function TransitionToJoined(){
m_eState="STATE_JOINED"
m_selfMessage.State=m_eState
m_selfMessage.Lable=m_nLabel
m_vecNodes[m_nLabel].State="ASSIGNED"
neighbors.ignore("reply")
m_eState="STATE_JOINED"
m_selfMessage.State=m_eState
m_selfMessage.Lable=m_nLabel
m_vecNodes[m_nLabel].State="ASSIGNED"
neighbors.ignore("reply")
#write statues
v_tag.put(m_nLabel, 1)
#write statues
v_tag.put(m_nLabel, 1)
m_navigation.x=0.0
m_navigation.y=0.0
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
m_navigation.x=0.0
m_navigation.y=0.0
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
}
#
#Transistion to state Lock, lock the current formation
#
function TransitionToLock(){
m_eState="STATE_LOCK"
m_selfMessage.State=m_eState
m_selfMessage.Lable=m_nLabel
m_vecNodes[m_nLabel].State="ASSIGNED"
m_eState="STATE_LOCK"
m_selfMessage.State=m_eState
m_selfMessage.Lable=m_nLabel
m_vecNodes[m_nLabel].State="ASSIGNED"
m_navigation.x=0.0
m_navigation.y=0.0
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
m_navigation.x=0.0
m_navigation.y=0.0
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
}
#
@ -554,19 +532,18 @@ function DoJoining(){
#the vector from self to target in global coordinate
var S2Target=math.vec2.add(S2Pred,P2Target)
#change the vector to local coordinate of self
var S2Target_dis=Length(S2Target)
var S2Target_bearing=Angle(S2Target)
m_bias=m_cMeToPred.Bearing-S2PGlobalBearing
S2Target_bearing=S2Target_bearing+m_bias
m_navigation=math.vec2.newp(S2Target_dis,S2Target_bearing)
#S2Target_bearing=S2Target_bearing+m_bias # commented out by DS'06/17
m_navigation=math.vec2.newp(math.vec2.length(S2Target),S2Target_bearing)
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
#test if is already in desired position
if(math.abs(S2Target.x)<m_fTargetDistanceTolerance and math.abs(S2Target.y)<m_fTargetDistanceTolerance){
log(S2Target_dis,S2Target_bearing)
#TransitionToJoined()
#log(S2Target_dis,S2Target_bearing)
TransitionToJoined()
return
}
} else{ #miss pred, there is a change the another robot block the sight, keep moving as before for sometime
@ -662,7 +639,7 @@ function DoJoined(){
if(v_tag.size()==ROBOTS){
#TransitionToLock()
TransitionToLock()
}
}
@ -728,11 +705,12 @@ if(m_nLabel>1){
log(";",m_nLabel,";",mypred1.range-m_vecNodes_fixed[m_nLabel].d1)
}
#move
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
uav_moveto(m_navigation.x,m_navigation.y)
}
function action(){
statef=action
UAVSTATE="GRAPH"
}
#
@ -744,7 +722,7 @@ function init() {
#
m_unResponseTimeThreshold=10
m_unLabelSearchWaitTime=10
m_fTargetDistanceTolerance=150
m_fTargetDistanceTolerance=10
m_unJoiningLostPeriod=100
#
@ -767,7 +745,7 @@ function step(){
#
#act according to current state
#
if(UAVSTATE=="IDLE"){
if(UAVSTATE=="GRAPH"){
if(m_eState=="STATE_FREE")
DoFree()
else if(m_eState=="STATE_ESCAPE")
@ -815,9 +793,9 @@ function step(){
#
function Reset(){
m_vecNodes={}
m_vecNodes = parse_graph("/home/dave/ROS_WS/src/rosbuzz/script/Graph_drone.graph")#change the .graph file when necessary
m_vecNodes = parse_graph("/home/dave/ROS_WS/src/rosbuzz/buzz_scripts/include/Graph_drone.graph")#change the .graph file when necessary
m_vecNodes_fixed={}
m_vecNodes_fixed=parse_graph_fixed("/home/dave/ROS_WS/src/rosbuzz/script/Graph_fixed.graph")
m_vecNodes_fixed=parse_graph_fixed("/home/dave/ROS_WS/src/rosbuzz/buzz_scripts/include/Graph_fixed.graph")
m_nLabel=-1
#start listening

5
buzz_scripts/include/Graph_drone.graph Normal file
View File

@ -0,0 +1,5 @@
0 -1 -1 -1 3000.0
1 0 1000.0 0.0 5000.0
2 0 1000.0 1.57 7000.0
3 0 1000.0 3.14 9000.0
4 0 1000.0 4.71 11000.0

5
buzz_scripts/include/Graph_fixed.graph Normal file
View File

@ -0,0 +1,5 @@
0 -1 -1 -1 -1
1 0 10.0 -1 -1
2 0 10.0 1 1414.2
3 0 10.0 2 1414.2
4 0 10.0 1 1414.2

0
script/include/barrier.bzz → buzz_scripts/include/barrier.bzz
View File

111
buzz_scripts/include/shapes.bzz Normal file
View File

@ -0,0 +1,111 @@
#Table of the nodes in the graph
m_vecNodes={}
m_vecNodes_fixed={}
m_vecNodes[0] = { # The .graph file is stored according the sequence of lable, predecessor, distance, bearing
.Lable = 0, # Lable of the point
.Pred = -1, # Lable of its predecessor
.distance = -1, # distance to the predecessor
.bearing = -1, # bearing form the predecessor to this dot
.height = 3000, # height of this dot
.State="UNASSIGNED",
.StateAge=0
}
m_vecNodes[1] = {
.Lable = 1,
.Pred = 0,
.distance = 1000,
.bearing = 0.0,
.height = 5000,
.State="UNASSIGNED",
.StateAge=0
}
m_vecNodes[2] = {
.Lable = 2,
.Pred = 0,
.distance = 1000,
.bearing = 1.57,
.height = 7000,
.State="UNASSIGNED",
.StateAge=0
}
m_vecNodes[3] = {
.Lable = 3,
.Pred = 0,
.distance = 1000,
.bearing = 3.14,
.height = 9000,
.State="UNASSIGNED",
.StateAge=0
}
m_vecNodes[4] = {
.Lable = 4,
.Pred = 0,
.distance = 1000,
.bearing = 4.71,
.height = 11000,
.State="UNASSIGNED",
.StateAge=0
}
#
# Graph parsing
#
function parse_graph(fname) {
# Graph data
var gd = {}
# Open the file
var fd = io.fopen(fname, "r")
if(not fd) {
log("Can't open '", fname, "'")
return nil
}
# Parse the file line by line
var rrec # Record read from line
var grec # Record parsed into graph
io.fforeach(fd, function(line) {
# Parse file line
rrec = string.split(line, "\t ")
# Make record
gd[string.toint(rrec[0])] = { # The .graph file is stored according the sequence of lable, predecessor, distance, bearing
.Lable = string.toint(rrec[0]), # Lable of the point
.Pred = string.toint(rrec[1]), # Lable of its predecessor
.distance = string.tofloat(rrec[2]), # distance to the predecessor
.bearing = string.tofloat(rrec[3]), # bearing form the predecessor to this dot
.height = string.tofloat(rrec[4]), # height of this dot
.State="UNASSIGNED",
.StateAge=0
}})
# All done
io.fclose(fd)
return gd
}
function parse_graph_fixed(fname) {
# Graph data
var gd = {}
# Open the file
var fd = io.fopen(fname, "r")
if(not fd) {
log("Can't open '", fname, "'")
return nil
}
# Parse the file line by line
var rrec # Record read from line
var grec # Record parsed into graph
io.fforeach(fd, function(line) {
# Parse file line
rrec = string.split(line, "\t ")
# Make record
gd[string.toint(rrec[0])] = { # The .graph file is stored according the sequence of lable, pre1, dis2pr1, pre2, ids2pre2
.Pred1 = string.toint(rrec[1]), # Pred 1 lable
.Pred2 = string.toint(rrec[3]), # Pred 2 lable
.d1 = string.tofloat(rrec[2]), # Pred 1 distance
.d2 = string.tofloat(rrec[4]), # Pred 2 distance
.Lable=string.toint(rrec[0]),
.State="UNASSIGNED",
.StateAge=0
}})
# All done
io.fclose(fd)
return gd
}

0
script/include/string.bzz → buzz_scripts/include/string.bzz
View File

24
script/include/uavstates.bzz → buzz_scripts/include/uavstates.bzz
View File

@ -50,7 +50,7 @@ function land() {
uav_land()
}
else {
barrier_set(ROBOTS,idle,land)
barrier_set(ROBOTS,turnedoff,land)
barrier_ready()
timeW=0
#barrier = nil
@ -58,6 +58,22 @@ function land() {
}
}
function follow() {
if(size(targets)>0) {
UAVSTATE = "FOLLOW"
statef=follow
attractor=math.vec2.newp(0,0)
foreach(targets, function(id, tab) {
force=(0.05)*(tab.range)^4
attractor=math.vec2.add(attractor, math.vec2.newp(force, tab.bearing))
})
uav_moveto(attractor.x, attractor.y)
} else {
log("No target in local table!")
#statef=idle
}
}
function uav_rccmd() {
if(flight.rc_cmd==22) {
log("cmd 22")
@ -74,9 +90,11 @@ function uav_rccmd() {
neighbors.broadcast("cmd", 21)
} else if(flight.rc_cmd==16) {
flight.rc_cmd=0
statef = idle
UAVSTATE = "FOLLOW"
log(rc_goto.latitude, " ", rc_goto.longitude)
add_targetrb(0,rc_goto.latitude,rc_goto.longitude)
statef = follow
#uav_goto()
add_user_rb(10,rc_goto.latitude,rc_goto.longitude)
} else if(flight.rc_cmd==400) {
flight.rc_cmd=0
uav_arm()

0
script/include/update.bzz → buzz_scripts/include/update.bzz
View File

0
script/include/vec2.bzz → buzz_scripts/include/vec2.bzz
View File

0
script/include/vstigenv.bzz → buzz_scripts/include/vstigenv.bzz
View File

0
script/stand_by.bzz → buzz_scripts/stand_by.bzz
View File

0
script/tentacle.bzz → buzz_scripts/tentacle.bzz
View File

0
script/testalone.bzz → buzz_scripts/testalone.bzz
View File

0
script/testflocksim.bzz → buzz_scripts/testflocksim.bzz
View File

9
include/buzz_utility.h
View File

@ -20,8 +20,15 @@ struct pos_struct
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,la,lo;
rb_struct(double la, double lo, double r,double b):la(la),lo(lo),r(r),b(b){};
rb_struct(){}
};
typedef struct rb_struct RB_struct;
typedef struct pos_struct Pos_struct ;
uint16_t* u64_cvt_u16(uint64_t u64);

20
include/buzzuav_closures.h
View File

@ -14,8 +14,8 @@
namespace buzzuav_closures{
typedef enum {
COMMAND_NIL = 0, // Dummy command
COMMAND_TAKEOFF, // Take off
COMMAND_NIL = 0, // Dummy command
COMMAND_TAKEOFF, // Take off
COMMAND_LAND,
COMMAND_GOHOME,
COMMAND_ARM,
@ -47,6 +47,11 @@ void rc_set_goto(double pos[]);
void rc_call(int rc_cmd);
/* sets the battery state */
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);
void set_filtered_packet_loss(float value);
void set_api_rssi(float value);
/* sets current position */
void set_currentpos(double latitude, double longitude, double altitude);
/*retuns the current go to position */
@ -64,7 +69,7 @@ void set_obstacle_dist(float dist[]);
*/
int buzzuav_takeoff(buzzvm_t vm);
/*
* Arm command from Buzz
* Arm command from Buzz
*/
int buzzuav_arm(buzzvm_t vm);
/*
@ -83,13 +88,18 @@ int buzzuav_gohome(buzzvm_t vm);
* Updates battery information in Buzz
*/
int buzzuav_update_battery(buzzvm_t vm);
/*
* Updates xbee_status information in Buzz
*/
int buzzuav_update_xbee_status(buzzvm_t vm);
/*
* Updates current position in Buzz
*/
int buzzuav_update_currentpos(buzzvm_t vm);
int buzzuav_adduserRB(buzzvm_t vm);
int buzzuav_update_targets(buzzvm_t vm);
int buzzuav_addtargetRB(buzzvm_t vm);
/*
* Updates flight status and rc command in Buzz, put it in a tabel to acess it
* Updates flight status and rc command in Buzz, put it in a tabel to acess it
* use flight.status for flight status
* use flight.rc_cmd for current rc cmd
*/

313
include/roscontroller.h
View File

@ -42,197 +42,210 @@
using namespace std;
namespace rosbzz_node{
class roscontroller{
namespace rosbzz_node
{
class roscontroller
{
public:
roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv);
~roscontroller();
//void RosControllerInit();
void RosControllerRun();
roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv);
~roscontroller();
//void RosControllerInit();
void RosControllerRun();
private:
struct num_robot_count
{
uint8_t history[10];
uint8_t index=0;
uint8_t current=0;
num_robot_count(){}
}; typedef struct num_robot_count Num_robot_count ;
struct num_robot_count
{
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
struct 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;
struct gps
{
double longitude=0.0;
double latitude=0.0;
float altitude=0.0;
}; typedef struct gps GPS ; // not useful in cpp
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::string robot_name = "";
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::string robot_name = "";
//int oldcmdID=0;
int rc_cmd;
float fcu_timeout;
int armstate;
int barrier;
int message_number=0;
uint8_t no_of_robots=0;
/*tmp to be corrected*/
uint8_t no_cnt=0;
uint8_t old_val=0;
std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by,xbeesrv_name, setpoint_name;
std::string stream_client_name;
std::string relative_altitude_sub_name;
std::string setpoint_nonraw;
bool rcclient;
bool xbeeplugged = false;
bool multi_msg;
Num_robot_count count_robots;
ros::ServiceClient mav_client;
ros::ServiceClient xbeestatus_srv;
ros::Publisher payload_pub;
ros::Publisher neigh_pos_pub;
ros::Publisher localsetpoint_nonraw_pub;
ros::ServiceServer service;
ros::Subscriber current_position_sub;
ros::Subscriber users_sub;
ros::Subscriber battery_sub;
ros::Subscriber payload_sub;
ros::Subscriber flight_status_sub;
ros::Subscriber obstacle_sub;
ros::Subscriber Robot_id_sub;
ros::Subscriber relative_altitude_sub;
std::string local_pos_sub_name;
ros::Subscriber local_pos_sub;
double local_pos_new[3];
int rc_cmd;
float fcu_timeout;
int armstate;
int barrier;
int message_number=0;
uint8_t no_of_robots=0;
/*tmp to be corrected*/
uint8_t no_cnt=0;
uint8_t old_val=0 ;
std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by,xbeesrv_name, setpoint_name;
std::string stream_client_name;
std::string relative_altitude_sub_name;
std::string setpoint_nonraw;
bool rcclient;
bool xbeeplugged = false;
bool multi_msg;
Num_robot_count count_robots;
ros::ServiceClient mav_client;
ros::ServiceClient xbeestatus_srv;
ros::Publisher payload_pub;
ros::Publisher neigh_pos_pub;
ros::Publisher localsetpoint_nonraw_pub;
ros::ServiceServer service;
ros::Subscriber current_position_sub;
ros::Subscriber users_sub;
ros::Subscriber battery_sub;
ros::Subscriber payload_sub;
ros::Subscriber flight_status_sub;
ros::Subscriber obstacle_sub;
ros::Subscriber Robot_id_sub;
ros::Subscriber relative_altitude_sub;
std::string local_pos_sub_name;
ros::Subscriber local_pos_sub;
double local_pos_new[3];
ros::ServiceClient stream_client;
ros::ServiceClient stream_client;
int setpoint_counter;
double my_x = 0, my_y = 0;
std::ofstream log;
int setpoint_counter;
double my_x = 0, my_y = 0;
/*Commands for flight controller*/
//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;
std::ofstream log;
mavros_msgs::CommandBool m_cmdBool;
ros::ServiceClient arm_client;
/*Commands for flight controller*/
//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;
mavros_msgs::SetMode m_cmdSetMode;
ros::ServiceClient mode_client;
/*Initialize publisher and subscriber, done in the constructor*/
void Initialize_pub_sub(ros::NodeHandle& n_c);
mavros_msgs::CommandBool m_cmdBool;
ros::ServiceClient arm_client;
std::string current_mode; // SOLO SPECIFIC: just so you don't call the switch to same mode
mavros_msgs::SetMode m_cmdSetMode;
ros::ServiceClient mode_client;
/*Obtain data from ros parameter server*/
void Rosparameters_get(ros::NodeHandle& n_c_priv);
/*Initialize publisher and subscriber, done in the constructor*/
void Initialize_pub_sub(ros::NodeHandle& n_c);
/*compiles buzz script from the specified .bzz file*/
std::string Compile_bzz(std::string bzzfile_name);
std::string current_mode; // SOLO SPECIFIC: just so you don't call the switch to same mode
/*Flight controller service call*/
void flight_controller_service_call();
/*Neighbours pos publisher*/
void neighbours_pos_publisher();
/*Obtain data from ros parameter server*/
void Rosparameters_get(ros::NodeHandle& n_c_priv);
/*Prepare messages and publish*/
void prepare_msg_and_publish();
/*compiles buzz script from the specified .bzz file*/
std::string Compile_bzz(std::string bzzfile_name);
/*Refresh neighbours Position for every ten step*/
void maintain_pos(int tim_step);
/*Flight controller service call*/
void flight_controller_service_call();
/*Puts neighbours position inside neigbours_pos_map*/
void neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr );
/*Neighbours pos publisher*/
void neighbours_pos_publisher();
/*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 );
/*Prepare messages and publish*/
void prepare_msg_and_publish();
/*Set the current position of the robot callback*/
void set_cur_pos(double latitude,
double longitude,
double altitude);
/*convert from spherical to cartesian coordinate system callback */
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);
/*battery status callback*/
void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg);
/*flight extended status callback*/
void flight_extended_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg);
/*Refresh neighbours Position for every ten step*/
void maintain_pos(int tim_step);
/*flight status callback*/
void flight_status_update(const mavros_msgs::State::ConstPtr& msg);
/*current position callback*/
void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg);
/*Puts neighbours position inside neigbours_pos_map*/
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 );
/*Set the current position of the robot callback*/
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);
/*battery status callback */
void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg);
/*flight extended status callback*/
void flight_extended_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg);
/*flight status callback*/
void flight_status_update(const mavros_msgs::State::ConstPtr& msg);
/*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);
/*current relative altitude callback*/
void current_rel_alt(const std_msgs::Float64::ConstPtr& msg);
/*payload callback callback*/
void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg);
/*payload callback callback*/
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);
/* RC commands service */
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);
/*robot id sub callback*/
void set_robot_id(const std_msgs::UInt8::ConstPtr& msg);
/*Obstacle distance table callback*/
void obstacle_dist(const sensor_msgs::LaserScan::ConstPtr& msg);
/*Obstacle distance table callback*/
void obstacle_dist(const sensor_msgs::LaserScan::ConstPtr& msg);
/*Get publisher and subscriber from YML file*/
void GetSubscriptionParameters(ros::NodeHandle& node_handle);
/*Get publisher and subscriber from YML file*/
void GetSubscriptionParameters(ros::NodeHandle& node_handle);
/*Arm/disarm method that can be called from buzz*/
void Arm();
/*Arm/disarm method that can be called from buzz*/
void Arm();
/*set mode like guided for solo*/
void SetMode(std::string mode, int delay_miliseconds);
/*set mode like guided for solo*/
void SetMode(std::string mode, int delay_miliseconds);
/*Robot independent subscribers*/
void Subscribe(ros::NodeHandle& n_c);
/*Robot independent subscribers*/
void Subscribe(ros::NodeHandle& n_c);
void local_pos_callback(const geometry_msgs::PoseStamped::ConstPtr& pose);
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();
void fc_command_setup();
void SetLocalPosition(float x, float y, float z, float yaw);
void SetLocalPosition(float x, float y, float z, float yaw);
void SetLocalPositionNonRaw(float x, float y, float z, float yaw);
void SetLocalPositionNonRaw(float x, float y, float z, float yaw);
void SetStreamRate(int id, int rate, int on_off);
void get_number_of_robots();
void GetRobotId();
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);
void get_xbee_status();
void SetStreamRate(int id, int rate, int on_off);
void get_number_of_robots();
void GetRobotId();
};
}

17
launch/rosbuzz-solo.launch
View File

@ -1,14 +1,14 @@
<launch>
<!-- RUN mavros -->
<arg name="fcu_url" default="/dev/ttyAMA0:115200" />
<arg name="fcu_url" default="/dev/ttyS0:115200" />
<arg name="gcs_url" default="" />
<arg name="tgt_system" default="1" />
<arg name="tgt_component" default="1" />
<arg name="log_output" default="screen" />
<include file="/home/pi/ros_catkinKin_ws/src/mavros/mavros/launch/node.launch">
<arg name="pluginlists_yaml" value="/home/pi/ros_catkinKin_ws/src/mavros/mavros/launch/apm_pluginlists.yaml" />
<arg name="config_yaml" value="/home/pi/ros_catkinKin_ws/src/mavros/mavros/launch/apm_config.yaml" />
<include file="$(find mavros)/launch/node.launch">
<arg name="pluginlists_yaml" value="$(find mavros)/launch/apm_pluginlists.yaml" />
<arg name="config_yaml" value="$(find mavros)/launch/apm_config.yaml" />
<arg name="fcu_url" value="$(arg fcu_url)" />
<arg name="gcs_url" value="$(arg gcs_url)" />
@ -36,13 +36,14 @@
<param name="Xbee_Out_To_Buzz" type="str" value="inMavlink" />
<param name="Xbee_In_From_Controller" type="str" value="xbee_cmd" />
<param name="Xbee_Out_To_Controller" type="str" value="mav_dji_cmd" />
<param name="USB_port" type="str" value="/dev/ttyUSB0" />
<param name="Baud_rate" type="double" value="230400" />
<!-- run rosbuzz -->
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
<rosparam file="/home/pi/ros_catkinKin_ws/src/ROSBuzz/launch/launch_config/solo.yaml"/>
<param name="bzzfile_name" value="/home/pi/ros_catkinKin_ws/src/ROSBuzz/script/testflockfev.bzz" />
<rosparam file="$(find rosbuzz)/launch/launch_config/solo.yaml"/>
<param name="bzzfile_name" value="$(find rosbuzz)/buzz_scripts/flock.bzz" />
<param name="rcclient" value="true" />
<param name="rcservice_name" value="buzzcmd" />
<param name="in_payload" value="inMavlink"/>
@ -50,7 +51,7 @@
<param name="xbee_status_srv" value="xbee_status"/>
<param name="xbee_plugged" value="true"/>
<param name="name" value="solos1"/>
<param name="stand_by" value="/home/pi/ros_catkinKin_ws/src/ROSBuzz/script/stand_by.bzz"/>
<param name="stand_by" value="$(find rosbuzz)/buzz_scripts/stand_by.bzz"/>
</node>
</launch>

16
misc/cmdlinectr.sh
View File

@ -11,8 +11,12 @@ function arm {
function disarm {
rosservice call $1/buzzcmd 0 400 0 0 0 0 0 0 0 0
}
function testWP {
rosservice call $1/buzzcmd 0 16 0 0 0 0 0 45.45782 -- -73.63608 10
}
function record {
rosbag record $1/flight_status $1/global_position $1/users_pos $1/local_position $1/neighbours_pos /power_status /guidance/obstacle_distance /guidance/front/depth/image_rect/compressedDepth /guidance/right/depth/image_rect/compressedDepth /guidance/front/depth/points /guidance/right/depth/points /guidance/front/right/image_rect/compressed /guidance/front/left/image_rect/compressed /guidance/right/right/image_rect/compressed /guidance/right/left/image_rect/compressed /guidance/front/left/camera_info /guidance/front/right/camera_info /guidance/right/right/camera_info /guidance/right/left/camera_info
}
function clean {
sudo rm /var/log/upstart/robot*
@ -20,10 +24,18 @@ function clean {
sudo rm /var/log/upstart/x3s*
}
function startrobot {
sudo service robot start
sudo service dji start
}
function stoprobot {
sudo service robot stop
sudo service dji stop
}
function updaterobot {
# rosrun robot_upstart install --logdir ~/ROS_WS/log/ robot_upstart/launch/m100buzzynocam.launch
rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch/m100buzzy.launch
}
function updaterobot {
# rosrun robot_upstart install --logdir ~/ROS_WS/log/ robot_upstart/launch/m100buzzynocam.launch
rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch/m100buzzy.launch
}
function updaterobot {
# rosrun robot_upstart install --logdir ~/ROS_WS/log/ robot_upstart/launch/m100buzzynocam.launch

4
script/Graph_drone.graph
View File

@ -1,4 +0,0 @@
0 -1 -1 -1
1 0 1000.0 0.0
2 0 1000.0 1.57
3 0 1000.0 3.14

5
script/Graph_fixed.graph
View File

@ -1,5 +0,0 @@
0 -1 -1 -1 -1
1 0 1000.0 -1 -1
2 0 1000.0 1 1414.2
3 0 1000.0 2 1414.2
4 0 1000.0 1 1414.2

120
src/buzz_utility.cpp
View File

@ -1,7 +1,7 @@
/** @file buzz_utility.cpp
* @version 1.0
* @version 1.0
* @date 27.09.2016
* @brief Buzz Implementation as a node in ROS for Dji M100 Drone.
* @brief Buzz Implementation as a node in ROS for Dji M100 Drone.
* @author Vivek Shankar Varadharajan
* @copyright 2016 MistLab. All rights reserved.
*/
@ -18,11 +18,11 @@ namespace buzz_utility{
static uint8_t* BO_BUF = 0;
static buzzdebug_t DBG_INFO = 0;
static uint32_t MSG_SIZE = 600;//250; // Only 100 bytes of Buzz messages every step
static uint32_t MAX_MSG_SIZE = 10000; // Maximum Msg size for sending update packets
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)
@ -40,10 +40,10 @@ namespace buzz_utility{
void update_users(){
if(users_map.size()>0) {
/* Reset users information */
buzzusers_reset();
// Reset users information
// buzzusers_reset();
// create_stig_tables();
/* Get user id and update user information */
// 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);
@ -52,21 +52,20 @@ namespace buzz_utility{
(it->second).y,
(it->second).z);
}
}/*else
ROS_INFO("[%i] No new users",Robot_id);*/
}
}
int buzzusers_reset() {
/*int buzzusers_reset() {
if(VM->state != BUZZVM_STATE_READY) return VM->state;
/* Make new table */
//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));
//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_tget(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
buzzvm_gload(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "dataG", 1));
@ -75,55 +74,47 @@ namespace buzz_utility{
//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 */
// 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_gload(VM);
//buzzvm_pushi(VM, 1);
//buzzvm_callc(VM);
buzzvm_type_assert(VM, 1, BUZZTYPE_TABLE);
buzzobj_t nbr = buzzvm_stack_at(VM, 1);
/* Get "data" field */
buzzvm_pushs(VM, buzzvm_string_register(VM, "dataG", 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_push(VM, nbr);
buzzvm_pushs(VM, buzzvm_string_register(VM, "dataG", 1));
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 */
// When we get here, the "data" table is on top of the stack
// Push user id
buzzvm_pushi(VM, id);
/* Create entry table */
// Create entry table
buzzobj_t entry = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE);
/* Insert latitude */
// Insert latitude
buzzvm_push(VM, entry);
buzzvm_pushs(VM, buzzvm_string_register(VM, "la", 1));
buzzvm_pushf(VM, latitude);
buzzvm_tput(VM);
/* Insert longitude */
// Insert longitude
buzzvm_push(VM, entry);
buzzvm_pushs(VM, buzzvm_string_register(VM, "lo", 1));
buzzvm_pushf(VM, longitude);
buzzvm_tput(VM);
/* Insert altitude */
// 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 */
// 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);
@ -176,24 +167,26 @@ namespace buzz_utility{
uint16_t* data= u64_cvt_u16((uint64_t)payload[0]);
/*Size is at first 2 bytes*/
uint16_t size=data[0]*sizeof(uint64_t);
delete[] data;
uint16_t neigh_id =(uint16_t) data[1];
delete[] data;
uint8_t* pl =(uint8_t*)malloc(size);
/* Copy packet into temporary buffer */
memcpy(pl, payload ,size);
IN_MSG.push_back(pl);
}
void in_message_process(){
while(!IN_MSG.empty()){
uint8_t* first_INmsg = (uint8_t*)IN_MSG.front();
/* Go through messages and append them to the FIFO */
uint16_t* data= u64_cvt_u16((uint64_t)first_INmsg[0]);
uint8_t* first_INmsg = (uint8_t*)IN_MSG.front();
size_t tot =0;
/*Size is at first 2 bytes*/
uint16_t size=data[0]*sizeof(uint64_t);
delete[] data;
/*size and robot id read*/
size_t tot = sizeof(uint32_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);
/* Go through the messages until there's nothing else to read */
uint16_t unMsgSize=0;
/*Obtain Buzz messages push it into queue*/
@ -204,12 +197,13 @@ namespace buzz_utility{
/* 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));
tot += unMsgSize;
}
}while(size - tot > sizeof(uint16_t) && unMsgSize > 0);
IN_MSG.erase(IN_MSG.begin());
free(first_INmsg);
free(first_INmsg);
IN_MSG.erase(IN_MSG.begin());
}
/* Process messages VM call*/
buzzvm_process_inmsgs(VM);
@ -220,7 +214,8 @@ namespace buzz_utility{
/***************************************************/
uint64_t* obt_out_msg(){
/* Process out messages */
buzzvm_process_outmsgs(VM);
buzzvm_process_outmsgs(VM);
/*TODO change*/
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*/
@ -329,8 +324,8 @@ namespace buzz_utility{
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_land", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_land));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "add_user_rb", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_adduserRB));
buzzvm_pushs(VM, buzzvm_string_register(VM, "add_targetrb", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_addtargetRB));
buzzvm_gstore(VM);
return VM->state;
@ -361,7 +356,7 @@ namespace buzz_utility{
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_disarm", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
@ -371,7 +366,7 @@ namespace buzz_utility{
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_land", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "add_user_rb", 1));
buzzvm_pushs(VM, buzzvm_string_register(VM, "add_targetrb", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
@ -414,7 +409,7 @@ int create_stig_tables() {
//buzzvm_gstore(VM);
//buzzvm_dump(VM);
/*buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
buzzvm_gload(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1));
buzzvm_tget(VM);
@ -435,7 +430,7 @@ int create_stig_tables() {
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));
@ -498,7 +493,8 @@ int create_stig_tables() {
ROS_ERROR("[%i] Error registering hooks", Robot_id);
return 0;
}
/* Create vstig tables
/* Create vstig tables
if(create_stig_tables() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
@ -507,10 +503,10 @@ int create_stig_tables() {
//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);
@ -527,7 +523,7 @@ int create_stig_tables() {
return 1;//buzz_update_set(BO_BUF, bdbg_filename, bcode_size);
}
/****************************************/
/*Sets a new update */
/****************************************/
@ -560,7 +556,7 @@ int create_stig_tables() {
fprintf(stdout, "%s: Error registering hooks\n\n", BO_FNAME);
return 0;
}
/* Create vstig tables
/* Create vstig tables
if(create_stig_tables() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
@ -569,7 +565,7 @@ int create_stig_tables() {
//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);
@ -616,7 +612,7 @@ int create_stig_tables() {
fprintf(stdout, "%s: Error registering hooks\n\n", BO_FNAME);
return 0;
}
/* Create vstig tables
/* Create vstig tables
if(create_stig_tables() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
@ -689,9 +685,11 @@ int create_stig_tables() {
void update_sensors(){
/* Update sensors*/
buzzuav_closures::buzzuav_update_battery(VM);
buzzuav_closures::buzzuav_update_xbee_status(VM);
buzzuav_closures::buzzuav_update_prox(VM);
buzzuav_closures::buzzuav_update_currentpos(VM);
buzzuav_closures::update_neighbors(VM);
buzzuav_closures::buzzuav_update_targets(VM);
//update_users();
buzzuav_closures::buzzuav_update_flight_status(VM);
}
@ -708,12 +706,12 @@ int create_stig_tables() {
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);
/* Check swarm state -- Not crashing thanks to test added in check_swarm_members */
//int status = 1;
@ -771,7 +769,7 @@ int create_stig_tables() {
buzz_error_info());
fprintf(stdout, "step test VM state %i\n",a);
}
return a == BUZZVM_STATE_READY;
}
@ -785,5 +783,3 @@ int create_stig_tables() {
buzzvm_gstore(VM);
}
}

310
src/buzzuav_closures.cpp
View File

@ -1,7 +1,7 @@
/** @file buzzuav_closures.cpp
* @version 1.0
* @version 1.0
* @date 27.09.2016
* @brief Buzz Implementation as a node in ROS for Dji M100 Drone.
* @brief Buzz Implementation as a node in ROS for Dji M100 Drone.
* @author Vivek Shankar Varadharajan
* @copyright 2016 MistLab. All rights reserved.
*/
@ -10,7 +10,6 @@
#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*/
@ -25,8 +24,13 @@ namespace buzzuav_closures{
static int rc_cmd=0;
static int buzz_cmd=0;
static float height=0;
static bool deque_full = false;
static float rssi = 0.0;
static float raw_packet_loss = 0.0;
static float filtered_packet_loss = 0.0;
static float api_rssi = 0.0;
std::map< int, buzz_utility::RB_struct> targets_map;
std::map< int, buzz_utility::Pos_struct> neighbors_map;
/****************************************/
@ -89,13 +93,20 @@ namespace buzzuav_closures{
out[2] = height; //constant height.
}
void rb_from_gps(double nei[], double out[], double cur[]){
float constrainAngle(float x){
x = fmod(x,2*M_PI);
if (x < 0.0)
x += 2*M_PI;
return x;
}
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] = atan2(ned_y,ned_x);
out[1] = constrainAngle(atan2(ned_y,ned_x));
out[2] = 0.0;
}
@ -126,57 +137,57 @@ namespace buzzuav_closures{
gps_from_rb(d, b, goto_pos);
cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;*/
//printf(" Vector for Goto: %.7f,%.7f\n",dx,dy);
//printf(" Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f \n",goto_pos[0], goto_pos[1], goto_pos[2]);
//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 users_add2localtable(buzzvm_t vm, int id, float range, float bearing) {
int buzzuav_update_targets(buzzvm_t vm) {
if(vm->state != BUZZVM_STATE_READY) return vm->state;
buzzvm_pushs(vm, buzzvm_string_register(vm, "users", 1));
buzzvm_gload(vm);
buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE);
buzzobj_t nbr = buzzvm_stack_at(vm, 1);
/* Get "data" field */
buzzvm_pushs(vm, buzzvm_string_register(vm, "dataL", 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_push(vm, nbr);
buzzvm_pushs(vm, buzzvm_string_register(vm, "dataL", 1));
buzzvm_pusht(vm);
buzzobj_t data = buzzvm_stack_at(vm, 1);
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).la;tmp[1]=(it->second).lo;tmp[2]=height;
rb_from_gps(tmp, rb, cur_pos);
ROS_WARN("----------Pushing target id %i (%f,%f)", rb[0], rb[1]);
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);
/* Push user id */
buzzvm_pushi(vm, it->first);
/* Create entry table */
buzzobj_t entry = buzzheap_newobj(vm->heap, BUZZTYPE_TABLE);
/* Insert range */
buzzvm_push(vm, entry);
buzzvm_pushs(vm, buzzvm_string_register(vm, "range", 1));
buzzvm_pushf(vm, rb[0]);
buzzvm_tput(vm);
/* Insert longitude */
buzzvm_push(vm, entry);
buzzvm_pushs(vm, buzzvm_string_register(vm, "bearing", 1));
buzzvm_pushf(vm, rb[1]);
buzzvm_tput(vm);
/* Save entry into data table */
buzzvm_push(vm, entry);
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 range */
buzzvm_push(vm, entry);
buzzvm_pushs(vm, buzzvm_string_register(vm, "r", 1));
buzzvm_pushf(vm, range);
buzzvm_tput(vm);
/* Insert longitude */
buzzvm_push(vm, entry);
buzzvm_pushs(vm, buzzvm_string_register(vm, "b", 1));
buzzvm_pushf(vm, bearing);
buzzvm_tput(vm);
/* Save entry into data table */
buzzvm_push(vm, entry);
buzzvm_tput(vm);
//printf("\tBuzz_closure saved new user: %i (%f,%f)\n", id, range, bearing);
buzzvm_gstore(vm);
return vm->state;
}
int buzzuav_adduserRB(buzzvm_t vm) {
int buzzuav_addtargetRB(buzzvm_t vm) {
buzzvm_lnum_assert(vm, 3);
buzzvm_lload(vm, 1); /* longitude */
buzzvm_lload(vm, 2); /* latitude */
buzzvm_lload(vm, 3); /* id */
buzzvm_lload(vm, 1); // longitude
buzzvm_lload(vm, 2); // latitude
buzzvm_lload(vm, 3); // id
buzzvm_type_assert(vm, 3, BUZZTYPE_INT);
buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
@ -189,10 +200,20 @@ namespace buzzuav_closures{
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]);
return users_add2localtable(vm, uid, rb[0], rb[1]);
//printf("\tGot new user from bzz stig: %i - %f, %f\n", uid, rb[0], rb[1]);
buzz_utility::RB_struct RB_arr;
RB_arr.la=tmp[0];
RB_arr.lo=tmp[1];
RB_arr.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);
return vm->state;
} else
printf(" ---------- User too far %f\n",rb[0]);
printf(" ---------- Target too far %f\n",rb[0]);
return 0;
}
@ -318,7 +339,60 @@ namespace buzzuav_closures{
buzzvm_gstore(vm);
return vm->state;
}
/****************************************/
void set_deque_full(bool state)
{
deque_full = state;
}
void set_rssi(float value)
{
rssi = value;
}
void set_raw_packet_loss(float value)
{
raw_packet_loss = value;
}
void set_filtered_packet_loss(float value)
{
filtered_packet_loss = value;
}
void set_api_rssi(float value)
{
api_rssi = value;
}
int buzzuav_update_xbee_status(buzzvm_t vm) {
buzzvm_pushs(vm, buzzvm_string_register(vm, "xbee_status", 1));
buzzvm_pusht(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "deque_full", 1));
buzzvm_pushi(vm, static_cast<uint8_t>(deque_full));
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "rssi", 1));
buzzvm_pushf(vm, rssi);
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "raw_packet_loss", 1));
buzzvm_pushf(vm, raw_packet_loss);
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "filtered_packet_loss", 1));
buzzvm_pushf(vm, filtered_packet_loss);
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "api_rssi", 1));
buzzvm_pushf(vm, api_rssi);
buzzvm_tput(vm);
buzzvm_gstore(vm);
return vm->state;
}
/***************************************/
/*current pos update*/
/***************************************/
void set_currentpos(double latitude, double longitude, double altitude){
@ -393,7 +467,7 @@ namespace buzzuav_closures{
rc_cmd=0;
buzzvm_pushs(vm, buzzvm_string_register(vm, "status", 1));
buzzvm_pushi(vm, status);
buzzvm_tput(vm);
buzzvm_tput(vm);
buzzvm_gstore(vm);
//also set rc_controllers goto
buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_goto", 1));
@ -418,7 +492,7 @@ namespace buzzuav_closures{
/******************************************************/
/*Create an obstacle Buzz table from proximity sensors*/
/* Acessing proximity in buzz script
/* Acessing proximity in buzz script
proximity[0].angle and proximity[0].value - front
"" "" "" - right and back
proximity[3].angle and proximity[3].value - left
@ -427,11 +501,11 @@ namespace buzzuav_closures{
int buzzuav_update_prox(buzzvm_t vm) {
buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity", 1));
buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity", 1));
buzzvm_pusht(vm);
buzzobj_t tProxTable = buzzvm_stack_at(vm, 1);
buzzvm_gstore(vm);
/* Fill into the proximity table */
buzzobj_t tProxRead;
float angle =0;
@ -441,80 +515,80 @@ namespace buzzuav_closures{
tProxRead = buzzvm_stack_at(vm, 1);
buzzvm_pop(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_tput(vm);
buzzvm_push(vm, tProxRead);
buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0));
buzzvm_pushf(vm, angle);
buzzvm_tput(vm);
buzzvm_push(vm, tProxRead);
buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0));
buzzvm_pushf(vm, obst[i+1]);
buzzvm_tput(vm);
buzzvm_push(vm, tProxRead);
buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0));
buzzvm_pushf(vm, angle);
buzzvm_tput(vm);
/* Store read table in the proximity table */
buzzvm_push(vm, tProxTable);
buzzvm_pushi(vm, i);
buzzvm_push(vm, tProxRead);
buzzvm_tput(vm);
angle+=1.5708;
buzzvm_pushi(vm, i);
buzzvm_push(vm, tProxRead);
buzzvm_tput(vm);
angle+=1.5708;
}
/* Create table for bottom read */
angle =-1;
/* Create table for bottom read */
angle =-1;
buzzvm_pusht(vm);
tProxRead = buzzvm_stack_at(vm, 1);
buzzvm_pop(vm);
/* Fill in the read */
buzzvm_push(vm, tProxRead);
buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0));
buzzvm_pushf(vm, obst[0]);
buzzvm_tput(vm);
buzzvm_push(vm, tProxRead);
buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0));
buzzvm_pushf(vm, angle);
buzzvm_tput(vm);
/*Store read table in the proximity table*/
buzzvm_push(vm, tProxTable);
buzzvm_pushi(vm, 4);
buzzvm_push(vm, tProxRead);
buzzvm_tput(vm);
buzzvm_push(vm, tProxRead);
buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0));
buzzvm_pushf(vm, obst[0]);
buzzvm_tput(vm);
buzzvm_push(vm, tProxRead);
buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0));
buzzvm_pushf(vm, angle);
buzzvm_tput(vm);
/*Store read table in the proximity table*/
buzzvm_push(vm, tProxTable);
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;
}
/*
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 */
/**********************************************/
/**********************************************/
/*Dummy closure for use during update testing */
/**********************************************/
int dummy_closure(buzzvm_t vm){ return buzzvm_ret0(vm);}
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;
//}
/***********************************************/
/* Store Ros controller object pointer */
/***********************************************/
//void set_ros_controller_ptr(const rosbzz_node::roscontroller* roscontroller_ptrin){
//roscontroller_ptr = roscontroller_ptrin;
//}
}

2125
src/roscontroller.cpp
View File

File diff suppressed because it is too large Load Diff