Merge commit 'f24fb637230800de47d5b978bbebca1d4338cec9' into sim
This commit is contained in:
commit
3d73e2e01c
|
@ -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) {
|
|
@ -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")
|
||||
}
|
|
@ -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,37 +119,37 @@ 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{
|
||||
var i=0
|
||||
var renturn_val=1
|
||||
if(exponent==0)
|
||||
return 1
|
||||
else{
|
||||
while(i<exponent){
|
||||
renturn_val=renturn_val*base
|
||||
i=i+1
|
||||
|
@ -272,7 +250,7 @@ neighbors.listen("msg",
|
|||
#
|
||||
#Function used to get the station info of the sender of the message
|
||||
function Get_DisAndAzi(id){
|
||||
neighbors.foreach(
|
||||
neighbors.foreach(
|
||||
function(rid, data) {
|
||||
if(rid==id){
|
||||
m_receivedMessage.Range=data.distance*100.0
|
||||
|
@ -285,73 +263,73 @@ neighbors.foreach(
|
|||
#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"){
|
||||
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"){
|
||||
}
|
||||
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)){
|
||||
}
|
||||
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",
|
||||
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
|
|
@ -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
|
|
@ -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,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
|
||||
}
|
|
@ -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()
|
|
@ -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);
|
||||
|
||||
|
|
|
@ -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 */
|
||||
|
@ -83,11 +88,16 @@ 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
|
||||
* use flight.status for flight status
|
||||
|
|
|
@ -42,9 +42,11 @@
|
|||
|
||||
using namespace std;
|
||||
|
||||
namespace rosbzz_node{
|
||||
namespace rosbzz_node
|
||||
{
|
||||
|
||||
class roscontroller{
|
||||
class roscontroller
|
||||
{
|
||||
|
||||
public:
|
||||
roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv);
|
||||
|
@ -56,18 +58,17 @@ private:
|
|||
struct num_robot_count
|
||||
{
|
||||
uint8_t history[10];
|
||||
uint8_t index=0;
|
||||
uint8_t current=0;
|
||||
uint8_t index = 0;
|
||||
uint8_t current = 0;
|
||||
num_robot_count(){}
|
||||
|
||||
}; typedef struct num_robot_count 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 ;
|
||||
}; typedef struct gps GPS ; // not useful in cpp
|
||||
|
||||
GPS target, home, cur_pos;
|
||||
double cur_rel_altitude;
|
||||
|
@ -80,6 +81,7 @@ private:
|
|||
int robot_id=0;
|
||||
std::string robot_name = "";
|
||||
//int oldcmdID=0;
|
||||
|
||||
int rc_cmd;
|
||||
float fcu_timeout;
|
||||
int armstate;
|
||||
|
@ -88,7 +90,7 @@ private:
|
|||
uint8_t no_of_robots=0;
|
||||
/*tmp to be corrected*/
|
||||
uint8_t no_cnt=0;
|
||||
uint8_t old_val=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;
|
||||
|
@ -171,6 +173,7 @@ private:
|
|||
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);
|
||||
|
@ -178,7 +181,7 @@ private:
|
|||
double gps_t_lon, double gps_t_lat,
|
||||
double gps_r_lon, double gps_r_lat);
|
||||
|
||||
/*battery status callback*/
|
||||
/*battery status callback */
|
||||
void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg);
|
||||
|
||||
/*flight extended status callback*/
|
||||
|
@ -232,7 +235,17 @@ private:
|
|||
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();
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
@ -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>
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
|
@ -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
|
|
@ -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,6 +167,7 @@ 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);
|
||||
uint16_t neigh_id =(uint16_t) data[1];
|
||||
delete[] data;
|
||||
uint8_t* pl =(uint8_t*)malloc(size);
|
||||
/* Copy packet into temporary buffer */
|
||||
|
@ -186,14 +178,15 @@ namespace buzz_utility{
|
|||
|
||||
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);
|
||||
IN_MSG.erase(IN_MSG.begin());
|
||||
}
|
||||
/* Process messages VM call*/
|
||||
buzzvm_process_inmsgs(VM);
|
||||
|
@ -221,6 +215,7 @@ namespace buzz_utility{
|
|||
uint64_t* obt_out_msg(){
|
||||
/* Process out messages */
|
||||
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;
|
||||
|
@ -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);
|
||||
|
@ -498,6 +493,7 @@ int create_stig_tables() {
|
|||
ROS_ERROR("[%i] Error registering hooks", Robot_id);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* Create vstig tables
|
||||
if(create_stig_tables() != BUZZVM_STATE_READY) {
|
||||
buzzvm_destroy(&VM);
|
||||
|
@ -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);
|
||||
}
|
||||
|
@ -785,5 +783,3 @@ int create_stig_tables() {
|
|||
buzzvm_gstore(VM);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
|
|
@ -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.
|
||||
}
|
||||
|
||||
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_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 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 */
|
||||
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, 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, "r", 1));
|
||||
buzzvm_pushf(vm, range);
|
||||
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, "b", 1));
|
||||
buzzvm_pushf(vm, bearing);
|
||||
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);
|
||||
//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);
|
||||
|
@ -190,9 +201,19 @@ 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]);
|
||||
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){
|
||||
|
|
File diff suppressed because it is too large
Load Diff
Loading…
Reference in New Issue