From af301a1c62bae52e6a401ed46f3c471f4bc3353a Mon Sep 17 00:00:00 2001 From: dave Date: Fri, 7 Jul 2017 00:04:40 -0400 Subject: [PATCH] enhance graph script --- buzz_scripts/graphform.bzz | 228 ++++--- buzz_scripts/graphform.bzz_old | 826 +++++++++++++++++++++++++ buzz_scripts/include/Graph_drone.graph | 10 +- buzz_scripts/include/Graph_fixed.graph | 8 +- buzz_scripts/include/shapes.bzz | 111 ++++ 5 files changed, 1055 insertions(+), 128 deletions(-) create mode 100644 buzz_scripts/graphform.bzz_old create mode 100644 buzz_scripts/include/shapes.bzz diff --git a/buzz_scripts/graphform.bzz b/buzz_scripts/graphform.bzz index 103c31b..dff7dd0 100644 --- a/buzz_scripts/graphform.bzz +++ b/buzz_scripts/graphform.bzz @@ -7,6 +7,8 @@ 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 @@ -22,18 +24,18 @@ 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} +#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),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"} +#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" @@ -47,19 +49,12 @@ m_navigation={.x=0,.y=0} #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 @@ -87,8 +82,9 @@ step_cunt=0 #virtual stigmergy v_tag = stigmergy.create(1) -# Lennard-Jones parameters -EPSILON = 13.5 #3.5 +# 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 @@ -130,6 +126,64 @@ function count(table,value){ 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 # @@ -158,90 +212,25 @@ function pow(base,exponent){ } } -# -# 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", +neighbors.listen("m", function(vid,value,rid){ #store the received message var temp_id=rid - m_receivedMessage.State=value.State + m_receivedMessage.State=i2s(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 + m_receivedMessage.Response=i2r(value.Response) Get_DisAndAzi(temp_id) #add the received message # - m_MessageState[m_neighbourCunt]=value.State + 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_MessageSide[m_neighbourCunt]=value.Side - m_MessageResponse[m_neighbourCunt]=value.Response + 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 @@ -296,7 +285,7 @@ function UpdateNodeInfo(){ function TransitionToFree(){ m_eState="STATE_FREE" m_unWaitCount=m_unLabelSearchWaitTime - m_selfMessage.State=m_eState + m_selfMessage.State=s2i(m_eState) } # @@ -306,7 +295,7 @@ 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.State=s2i(m_eState) m_selfMessage.ReqLable=m_nLabel m_selfMessage.ReqID=m_unRequestId @@ -318,15 +307,15 @@ function TransitionToAsking(un_label){ # function TransitionToJoining(){ m_eState="STATE_JOINING" - m_selfMessage.State=m_eState + m_selfMessage.State=s2i(m_eState) m_selfMessage.Lable=m_nLabel m_unWaitCount=m_unJoiningLostPeriod - neighbors.listen("reply", + neighbors.listen("r", function(vid,value,rid){ #store the received message - if(value.Lable==m_nLabel){ - m_cMeToPred.GlobalBearing=value.GlobalBearing + if(value.Label==m_nLabel){ + m_cMeToPred.GlobalBearing=value.Bearing } }) @@ -338,10 +327,10 @@ function TransitionToJoining(){ # function TransitionToJoined(){ m_eState="STATE_JOINED" - m_selfMessage.State=m_eState + m_selfMessage.State=s2i(m_eState) m_selfMessage.Lable=m_nLabel m_vecNodes[m_nLabel].State="ASSIGNED" - neighbors.ignore("reply") + neighbors.ignore("r") #write statues v_tag.put(m_nLabel, 1) @@ -356,7 +345,7 @@ function TransitionToJoined(){ # function TransitionToLock(){ m_eState="STATE_LOCK" - m_selfMessage.State=m_eState + m_selfMessage.State=s2i(m_eState) m_selfMessage.Lable=m_nLabel m_vecNodes[m_nLabel].State="ASSIGNED" @@ -369,7 +358,8 @@ function TransitionToLock(){ # Do free # function DoFree() { - m_selfMessage.State=m_eState + m_selfMessage.State=s2i(m_eState) + #wait for a while before looking for a lable if(m_unWaitCount>0) m_unWaitCount=m_unWaitCount-1 @@ -442,7 +432,8 @@ function DoFree() { uav_moveto(0.0,0.0) } #set message - m_selfMessage.State=m_eState + m_selfMessage.State=s2i(m_eState) + } @@ -470,7 +461,7 @@ function DoAsking(){ if(psResponse==-1){ #no response, wait m_unWaitCount=m_unWaitCount-1 - m_selfMessage.State=m_eState + m_selfMessage.State=s2i(m_eState) m_selfMessage.ReqLable=m_nLabel m_selfMessage.ReqID=m_unRequestId if(m_unWaitCount==0){ @@ -520,7 +511,7 @@ function DoJoining(){ #attention, m_cMeToPred.GlobalBearing is the bearing of self wrt pred in global coordinate var S2PGlobalBearing=0 - #m_cMeToPred.GlobalBearing=LimitAngle(m_cMeToPred.GlobalBearing) + m_cMeToPred.GlobalBearing=LimitAngle(m_cMeToPred.GlobalBearing) if(m_cMeToPred.GlobalBearing>math.pi) S2PGlobalBearing=m_cMeToPred.GlobalBearing-math.pi @@ -534,7 +525,7 @@ function DoJoining(){ #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 + 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) @@ -557,7 +548,7 @@ function DoJoining(){ } #pack the communication package - m_selfMessage.State=m_eState + m_selfMessage.State=s2i(m_eState) m_selfMessage.Lable=m_nLabel } @@ -566,7 +557,7 @@ function DoJoining(){ #Do joined # function DoJoined(){ - m_selfMessage.State=m_eState + m_selfMessage.State=s2i(m_eState) m_selfMessage.Lable=m_nLabel #collect all requests @@ -590,8 +581,8 @@ function DoJoined(){ 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) + var m_messageForJoining={.Label=JoiningLable,.Bearing=m_MessageBearing[i]-m_bias} + neighbors.broadcast("r",m_messageForJoining) } } #if it is the pred @@ -617,7 +608,7 @@ function DoJoined(){ var ReqID=m_MessageReqID[mapRequests[ReqIndex]] m_selfMessage.ReqLable=ReqLable m_selfMessage.ReqID=ReqID - m_selfMessage.Response="REQ_GRANTED" + m_selfMessage.Response=r2i("REQ_GRANTED") } #lost pred, wait for some time and transit to free @@ -638,16 +629,16 @@ function DoJoined(){ #check if should to transists to lock - if(v_tag.size()==ROBOTS){ - TransitionToLock() - } +# if(v_tag.size()==ROBOTS){ +# TransitionToLock() +# } } # #Do Lock # function DoLock(){ -m_selfMessage.State=m_eState +m_selfMessage.State=s2i(m_eState) m_selfMessage.Lable=m_nLabel m_navigation.x=0.0 @@ -675,14 +666,14 @@ i=i+1 #calculate motion vection if(m_nLabel==0){ - m_navigation.x=0.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,10) + 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) @@ -698,14 +689,14 @@ if(m_nLabel>1){ #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) + 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,m_navigation.y) +uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) } function action(){ @@ -741,7 +732,7 @@ function step(){ #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"} + m_selfMessage={.State=s2i("STATE_FREE"),.Lable=0,.ReqLable=0,.ReqID=0,.Response=r2i("REQ_NONE")} # #act according to current state # @@ -769,7 +760,7 @@ function step(){ #navigation #broadcast messag - neighbors.broadcast("msg",m_selfMessage) + neighbors.broadcast("m",m_selfMessage) # #clean message storage @@ -777,7 +768,6 @@ function step(){ 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 @@ -792,10 +782,10 @@ function step(){ # Executed when reset # function Reset(){ - m_vecNodes={} - m_vecNodes = parse_graph("/home/ubuntu/ROS_WS/src/rosbuzz/buzz_scripts/include/Graph_drone.graph")#change the .graph file when necessary + #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/ubuntu/ROS_WS/src/rosbuzz/buzz_scripts/include/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 @@ -822,5 +812,5 @@ function destroy() { uav_moveto(0.0,0.0) m_vecNodes={} #stop listening - neighbors.ignore("msg") -} + neighbors.ignore("m") +} \ No newline at end of file diff --git a/buzz_scripts/graphform.bzz_old b/buzz_scripts/graphform.bzz_old new file mode 100644 index 0000000..937d69c --- /dev/null +++ b/buzz_scripts/graphform.bzz_old @@ -0,0 +1,826 @@ +# +# Include files +# +include "string.bzz" +include "vec2.bzz" +include "update.bzz" +include "barrier.bzz" # don't use a stigmergy id=11 with this header. +include "uavstates.bzz" # require an 'action' function to be defined here. + +ROBOT_RADIUS=50 +ROBOT_DIAMETER=2.0*ROBOT_RADIUS +ROBOT_SAFETYDIST=2.0*ROBOT_DIAMETER + +# +# Global variables +# + +# +#Save message from all neighours +#the indexes are as 1,2,3..., while each value is a table that store the information of a neighbour robot +m_MessageState={}#store received neighbour message +m_MessageLable={}#store received neighbour message +m_MessageReqLable={}#store received neighbour message +m_MessageReqID={}#store received neighbour message +m_MessageSide={}#store received neighbour message +m_MessageResponse={}#store received neighbour message +m_MessageRange={}#store received neighbour message +m_MessageBearing={}#store received neighbour message +m_neighbourCunt=0#used to cunt neighbours +#Save message from one neighbour +#the indexes are as State(received state),Lable(received lable),ReqLable,ReqID,Side,Response,Range,Bearing +m_receivedMessage={.State="STATE_FREE",.Lable=0,.ReqLable=0,.ReqID=0,.Side=0,.Response="REQ_NONE",.Range=0,.Bearing=0} +# +#Save the message to send +#The keys of the talbe is State(current state),Lable(current lable),ReqLable(requested lable),ReqID(request id),Side(side),Response(reply message{REQ_NONE,REQ_GRANTED,REQ_RESEND}) +m_selfMessage={.State="STATE_FREE",.Lable=0,.ReqLable=0,.ReqID=0,.Side=0,.Response="REQ_NONE"} + +#Current robot state +m_eState="STATE_FREE" + +#navigation vector +m_navigation={.x=0,.y=0} + +#Debug message to be displayed in qt-opengl +#m_ossDebugMsg + +#Debug vector to draw +#CVector2 m_cDebugVector + +#Table of the nodes in the graph +m_vecNodes={} +m_vecNodes_fixed={} + +#Current label being requested or chosen (-1 when none) +m_nLabel=-1 + +#Label request id +m_unRequestId=0 + +#Side +m_unbSide=0 + +#Global bias, used to map local coordinate to global coordinate +m_bias=0 + +#Vector to predecessor,range is the distance between robots, bearing is the angle of pred wrt self in local coordinate of self, globalbearing is the angle of self wrt pred in global coordinate +m_cMeToPred={.Range=0.0,.Bearing=0.0,.GlobalBearing=0.0} + +#Counter to wait for something to happen +m_unWaitCount=0 + +#Number of steps to wait before looking for a free label +m_unLabelSearchWaitTime=0 + +#Number of steps to wait for an answer to be received +m_unResponseTimeThreshold=0 + +#Number of steps to wait until giving up joining +m_unJoiningLostPeriod=0 + +#Tolerance distance to a target location +m_fTargetDistanceTolerance=0 + +#step cunt +step_cunt=0 + +#virtual stigmergy +v_tag = stigmergy.create(1) + +# Lennard-Jones parameters +EPSILON = 13.5 #3.5 + +# Lennard-Jones interaction magnitude + +function FlockInteraction(dist,target,epsilon){ + var mag = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) + return mag +} + +function LimitAngle(angle){ + if(angle>2*math.pi) + return angle-2*math.pi + else if (angle<0) + return angle+2*math.pi + else + return angle +} + +# +# Calculates the angle of the given vector2. +# PARAM v: The vector2. +# RETURN: The angle of the vector. +# +Angle = function(v) { + return math.atan(v.y, v.x) +} + +# +#return the number of value in table +# +function count(table,value){ + var number=0 + var i=0 + while(i0) and (m_vecNodes[i].State=="ASSIGNING")){ + m_vecNodes[i].StateAge=m_vecNodes[i].StateAge-1 + if(m_vecNodes[i].StateAge==0) + m_vecNodes[i].State="UNASSIGNED" + } + i=i+1 + } +} + +# +#Transistion to state free +# +function TransitionToFree(){ + m_eState="STATE_FREE" + m_unWaitCount=m_unLabelSearchWaitTime + m_selfMessage.State=m_eState +} + +# +#Transistion to state asking +# +function TransitionToAsking(un_label){ + m_eState="STATE_ASKING" + m_nLabel=un_label + m_unRequestId=rng.uniform(0,65536)+id#don't know why the random numbers are the same, add id to make the ReqID different + m_selfMessage.State=m_eState + m_selfMessage.ReqLable=m_nLabel + m_selfMessage.ReqID=m_unRequestId + + m_unWaitCount=m_unResponseTimeThreshold +} + +# +#Transistion to state joining +# +function TransitionToJoining(){ + m_eState="STATE_JOINING" + m_selfMessage.State=m_eState + m_selfMessage.Lable=m_nLabel + m_unWaitCount=m_unJoiningLostPeriod + + neighbors.listen("reply", + function(vid,value,rid){ + #store the received message + if(value.Lable==m_nLabel){ + m_cMeToPred.GlobalBearing=value.GlobalBearing + + } + }) + +} + +# +#Transistion to state joined +# +function TransitionToJoined(){ + m_eState="STATE_JOINED" + m_selfMessage.State=m_eState + m_selfMessage.Lable=m_nLabel + m_vecNodes[m_nLabel].State="ASSIGNED" + neighbors.ignore("reply") + + #write statues + v_tag.put(m_nLabel, 1) + + m_navigation.x=0.0 + m_navigation.y=0.0 + uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) +} + +# +#Transistion to state Lock, lock the current formation +# +function TransitionToLock(){ + m_eState="STATE_LOCK" + m_selfMessage.State=m_eState + m_selfMessage.Lable=m_nLabel + m_vecNodes[m_nLabel].State="ASSIGNED" + + m_navigation.x=0.0 + m_navigation.y=0.0 + uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) +} + +# +# Do free +# +function DoFree() { + m_selfMessage.State=m_eState + #wait for a while before looking for a lable + if(m_unWaitCount>0) + m_unWaitCount=m_unWaitCount-1 + + #find a set of joined robots + var setJoinedLables={} + var setJoinedIndexes={} + var i=0 + var j=0 + while(i0){ + 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(imath.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_MessageRange[mapRequests[i]]) + ReqIndex=i + i=i+1 + } + #get the best index, whose Reqlable and Reqid are + ReqLable=m_MessageReqLable[mapRequests[ReqIndex]] + var ReqID=m_MessageReqID[mapRequests[ReqIndex]] + m_selfMessage.ReqLable=ReqLable + m_selfMessage.ReqID=ReqID + m_selfMessage.Response="REQ_GRANTED" + } + + #lost pred, wait for some time and transit to free + #if(seenPred==0){ + #m_unWaitCount=m_unWaitCount-1 + #if(m_unWaitCount==0){ + #TransitionToFree() + #return + #} + #} + + + m_navigation.x=0.0 + m_navigation.y=0.0 + uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) + + + #check if should to transists to lock + + + if(v_tag.size()==ROBOTS){ + TransitionToLock() + } +} + +# +#Do Lock +# +function DoLock(){ +m_selfMessage.State=m_eState +m_selfMessage.Lable=m_nLabel + +m_navigation.x=0.0 +m_navigation.y=0.0 + +#collect preds information +var i=0 +var mypred1={.range=0,.bearing=0} +var mypred2={.range=0,.bearing=0} + +while(i1){ + var cDir={.x=0.0,.y=0.0} + var cDir1={.x=0.0,.y=0.0} + var cDir2={.x=0.0,.y=0.0} + cDir1=math.vec2.newp(FlockInteraction(mypred1.range,m_vecNodes_fixed[m_nLabel].d1,EPSILON),mypred1.bearing) + cDir2=math.vec2.newp(FlockInteraction(mypred2.range,m_vecNodes_fixed[m_nLabel].d2,EPSILON),mypred2.bearing) + #cDir1=math.vec2.newp((mypred1.range-m_vecNodes_fixed[m_nLabel].d1),mypred1.bearing) + #cDir2=math.vec2.newp((mypred2.range-m_vecNodes_fixed[m_nLabel].d2),mypred2.bearing) + cDir=math.vec2.add(cDir1,cDir2) + + cDir=math.vec2.scale(cDir,100) + m_navigation.x=cDir.x + m_navigation.y=cDir.y + #log(m_nLabel,"error:",mypred1.range-m_vecNodes_fixed[m_nLabel].d1,"and",mypred2.range-m_vecNodes_fixed[m_nLabel].d2) + log(";",m_nLabel,";",mypred1.range-m_vecNodes_fixed[m_nLabel].d1) +} +#move +uav_moveto(m_navigation.x,m_navigation.y) +} + +function action(){ + statef=action + UAVSTATE="GRAPH" +} + +# +# Executed at init +# +function init() { + # + #Adjust parameters here + # + m_unResponseTimeThreshold=10 + m_unLabelSearchWaitTime=10 + m_fTargetDistanceTolerance=10 + m_unJoiningLostPeriod=100 + + # + # Join Swarm + # + uav_initswarm() + Reset(); +} + +# +# Executed every step +# +function step(){ + uav_rccmd() + uav_neicmd() + #update the graph + UpdateNodeInfo() + #reset message package to be sent + m_selfMessage={.State="STATE_FREE",.Lable=0,.ReqLable=0,.ReqID=0,.Side=0,.Response="REQ_NONE"} + # + #act according to current state + # + if(UAVSTATE=="GRAPH"){ + if(m_eState=="STATE_FREE") + DoFree() + else if(m_eState=="STATE_ESCAPE") + DoEscape() + else if(m_eState=="STATE_ASKING") + DoAsking() + else if(m_eState=="STATE_JOINING") + DoJoining() + else if(m_eState=="STATE_JOINED") + DoJoined() + else if(m_eState=="STATE_LOCK") + DoLock() + } + + statef() + + + debug(m_eState,m_nLabel) + log("Current state: ", UAVSTATE) + log("Swarm size: ", ROBOTS) + #navigation + + #broadcast messag + neighbors.broadcast("msg",m_selfMessage) + + # + #clean message storage + m_MessageState={}#store received neighbour message + m_MessageLable={}#store received neighbour message + m_MessageReqLable={}#store received neighbour message + m_MessageReqID={}#store received neighbour message + m_MessageSide={}#store received neighbour message + m_MessageResponse={}#store received neighbour message + m_MessageRange={}#store received neighbour message + m_MessageBearing={}#store received neighbour message + m_neighbourCunt=0 + + + #step cunt+1 + step_cunt=step_cunt+1 +} + +# +# Executed when reset +# +function Reset(){ + m_vecNodes={} + m_vecNodes = parse_graph("/home/dave/ROS_WS/src/rosbuzz/buzz_scripts/include/Graph_drone.graph")#change the .graph file when necessary + m_vecNodes_fixed={} + m_vecNodes_fixed=parse_graph_fixed("/home/dave/ROS_WS/src/rosbuzz/buzz_scripts/include/Graph_fixed.graph") + m_nLabel=-1 + + #start listening + start_listen() + # + #set initial state, only one robot choose [A], while the rest choose [B] + # + #[A]The robot used to triger the formation process is defined as joined, + if(id==0){ + m_nLabel=0 + TransitionToJoined() + } + #[B]Other robots are defined as free. + else{ + TransitionToFree() + } +} + +# +# Executed upon destroy +# +function destroy() { + #clear neighbour message + uav_moveto(0.0,0.0) + m_vecNodes={} + #stop listening + neighbors.ignore("msg") +} diff --git a/buzz_scripts/include/Graph_drone.graph b/buzz_scripts/include/Graph_drone.graph index f809476..edd6764 100644 --- a/buzz_scripts/include/Graph_drone.graph +++ b/buzz_scripts/include/Graph_drone.graph @@ -1,5 +1,5 @@ -0 -1 -1 -1 -1 0 1000.0 0.0 -2 0 1000.0 1.57 -3 0 1000.0 3.14 -4 0 1000.0 4.71 +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 diff --git a/buzz_scripts/include/Graph_fixed.graph b/buzz_scripts/include/Graph_fixed.graph index 6935e27..c0d6de7 100644 --- a/buzz_scripts/include/Graph_fixed.graph +++ b/buzz_scripts/include/Graph_fixed.graph @@ -1,5 +1,5 @@ 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 \ No newline at end of file +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 \ No newline at end of file diff --git a/buzz_scripts/include/shapes.bzz b/buzz_scripts/include/shapes.bzz new file mode 100644 index 0000000..53cdd3b --- /dev/null +++ b/buzz_scripts/include/shapes.bzz @@ -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 +} \ No newline at end of file