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

Merge commit '02f76b8f67eab43d655ac008df19f681410a6238' into ros_drones_ws

This commit is contained in:
dave 2018-11-06 04:03:20 -05:00
parent b119ec7df0 02f76b8f67
commit bc4a50e2fe
6 changed files with 114 additions and 72 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -63,14 +63,14 @@ function barrier_wait(threshold, transf, resumef, bc) {
barrier.put(id, bc)
barrier.get(id)
log("--->BS: ", barrier.size(), " / ", threshold, " (", BARRIER_VSTIG, " - ", barrier.get("d"), ") t= ", timeW)
if(barrier.size()-1 >= threshold or barrier.get("d") == 1) {
#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(timeW >= BARRIER_TIMEOUT) {
log("------> Barrier Timeout !!!!")
@ -86,13 +86,13 @@ function barrier_wait(threshold, transf, resumef, bc) {
# Check all members state
function barrier_allgood(barrier, bc) {
barriergood = 1
barrier.foreach(
function(key, value, robot){
#log("VS entry : ", key, " ", value, " ", robot)
if(value != bc and key != "d"){
barrier.foreach(function(key, value, robot){
log("VS entry : ", key, " ", value, " ", robot)
if(key == "d"){
if(value == 1)
return 1
} else if(value != bc)
barriergood = 0
}
}
)
})
return barriergood
}

61
buzz_scripts/include/act/neighborcomm.bzz
View File

@ -50,15 +50,22 @@ function rc_cmd_listen() {
} else if (flight.rc_cmd==902){
flight.rc_cmd=0
destroyGraph()
barrier_set(ROBOTS, "AGGREGATE", BVMSTATE, 902)
barrier_set(ROBOTS, "WAYPOINT", BVMSTATE, 902)
barrier_ready(902)
neighbors.broadcast("cmd", 902)
} else if (flight.rc_cmd==903){
flight.rc_cmd=0
destroyGraph()
barrier_set(ROBOTS, "FORMATION", BVMSTATE, 903)
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)
}
}
@ -85,22 +92,28 @@ function nei_cmd_listen() {
}else if(value==900){ # Shapes
barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900)
#barrier_ready(900)
neighbors.broadcast("cmd", 900)
} else if(value==901){ # Pursuit
#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){ # Agreggate
#neighbors.broadcast("cmd", 901)
} else if(value==902 and BVMSTATE!="BARRIERWAIT" and BVMSTATE!="WAYPOINT"){ # Waypoint
destroyGraph()
barrier_set(ROBOTS, "AGGREGATE", BVMSTATE, 902)
barrier_set(ROBOTS, "WAYPOINT", BVMSTATE, 902)
#barrier_ready(902)
neighbors.broadcast("cmd", 902)
} else if(value==903){ # Formation
#neighbors.broadcast("cmd", 902)
} else if(value==903 and BVMSTATE!="BARRIERWAIT" and BVMSTATE!="POTENTIAL"){ # Formation
destroyGraph()
barrier_set(ROBOTS, "FORMATION", BVMSTATE, 903)
resetWP()
barrier_set(ROBOTS, "POTENTIAL", BVMSTATE, 903)
#barrier_ready(903)
neighbors.broadcast("cmd", 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)
@ -110,3 +123,29 @@ function nei_cmd_listen() {
#}
})
}
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})
}

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

@ -97,40 +97,23 @@ function stop() {
}
# State functions for individual waypoint control
firsttimeinwp = 1
wpreached = 1
function indiWP() {
if(firsttimeinwp) {
v_wp = stigmergy.create(WP_STIG)
storegoal(pose.position.latitude, pose.position.longitude, pose.position.altitude)
firsttimeinwp = 0
}
BVMSTATE = "INDIWP"
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()
}
}
check_rc_wp()
#if(vstig_buzzy == 0) {
wpi = v_wp.get(id)
if(wpi!=nil) {
wp = unpackWP2i(wpi)
if(wp.pro == 0) {
wpreached = 0
storegoal(wp.lat, wp.lon, pose.position.altitude)
var ls = packWP2i(wp.lat, wp.lon, 1)
v_wp.put(id,ls)
return
}
}
#}
wpi = v_wp.get(id)
if(wpi!=nil) {
wp = unpackWP2i(wpi)
if(wp.pro == 0) {
wpreached = 0
storegoal(wp.lat, wp.lon, pose.position.altitude)
var ls = packWP2i(wp.lat, wp.lon, 1)
v_wp.put(id,ls)
return
}
}
if(wpreached!=1)
goto_gps(indiWP_done)
@ -138,7 +121,7 @@ function indiWP() {
}
function indiWP_done() {
BVMSTATE = "INDIWP"
BVMSTATE = "WAYPOINT"
wpreached = 1
}
@ -208,10 +191,12 @@ function pursuit() {
}
# Lennard-Jones interaction magnitude
TARGET = 8.0
EPSILON = 30.0
TARGET = 16.0
EPSILON = 30.0 #30
GAIN_ATT = 50.0
GAIN_REP = 30.0
function lj_magnitude(dist, target, epsilon) {
lj = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
lj = -(epsilon / dist) * ((target / dist)^4)# - (target / dist)^2) #repulse only to avoid each other
return lj
}
@ -225,14 +210,32 @@ function lj_sum(rid, data, accum) {
return math.vec2.add(data, accum)
}
# Sate function that calculates and actuates the flocking interaction
function formation() {
BVMSTATE="FORMATION"
# State function that calculates and actuates LJ flocking interaction with vstig targets (attractor/repulsors)
function lennardjones() {
BVMSTATE="POTENTIAL"
check_rc_wp()
# 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())
accum_lj = LimitSpeed(accum_lj, 1.0/3.0)
# Add attractor/repulsor effects
log(v_wp.size())
accum_t = math.vec2.new(0.0, 0.0);
v_wp.foreach(
function(key, value, robot){
wp = unpackWP2i(value)
dvec = vec_from_gps(wp.lat, wp.lon, 0)
Tdist = math.vec2.length(dvec)
if(key > 99 and Tdist < 40)
accum_t = math.vec2.sub(accum_t, math.vec2.newp(GAIN_REP*(TARGET/Tdist)^4, math.vec2.angle(dvec)))
else if(key > 49 and Tdist > 10)
accum_t = math.vec2.add(accum_t, math.vec2.newp(GAIN_ATT*(TARGET/Tdist)^2, math.vec2.angle(dvec)))
})
if(v_wp.size() > 0)
accum_t = math.vec2.scale(accum_t, 1.0 / v_wp.size())
#log(math.vec2.length(accum_t),math.vec2.length(accum_lj))
accum_lj = LimitSpeed(math.vec2.add(accum_t,accum_lj), 1.0) #1/3
goto_abs(accum_lj.x, accum_lj.y, 0.0, 0.0)
}

6
buzz_scripts/include/utils/table.bzz
View File

@ -55,7 +55,7 @@ function i2s(value){
return "LAUNCH"
}
else if(value==9){
return "STOP"
return "FORMATION"
}
else {
return "UNDETERMINED"
@ -80,7 +80,7 @@ function s2i(value){
else if(value=="BARRIERWAIT"){
return 5
}
else if(value=="INDIWP"){
else if(value=="WAYPOINT"){
return 6
}
else if(value=="TASK_ALLOCATE"){
@ -89,7 +89,7 @@ function s2i(value){
else if(value=="LAUNCH"){
return 8
}
else if(value=="STOP"){
else if(value=="POTENTIAL"){
return 9
}
else

16
buzz_scripts/include/utils/takeoff_heights.bzz
View File

@ -1,12 +1,12 @@
takeoff_heights ={
.0 = 30.0,
.1 = 21.0,
.2 = 18.0,
.3 = 12.0,
.4 = 28.0,
.5 = 12.0,
.6 = 3.0,
.9 = 15.0,
.0 = 0.0,
.1 = 32.0,
.2 = 28.0,
.3 = 24.0,
.4 = 20.0,
.5 = 16.0,
.6 = 12.0,
.9 = 8.0,
.11 = 6.0,
.14 = 9.0,
.16 = 9.0,

6
buzz_scripts/main.bzz
View File

@ -12,7 +12,7 @@ include "utils/takeoff_heights.bzz"
#State launched after takeoff
AUTO_LAUNCH_STATE = "INDIWP"
AUTO_LAUNCH_STATE = "IDLE"
TARGET = 9.0
EPSILON = 30.0
ROOT_ID = 3
@ -78,8 +78,8 @@ function step() {
statef=idle
else if(BVMSTATE=="AGGREGATE")
statef=aggregate
else if(BVMSTATE=="FORMATION")
statef=formation
else if(BVMSTATE=="POTENTIAL")
statef=lennardjones
else if(BVMSTATE=="PURSUIT")
statef=pursuit
else if(BVMSTATE=="TASK_ALLOCATE") # or bidding ?