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

removed state transitions

This commit is contained in:
dave 2018-09-08 19:15:46 -04:00
parent d055d8939f
commit 602d744140
4 changed files with 254 additions and 269 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
buzz_scripts/include/act/barrier.bzz
View File

@ -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(allgood) {
if(barrier.size()-1 >= threshold or barrier.get("d") == 1) {
if(barrier_allgood(barrier,bc)) {
barrier.put("d", 1)
timeW = 0
BVMSTATE = transf
} else if(timeW >= BARRIER_TIMEOUT) {
} else
barrier.put("d", 0)
}
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(

12
buzz_scripts/include/act/states.bzz
View File

@ -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
# })
}
}
#}
})
}

195
buzz_scripts/include/taskallocate/graphformGPS.bzz
View File

@ -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,82 +349,16 @@ 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() {
if(BVMSTATE!="GRAPH_FREE"){
BVMSTATE="GRAPH_FREE"
m_unWaitCount=m_unLabelSearchWaitTime
m_selfMessage.State=s2i(BVMSTATE)
}else{
UpdateGraph()
m_selfMessage.State=s2i(BVMSTATE)
@ -458,7 +394,8 @@ function DoFree() {
}
if(unFoundLabel>0){
TransitionToAsking(unFoundLabel)
m_nLabel=unFoundLabel
DoAsking()
return
}
@ -466,11 +403,20 @@ function DoFree() {
m_selfMessage.State=s2i(BVMSTATE)
BroadcastGraph()
}
}
#
#Do asking
#
function DoAsking(){
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
@ -486,7 +432,7 @@ function DoAsking(){
psResponse=i
}}
if(m_MessageState[i]=="GRAPH_JOINING" and m_MessageLabel[i]==m_nLabel){
TransitionToFree()
DoFree()
return
}
i=i+1
@ -500,7 +446,7 @@ function DoAsking(){
m_selfMessage.ReqLabel=m_nLabel
m_selfMessage.ReqID=m_unRequestId
#if(m_unWaitCount==0){
#TransitionToFree()
#DoFree()
#return
#}
}
@ -509,15 +455,15 @@ function DoAsking(){
if(m_MessageReqID[psResponse]!=m_unRequestId){
m_vecNodes[m_nLabel].State="ASSIGNING"
m_vecNodes[m_nLabel].StateAge=m_unJoiningLostPeriod
TransitionToFree()
DoFree()
}
if(m_MessageReqID[psResponse]==m_unRequestId){
if(m_MessageResponse[psResponse]=="REQ_GRANTED"){
TransitionToJoining()
DoJoining()
return
}
else{
TransitionToAsking(m_nLabel)
DoAsking()
return
}
}
@ -528,21 +474,28 @@ function DoAsking(){
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(){
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(TransitionToJoined)
goto_gps(DoJoined)
#pack the communication package
m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.Label=m_nLabel
BroadcastGraph()
}
}
function set_rc_goto() {
@ -576,6 +529,20 @@ function set_rc_goto() {
#Do joined
#
function DoJoined(){
if(BVMSTATE!="GRAPH_JOINED"){
BVMSTATE="GRAPH_JOINED"
m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.Label=m_nLabel
m_vecNodes[m_nLabel].State="ASSIGNED"
#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
@ -651,18 +618,41 @@ function DoJoined(){
if(seenPred==0){
m_unWaitCount=m_unWaitCount-1
if(m_unWaitCount==0){
TransitionToFree()
DoFree()
return
}
}
barrier_wait(ROBOTS, "GRAPH_TRANSTOLOCK", "GRAPH_JOINED",941)
# 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() {
#[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)
#stop listening
neighbors.ignore("m")
#..]
UpdateGraph()
m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.Label=m_nLabel
@ -676,22 +666,22 @@ function DoLock() {
if(m_nLabel!=0){
m_navigation=motion_vector()
}
# #move
# #move
# 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()
# 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
# }
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()
}
}
#

9
buzz_scripts/main.bzz
View File

@ -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