ROSBuzz_MISTLab/buzz_scripts/include/act/neighborcomm.bzz

152 lines
4.4 KiB
Plaintext

# listens to commands from the remote control (web, commandline, rcclient node, etc)
function rc_cmd_listen() {
if(flight.rc_cmd==22) {
log("cmd 22")
flight.rc_cmd=0
BVMSTATE = "LAUNCH"
neighbors.broadcast("cmd", 22)
} else if(flight.rc_cmd==21) {
flight.rc_cmd=0
AUTO_LAUNCH_STATE = "TURNEDOFF"
#barrier_set(ROBOTS, "GOHOME", BVMSTATE, 21)
#barrier_ready(21)
BVMSTATE = "STOP"
neighbors.broadcast("cmd", 21)
} else if(flight.rc_cmd==20) {
flight.rc_cmd=0
AUTO_LAUNCH_STATE = "IDLE"
barrier_set(ROBOTS, "GOHOME", BVMSTATE, 20)
barrier_ready(20)
neighbors.broadcast("cmd", 20)
# } else if(flight.rc_cmd==16) {
# flight.rc_cmd=0
# BVMSTATE = "PATHPLAN"
} else if(flight.rc_cmd==400) {
flight.rc_cmd=0
arm()
neighbors.broadcast("cmd", 400)
} else if (flight.rc_cmd==401){
flight.rc_cmd=0
disarm()
neighbors.broadcast("cmd", 401)
} else if (flight.rc_cmd==666){
flight.rc_cmd=0
stattab_send()
} else if (flight.rc_cmd==777){
flight.rc_cmd=0
reinit_time_sync()
neighbors.broadcast("cmd", 777)
}else if (flight.rc_cmd==900){
flight.rc_cmd=0
barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900)
barrier_ready(900)
neighbors.broadcast("cmd", 900)
} else if (flight.rc_cmd==901){
flight.rc_cmd=0
destroyGraph()
barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 901)
barrier_ready(901)
neighbors.broadcast("cmd", 901)
} else if (flight.rc_cmd==902){
flight.rc_cmd=0
destroyGraph()
barrier_set(ROBOTS, "WAYPOINT", BVMSTATE, 902)
barrier_ready(902)
neighbors.broadcast("cmd", 902)
} else if (flight.rc_cmd==903){
flight.rc_cmd=0
destroyGraph()
resetWP()
barrier_set(ROBOTS, "POTENTIAL", BVMSTATE, 903)
barrier_ready(903)
neighbors.broadcast("cmd", 903)
} else if (flight.rc_cmd==904){
flight.rc_cmd=0
destroyGraph()
barrier_set(ROBOTS, "IDLE", BVMSTATE, 904)
barrier_ready(904)
neighbors.broadcast("cmd", 904)
}
}
# listens to neighbors broadcasting commands
function nei_cmd_listen() {
neighbors.listen("cmd",
function(vid, value, rid) {
print("Got (", vid, ",", value, ") #", rid, "(", BVMSTATE, ")")
#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 and BVMSTATE!="TURNEDOFF") {
BVMSTATE = "STOP"
} else if(value==400 and BVMSTATE=="TURNEDOFF") {
arm()
} else if(value==401 and BVMSTATE=="TURNEDOFF"){
disarm()
} else if(value==777 and BVMSTATE=="TURNEDOFF"){
reinit_time_sync()
#neighbors.broadcast("cmd", 777)
}else if(value==900){ # Shapes
barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900)
#barrier_ready(900)
#neighbors.broadcast("cmd", 900)
} else if(value==901 and BVMSTATE!="BARRIERWAIT"){ # Pursuit
destroyGraph()
barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 901)
#barrier_ready(901)
#neighbors.broadcast("cmd", 901)
} else if(value==902 and BVMSTATE!="BARRIERWAIT" and BVMSTATE!="WAYPOINT"){ # Waypoint
destroyGraph()
barrier_set(ROBOTS, "WAYPOINT", BVMSTATE, 902)
#barrier_ready(902)
#neighbors.broadcast("cmd", 902)
} else if(value==903 and BVMSTATE!="BARRIERWAIT" and BVMSTATE!="POTENTIAL"){ # Formation
destroyGraph()
resetWP()
barrier_set(ROBOTS, "POTENTIAL", BVMSTATE, 903)
#barrier_ready(903)
#neighbors.broadcast("cmd", 903)
} else if(value==904 and BVMSTATE!="BARRIERWAIT" and BVMSTATE!="IDLE"){ # idle
destroyGraph()
barrier_set(ROBOTS, "IDLE", BVMSTATE, 904)
#barrier_ready(904)
#neighbors.broadcast("cmd", 904)
} else if(value==16 and BVMSTATE=="IDLE"){
# neighbors.listen("gt",function(vid, value, rid) {
# print("Got (", vid, ",", value, ") from robot #", rid)
# # if(gt.id == id) statef=goto
# })
}
#}
})
}
firsttimeinwp = 1
function check_rc_wp() {
if(firsttimeinwp) {
v_wp = stigmergy.create(WP_STIG)
storegoal(pose.position.latitude, pose.position.longitude, pose.position.altitude)
firsttimeinwp = 0
}
if(rc_goto.id != -1) {
if(rc_goto.id == id) {
wpreached = 0
storegoal(rc_goto.latitude, rc_goto.longitude, pose.position.altitude)
return
} else {
var ls = packWP2i(rc_goto.latitude, rc_goto.longitude, 0)
v_wp.put(rc_goto.id,ls)
reset_rc()
}
}
}
function resetWP() {
firsttimeinwp = 1
if(v_wp!=nil)
v_wp.foreach(function(key, value, robot){v_wp[key]=nil})
}