420 lines
12 KiB
Plaintext
420 lines
12 KiB
Plaintext
########################################
|
|
#
|
|
# FLIGHT-RELATED FUNCTIONS
|
|
#
|
|
########################################
|
|
include "utils/vec2.bzz"
|
|
include "act/barrier.bzz"
|
|
include "utils/conversions.bzz"
|
|
include "utils/quickhull.bzz"
|
|
include "act/naviguation.bzz"
|
|
include "act/CA.bzz"
|
|
include "act/neighborcomm.bzz"
|
|
include "taskallocate/discomp.bzz"
|
|
|
|
TARGET_ALTITUDE = 15.0 # m.
|
|
BVMSTATE = "TURNEDOFF"
|
|
PICTURE_WAIT = 20 # steps
|
|
WP_STIG = 8
|
|
|
|
path_it = 0
|
|
pic_time = 0
|
|
g_it = 0
|
|
homegps = {}
|
|
|
|
YOLO_TARGET = 1
|
|
YOLO_NEI_TARGET = 0
|
|
yolo_init = 0
|
|
yolo_left_buff = 0
|
|
yolo_right_buff = 0
|
|
YOLO_L_GOAL_1 = math.vec2.new(20,0,0)
|
|
YOLO_L_GOAL_2 = math.vec2.new(-20,0,0)
|
|
YOLO_GPS_1 = {.lat =0, .long=0}
|
|
YOLO_GPS_2 = {.lat =0, .long=0}
|
|
|
|
# Core state function when on the ground
|
|
function turnedoff() {
|
|
BVMSTATE = "TURNEDOFF"
|
|
}
|
|
|
|
# Core state function when hovering
|
|
function idle() {
|
|
BVMSTATE = "IDLE"
|
|
}
|
|
|
|
# yolo Demostration
|
|
function yolo_demo(){
|
|
var left_count = 0
|
|
var right_count = 0
|
|
var img_bound_x={.l=0,.r=320} # assumed left image pixel x boundary
|
|
var img_bound_y={.t=0,.b=480} # assumed left image pixel y boundary
|
|
if(yolo_init == 0){
|
|
yolo_init = 1
|
|
yolo_gps_in = {.latitude=pose.position.latitude, .longitude=pose.position.longitude, .altitude= pose.position.altitude}
|
|
yolo_left_buff = 0
|
|
yolo_right_buff = 0
|
|
YOLO_GPS_1 = gps_from_vec(YOLO_L_GOAL_1)
|
|
YOLO_GPS_2 = gps_from_vec(YOLO_L_GOAL_2)
|
|
}
|
|
#log("POS lat : ",pose.position.latitude, " long: ", pose.position.longitude )
|
|
if(yolo_boxes != nil){
|
|
# log("yolo tab size",size(yolo_boxes))
|
|
var yol_i = 0
|
|
while(yol_i < yolo_boxes.size){
|
|
var table_id = string.tostring(yol_i)
|
|
if(yolo_boxes[table_id].class == "person"){
|
|
var diff_x = (yolo_boxes[table_id].xmax - yolo_boxes[table_id].xmin) / 2
|
|
var mid_x = yolo_boxes[table_id].xmax - diff_x
|
|
if( (mid_x >=img_bound_x.l and mid_x <=img_bound_x.r)){
|
|
# person is in the left side of the image
|
|
left_count = left_count + 1
|
|
}
|
|
else{
|
|
# person is in the right side of the image
|
|
right_count = right_count + 1
|
|
}
|
|
}
|
|
yol_i = yol_i + 1
|
|
}
|
|
# log("left_count: ", left_count, " right_count: ", right_count)
|
|
if(left_count > right_count){
|
|
if(1){
|
|
YOLO_TARGET = 1
|
|
neighbors.broadcast("cmd", 555)
|
|
#storegoal(YOLO_GPS_1.latitude, YOLO_GPS_1.longitude, pose.position.altitude)
|
|
yolo_gps_in = {.latitude=YOLO_GPS_1.latitude, .longitude=YOLO_GPS_1.longitude, .altitude= pose.position.altitude}
|
|
# log("left dominating")
|
|
yolo_left_buff = 0
|
|
yolo_right_buff = 0
|
|
}
|
|
else{
|
|
yolo_left_buff = yolo_left_buff + 1
|
|
yolo_right_buff = 0
|
|
}
|
|
}
|
|
else if(left_count < right_count){
|
|
if(1){
|
|
YOLO_TARGET = 2
|
|
neighbors.broadcast("cmd", 556)
|
|
yolo_gps_in = {.latitude=YOLO_GPS_2.latitude, .longitude=YOLO_GPS_2.longitude, .altitude= pose.position.altitude}
|
|
# log("right dominating")
|
|
yolo_right_buff = 0
|
|
yolo_left_buff = 0
|
|
}
|
|
else{
|
|
yolo_right_buff = yolo_right_buff + 1
|
|
yolo_left_buff = 0
|
|
}
|
|
}
|
|
goto_gps_in(yolo_demo, yolo_gps_in)
|
|
}
|
|
if( YOLO_NEI_TARGET == 1){
|
|
# YOLO_NEI_TARGET =0
|
|
YOLO_TARGET = 1
|
|
yolo_gps_in = {.latitude=YOLO_GPS_1.latitude, .longitude=YOLO_GPS_1.longitude, .altitude= pose.position.altitude}
|
|
# log("left dominating")
|
|
goto_gps_in(yolo_demo, yolo_gps_in)
|
|
}
|
|
else if(YOLO_NEI_TARGET == 2){
|
|
YOLO_TARGET = 2
|
|
# YOLO_NEI_TARGET = 0
|
|
yolo_gps_in = {.latitude=YOLO_GPS_2.latitude, .longitude=YOLO_GPS_2.longitude, .altitude= pose.position.altitude}
|
|
# log("right dominating")
|
|
goto_gps_in(yolo_demo, yolo_gps_in)
|
|
}
|
|
}
|
|
|
|
|
|
# Core state function to launch the robot: takeoff and wait for others, or stop (land)
|
|
function launch() {
|
|
BVMSTATE = "LAUNCH"
|
|
neighbors.broadcast("cmd", 22)
|
|
log("AUTO_LAUNCH_STATE: ", AUTO_LAUNCH_STATE)
|
|
if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so TAKE_OFF
|
|
homegps = {.lat=pose.position.latitude, .lng=pose.position.longitude}
|
|
log("Recorded home point: ",homegps.lat, homegps.lng)
|
|
if(pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
|
|
barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "STOP", 22)
|
|
barrier_ready(22)
|
|
} else {
|
|
log("Altitude: ", pose.position.altitude)
|
|
uav_takeoff(TARGET_ALTITUDE)
|
|
}
|
|
} else {
|
|
barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "STOP", 22)
|
|
barrier_ready(22)
|
|
}
|
|
}
|
|
|
|
# Launch function version without the timeout and stop state.
|
|
function launch_switch() {
|
|
BVMSTATE = "LAUNCH_SWITCH"
|
|
if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so TAKE_OFF
|
|
homegps = {.lat=pose.position.latitude, .lng=pose.position.longitude}
|
|
if(pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
|
|
barrier_set(ROBOTS, AUTO_LAUNCH_STATE, AUTO_LAUNCH_STATE, 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, AUTO_LAUNCH_STATE, 22)
|
|
barrier_ready(22)
|
|
}
|
|
}
|
|
|
|
gohomeT=100
|
|
# State function to go back to ROSBuzz recorded home GPS position (at takeoff)
|
|
function goinghome() {
|
|
BVMSTATE = "GOHOME"
|
|
if(V_TYPE == 0 or V_TYPE == 1) { # vehicle
|
|
storegoal(homegps.lat, homegps.lng, pose.position.altitude)
|
|
goto_gps(str2fct(AUTO_LAUNCH_STATE))
|
|
} else
|
|
BVMSTATE = AUTO_LAUNCH_STATE
|
|
}
|
|
|
|
# Core state function to stop and land.
|
|
function stop() {
|
|
BVMSTATE = "STOP"
|
|
neighbors.broadcast("cmd", 21)
|
|
if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so LAND
|
|
uav_land()
|
|
if(pose.position.altitude <= 0.2) {
|
|
BVMSTATE = "STOP"
|
|
barrier_set(ROBOTS,"TURNEDOFF","STOP", 21)
|
|
barrier_ready(21)
|
|
}
|
|
} else {
|
|
BVMSTATE = "STOP"
|
|
barrier_set(ROBOTS,"TURNEDOFF","STOP", 21)
|
|
barrier_ready(21)
|
|
}
|
|
}
|
|
|
|
# State functions for individual waypoint control
|
|
|
|
wpreached = 1
|
|
function indiWP() {
|
|
BVMSTATE = "WAYPOINT"
|
|
check_rc_wp()
|
|
|
|
wpi = v_wp.get(id)
|
|
if(wpi!=nil) {
|
|
wp = unpackWP2i(wpi)
|
|
if(wp.pro == 0) {
|
|
wpreached = 0
|
|
storegoal(wp.lat, wp.lng, pose.position.altitude)
|
|
var ls = packWP2i(wp.lat, wp.lng, 1)
|
|
v_wp.put(id,ls)
|
|
return
|
|
}
|
|
}
|
|
|
|
if(wpreached!=1)
|
|
goto_gps(indiWP_done)
|
|
|
|
}
|
|
|
|
function indiWP_done() {
|
|
BVMSTATE = "WAYPOINT"
|
|
wpreached = 1
|
|
}
|
|
|
|
# State function to take a picture from the camera server (developed by HS)
|
|
function take_picture() {
|
|
BVMSTATE="PICTURE"
|
|
setgimbal(0.0, 0.0, -90.0, 20.0)
|
|
if(pic_time==PICTURE_WAIT/2) { # wait for the drone to stabilize
|
|
takepicture()
|
|
} else if(pic_time>=PICTURE_WAIT) { # wait for the picture
|
|
BVMSTATE="IDLE"
|
|
pic_time=0
|
|
}
|
|
pic_time=pic_time+1
|
|
}
|
|
|
|
# State function to follow a moving attractor (GPS sent from a user phone)
|
|
function follow() {
|
|
if(size(targets)>0) {
|
|
BVMSTATE = "FOLLOW"
|
|
attractor=math.vec2.newp(0,0)
|
|
foreach(targets, function(id, tab) {
|
|
force=(0.05)*(tab.range)^4
|
|
attractor=math.vec2.add(attractor, math.vec2.newp(force, tab.bearing))
|
|
})
|
|
goto_abs(attractor.x, attractor.y, 0.0, 0.0)
|
|
} else {
|
|
log("No target in local table!")
|
|
BVMSTATE = "IDLE"
|
|
}
|
|
}
|
|
|
|
# State function to converge to centroid
|
|
function aggregate() {
|
|
BVMSTATE="AGGREGATE"
|
|
centroid = neighbors.reduce(function(rid, data, centroid) {
|
|
centroid = math.vec2.add(centroid, math.vec2.newp(data.distance, data.azimuth))
|
|
return centroid
|
|
}, {.x=0, .y=0})
|
|
if(neighbors.count() > 0)
|
|
centroid = math.vec2.scale(centroid, 1.0 / neighbors.count())
|
|
cmdbin = math.vec2.sub(centroid, math.vec2.newp(3.0, id * 2 * math.pi / ROBOTS))
|
|
cmdbin = LimitSpeed(cmdbin, 1.0/2.0)
|
|
goto_abs(cmdbin.x, cmdbin.y, 0.0, 0.0)
|
|
}
|
|
|
|
# State fucntion to circle all together around centroid
|
|
function pursuit() {
|
|
BVMSTATE="PURSUIT"
|
|
rd = 30.0
|
|
pc = 3.2
|
|
vd = 15.0
|
|
r_vec = neighbors.reduce(function(rid, data, r_vec) {
|
|
r_vec = math.vec2.add(r_vec, math.vec2.newp(data.distance, data.azimuth))
|
|
return r_vec
|
|
}, {.x=0, .y=0})
|
|
if(neighbors.count() > 0)
|
|
r_vec = math.vec2.scale(r_vec, 1.0 / neighbors.count())
|
|
r = math.vec2.length(r_vec)
|
|
var sqr = (r-rd)*(r-rd)+pc*pc*r*r
|
|
gamma = vd / math.sqrt(sqr)
|
|
dr = -gamma * (r-rd)
|
|
dT = gamma * pc
|
|
vfg = math.vec2.newp(r+dr*0.1, math.vec2.angle(r_vec)+dT*0.1)
|
|
vfg = LimitSpeed(vfg, 2.0)
|
|
goto_abs(vfg.x, vfg.y, 0.0, 0.0)
|
|
}
|
|
|
|
# Lennard-Jones interaction magnitude
|
|
TARGET = 8.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) #repulse only to avoid each other
|
|
return lj
|
|
}
|
|
|
|
# Neighbor data to LJ interaction vector
|
|
function lj_vector(rid, data) {
|
|
return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth)
|
|
}
|
|
|
|
# Accumulator of neighbor LJ interactions
|
|
function lj_sum(rid, data, accum) {
|
|
return math.vec2.add(data, accum)
|
|
}
|
|
|
|
# State function that calculates and actuates LJ flocking interaction with vstig targets (attractor/repulsors)
|
|
function lennardjones() {
|
|
BVMSTATE="POTENTIAL"
|
|
check_rc_wp()
|
|
if(V_TYPE == 2){ # NOT MOVING!
|
|
return
|
|
}
|
|
# 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())
|
|
# 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)
|
|
}
|
|
|
|
# State function that calculates and actuates Voronoi Centroidal tessellation coverage (attractor/repulsors)
|
|
counter = 0
|
|
function voronoicentroid() {
|
|
BVMSTATE="DEPLOY"
|
|
check_rc_wp()
|
|
wptab = v_wp.get(WPtab_id)
|
|
if(wptab==nil){
|
|
return
|
|
}
|
|
else if(not(size(wptab) > 0)){
|
|
return
|
|
}
|
|
log("WP table size:", size(wptab))
|
|
if(V_TYPE == 2){
|
|
return
|
|
}
|
|
it_pts = 0
|
|
att = {}
|
|
foreach(wptab, function(key, value){
|
|
wp = unpackWP2i(value)
|
|
if(key > 99)
|
|
log("Nothing planed for the repulsors yet....")
|
|
else if(key > 49)
|
|
att[it_pts]=vec_from_gps(wp.lat, wp.lng, 0)
|
|
it_pts = it_pts + 1
|
|
})
|
|
# Boundaries from Geofence
|
|
#it_pts = 0
|
|
#foreach(GPSlimit, function(key, value) {
|
|
# bounds[it_pts]=vec_from_gps(value.lat, value.lng, 0)
|
|
# it_pts = it_pts + 1
|
|
#})
|
|
# Boundaries from user attractors
|
|
#att = {.0=vec_from_gps(45.510283, -73.609633, 0), .1=vec_from_gps(45.510398, -73.609281, 0)}
|
|
bounds = QuickHull(att)
|
|
if(size(bounds)<3 ){
|
|
return
|
|
}
|
|
if(counter==0) {
|
|
pts = {.np=size(bounds)}
|
|
it_pts = 0
|
|
foreach(bounds, function(key, value) {
|
|
pts[it_pts]=value
|
|
it_pts = it_pts + 1
|
|
})
|
|
pts[it_pts] = {.x=0, .y=0} #add itself
|
|
it_pts = it_pts + 1
|
|
if(neighbors.count() > 0) {
|
|
neighbors.foreach(function(rid, data) {
|
|
if(rid!=0){ #remove GS (?)
|
|
pts[it_pts]=math.vec2.newp(data.distance,data.azimuth)
|
|
it_pts = it_pts + 1
|
|
}
|
|
})
|
|
#table_print(pts)
|
|
voronoi(pts)
|
|
}
|
|
counter=4
|
|
}
|
|
counter=counter-1
|
|
|
|
goto_gps(voronoicentroid_done)
|
|
}
|
|
|
|
function voronoicentroid_done() {
|
|
BVMSTATE="DEPLOY"
|
|
}
|
|
|
|
# Custom state function for the developer to play with
|
|
firstincus = 1
|
|
function cusfun(){
|
|
BVMSTATE="CUSFUN"
|
|
if(firstincus) {
|
|
discomp_init()
|
|
firstincus = 0
|
|
}
|
|
discomp_step()
|
|
} |