graph integrated, not working
This commit is contained in:
dave
parent
cf35d9696b
commit
0093556096
|
|
@ -0,0 +1,5 @@
|
|||
0 -1 -1 -1
|
||||
1 0 10.0 0.0
|
||||
2 0 10.0 1.57
|
||||
3 0 10.0 3.14
|
||||
4 0 10.0 4.71
|
||||
|
|
@ -0,0 +1,5 @@
|
|||
0 -1 -1 -1 -1
|
||||
1 0 10.0 -1 -1
|
||||
2 0 10.0 1 12.0
|
||||
3 0 10.0 2 12.0
|
||||
4 0 10.0 1 12.0
|
||||
|
|
@ -0,0 +1,879 @@
|
|||
#
|
||||
# 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"
|
||||
|
||||
#
|
||||
#Constant parameters, need to be adjust
|
||||
#parameters for set_wheels
|
||||
m_sWheelTurningParams={.MaxSpeed=1.0}
|
||||
|
||||
ROBOT_RADIUS=1.0
|
||||
ROBOT_DIAMETER=2.0*ROBOT_RADIUS
|
||||
ROBOT_SAFETYDIST=3.0*ROBOT_DIAMETER
|
||||
|
||||
#
|
||||
# Global variables
|
||||
#
|
||||
|
||||
#
|
||||
#Save message from all neighours
|
||||
#the indexes are as 1,2,3..., while each value is a table that store the information of a neighbour robot
|
||||
m_MessageState={}#store received neighbour message
|
||||
m_MessageLable={}#store received neighbour message
|
||||
m_MessageReqLable={}#store received neighbour message
|
||||
m_MessageReqID={}#store received neighbour message
|
||||
m_MessageSide={}#store received neighbour message
|
||||
m_MessageResponse={}#store received neighbour message
|
||||
m_MessageRange={}#store received neighbour message
|
||||
m_MessageBearing={}#store received neighbour message
|
||||
m_neighbourCunt=0#used to cunt neighbours
|
||||
#Save message from one neighbour
|
||||
#the indexes are as State(received state),Lable(received lable),ReqLable,ReqID,Side,Response,Range,Bearing
|
||||
m_receivedMessage={.State="STATE_FREE",.Lable=0,.ReqLable=0,.ReqID=0,.Side=0,.Response="REQ_NONE",.Range=0,.Bearing=0}
|
||||
#
|
||||
#Save the message to send
|
||||
#The keys of the talbe is State(current state),Lable(current lable),ReqLable(requested lable),ReqID(request id),Side(side),Response(reply message{REQ_NONE,REQ_GRANTED,REQ_RESEND})
|
||||
m_selfMessage={.State="STATE_FREE",.Lable=0,.ReqLable=0,.ReqID=0,.Side=0,.Response="REQ_NONE"}
|
||||
|
||||
#Current robot state
|
||||
m_eState="STATE_FREE"
|
||||
|
||||
#navigation vector
|
||||
m_navigation={.x=0,.y=0}
|
||||
|
||||
#Debug message to be displayed in qt-opengl
|
||||
#m_ossDebugMsg
|
||||
|
||||
#Debug vector to draw
|
||||
#CVector2 m_cDebugVector
|
||||
|
||||
#Table of the nodes in the graph
|
||||
m_vecNodes={}
|
||||
m_vecNodes_fixed={}
|
||||
|
||||
#Current label being requested or chosen (-1 when none)
|
||||
m_nLabel=-1
|
||||
|
||||
#Label request id
|
||||
m_unRequestId=0
|
||||
|
||||
#Side
|
||||
m_unbSide=0
|
||||
|
||||
#Global bias, used to map local coordinate to global coordinate
|
||||
m_bias=0
|
||||
|
||||
#Vector to predecessor,range is the distance between robots, bearing is the angle of pred wrt self in local coordinate of self, globalbearing is the angle of self wrt pred in global coordinate
|
||||
m_cMeToPred={.Range=0.0,.Bearing=0.0,.GlobalBearing=0.0}
|
||||
|
||||
#Counter to wait for something to happen
|
||||
m_unWaitCount=0
|
||||
|
||||
#Number of steps to wait before looking for a free label
|
||||
m_unLabelSearchWaitTime=0
|
||||
|
||||
#Number of steps to wait for an answer to be received
|
||||
m_unResponseTimeThreshold=0
|
||||
|
||||
#Number of steps to wait until giving up joining
|
||||
m_unJoiningLostPeriod=0
|
||||
|
||||
#Tolerance distance to a target location
|
||||
m_fTargetDistanceTolerance=0
|
||||
|
||||
#step cunt
|
||||
step_cunt=0
|
||||
|
||||
#virtual stigmergy
|
||||
v_tag = stigmergy.create(1)
|
||||
|
||||
# Lennard-Jones parameters
|
||||
EPSILON = 3.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
|
||||
}
|
||||
}
|
||||
|
||||
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)
|
||||
}
|
||||
|
||||
#
|
||||
# Calculates the angle of the given vector2.
|
||||
# PARAM v: The vector2.
|
||||
# RETURN: The angle of the vector.
|
||||
#
|
||||
Angle = function(v) {
|
||||
return math.atan(v.y, v.x)
|
||||
}
|
||||
|
||||
#
|
||||
#return the number of value in table
|
||||
#
|
||||
function count(table,value){
|
||||
var number=0
|
||||
var i=0
|
||||
while(i<size(table)){
|
||||
if(table[i]==value){
|
||||
number=number+1
|
||||
}
|
||||
i=i+1
|
||||
}
|
||||
return number
|
||||
}
|
||||
|
||||
#
|
||||
#return the index of value
|
||||
#
|
||||
function find(table,value){
|
||||
var index=nil
|
||||
var i=0
|
||||
while(i<size(table)){
|
||||
if(table[i]==value)
|
||||
index=i
|
||||
i=i+1
|
||||
}
|
||||
return index
|
||||
}
|
||||
|
||||
function pow(base,exponent){
|
||||
var i=0
|
||||
var renturn_val=1
|
||||
if(exponent==0)
|
||||
return 1
|
||||
else{
|
||||
while(i<exponent){
|
||||
renturn_val=renturn_val*base
|
||||
i=i+1
|
||||
}
|
||||
return renturn_val
|
||||
}
|
||||
}
|
||||
|
||||
#
|
||||
# Graph parsing
|
||||
#
|
||||
function parse_graph(fname) {
|
||||
# Graph data
|
||||
var gd = {}
|
||||
# Open the file
|
||||
var fd = io.fopen(fname, "r")
|
||||
if(not fd) {
|
||||
log("Can't open '", fname, "'")
|
||||
return nil
|
||||
}
|
||||
# Parse the file line by line
|
||||
var rrec # Record read from line
|
||||
var grec # Record parsed into graph
|
||||
io.fforeach(fd, function(line) {
|
||||
# Parse file line
|
||||
rrec = string.split(line, "\t ")
|
||||
# Make record
|
||||
gd[string.toint(rrec[0])] = { # The .graph file is stored according the sequence of lable, predecessor, distance, bearing
|
||||
.Lable = string.toint(rrec[0]), # Lable of the point
|
||||
.Pred = string.toint(rrec[1]), # Lable of its predecessor
|
||||
.distance = string.tofloat(rrec[2]), # distance to the predecessor
|
||||
.bearing = string.tofloat(rrec[3]), # bearing form the predecessor to this dot
|
||||
.State="UNASSIGNED",
|
||||
.StateAge=0
|
||||
}})
|
||||
# All done
|
||||
io.fclose(fd)
|
||||
return gd
|
||||
}
|
||||
|
||||
function parse_graph_fixed(fname) {
|
||||
# Graph data
|
||||
var gd = {}
|
||||
# Open the file
|
||||
var fd = io.fopen(fname, "r")
|
||||
if(not fd) {
|
||||
log("Can't open '", fname, "'")
|
||||
return nil
|
||||
}
|
||||
# Parse the file line by line
|
||||
var rrec # Record read from line
|
||||
var grec # Record parsed into graph
|
||||
io.fforeach(fd, function(line) {
|
||||
# Parse file line
|
||||
rrec = string.split(line, "\t ")
|
||||
# Make record
|
||||
gd[string.toint(rrec[0])] = { # The .graph file is stored according the sequence of lable, pre1, dis2pr1, pre2, ids2pre2, side
|
||||
.Pred1 = string.toint(rrec[1]), # Pred 1 lable
|
||||
.Pred2 = string.toint(rrec[3]), # Pred 2 lable
|
||||
.d1 = string.tofloat(rrec[2]), # Pred 1 distance
|
||||
.d2 = string.tofloat(rrec[4]), # Pred 2 distance
|
||||
#.S = string.toint(rrec[5]), # Side
|
||||
.Lable=string.toint(rrec[0]),
|
||||
.State="UNASSIGNED",
|
||||
.StateAge=0
|
||||
}})
|
||||
# All done
|
||||
io.fclose(fd)
|
||||
return gd
|
||||
}
|
||||
|
||||
|
||||
function start_listen(){
|
||||
neighbors.listen("msg",
|
||||
function(vid,value,rid){
|
||||
#store the received message
|
||||
var temp_id=rid
|
||||
m_receivedMessage.State=value.State
|
||||
m_receivedMessage.Lable=value.Lable
|
||||
m_receivedMessage.ReqLable=value.ReqLable
|
||||
m_receivedMessage.ReqID=value.ReqID
|
||||
m_receivedMessage.Side=value.Side
|
||||
m_receivedMessage.Response=value.Response
|
||||
Get_DisAndAzi(temp_id)
|
||||
#add the received message
|
||||
#
|
||||
m_MessageState[m_neighbourCunt]=value.State
|
||||
m_MessageLable[m_neighbourCunt]=value.Lable
|
||||
m_MessageReqLable[m_neighbourCunt]=value.ReqLable
|
||||
m_MessageReqID[m_neighbourCunt]=value.ReqID
|
||||
m_MessageSide[m_neighbourCunt]=value.Side
|
||||
m_MessageResponse[m_neighbourCunt]=value.Response
|
||||
m_MessageRange[m_neighbourCunt]=m_receivedMessage.Range
|
||||
m_MessageBearing[m_neighbourCunt]=m_receivedMessage.Bearing
|
||||
m_neighbourCunt=m_neighbourCunt+1
|
||||
})
|
||||
}
|
||||
#
|
||||
#Function used to get the station info of the sender of the message
|
||||
function Get_DisAndAzi(id){
|
||||
neighbors.foreach(
|
||||
function(rid, data) {
|
||||
if(rid==id){
|
||||
m_receivedMessage.Range=data.distance
|
||||
m_receivedMessage.Bearing=data.azimuth
|
||||
}
|
||||
})
|
||||
}
|
||||
|
||||
#
|
||||
#Update node info according to neighbour robots
|
||||
#
|
||||
function UpdateNodeInfo(){
|
||||
#Collect informaiton
|
||||
#Update information
|
||||
var i=0
|
||||
|
||||
while(i<m_neighbourCunt){
|
||||
if(m_MessageState[i]=="STATE_JOINED"){
|
||||
m_vecNodes[m_MessageLable[i]].State="ASSIGNED"
|
||||
m_vecNodes[m_MessageLable[i]].StateAge=m_unJoiningLostPeriod
|
||||
}
|
||||
else if(m_MessageState[i]=="STATE_JOINING"){
|
||||
m_vecNodes[m_MessageLable[i]].State="ASSIGNING"
|
||||
m_vecNodes[m_MessageLable[i]].StateAge=m_unJoiningLostPeriod
|
||||
}
|
||||
i=i+1
|
||||
}
|
||||
#Forget old information
|
||||
i=0
|
||||
while(i<size(m_vecNodes)){
|
||||
if((m_vecNodes[i].StateAge>0) and (m_vecNodes[i].State=="ASSIGNING")){
|
||||
m_vecNodes[i].StateAge=m_vecNodes[i].StateAge-1
|
||||
if(m_vecNodes[i].StateAge==0)
|
||||
m_vecNodes[i].State="UNASSIGNED"
|
||||
}
|
||||
i=i+1
|
||||
}
|
||||
}
|
||||
|
||||
#
|
||||
#Transistion to state free
|
||||
#
|
||||
function TransitionToFree(){
|
||||
m_eState="STATE_FREE"
|
||||
m_unWaitCount=m_unLabelSearchWaitTime
|
||||
m_selfMessage.State=m_eState
|
||||
}
|
||||
|
||||
#
|
||||
#Transistion to state asking
|
||||
#
|
||||
function TransitionToAsking(un_label){
|
||||
m_eState="STATE_ASKING"
|
||||
m_nLabel=un_label
|
||||
m_unRequestId=rng.uniform(0,65536)+id#don't know why the random numbers are the same, add id to make the ReqID different
|
||||
m_selfMessage.State=m_eState
|
||||
m_selfMessage.ReqLable=m_nLabel
|
||||
m_selfMessage.ReqID=m_unRequestId
|
||||
|
||||
m_unWaitCount=m_unResponseTimeThreshold
|
||||
}
|
||||
|
||||
#
|
||||
#Transistion to state joining
|
||||
#
|
||||
function TransitionToJoining(){
|
||||
m_eState="STATE_JOINING"
|
||||
m_selfMessage.State=m_eState
|
||||
m_selfMessage.Lable=m_nLabel
|
||||
m_unWaitCount=m_unJoiningLostPeriod
|
||||
|
||||
neighbors.listen("reply",
|
||||
function(vid,value,rid){
|
||||
#store the received message
|
||||
if(value.Lable==m_nLabel){
|
||||
m_cMeToPred.GlobalBearing=value.GlobalBearing
|
||||
|
||||
}
|
||||
})
|
||||
|
||||
}
|
||||
|
||||
#
|
||||
#Transistion to state joined
|
||||
#
|
||||
function TransitionToJoined(){
|
||||
m_eState="STATE_JOINED"
|
||||
m_selfMessage.State=m_eState
|
||||
m_selfMessage.Lable=m_nLabel
|
||||
m_vecNodes[m_nLabel].State="ASSIGNED"
|
||||
neighbors.ignore("reply")
|
||||
|
||||
#write statues
|
||||
v_tag.put(m_nLabel, 1)
|
||||
|
||||
m_navigation.x=0.0
|
||||
m_navigation.y=0.0
|
||||
uav_moveto(m_navigation.x,m_navigation.y)
|
||||
}
|
||||
|
||||
#
|
||||
#Transistion to state Lock, lock the current formation
|
||||
#
|
||||
function TransitionToLock(){
|
||||
m_eState="STATE_LOCK"
|
||||
m_selfMessage.State=m_eState
|
||||
m_selfMessage.Lable=m_nLabel
|
||||
m_vecNodes[m_nLabel].State="ASSIGNED"
|
||||
|
||||
m_navigation.x=0.0
|
||||
m_navigation.y=0.0
|
||||
uav_moveto(m_navigation.x,m_navigation.y)
|
||||
}
|
||||
|
||||
#
|
||||
# Do free
|
||||
#
|
||||
function DoFree() {
|
||||
m_selfMessage.State=m_eState
|
||||
#wait for a while before looking for a lable
|
||||
if(m_unWaitCount>0)
|
||||
m_unWaitCount=m_unWaitCount-1
|
||||
|
||||
#find a set of joined robots
|
||||
var setJoinedLables={}
|
||||
var setJoinedIndexes={}
|
||||
var i=0
|
||||
var j=0
|
||||
while(i<m_neighbourCunt){
|
||||
if(m_MessageState[i]=="STATE_JOINED"){
|
||||
setJoinedLables[j]=m_MessageLable[i]
|
||||
setJoinedIndexes[j]=i
|
||||
j=j+1
|
||||
}
|
||||
i=i+1
|
||||
}
|
||||
|
||||
#go through the graph to look for a proper lable
|
||||
var unFoundLable=0
|
||||
var IDofPred=0
|
||||
i=1
|
||||
while(i<size(m_vecNodes) and (unFoundLable==0)){
|
||||
#if the node is unassigned and the predecessor is insight
|
||||
if(m_vecNodes[i].State=="UNASSIGNED" and count(setJoinedLables,m_vecNodes[i].Pred)==1){
|
||||
unFoundLable=m_vecNodes[i].Lable
|
||||
IDofPred=find(m_MessageLable,m_vecNodes[unFoundLable].Pred)
|
||||
}
|
||||
i=i+1
|
||||
}
|
||||
|
||||
if(unFoundLable>0){
|
||||
TransitionToAsking(unFoundLable)
|
||||
return
|
||||
}
|
||||
|
||||
#navigation
|
||||
#if there is a joined robot within sight, move around joined robots
|
||||
#else, gather with other free robots
|
||||
if(size(setJoinedIndexes)>0){
|
||||
var tempvec_P={.x=0.0,.y=0.0}
|
||||
var tempvec_N={.x=0.0,.y=0.0}
|
||||
i=0
|
||||
while(i<size(setJoinedIndexes)){
|
||||
var index=setJoinedIndexes[i]
|
||||
tempvec_P=math.vec2.add(tempvec_P,math.vec2.newp(m_MessageRange[index],m_MessageBearing[index]+0.5*math.pi))
|
||||
tempvec_N=math.vec2.add(tempvec_N,math.vec2.newp(m_MessageRange[index]-5.0*ROBOT_SAFETYDIST,m_MessageBearing[index]))
|
||||
i=i+1
|
||||
}
|
||||
tempvec_P=math.vec2.scale(tempvec_P,size(setJoinedIndexes))
|
||||
tempvec_N=math.vec2.scale(tempvec_N,size(setJoinedIndexes))
|
||||
m_navigation=math.vec2.add(tempvec_P,tempvec_N)
|
||||
uav_moveto(m_navigation.x,m_navigation.y)
|
||||
}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,m_navigation.y)
|
||||
}
|
||||
|
||||
#collision avoidence
|
||||
i=0
|
||||
var turnAngle=0.0
|
||||
var needHide=0
|
||||
while(i<m_neighbourCunt){
|
||||
#if there is a robot within tolerance before, turn 90 degree to hide
|
||||
if(m_MessageRange[i]<ROBOT_SAFETYDIST){
|
||||
turnAngle=m_MessageBearing[i]+math.pi/2.0
|
||||
needHide=1
|
||||
}
|
||||
i=i+1
|
||||
}
|
||||
|
||||
if(needHide==1){
|
||||
m_navigation.x=0.0
|
||||
m_navigation.y=0.0
|
||||
m_navigation=math.vec2.newp(m_sWheelTurningParams.MaxSpeed,turnAngle)
|
||||
uav_moveto(m_navigation.x,m_navigation.y)
|
||||
}
|
||||
|
||||
#jump the first step
|
||||
if(step_cunt<=1){
|
||||
uav_moveto(0.0,0.0)
|
||||
}
|
||||
#set message
|
||||
m_selfMessage.State=m_eState
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
#
|
||||
#Do asking
|
||||
#
|
||||
function DoAsking(){
|
||||
#look for response from predecessor
|
||||
var i=0
|
||||
var psResponse=-1
|
||||
while(i<m_neighbourCunt and psResponse==-1){
|
||||
#the respond robot in joined state
|
||||
#the request lable be the same as requesed
|
||||
#get a respond
|
||||
if(m_MessageState[i]=="STATE_JOINED"){
|
||||
if(m_MessageReqLable[i]==m_nLabel)
|
||||
if(m_MessageResponse[i]!="REQ_NONE"){
|
||||
psResponse=i
|
||||
}}
|
||||
i=i+1
|
||||
}
|
||||
#analyse response
|
||||
if(psResponse==-1){
|
||||
#no response, wait
|
||||
m_unWaitCount=m_unWaitCount-1
|
||||
m_selfMessage.State=m_eState
|
||||
m_selfMessage.ReqLable=m_nLabel
|
||||
m_selfMessage.ReqID=m_unRequestId
|
||||
if(m_unWaitCount==0){
|
||||
TransitionToFree()
|
||||
return
|
||||
}
|
||||
}
|
||||
else{
|
||||
if(m_MessageReqID[psResponse]!=m_unRequestId)
|
||||
TransitionToFree()
|
||||
if(m_MessageReqID[psResponse]==m_unRequestId){
|
||||
if(m_MessageResponse[psResponse]=="REQ_GRANTED"){
|
||||
TransitionToJoining()
|
||||
#TransitionToJoined()
|
||||
return
|
||||
}
|
||||
else{
|
||||
TransitionToAsking(m_nLabel)
|
||||
return
|
||||
}
|
||||
}
|
||||
}
|
||||
uav_moveto(0.0,0.0)
|
||||
}
|
||||
|
||||
#
|
||||
#Do joining
|
||||
#
|
||||
function DoJoining(){
|
||||
#get information of pred
|
||||
var i=0
|
||||
var IDofPred=-1
|
||||
while(i<m_neighbourCunt and IDofPred==-1){
|
||||
if(m_MessageLable[i]==m_vecNodes[m_nLabel].Pred and m_MessageState[i]=="STATE_JOINED")
|
||||
IDofPred=i
|
||||
i=i+1
|
||||
}
|
||||
|
||||
#found pred
|
||||
if(IDofPred!=-1){
|
||||
m_unWaitCount=m_unJoiningLostPeriod#if see pred, reset the timer
|
||||
|
||||
var P2Target=math.vec2.newp(m_vecNodes[m_nLabel].distance,m_vecNodes[m_nLabel].bearing)
|
||||
m_cMeToPred.Range=m_MessageRange[IDofPred]#the poition of self to pred in local coordinate
|
||||
m_cMeToPred.Bearing=m_MessageBearing[IDofPred]
|
||||
|
||||
#attention, m_cMeToPred.GlobalBearing is the bearing of self wrt pred in global coordinate
|
||||
var S2PGlobalBearing=0
|
||||
|
||||
#m_cMeToPred.GlobalBearing=LimitAngle(m_cMeToPred.GlobalBearing)
|
||||
|
||||
if(m_cMeToPred.GlobalBearing>math.pi)
|
||||
S2PGlobalBearing=m_cMeToPred.GlobalBearing-math.pi
|
||||
else
|
||||
S2PGlobalBearing=m_cMeToPred.GlobalBearing+math.pi
|
||||
|
||||
var S2Pred=math.vec2.newp(m_cMeToPred.Range,S2PGlobalBearing)
|
||||
|
||||
#the vector from self to target in global coordinate
|
||||
var S2Target=math.vec2.add(S2Pred,P2Target)
|
||||
#change the vector to local coordinate of self
|
||||
var S2Target_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)
|
||||
uav_moveto(m_navigation.x,m_navigation.y)
|
||||
|
||||
|
||||
#test if is already in desired position
|
||||
if(math.abs(S2Target.x)<m_fTargetDistanceTolerance and math.abs(S2Target.y)<m_fTargetDistanceTolerance){
|
||||
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
|
||||
}
|
||||
|
||||
#avoide collision
|
||||
i=0
|
||||
var turnAngle=0.0
|
||||
var needHide=0
|
||||
while(i<m_neighbourCunt){
|
||||
#if there is a robot within tolerance before, turn 90 degree to hide
|
||||
if(m_MessageRange[i]<ROBOT_SAFETYDIST){
|
||||
turnAngle=m_MessageBearing[i]+math.pi/2.0
|
||||
needHide=1
|
||||
}
|
||||
i=i+1
|
||||
}
|
||||
|
||||
if(needHide==1){
|
||||
m_navigation.x=0.0
|
||||
m_navigation.y=0.0
|
||||
m_navigation=math.vec2.newp(m_sWheelTurningParams.MaxSpeed,turnAngle)
|
||||
uav_moveto(m_navigation.x,m_navigation.y)
|
||||
}
|
||||
if(m_unWaitCount==0){
|
||||
TransitionToFree()
|
||||
return
|
||||
}
|
||||
|
||||
#pack the communication package
|
||||
m_selfMessage.State=m_eState
|
||||
m_selfMessage.Lable=m_nLabel
|
||||
|
||||
}
|
||||
|
||||
#
|
||||
#Do joined
|
||||
#
|
||||
function DoJoined(){
|
||||
m_selfMessage.State=m_eState
|
||||
m_selfMessage.Lable=m_nLabel
|
||||
|
||||
#collect all requests
|
||||
var mapRequests={}
|
||||
var i=0
|
||||
var j=0
|
||||
var ReqLable
|
||||
var JoiningLable
|
||||
var seenPred=0
|
||||
while(i<m_neighbourCunt){
|
||||
if(m_MessageState[i]=="STATE_ASKING"){
|
||||
ReqLable=m_MessageReqLable[i]
|
||||
if(m_vecNodes[ReqLable].State=="UNASSIGNED")
|
||||
if(m_nLabel==m_vecNodes[ReqLable].Pred){
|
||||
#is a request, store the index
|
||||
mapRequests[j]=i
|
||||
j=j+1
|
||||
}
|
||||
}
|
||||
if(m_MessageState[i]=="STATE_JOINING"){
|
||||
JoiningLable=m_MessageLable[i]
|
||||
if(m_nLabel==m_vecNodes[JoiningLable].Pred){
|
||||
##joining wrt this dot,send the global bearing
|
||||
var m_messageForJoining={.Lable=JoiningLable,.GlobalBearing=m_MessageBearing[i]-m_bias}
|
||||
neighbors.broadcast("reply",m_messageForJoining)
|
||||
}
|
||||
}
|
||||
#if it is the pred
|
||||
if(m_MessageState[i]=="STATE_JOINED" and m_MessageLable[i]==m_vecNodes[m_nLabel].Pred){
|
||||
seenPred=1
|
||||
m_unWaitCount=m_unJoiningLostPeriod
|
||||
}
|
||||
i=i+1
|
||||
}
|
||||
|
||||
#get request
|
||||
if(size(mapRequests)!=0){
|
||||
i=1
|
||||
var ReqIndex=0
|
||||
while(i<size(mapRequests)){
|
||||
#compare the distance
|
||||
if(m_MessageRange[mapRequests[ReqIndex]]>m_MessageRange[mapRequests[i]])
|
||||
ReqIndex=i
|
||||
i=i+1
|
||||
}
|
||||
#get the best index, whose Reqlable and Reqid are
|
||||
ReqLable=m_MessageReqLable[mapRequests[ReqIndex]]
|
||||
var ReqID=m_MessageReqID[mapRequests[ReqIndex]]
|
||||
m_selfMessage.ReqLable=ReqLable
|
||||
m_selfMessage.ReqID=ReqID
|
||||
m_selfMessage.Response="REQ_GRANTED"
|
||||
}
|
||||
|
||||
#lost pred, wait for some time and transit to free
|
||||
#if(seenPred==0){
|
||||
#m_unWaitCount=m_unWaitCount-1
|
||||
#if(m_unWaitCount==0){
|
||||
#TransitionToFree()
|
||||
#return
|
||||
#}
|
||||
#}
|
||||
|
||||
|
||||
m_navigation.x=0.0
|
||||
m_navigation.y=0.0
|
||||
uav_moveto(m_navigation.x,m_navigation.y)
|
||||
|
||||
|
||||
#check if should to transists to lock
|
||||
|
||||
|
||||
if(v_tag.size()==ROBOTS){
|
||||
TransitionToLock()
|
||||
}
|
||||
}
|
||||
|
||||
#
|
||||
#Do Lock
|
||||
#
|
||||
function DoLock(){
|
||||
m_selfMessage.State=m_eState
|
||||
m_selfMessage.Lable=m_nLabel
|
||||
|
||||
m_navigation.x=0.0
|
||||
m_navigation.y=0.0
|
||||
|
||||
#collect preds information
|
||||
var i=0
|
||||
var mypred1={.range=0,.bearing=0}
|
||||
var mypred2={.range=0,.bearing=0}
|
||||
|
||||
while(i<m_neighbourCunt){
|
||||
|
||||
#is the first predecessor
|
||||
if(m_MessageLable[i]==m_vecNodes_fixed[m_nLabel].Pred1){
|
||||
mypred1.range=m_MessageRange[i]
|
||||
mypred1.bearing=m_MessageBearing[i]
|
||||
}
|
||||
#is the second predecessor
|
||||
if(m_MessageLable[i]==m_vecNodes_fixed[m_nLabel].Pred2){
|
||||
mypred2.range=m_MessageRange[i]
|
||||
mypred2.bearing=m_MessageBearing[i]
|
||||
}
|
||||
i=i+1
|
||||
}
|
||||
|
||||
#calculate motion vection
|
||||
if(m_nLabel==0){
|
||||
m_navigation.x=0.0
|
||||
m_navigation.y=0.0
|
||||
log(";",m_nLabel,";",0)
|
||||
}
|
||||
|
||||
if(m_nLabel==1){
|
||||
var tempvec={.Range=0.0,.Bearing=0.0}
|
||||
tempvec.Range=FlockInteraction(mypred1.range,m_vecNodes_fixed[m_nLabel].d1,10)
|
||||
#tempvec.Range=mypred1.range-m_vecNodes_fixed[m_nLabel].d1
|
||||
tempvec.Bearing=mypred1.bearing
|
||||
m_navigation=math.vec2.newp(tempvec.Range,tempvec.Bearing)
|
||||
log(";",m_nLabel,";",mypred1.range-m_vecNodes_fixed[m_nLabel].d1)
|
||||
}
|
||||
if(m_nLabel>1){
|
||||
var cDir={.x=0.0,.y=0.0}
|
||||
var cDir1={.x=0.0,.y=0.0}
|
||||
var cDir2={.x=0.0,.y=0.0}
|
||||
cDir1=math.vec2.newp(FlockInteraction(mypred1.range,m_vecNodes_fixed[m_nLabel].d1,EPSILON),mypred1.bearing)
|
||||
cDir2=math.vec2.newp(FlockInteraction(mypred2.range,m_vecNodes_fixed[m_nLabel].d2,EPSILON),mypred2.bearing)
|
||||
#cDir1=math.vec2.newp((mypred1.range-m_vecNodes_fixed[m_nLabel].d1),mypred1.bearing)
|
||||
#cDir2=math.vec2.newp((mypred2.range-m_vecNodes_fixed[m_nLabel].d2),mypred2.bearing)
|
||||
cDir=math.vec2.add(cDir1,cDir2)
|
||||
|
||||
cDir=math.vec2.scale(cDir,100)
|
||||
m_navigation.x=cDir.x
|
||||
m_navigation.y=cDir.y
|
||||
#log(m_nLabel,"error:",mypred1.range-m_vecNodes_fixed[m_nLabel].d1,"and",mypred2.range-m_vecNodes_fixed[m_nLabel].d2)
|
||||
log(";",m_nLabel,";",mypred1.range-m_vecNodes_fixed[m_nLabel].d1)
|
||||
}
|
||||
#move
|
||||
uav_moveto(m_navigation.x,m_navigation.y)
|
||||
}
|
||||
|
||||
#
|
||||
# Executed at init
|
||||
#
|
||||
function init() {
|
||||
#
|
||||
#Adjust parameters here
|
||||
#
|
||||
m_unResponseTimeThreshold=10
|
||||
m_unLabelSearchWaitTime=10
|
||||
m_fTargetDistanceTolerance=15
|
||||
m_unJoiningLostPeriod=100
|
||||
|
||||
#
|
||||
# Join Swarm
|
||||
#
|
||||
s = swarm.create(1)
|
||||
s.join()
|
||||
Reset();
|
||||
statef=idle
|
||||
}
|
||||
|
||||
#
|
||||
# Executed every step
|
||||
#
|
||||
function step(){
|
||||
uavcmd()
|
||||
#update the graph
|
||||
UpdateNodeInfo()
|
||||
#reset message package to be sent
|
||||
m_selfMessage={.State="STATE_FREE",.Lable=0,.ReqLable=0,.ReqID=0,.Side=0,.Response="REQ_NONE"}
|
||||
#
|
||||
#act according to current state
|
||||
#
|
||||
if(UAVSTATE=="IDLE"){
|
||||
if(m_eState=="STATE_FREE")
|
||||
DoFree()
|
||||
else if(m_eState=="STATE_ESCAPE")
|
||||
DoEscape()
|
||||
else if(m_eState=="STATE_ASKING")
|
||||
DoAsking()
|
||||
else if(m_eState=="STATE_JOINING")
|
||||
DoJoining()
|
||||
else if(m_eState=="STATE_JOINED")
|
||||
DoJoined()
|
||||
else if(m_eState=="STATE_LOCK")
|
||||
DoLock()
|
||||
}
|
||||
|
||||
statef()
|
||||
|
||||
|
||||
debug(m_eState,m_nLabel)
|
||||
log("Current state: ", UAVSTATE)
|
||||
log("Swarm size: ", ROBOTS)
|
||||
#navigation
|
||||
|
||||
#broadcast messag
|
||||
neighbors.broadcast("msg",m_selfMessage)
|
||||
|
||||
#
|
||||
#clean message storage
|
||||
m_MessageState={}#store received neighbour message
|
||||
m_MessageLable={}#store received neighbour message
|
||||
m_MessageReqLable={}#store received neighbour message
|
||||
m_MessageReqID={}#store received neighbour message
|
||||
m_MessageSide={}#store received neighbour message
|
||||
m_MessageResponse={}#store received neighbour message
|
||||
m_MessageRange={}#store received neighbour message
|
||||
m_MessageBearing={}#store received neighbour message
|
||||
m_neighbourCunt=0
|
||||
|
||||
|
||||
#step cunt+1
|
||||
step_cunt=step_cunt+1
|
||||
}
|
||||
|
||||
#
|
||||
# Executed when reset
|
||||
#
|
||||
function Reset(){
|
||||
m_vecNodes={}
|
||||
m_vecNodes = parse_graph("/home/dave/ROS_WS/src/rosbuzz/script/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_nLabel=-1
|
||||
|
||||
#start listening
|
||||
start_listen()
|
||||
#
|
||||
#set initial state, only one robot choose [A], while the rest choose [B]
|
||||
#
|
||||
#[A]The robot used to triger the formation process is defined as joined,
|
||||
if(id==0){
|
||||
m_nLabel=0
|
||||
TransitionToJoined()
|
||||
}
|
||||
#[B]Other robots are defined as free.
|
||||
else{
|
||||
TransitionToFree()
|
||||
}
|
||||
}
|
||||
|
||||
#
|
||||
# Executed upon destroy
|
||||
#
|
||||
function destroy() {
|
||||
#clear neighbour message
|
||||
uav_moveto(0.0,0.0)
|
||||
m_vecNodes={}
|
||||
#stop listening
|
||||
neighbors.ignore("msg")
|
||||
}
|
||||
|
|
@ -0,0 +1,47 @@
|
|||
########################################
|
||||
#
|
||||
# BARRIER-RELATED FUNCTIONS
|
||||
#
|
||||
########################################
|
||||
|
||||
#
|
||||
# Constants
|
||||
#
|
||||
BARRIER_VSTIG = 11
|
||||
|
||||
#
|
||||
# Sets a barrier
|
||||
#
|
||||
function barrier_set(threshold, transf, resumef) {
|
||||
statef = function() {
|
||||
barrier_wait(threshold, transf, resumef);
|
||||
}
|
||||
barrier = stigmergy.create(BARRIER_VSTIG)
|
||||
}
|
||||
|
||||
#
|
||||
# Make yourself ready
|
||||
#
|
||||
function barrier_ready() {
|
||||
barrier.put(id, 1)
|
||||
}
|
||||
|
||||
#
|
||||
# Executes the barrier
|
||||
#
|
||||
BARRIER_TIMEOUT = 200
|
||||
timeW=0
|
||||
function barrier_wait(threshold, transf, resumef) {
|
||||
barrier.get(id)
|
||||
barrier.put(id, 1)
|
||||
UAVSTATE = "BARRIERWAIT"
|
||||
if(barrier.size() >= threshold) {
|
||||
#barrier = nil
|
||||
transf()
|
||||
} else if(timeW>=BARRIER_TIMEOUT) {
|
||||
barrier = nil
|
||||
resumef()
|
||||
timeW=0
|
||||
}
|
||||
timeW = timeW+1
|
||||
}
|
||||
|
|
@ -0,0 +1,91 @@
|
|||
########################################
|
||||
#
|
||||
# FLIGHT-RELATED FUNCTIONS
|
||||
#
|
||||
########################################
|
||||
TARGET_ALTITUDE = 5.0
|
||||
UAVSTATE = "TURNEDOFF"
|
||||
|
||||
function idle() {
|
||||
statef=idle
|
||||
UAVSTATE = "IDLE"
|
||||
}
|
||||
|
||||
function takeoff() {
|
||||
UAVSTATE = "TAKEOFF"
|
||||
statef=takeoff
|
||||
#log("TakeOff: ", flight.status)
|
||||
#log("Relative position: ", position.altitude)
|
||||
|
||||
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
|
||||
barrier_set(ROBOTS,idle,land)
|
||||
barrier_ready()
|
||||
#statef=hexagon
|
||||
}
|
||||
else {
|
||||
log("Altitude: ", TARGET_ALTITUDE)
|
||||
neighbors.broadcast("cmd", 22)
|
||||
uav_takeoff(TARGET_ALTITUDE)
|
||||
}
|
||||
}
|
||||
|
||||
function land() {
|
||||
UAVSTATE = "LAND"
|
||||
statef=land
|
||||
#log("Land: ", flight.status)
|
||||
if(flight.status == 2 or flight.status == 3){
|
||||
neighbors.broadcast("cmd", 21)
|
||||
uav_land()
|
||||
}
|
||||
else {
|
||||
barrier_set(ROBOTS,idle,land)
|
||||
barrier_ready()
|
||||
timeW=0
|
||||
#barrier = nil
|
||||
#statef=idle
|
||||
}
|
||||
}
|
||||
|
||||
function uavcmd() {
|
||||
if(flight.rc_cmd==22) {
|
||||
log("cmd 22")
|
||||
flight.rc_cmd=0
|
||||
statef = takeoff
|
||||
UAVSTATE = "TAKEOFF"
|
||||
neighbors.broadcast("cmd", 22)
|
||||
} else if(flight.rc_cmd==21) {
|
||||
log("cmd 21")
|
||||
log("To land")
|
||||
flight.rc_cmd=0
|
||||
statef = land
|
||||
UAVSTATE = "LAND"
|
||||
neighbors.broadcast("cmd", 21)
|
||||
} else if(flight.rc_cmd==16) {
|
||||
flight.rc_cmd=0
|
||||
statef = idle
|
||||
#uav_goto()
|
||||
add_user_rb(10,rc_goto.latitude,rc_goto.longitude)
|
||||
} else if(flight.rc_cmd==400) {
|
||||
flight.rc_cmd=0
|
||||
uav_arm()
|
||||
neighbors.broadcast("cmd", 400)
|
||||
} else if (flight.rc_cmd==401){
|
||||
flight.rc_cmd=0
|
||||
uav_disarm()
|
||||
neighbors.broadcast("cmd", 401)
|
||||
}
|
||||
|
||||
neighbors.listen("cmd",
|
||||
function(vid, value, rid) {
|
||||
print("Got (", vid, ",", value, ") from robot #", rid)
|
||||
if(value==22 and UAVSTATE!="TAKEOFF") {
|
||||
statef=takeoff
|
||||
} else if(value==21) {
|
||||
statef=land
|
||||
} else if(value==400 and UAVSTATE=="IDLE") {
|
||||
uav_arm()
|
||||
} else if(value==401 and UAVSTATE=="IDLE"){
|
||||
uav_disarm()
|
||||
}
|
||||
})
|
||||
}
|
||||
|
|
@ -0,0 +1,9 @@
|
|||
####################################################################################################
|
||||
# Updater related
|
||||
# This should be here for the updater to work, changing position of code will crash the updater
|
||||
####################################################################################################
|
||||
updated="update_ack"
|
||||
update_no=0
|
||||
function updated_neigh(){
|
||||
neighbors.broadcast(updated, update_no)
|
||||
}
|
||||
|
|
@ -257,13 +257,13 @@ neighbors.listen("cmd",
|
|||
# Read a value from the structure
|
||||
# log(users)
|
||||
#users_print(users.dataG)
|
||||
if(size(users.dataG)>0)
|
||||
vt.put("p", users.dataG)
|
||||
# if(size(users.dataG)>0)
|
||||
# vt.put("p", users.dataG)
|
||||
|
||||
# Get the number of keys in the structure
|
||||
#log("The vstig has ", vt.size(), " elements")
|
||||
users_save(vt.get("p"))
|
||||
table_print(users.dataL)
|
||||
# users_save(vt.get("p"))
|
||||
# table_print(users.dataL)
|
||||
}
|
||||
|
||||
# Executed once when the robot (or the simulator) is reset.
|
||||
|
|
|
|||
|
|
@ -29,7 +29,7 @@ static int updated=0;
|
|||
|
||||
/*Initialize updater*/
|
||||
void init_update_monitor(const char* bo_filename, const char* stand_by_script){
|
||||
ROS_INFO("intiialized file monitor.\n");
|
||||
ROS_INFO("Initializing file monitor...");
|
||||
fd=inotify_init1(IN_NONBLOCK);
|
||||
if ( fd < 0 ) {
|
||||
perror( "inotify_init error" );
|
||||
|
|
|
|||
|
|
@ -305,6 +305,9 @@ namespace buzz_utility{
|
|||
buzzvm_gstore(VM);
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "log", 1));
|
||||
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
|
||||
buzzvm_gstore(VM);
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "debug", 1));
|
||||
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
|
||||
buzzvm_gstore(VM);
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1));
|
||||
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_moveto));
|
||||
|
|
@ -344,6 +347,9 @@ namespace buzz_utility{
|
|||
buzzvm_gstore(VM);
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "log", 1));
|
||||
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
|
||||
buzzvm_gstore(VM);
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "debug", 1));
|
||||
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
|
||||
buzzvm_gstore(VM);
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1));
|
||||
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
|
||||
|
|
@ -446,7 +452,7 @@ int create_stig_tables() {
|
|||
/****************************************/
|
||||
int buzz_script_set(const char* bo_filename,
|
||||
const char* bdbg_filename, int robot_id) {
|
||||
ROS_INFO(" Robot ID: " , robot_id);
|
||||
ROS_INFO(" Robot ID: %i" , robot_id);
|
||||
/* Reset the Buzz VM */
|
||||
if(VM) buzzvm_destroy(&VM);
|
||||
Robot_id = robot_id;
|
||||
|
|
@ -493,7 +499,7 @@ 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);
|
||||
|
|
@ -501,7 +507,7 @@ int create_stig_tables() {
|
|||
//cout << "ERROR!!!! ---------- " << VM->errormsg << endl;
|
||||
//cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl;
|
||||
return 0;
|
||||
}
|
||||
}*/
|
||||
|
||||
/* Save bytecode file name */
|
||||
BO_FNAME = strdup(bo_filename);
|
||||
|
|
@ -555,7 +561,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);
|
||||
|
|
@ -563,7 +569,7 @@ int create_stig_tables() {
|
|||
//cout << "ERROR!!!! ---------- " << VM->errormsg << endl;
|
||||
//cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl;
|
||||
return 0;
|
||||
}
|
||||
}*/
|
||||
|
||||
// Execute the global part of the script
|
||||
if(buzzvm_execute_script(VM)!= BUZZVM_STATE_DONE){
|
||||
|
|
@ -611,7 +617,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);
|
||||
|
|
@ -619,7 +625,7 @@ int create_stig_tables() {
|
|||
//cout << "ERROR!!!! ---------- " << VM->errormsg << endl;
|
||||
//cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl;
|
||||
return 0;
|
||||
}
|
||||
}*/
|
||||
// Execute the global part of the script
|
||||
if(buzzvm_execute_script(VM)!= BUZZVM_STATE_DONE){
|
||||
ROS_ERROR("Error executing global part, VM state : %i",VM->state);
|
||||
|
|
@ -687,7 +693,7 @@ int create_stig_tables() {
|
|||
buzzuav_closures::buzzuav_update_prox(VM);
|
||||
buzzuav_closures::buzzuav_update_currentpos(VM);
|
||||
buzzuav_closures::update_neighbors(VM);
|
||||
update_users();
|
||||
//update_users();
|
||||
buzzuav_closures::buzzuav_update_flight_status(VM);
|
||||
}
|
||||
|
||||
|
|
@ -698,7 +704,7 @@ int create_stig_tables() {
|
|||
update_sensors();
|
||||
/* Call Buzz step() function */
|
||||
if(buzzvm_function_call(VM, "step", 0) != BUZZVM_STATE_READY) {
|
||||
fprintf(stderr, "%s: execution terminated abnormally: %s\n\n",
|
||||
ROS_ERROR("%s: execution terminated abnormally: %s",
|
||||
BO_FNAME,
|
||||
buzz_error_info());
|
||||
buzzvm_dump(VM);
|
||||
|
|
@ -722,7 +728,7 @@ int create_stig_tables() {
|
|||
void buzz_script_destroy() {
|
||||
if(VM) {
|
||||
if(VM->state != BUZZVM_STATE_READY) {
|
||||
fprintf(stderr, "%s: execution terminated abnormally: %s\n\n",
|
||||
ROS_ERROR("%s: execution terminated abnormally: %s",
|
||||
BO_FNAME,
|
||||
buzz_error_info());
|
||||
buzzvm_dump(VM);
|
||||
|
|
@ -732,7 +738,7 @@ int create_stig_tables() {
|
|||
free(BO_FNAME);
|
||||
buzzdebug_destroy(&DBG_INFO);
|
||||
}
|
||||
fprintf(stdout, "Script execution stopped.\n");
|
||||
ROS_INFO("Script execution stopped.");
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -754,7 +760,7 @@ int create_stig_tables() {
|
|||
buzzuav_closures::buzzuav_update_prox(VM);
|
||||
buzzuav_closures::buzzuav_update_currentpos(VM);
|
||||
buzzuav_closures::update_neighbors(VM);
|
||||
update_users();
|
||||
//update_users();
|
||||
buzzuav_closures::buzzuav_update_flight_status(VM);
|
||||
//set_robot_var(buzzdict_size(VM->swarmmembers)+1);
|
||||
|
||||
|
|
|
|||
|
|
@ -86,12 +86,13 @@ namespace rosbzz_node{
|
|||
|
||||
/* Set the Buzz bytecode */
|
||||
if(buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(),robot_id)) {
|
||||
fprintf(stdout, "Bytecode file found and set\n");
|
||||
ROS_INFO("[%i] Bytecode file found and set", robot_id);
|
||||
std::string standby_bo = Compile_bzz(stand_by) + ".bo";
|
||||
init_update_monitor(bcfname.c_str(),standby_bo.c_str());
|
||||
///////////////////////////////////////////////////////
|
||||
// MAIN LOOP
|
||||
//////////////////////////////////////////////////////
|
||||
ROS_INFO("[%i] STARTING MAIN LOOP!", robot_id);
|
||||
while (ros::ok() && !buzz_utility::buzz_script_done())
|
||||
{
|
||||
/*Update neighbors position inside Buzz*/
|
||||
|
|
|
|||
Loading…
Reference in New Issue