removed state transitions
This commit is contained in:
dave
parent
d055d8939f
commit
602d744140
|
|
@ -50,19 +50,22 @@ function barrier_ready(bc) {
|
|||
# Executes the barrier
|
||||
#
|
||||
function barrier_wait(threshold, transf, resumef, bc) {
|
||||
if(barrier==nil) #failsafe
|
||||
barrier_create()
|
||||
|
||||
barrier.put(id, bc)
|
||||
barrier.get(id)
|
||||
var allgood = 0
|
||||
log("--->BS: ", barrier.size(), " / ", threshold, " (", BARRIER_VSTIG, " - ", barrier.get("d"), ") t= ", timeW)
|
||||
if(barrier.size() - 1 >= threshold or barrier.get("d") == 1) {
|
||||
allgood = barrier_allgood(barrier,bc)
|
||||
if(barrier.size()-1 >= threshold or barrier.get("d") == 1) {
|
||||
if(barrier_allgood(barrier,bc)) {
|
||||
barrier.put("d", 1)
|
||||
timeW = 0
|
||||
BVMSTATE = transf
|
||||
} else
|
||||
barrier.put("d", 0)
|
||||
}
|
||||
|
||||
if(allgood) {
|
||||
barrier.put("d", 1)
|
||||
timeW = 0
|
||||
BVMSTATE = transf
|
||||
} else if(timeW >= BARRIER_TIMEOUT) {
|
||||
if(timeW >= BARRIER_TIMEOUT) {
|
||||
log("------> Barrier Timeout !!!!")
|
||||
barrier = nil
|
||||
timeW = 0
|
||||
|
|
@ -73,9 +76,7 @@ function barrier_wait(threshold, transf, resumef, bc) {
|
|||
timeW = timeW+1
|
||||
}
|
||||
|
||||
barriergood = 1
|
||||
|
||||
# Barrer check all entries
|
||||
# Check all members state
|
||||
function barrier_allgood(barrier, bc) {
|
||||
barriergood = 1
|
||||
barrier.foreach(
|
||||
|
|
|
|||
|
|
@ -17,7 +17,6 @@ GOTO_MAXDIST = 150 # m.
|
|||
GOTODIST_TOL = 1.0 # m.
|
||||
GOTOANG_TOL = 0.1 # rad.
|
||||
path_it = 0
|
||||
graphid = 3
|
||||
pic_time = 0
|
||||
g_it = 0
|
||||
|
||||
|
|
@ -103,9 +102,8 @@ function take_picture() {
|
|||
}
|
||||
|
||||
function goto_gps(transf) {
|
||||
log(" has to move to lat : ", rc_goto.latitude, " long: ", rc_goto.longitude, "Current lat : ", pose.position.latitude," lon ",pose.position.longitude)
|
||||
m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0)
|
||||
print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation))
|
||||
#print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation))
|
||||
if(math.vec2.length(m_navigation)>GOTO_MAXDIST)
|
||||
log("Sorry this is too far.")
|
||||
else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination
|
||||
|
|
@ -265,13 +263,13 @@ function nei_cmd_listen() {
|
|||
neighbors.listen("cmd",
|
||||
function(vid, value, rid) {
|
||||
print("Got (", vid, ",", value, ") #", rid, "(", BVMSTATE, ")")
|
||||
if(BVMSTATE!="BARRIERWAIT") {
|
||||
if(value==22) {
|
||||
#if(BVMSTATE!="BARRIERWAIT") {
|
||||
if(value==22 and BVMSTATE=="TURNEDOFF") {
|
||||
BVMSTATE = "LAUNCH"
|
||||
}else if(value==20) {
|
||||
AUTO_LAUNCH_STATE = "IDLE"
|
||||
BVMSTATE = "GOHOME"
|
||||
} else if(value==21) {
|
||||
} else if(value==21 and BVMSTATE!="TURNEDOFF") {
|
||||
AUTO_LAUNCH_STATE = "TURNEDOFF"
|
||||
BVMSTATE = "STOP"
|
||||
} else if(value==400 and BVMSTATE=="TURNEDOFF") {
|
||||
|
|
@ -306,6 +304,6 @@ function nei_cmd_listen() {
|
|||
# # if(gt.id == id) statef=goto
|
||||
# })
|
||||
}
|
||||
}
|
||||
#}
|
||||
})
|
||||
}
|
||||
|
|
|
|||
|
|
@ -11,6 +11,8 @@ ROBOT_RADIUS = 50
|
|||
ROBOT_DIAMETER = 2.0*ROBOT_RADIUS
|
||||
ROBOT_SAFETYDIST = 2.0*ROBOT_DIAMETER
|
||||
ROOT_ID = 2
|
||||
graph_id = 3
|
||||
graph_loop = 1
|
||||
|
||||
#
|
||||
# Global variables
|
||||
|
|
@ -347,83 +349,17 @@ function UpdateNodeInfo(){
|
|||
i=i+1
|
||||
}
|
||||
}
|
||||
#
|
||||
#Transistion to state free
|
||||
#
|
||||
function TransitionToFree(){
|
||||
BVMSTATE="GRAPH_FREE"
|
||||
m_unWaitCount=m_unLabelSearchWaitTime
|
||||
m_selfMessage.State=s2i(BVMSTATE)
|
||||
}
|
||||
#
|
||||
#Transistion to state asking
|
||||
#
|
||||
function TransitionToAsking(un_label){
|
||||
BVMSTATE="GRAPH_ASKING"
|
||||
m_nLabel=un_label
|
||||
m_unRequestId=id #don't know why the random numbers are the same, add id to make the ReqID different
|
||||
m_selfMessage.State=s2i(BVMSTATE)
|
||||
m_selfMessage.ReqLabel=m_nLabel
|
||||
m_selfMessage.ReqID=m_unRequestId
|
||||
|
||||
m_unWaitCount=m_unResponseTimeThreshold
|
||||
}
|
||||
#
|
||||
#Transistion to state joining
|
||||
#
|
||||
function TransitionToJoining(){
|
||||
BVMSTATE="GRAPH_JOINING"
|
||||
m_selfMessage.State=s2i(BVMSTATE)
|
||||
m_selfMessage.Label=m_nLabel
|
||||
m_unWaitCount=m_unJoiningLostPeriod
|
||||
}
|
||||
#
|
||||
#Transistion to state joined
|
||||
#
|
||||
function TransitionToJoined(){
|
||||
BVMSTATE="GRAPH_JOINED"
|
||||
m_selfMessage.State=s2i(BVMSTATE)
|
||||
m_selfMessage.Label=m_nLabel
|
||||
m_vecNodes[m_nLabel].State="ASSIGNED"
|
||||
|
||||
#write statues
|
||||
#v_tag.put(m_nLabel, m_lockstig)
|
||||
barrier_create()
|
||||
barrier_ready(900+50)
|
||||
|
||||
m_navigation.x=0.0
|
||||
m_navigation.y=0.0
|
||||
goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0, 0.0)
|
||||
}
|
||||
#
|
||||
#Transistion to state Lock, lock the current formation
|
||||
#
|
||||
function TransitionToLock(){
|
||||
BVMSTATE="GRAPH_LOCK"
|
||||
m_selfMessage.State=s2i(BVMSTATE)
|
||||
m_selfMessage.Label=m_nLabel
|
||||
m_vecNodes[m_nLabel].State="ASSIGNED"
|
||||
#record neighbor distance
|
||||
lock_neighbor_id={}
|
||||
lock_neighbor_dis={}
|
||||
var i=0
|
||||
while(i<m_neighbourCount){
|
||||
lock_neighbor_id[i]=m_messageID[i]
|
||||
lock_neighbor_dis[i]=m_MessageRange[i]
|
||||
i=i+1
|
||||
}
|
||||
m_navigation.x=0.0
|
||||
m_navigation.y=0.0
|
||||
goto_abs(m_navigation.x, m_navigation.y, 0.0, 0.0)
|
||||
|
||||
#stop listening
|
||||
neighbors.ignore("m")
|
||||
}
|
||||
#
|
||||
# Do free
|
||||
#
|
||||
function DoFree() {
|
||||
UpdateGraph()
|
||||
if(BVMSTATE!="GRAPH_FREE"){
|
||||
BVMSTATE="GRAPH_FREE"
|
||||
m_unWaitCount=m_unLabelSearchWaitTime
|
||||
m_selfMessage.State=s2i(BVMSTATE)
|
||||
}else{
|
||||
UpdateGraph()
|
||||
m_selfMessage.State=s2i(BVMSTATE)
|
||||
|
||||
#wait for a while before looking for a Label
|
||||
|
|
@ -458,91 +394,108 @@ function DoFree() {
|
|||
}
|
||||
|
||||
if(unFoundLabel>0){
|
||||
TransitionToAsking(unFoundLabel)
|
||||
m_nLabel=unFoundLabel
|
||||
DoAsking()
|
||||
return
|
||||
}
|
||||
|
||||
#set message
|
||||
m_selfMessage.State=s2i(BVMSTATE)
|
||||
|
||||
BroadcastGraph()
|
||||
BroadcastGraph()
|
||||
}
|
||||
}
|
||||
#
|
||||
#Do asking
|
||||
#
|
||||
function DoAsking(){
|
||||
UpdateGraph()
|
||||
#look for response from predecessor
|
||||
var i=0
|
||||
var psResponse=-1
|
||||
while(i<m_neighbourCount and psResponse==-1){
|
||||
#the respond robot in joined state
|
||||
#the request Label be the same as requesed
|
||||
#get a respond
|
||||
if(m_MessageState[i]=="GRAPH_JOINED"){
|
||||
#log("received label = ",m_MessageReqLabel[i])
|
||||
if(m_MessageReqLabel[i]==m_nLabel)
|
||||
if(m_MessageResponse[i]!="REQ_NONE"){
|
||||
psResponse=i
|
||||
}}
|
||||
if(m_MessageState[i]=="GRAPH_JOINING" and m_MessageLabel[i]==m_nLabel){
|
||||
TransitionToFree()
|
||||
return
|
||||
if(BVMSTATE!="GRAPH_ASKING"){
|
||||
BVMSTATE="GRAPH_ASKING"
|
||||
m_unRequestId=id #don't know why the random numbers are the same, add id to make the ReqID different
|
||||
m_selfMessage.State=s2i(BVMSTATE)
|
||||
m_selfMessage.ReqLabel=m_nLabel
|
||||
m_selfMessage.ReqID=m_unRequestId
|
||||
m_unWaitCount=m_unResponseTimeThreshold
|
||||
}else {
|
||||
UpdateGraph()
|
||||
#look for response from predecessor
|
||||
var i=0
|
||||
var psResponse=-1
|
||||
while(i<m_neighbourCount and psResponse==-1){
|
||||
#the respond robot in joined state
|
||||
#the request Label be the same as requesed
|
||||
#get a respond
|
||||
if(m_MessageState[i]=="GRAPH_JOINED"){
|
||||
#log("received label = ",m_MessageReqLabel[i])
|
||||
if(m_MessageReqLabel[i]==m_nLabel)
|
||||
if(m_MessageResponse[i]!="REQ_NONE"){
|
||||
psResponse=i
|
||||
}}
|
||||
if(m_MessageState[i]=="GRAPH_JOINING" and m_MessageLabel[i]==m_nLabel){
|
||||
DoFree()
|
||||
return
|
||||
}
|
||||
i=i+1
|
||||
}
|
||||
i=i+1
|
||||
}
|
||||
#analyse response
|
||||
if(psResponse==-1){
|
||||
#no response, wait
|
||||
#analyse response
|
||||
if(psResponse==-1){
|
||||
#no response, wait
|
||||
|
||||
m_unWaitCount=m_unWaitCount-1
|
||||
m_selfMessage.State=s2i(BVMSTATE)
|
||||
m_selfMessage.ReqLabel=m_nLabel
|
||||
m_selfMessage.ReqID=m_unRequestId
|
||||
#if(m_unWaitCount==0){
|
||||
#TransitionToFree()
|
||||
#return
|
||||
#}
|
||||
}
|
||||
else{
|
||||
log("respond id=",m_MessageReqID[psResponse])
|
||||
if(m_MessageReqID[psResponse]!=m_unRequestId){
|
||||
m_vecNodes[m_nLabel].State="ASSIGNING"
|
||||
m_vecNodes[m_nLabel].StateAge=m_unJoiningLostPeriod
|
||||
TransitionToFree()
|
||||
}
|
||||
if(m_MessageReqID[psResponse]==m_unRequestId){
|
||||
if(m_MessageResponse[psResponse]=="REQ_GRANTED"){
|
||||
TransitionToJoining()
|
||||
return
|
||||
m_unWaitCount=m_unWaitCount-1
|
||||
m_selfMessage.State=s2i(BVMSTATE)
|
||||
m_selfMessage.ReqLabel=m_nLabel
|
||||
m_selfMessage.ReqID=m_unRequestId
|
||||
#if(m_unWaitCount==0){
|
||||
#DoFree()
|
||||
#return
|
||||
#}
|
||||
}
|
||||
else{
|
||||
TransitionToAsking(m_nLabel)
|
||||
return
|
||||
log("respond id=",m_MessageReqID[psResponse])
|
||||
if(m_MessageReqID[psResponse]!=m_unRequestId){
|
||||
m_vecNodes[m_nLabel].State="ASSIGNING"
|
||||
m_vecNodes[m_nLabel].StateAge=m_unJoiningLostPeriod
|
||||
DoFree()
|
||||
}
|
||||
if(m_MessageReqID[psResponse]==m_unRequestId){
|
||||
if(m_MessageResponse[psResponse]=="REQ_GRANTED"){
|
||||
DoJoining()
|
||||
return
|
||||
}
|
||||
else{
|
||||
DoAsking()
|
||||
return
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
m_selfMessage.Label=m_nLabel
|
||||
}
|
||||
m_selfMessage.Label=m_nLabel
|
||||
|
||||
m_navigation.x=0.0
|
||||
m_navigation.y=0.0
|
||||
goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0, 0.0)
|
||||
BroadcastGraph()
|
||||
m_navigation.x=0.0
|
||||
m_navigation.y=0.0
|
||||
goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0, 0.0)
|
||||
BroadcastGraph()
|
||||
}
|
||||
}
|
||||
#
|
||||
#Do joining
|
||||
#
|
||||
function DoJoining(){
|
||||
UpdateGraph()
|
||||
|
||||
if(m_gotjoinedparent!=1)
|
||||
set_rc_goto()
|
||||
else
|
||||
goto_gps(TransitionToJoined)
|
||||
#pack the communication package
|
||||
m_selfMessage.State=s2i(BVMSTATE)
|
||||
m_selfMessage.Label=m_nLabel
|
||||
BroadcastGraph()
|
||||
if(BVMSTATE!="GRAPH_JOINING") {
|
||||
BVMSTATE="GRAPH_JOINING"
|
||||
m_selfMessage.State=s2i(BVMSTATE)
|
||||
m_selfMessage.Label=m_nLabel
|
||||
m_unWaitCount=m_unJoiningLostPeriod
|
||||
} else {
|
||||
UpdateGraph()
|
||||
if(m_gotjoinedparent!=1)
|
||||
set_rc_goto()
|
||||
else
|
||||
goto_gps(DoJoined)
|
||||
#pack the communication package
|
||||
m_selfMessage.State=s2i(BVMSTATE)
|
||||
m_selfMessage.Label=m_nLabel
|
||||
BroadcastGraph()
|
||||
}
|
||||
}
|
||||
|
||||
function set_rc_goto() {
|
||||
|
|
@ -576,122 +529,159 @@ function set_rc_goto() {
|
|||
#Do joined
|
||||
#
|
||||
function DoJoined(){
|
||||
UpdateGraph()
|
||||
m_selfMessage.State=s2i(BVMSTATE)
|
||||
m_selfMessage.Label=m_nLabel
|
||||
if(BVMSTATE!="GRAPH_JOINED"){
|
||||
BVMSTATE="GRAPH_JOINED"
|
||||
m_selfMessage.State=s2i(BVMSTATE)
|
||||
m_selfMessage.Label=m_nLabel
|
||||
m_vecNodes[m_nLabel].State="ASSIGNED"
|
||||
|
||||
#collect all requests
|
||||
var mapRequests={}
|
||||
var i=0
|
||||
var j=0
|
||||
var ReqLabel
|
||||
var JoiningLabel
|
||||
var seenPred=0
|
||||
while(i<m_neighbourCount){
|
||||
if(m_MessageState[i]=="GRAPH_ASKING"){
|
||||
ReqLabel=m_MessageReqLabel[i]
|
||||
#log("ReqLabel var:",ReqLabel)
|
||||
#log("M_vec var",m_vecNodes[ReqLabel].State)
|
||||
if(m_vecNodes[ReqLabel].State=="UNASSIGNED")
|
||||
if(m_nLabel==m_vecNodes[ReqLabel].Pred){
|
||||
#is a request, store the index
|
||||
mapRequests[j]=i
|
||||
j=j+1
|
||||
}
|
||||
}
|
||||
if(m_MessageState[i]=="GRAPH_JOINING"){
|
||||
JoiningLabel=m_MessageLabel[i]
|
||||
if(m_nLabel==m_vecNodes[JoiningLabel].Pred){
|
||||
##joining wrt this dot,send the global bearing
|
||||
var m_messageForJoining={.Label=JoiningLabel,.Bearing=m_MessageBearing[i]-m_bias}
|
||||
#write statues
|
||||
barrier_create()
|
||||
barrier_ready(-1)
|
||||
|
||||
m_navigation.x=0.0
|
||||
m_navigation.y=0.0
|
||||
goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0, 0.0)
|
||||
} else {
|
||||
UpdateGraph()
|
||||
m_selfMessage.State=s2i(BVMSTATE)
|
||||
m_selfMessage.Label=m_nLabel
|
||||
|
||||
#collect all requests
|
||||
var mapRequests={}
|
||||
var i=0
|
||||
var j=0
|
||||
var ReqLabel
|
||||
var JoiningLabel
|
||||
var seenPred=0
|
||||
while(i<m_neighbourCount){
|
||||
if(m_MessageState[i]=="GRAPH_ASKING"){
|
||||
ReqLabel=m_MessageReqLabel[i]
|
||||
#log("ReqLabel var:",ReqLabel)
|
||||
#log("M_vec var",m_vecNodes[ReqLabel].State)
|
||||
if(m_vecNodes[ReqLabel].State=="UNASSIGNED")
|
||||
if(m_nLabel==m_vecNodes[ReqLabel].Pred){
|
||||
#is a request, store the index
|
||||
mapRequests[j]=i
|
||||
j=j+1
|
||||
}
|
||||
}
|
||||
if(m_MessageState[i]=="GRAPH_JOINING"){
|
||||
JoiningLabel=m_MessageLabel[i]
|
||||
if(m_nLabel==m_vecNodes[JoiningLabel].Pred){
|
||||
##joining wrt this dot,send the global bearing
|
||||
var m_messageForJoining={.Label=JoiningLabel,.Bearing=m_MessageBearing[i]-m_bias}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if(m_MessageState[i]=="GRAPH_JOINING" and repeat_assign==1 and m_MessageLabel[i]==assign_label){
|
||||
repeat_assign=0
|
||||
}
|
||||
if(m_MessageState[i]=="GRAPH_JOINING" and repeat_assign==1 and m_MessageLabel[i]==assign_label){
|
||||
repeat_assign=0
|
||||
}
|
||||
|
||||
|
||||
#if it is the pred
|
||||
if((m_MessageState[i]=="GRAPH_JOINED" or m_MessageState[i]=="GRAPH_LOCK") and m_MessageLabel[i]==m_vecNodes[m_nLabel].Pred){
|
||||
seenPred=1
|
||||
m_unWaitCount=m_unJoiningLostPeriod
|
||||
}
|
||||
i=i+1
|
||||
}
|
||||
|
||||
#get request
|
||||
if(size(mapRequests)!=0){
|
||||
i=1
|
||||
var ReqIndex=0
|
||||
while(i<size(mapRequests)){
|
||||
#compare the distance
|
||||
if(m_MessageRange[mapRequests[ReqIndex]]>m_MessageRange[mapRequests[i]])
|
||||
ReqIndex=i
|
||||
#if it is the pred
|
||||
if((m_MessageState[i]=="GRAPH_JOINED" or m_MessageState[i]=="GRAPH_LOCK") and m_MessageLabel[i]==m_vecNodes[m_nLabel].Pred){
|
||||
seenPred=1
|
||||
m_unWaitCount=m_unJoiningLostPeriod
|
||||
}
|
||||
i=i+1
|
||||
}
|
||||
if(repeat_assign==0){
|
||||
#get the best index, whose ReqLabel and Reqid are
|
||||
ReqLabel=m_MessageReqLabel[mapRequests[ReqIndex]]
|
||||
var ReqID=m_MessageReqID[mapRequests[ReqIndex]]
|
||||
assign_label=ReqLabel
|
||||
assign_id=ReqID
|
||||
repeat_assign=1
|
||||
}
|
||||
m_selfMessage.ReqLabel=assign_label
|
||||
m_selfMessage.ReqID=assign_id
|
||||
m_selfMessage.Response=r2i("REQ_GRANTED")
|
||||
#m_vecNodes[ReqLabel].State="ASSIGNING"
|
||||
log("Label=",assign_label)
|
||||
log("ID=",assign_id)
|
||||
m_vecNodes[ReqLabel].StateAge=m_unJoiningLostPeriod
|
||||
}
|
||||
|
||||
#lost pred, wait for some time and transit to free
|
||||
if(seenPred==0){
|
||||
m_unWaitCount=m_unWaitCount-1
|
||||
if(m_unWaitCount==0){
|
||||
TransitionToFree()
|
||||
return
|
||||
}
|
||||
}
|
||||
barrier_wait(ROBOTS, "GRAPH_TRANSTOLOCK", "GRAPH_JOINED",941)
|
||||
BroadcastGraph()
|
||||
#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
|
||||
}
|
||||
if(repeat_assign==0){
|
||||
#get the best index, whose ReqLabel and Reqid are
|
||||
ReqLabel=m_MessageReqLabel[mapRequests[ReqIndex]]
|
||||
var ReqID=m_MessageReqID[mapRequests[ReqIndex]]
|
||||
assign_label=ReqLabel
|
||||
assign_id=ReqID
|
||||
repeat_assign=1
|
||||
}
|
||||
m_selfMessage.ReqLabel=assign_label
|
||||
m_selfMessage.ReqID=assign_id
|
||||
m_selfMessage.Response=r2i("REQ_GRANTED")
|
||||
#m_vecNodes[ReqLabel].State="ASSIGNING"
|
||||
log("Label=",assign_label)
|
||||
log("ID=",assign_id)
|
||||
m_vecNodes[ReqLabel].StateAge=m_unJoiningLostPeriod
|
||||
}
|
||||
|
||||
#lost pred, wait for some time and transit to free
|
||||
if(seenPred==0){
|
||||
m_unWaitCount=m_unWaitCount-1
|
||||
if(m_unWaitCount==0){
|
||||
DoFree()
|
||||
return
|
||||
}
|
||||
}
|
||||
# using JOINED as the resume state kinds of just reset the barrier timeout, should be IDLE/ALLOCATE...
|
||||
barrier_wait(ROBOTS, "GRAPH_LOCK", "GRAPH_JOINED", -1)
|
||||
BroadcastGraph()
|
||||
}
|
||||
}
|
||||
#
|
||||
#Do Lock
|
||||
#
|
||||
timeout_graph = 40
|
||||
function DoLock() {
|
||||
UpdateGraph()
|
||||
m_selfMessage.State=s2i(BVMSTATE)
|
||||
m_selfMessage.Label=m_nLabel
|
||||
m_navigation.x=0.0
|
||||
m_navigation.y=0.0
|
||||
#calculate motion vection
|
||||
if(m_nLabel==0){
|
||||
m_navigation.x=0.0 #change value so that robot 0 will move
|
||||
m_navigation.y=0.0
|
||||
}
|
||||
if(m_nLabel!=0){
|
||||
m_navigation=motion_vector()
|
||||
}
|
||||
# #move
|
||||
#[transition to lock...
|
||||
BVMSTATE="GRAPH_LOCK"
|
||||
m_selfMessage.State=s2i(BVMSTATE)
|
||||
m_selfMessage.Label=m_nLabel
|
||||
m_vecNodes[m_nLabel].State="ASSIGNED"
|
||||
#record neighbor distance
|
||||
lock_neighbor_id={}
|
||||
lock_neighbor_dis={}
|
||||
var i=0
|
||||
while(i<m_neighbourCount){
|
||||
lock_neighbor_id[i]=m_messageID[i]
|
||||
lock_neighbor_dis[i]=m_MessageRange[i]
|
||||
i=i+1
|
||||
}
|
||||
m_navigation.x=0.0
|
||||
m_navigation.y=0.0
|
||||
goto_abs(m_navigation.x, m_navigation.y, 0.0, 0.0)
|
||||
|
||||
# goto_abs(m_navigation.x, m_navigation.y, 0.0, 0.0)
|
||||
BroadcastGraph()
|
||||
#stop listening
|
||||
neighbors.ignore("m")
|
||||
#..]
|
||||
UpdateGraph()
|
||||
m_selfMessage.State=s2i(BVMSTATE)
|
||||
m_selfMessage.Label=m_nLabel
|
||||
m_navigation.x=0.0
|
||||
m_navigation.y=0.0
|
||||
#calculate motion vection
|
||||
if(m_nLabel==0){
|
||||
m_navigation.x=0.0 #change value so that robot 0 will move
|
||||
m_navigation.y=0.0
|
||||
}
|
||||
if(m_nLabel!=0){
|
||||
m_navigation=motion_vector()
|
||||
}
|
||||
# #move
|
||||
|
||||
# if(loop) {
|
||||
# if(timeout_graph==0) {
|
||||
# if(graphid < 3)
|
||||
# graphid = graphid + 1
|
||||
# else
|
||||
# graphid = 0
|
||||
# timeout_graph = 40
|
||||
# BVMSTATE="TASK_ALLOCATE"
|
||||
# }
|
||||
# timeout_graph = timeout_graph - 1
|
||||
# }
|
||||
# goto_abs(m_navigation.x, m_navigation.y, 0.0, 0.0)
|
||||
BroadcastGraph()
|
||||
|
||||
if(graph_loop) {
|
||||
if(timeout_graph==0) {
|
||||
if(graph_id < 3)
|
||||
graph_id = graph_id + 1
|
||||
else
|
||||
graph_id = 0
|
||||
timeout_graph = 40
|
||||
BVMSTATE="TASK_ALLOCATE"
|
||||
}
|
||||
timeout_graph = timeout_graph - 1
|
||||
}
|
||||
}
|
||||
|
||||
#
|
||||
|
|
@ -738,7 +728,6 @@ function BroadcastGraph() {
|
|||
# Executed when reset
|
||||
#
|
||||
function resetGraph(){
|
||||
BVMSTATE="GRAPH_FREE"
|
||||
m_receivedMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0}
|
||||
m_selfMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
|
||||
m_navigation={.x=0,.y=0}
|
||||
|
|
@ -755,16 +744,16 @@ function resetGraph(){
|
|||
assign_id=-1
|
||||
m_gotjoinedparent = 0
|
||||
|
||||
if(graphid==0){
|
||||
if(graph_id==0){
|
||||
log("Loading P graph")
|
||||
Read_GraphP()
|
||||
} else if(graphid==1) {
|
||||
} else if(graph_id==1) {
|
||||
log("Loading O graph")
|
||||
Read_GraphO()
|
||||
} else if(graphid==2) {
|
||||
} else if(graph_id==2) {
|
||||
log("Loading L graph")
|
||||
Read_GraphL()
|
||||
} else if(graphid==3) {
|
||||
} else if(graph_id==3) {
|
||||
log("Loading Y graph")
|
||||
Read_GraphY()
|
||||
}
|
||||
|
|
@ -777,11 +766,9 @@ function resetGraph(){
|
|||
#[A]The robot used to triger the formation process is defined as joined,
|
||||
if(id==ROOT_ID){
|
||||
m_nLabel=0
|
||||
TransitionToJoined()
|
||||
}
|
||||
#[B]Other robots are defined as free.
|
||||
else{
|
||||
TransitionToFree()
|
||||
DoJoined()
|
||||
}else{ #[B]Other robots are defined as free.
|
||||
DoFree()
|
||||
}
|
||||
}
|
||||
#
|
||||
|
|
|
|||
|
|
@ -14,6 +14,9 @@ AUTO_LAUNCH_STATE = "TASK_ALLOCATE"
|
|||
LOWEST_ROBOT_ID = 0
|
||||
TARGET = 9.0
|
||||
EPSILON = 30.0
|
||||
ROOT_ID = 2
|
||||
graph_id = 3
|
||||
graph_loop = 0
|
||||
|
||||
#####
|
||||
# Vehicule type:
|
||||
|
|
@ -34,9 +37,7 @@ function init() {
|
|||
|
||||
# initGraph()
|
||||
|
||||
TARGET_ALTITUDE = 5 + (id-LOWEST_ROBOT_ID)*4.0 # m
|
||||
|
||||
loop = 1
|
||||
TARGET_ALTITUDE = 4 + (id-LOWEST_ROBOT_ID)*3.0 # m
|
||||
|
||||
# start the swarm command listener
|
||||
nei_cmd_listen()
|
||||
|
|
@ -88,8 +89,6 @@ function step() {
|
|||
statef=DoJoining
|
||||
else if(BVMSTATE=="GRAPH_JOINED")
|
||||
statef=DoJoined
|
||||
else if(BVMSTATE=="GRAPH_TRANSTOLOCK")
|
||||
statef=TransitionToLock
|
||||
else if(BVMSTATE=="GRAPH_LOCK") # move all together (TODO: not tested), but auto-loop in graph list
|
||||
statef=DoLock
|
||||
else if(BVMSTATE=="PATHPLAN") # ends on navigate, defined in rrtstar
|
||||
|
|
|
|||
Loading…
Reference in New Issue