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

adapted bzz files from sim.

This commit is contained in:
vivek-shankar 2018-07-04 21:17:42 -04:00
parent 392531e131
commit 1bfeada346
8 changed files with 229 additions and 91 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -7,10 +7,11 @@
#
# Constants
#
BARRIER_TIMEOUT = 1200 # in steps
BARRIER_TIMEOUT = 200 # in steps
BARRIER_VSTIG = 80
timeW = 0
barrier = nil
bc = 0;
#
# Sets a barrier
@ -22,15 +23,15 @@ function barrier_create() {
#log("---> Prev. br. ", barrier, " ", BARRIER_VSTIG)
if(barrier!=nil) {
barrier=nil
BARRIER_VSTIG = BARRIER_VSTIG +1
# BARRIER_VSTIG = BARRIER_VSTIG +1
}
#log("---> New. br. ", barrier, " ", BARRIER_VSTIG)
barrier = stigmergy.create(BARRIER_VSTIG)
}
function barrier_set(threshold, transf, resumef) {
function barrier_set(threshold, transf, resumef, bc) {
statef = function() {
barrier_wait(threshold, transf, resumef);
barrier_wait(threshold, transf, resumef, bc);
}
BVMSTATE = "BARRIERWAIT"
barrier_create()
@ -39,30 +40,41 @@ function barrier_set(threshold, transf, resumef) {
#
# Make yourself ready
#
function barrier_ready() {
function barrier_ready(bc) {
#log("BARRIER READY -------")
barrier.put(id, 1)
barrier.put(id, bc)
barrier.put("d", 0)
}
#
# Executes the barrier
#
function barrier_wait(threshold, transf, resumef) {
barrier.put(id, 1)
function barrier_wait(threshold, transf, resumef, bc) {
barrier.put(id, bc)
barrier.get(id)
#log("--->BS: ", barrier.size(), " (", BARRIER_VSTIG, ")")
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) {
var bi = LOWEST_ROBOT_ID
allgood = 1
while (bi<LOWEST_ROBOT_ID+threshold) {
if(barrier.get(bi) != bc)
allgood = 0
bi = bi + 1
}
}
if(allgood) {
barrier.put("d", 1)
timeW = 0
BVMSTATE = transf
} else if(timeW >= BARRIER_TIMEOUT) {
log("------> Barrier Timeout !!!!")
barrier=nil
barrier = nil
timeW = 0
BVMSTATE = resumef
}
} else if(timeW % 10 == 0 and bc > 0)
neighbors.broadcast("cmd", bc)
timeW = timeW+1
}

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

@ -27,44 +27,60 @@ function idle() {
BVMSTATE = "IDLE"
}
# Custom function for the user.
function cusfun(){
BVMSTATE="CUSFUN"
log("cusfun: yay!!!")
}
function launch() {
BVMSTATE = "LAUNCH"
if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so LAND
if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so TAKE_OFF
homegps = {.lat=pose.position.latitude, .long=pose.position.longitude}
if( flight.status == 2 and pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "LAUNCH")
barrier_ready()
barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "STOP", 22)
barrier_ready(22)
} else {
log("Altitude: ", pose.position.altitude)
neighbors.broadcast("cmd", 22)
uav_takeoff(TARGET_ALTITUDE)
}
} else {
barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "LAUNCH")
barrier_ready()
barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "STOP", 22)
barrier_ready(22)
}
}
gohomeT=100
function goinghome() {
BVMSTATE = "GOHOME"
if(gohomeT > 0) { # TODO: Make a real check if home is reached
gohome()
gohomeT = gohomeT - 1
} else
BVMSTATE = AUTO_LAUNCH_STATE
}
function stop() {
BVMSTATE = "STOP"
BVMSTATE = "STOP"
if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so LAND
neighbors.broadcast("cmd", 21)
uav_land()
if(flight.status != 2 and flight.status != 3) {
barrier_set(ROBOTS,"TURNEDOFF","STOP")
barrier_ready()
barrier_set(ROBOTS,"TURNEDOFF","STOP", 21)
barrier_ready(21)
}
} else {
barrier_set(ROBOTS,"TURNEDOFF","STOP")
barrier_ready()
}
barrier_set(ROBOTS,"TURNEDOFF","STOP", 21)
barrier_ready(21)
}
}
function take_picture() {
BVMSTATE="PICTURE"
uav_setgimbal(0.0, 0.0, -90.0, 20.0)
setgimbal(0.0, 0.0, -90.0, 20.0)
if(pic_time==PICTURE_WAIT/2) { # wait for the drone to stabilize
uav_takepicture()
takepicture()
} else if(pic_time>=PICTURE_WAIT) { # wait for the picture
BVMSTATE="IDLE"
pic_time=0
@ -117,7 +133,7 @@ function aggregate() {
# circle all together around centroid
function pursuit() {
BVMSTATE="PURSUIT"
rd = 15.0
rd = 20.0
pc = 3.2
vd = 15.0
r_vec = neighbors.reduce(function(rid, data, r_vec) {
@ -127,8 +143,7 @@ function pursuit() {
if(neighbors.count() > 0)
r_vec = math.vec2.scale(r_vec, 1.0 / neighbors.count())
r = math.vec2.length(r_vec)
sqr = (r-rd)*(r-rd)+pc*pc*r*r
gamma = vd / math.sqrt(sqr)
gamma = vd / math.sqrt((r-rd)*(r-rd)+pc*pc*r*r)
dr = -gamma * (r-rd)
dT = gamma * pc
vfg = math.vec2.newp(r+dr*0.1, math.vec2.angle(r_vec)+dT*0.1)
@ -138,9 +153,9 @@ function pursuit() {
# Lennard-Jones interaction magnitude
TARGET = 8.0
EPSILON = 3.0
EPSILON = 30.0
function lj_magnitude(dist, target, epsilon) {
lj = -(epsilon / dist) * ((target / dist)^4 - 1.2 * (target / dist)^2)
lj = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
return lj
}
@ -155,92 +170,119 @@ function lj_sum(rid, data, accum) {
}
# Calculates and actuates the flocking interaction
old_lj = math.vec2.newp(0.0, 0.0) # 1-step filter, to smoothen dir. changes
function formation() {
BVMSTATE="FORMATION"
# Calculate accumulator
accum_lj = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
if(neighbors.count() > 0)
accum_lj = math.vec2.scale(accum_lj, 1.0 / neighbors.count())
old_lj = LimitSpeed(math.vec2.add(old_lj, accum_lj), 1.0/4.0)
goto_abs(old_lj.x, old_lj.y, 0.0, 0.0)
accum_lj = LimitSpeed(accum_lj, 1.0/3.0)
goto_abs(accum_lj.x, accum_lj.y, 0.0, 0.0)
}
# listens to commands from the remote control (web, commandline, etc)
function rc_cmd_listen() {
if(flight.rc_cmd==22) {
if(flight.rc_cmd==22) {
log("cmd 22")
flight.rc_cmd=0
BVMSTATE = "LAUNCH"
neighbors.broadcast("cmd", 22)
} else if(flight.rc_cmd==21) {
log("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
uav_arm()
arm()
neighbors.broadcast("cmd", 400)
} else if (flight.rc_cmd==401){
flight.rc_cmd=0
uav_disarm()
disarm()
neighbors.broadcast("cmd", 401)
} else if (flight.rc_cmd==666){
flight.rc_cmd=0
stattab_send()
} else if (flight.rc_cmd==900){
flight.rc_cmd=0
BVMSTATE = "TASK_ALLOCATE"
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()
BVMSTATE = "PURSUIT"
barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 901)
barrier_ready(901)
neighbors.broadcast("cmd", 901)
} else if (flight.rc_cmd==902){
flight.rc_cmd=0
destroyGraph()
BVMSTATE = "AGGREGATE"
barrier_set(ROBOTS, "AGGREGATE", BVMSTATE, 902)
barrier_ready(902)
neighbors.broadcast("cmd", 902)
} else if (flight.rc_cmd==903){
flight.rc_cmd=0
destroyGraph()
BVMSTATE = "FORMATION"
barrier_set(ROBOTS, "FORMATION", BVMSTATE, 903)
barrier_ready(903)
neighbors.broadcast("cmd", 903)
}
}
# listens to other fleet members broadcasting commands
function nei_cmd_listen() {
neighbors.listen("cmd",
function(vid, value, rid) {
print("Got (", vid, ",", value, ") #", rid, "(", BVMSTATE, ")")
if(value==22 and BVMSTATE!="BARRIERWAIT") {
BVMSTATE = "LAUNCH"
} else if(value==21 and BVMSTATE!="BARRIERWAIT") {
BVMSTATE = "STOP"
} else if(value==400 and BVMSTATE=="TURNEDOFF") {
uav_arm()
} else if(value==401 and BVMSTATE=="TURNEDOFF"){
uav_disarm()
} else if(value==900){ # Shapes
BVMSTATE = "TASK_ALLOCATE"
} else if(value==901){ # Pursuit
destroyGraph()
BVMSTATE = "PURSUIT"
} else if(value==902){ # Agreggate
destroyGraph()
BVMSTATE = "AGGREGATE"
} else if(value==903){ # Formation
destroyGraph()
BVMSTATE = "FORMATION"
} 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
# })
if(BVMSTATE!="BARRIERWAIT") {
if(value==22) {
BVMSTATE = "LAUNCH"
}else if(value==20) {
AUTO_LAUNCH_STATE = "IDLE"
BVMSTATE = "GOHOME"
} else if(value==21) {
AUTO_LAUNCH_STATE = "TURNEDOFF"
BVMSTATE = "STOP"
} else if(value==400 and BVMSTATE=="TURNEDOFF") {
arm()
} else if(value==401 and BVMSTATE=="TURNEDOFF"){
disarm()
} else if(value==900){ # Shapes
barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900)
#barrier_ready(900)
neighbors.broadcast("cmd", 900)
} else if(value==901){ # Pursuit
destroyGraph()
barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 901)
#barrier_ready(901)
neighbors.broadcast("cmd", 901)
} else if(value==902){ # Agreggate
destroyGraph()
barrier_set(ROBOTS, "AGGREGATE", BVMSTATE, 902)
#barrier_ready(902)
neighbors.broadcast("cmd", 902)
} else if(value==903){ # Formation
destroyGraph()
barrier_set(ROBOTS, "FORMATION", BVMSTATE, 903)
#barrier_ready(903)
neighbors.broadcast("cmd", 903)
} 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
# })
}
}
})
}

2
buzz_scripts/include/plan/rrtstar.bzz
View File

@ -20,7 +20,7 @@ mapgoal = {}
function navigate() {
BVMSTATE="NAVIGATE"
if(V_TYPE == 0) { # TODO: place these in a generic function in conversion.bzz
uav_storegoal(goal_list[g_it].x, goal_list[g_it].y, pose.position.altitude)
storegoal(goal_list[g_it].x, goal_list[g_it].y, pose.position.altitude)
cur_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0)
m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long, 0),-1)
}

3
buzz_scripts/include/update.bzz
View File

@ -4,6 +4,7 @@
####################################################################################################
updated="update_ack"
update_no=0
function updated_neigh(){
updates_active = 1
function updated_no_bct(){
neighbors.broadcast(updated, update_no)
}

13
buzz_scripts/main.bzz
View File

@ -7,7 +7,11 @@ include "taskallocate/graphformGPS.bzz"
include "vstigenv.bzz"
#State launched after takeoff
AUTO_LAUNCH_STATE = "PURSUIT"
AUTO_LAUNCH_STATE = "CUSFUN"
#Lowest robot id in the network
LOWEST_ROBOT_ID = 97
TARGET = 9.0
EPSILON = 30.0
#####
# Vehicule type:
@ -21,12 +25,13 @@ goal_list = {
.0={.x = 45.5088103899, .y = -73.1540826153, .z = 2.5}
}
# Executed once at init time.
function init() {
init_stig()
init_swarm()
TARGET_ALTITUDE = 10 + id*2.0 # m
TARGET_ALTITUDE = 5 + (id-LOWEST_ROBOT_ID)*3.0 # m
loop = 1
# start the swarm command listener
@ -49,10 +54,14 @@ function step() {
#
if(BVMSTATE=="TURNEDOFF")
statef=turnedoff
else if(BVMSTATE=="CUSFUN")
statef=cusfun
else if(BVMSTATE=="STOP") # ends on turnedoff
statef=stop
else if(BVMSTATE=="LAUNCH") # ends on AUTO_LAUNCH_STATE
statef=launch
else if(BVMSTATE=="GOHOME") # ends on AUTO_LAUNCH_STATE
statef=goinghome
else if(BVMSTATE=="IDLE")
statef=idle
else if(BVMSTATE=="AGGREGATE")

1
buzz_scripts/minimal.bzz
View File

@ -51,7 +51,6 @@ function step() {
statef=action
statef()
log("Current state: ", BVMSTATE)
}

45
buzz_scripts/stand_by.bzz
View File

@ -1,35 +1,38 @@
include "act/states.bzz"
include "vstigenv.bzz"
updated="update_ack"
update_no=0
function init(){
barrier = stigmergy.create(101)
barrier.put(id,1)
barrier_val=0
barrier.onconflict(function (k,l,r) {
if(r. data < l. data or (r. data == l. data )) return l
else return r
})
BVMSTATE = "UPDATESTANDBY"
s = swarm.create(1)
s.join()
# Executed once at init time.
function init(){
barrier = stigmergy.create(101)
barrier.put(id,1)
barrier_val=0
barrier.onconflict(function (k,l,r) {
if(r. data < l. data or (r. data == l. data )) return l
else return r
})
init_swarm()
# start the swarm command listener
nei_cmd_listen()
}
function stand_by(){
barrier.get(id)
barrier_val = barrier.size()
barrier.get(id)
barrier_val = barrier.size()
neighbors.listen(updated,
function(vid, value, rid) {
print(" got from",vid," ", " value = ",value," ","rid"," " )
if(value==update_no) barrier.put(rid,1)
}
neighbors.listen(updated,
function(vid, value, rid) {
if(value==update_no) barrier.put(rid,1)
}
)
}
# Executed at each time step.
function step() {
stand_by()
stand_by()
}

72
buzz_scripts/testLJ.bzz Executable file
View File

@ -0,0 +1,72 @@
include "update.bzz"
# don't use a stigmergy id=11 with this header, for barrier
# it requires an 'action' function to be defined here.
include "act/states.bzz"
include "vstigenv.bzz"
V_TYPE = 0
#State launched after takeoff
AUTO_LAUNCH_STATE = "FORMATION"
TARGET = 8.0
EPSILON = 3.0
GOTO_MAXVEL = 2.5 # m/steps
# Executed once at init time.
function init() {
init_stig()
init_swarm()
# start the swarm command listener
nei_cmd_listen()
# Starting state: TURNEDOFF to wait for user input.
BVMSTATE = "TURNEDOFF"
TAKEOFF_COUNTER = 20
}
# Executed at each time step.
function step() {
rc_cmd_listen()
# update the vstig (status/net/batt/...)
# uav_updatestig()
#
# State machine
#
if(BVMSTATE=="TURNEDOFF")
statef=turnedoff
else if(BVMSTATE=="STOP") # ends on turnedoff
statef=stop
else if(BVMSTATE=="LAUNCH") # ends on AUTO_LAUNCH_STATE
statef=launch
else if(BVMSTATE=="IDLE")
statef=idle
else if(BVMSTATE=="FORMATION")
statef=formation
statef()
log("Current state: ", BVMSTATE)
# Auto-takeoff (delayed for simulator boot)
if(id == 0) {
if(TAKEOFF_COUNTER>0)
TAKEOFF_COUNTER = TAKEOFF_COUNTER - 1
else if(TAKEOFF_COUNTER == 0) {
BVMSTATE="LAUNCH"
TAKEOFF_COUNTER = -1
}
}
}
# Executed once when the robot (or the simulator) is reset.
function reset() {
}
# Executed once at the end of experiment.
function destroy() {
}