Merge commit 'fd8e8022744c30cb9d59b1dd4cb6cf90f1c728f8' into ros_drones_ws
This commit is contained in:
dave
commit
4b293afac5
|
|
@ -0,0 +1,45 @@
|
|||
function LCA(vel_vec) {
|
||||
var safety_radius = 3.0
|
||||
var default_velocity = 2.0
|
||||
collide = 0
|
||||
|
||||
var k_v = 5 # velocity gain
|
||||
var k_w = 10 # angular velocity gain
|
||||
|
||||
cart = neighbors.map(
|
||||
function(rid, data) {
|
||||
var c = {}
|
||||
c.distance = data.distance
|
||||
c.azimuth = data.azimuth
|
||||
if (c.distance < (safety_radius * 2.0) )
|
||||
collide = 1
|
||||
return c
|
||||
})
|
||||
if (collide) {
|
||||
log("")
|
||||
log("------> AVOIDING NEIGHBOR! <------")
|
||||
log("")
|
||||
result = cart.reduce(function(rid, data, accum) {
|
||||
if(data.distance < accum.distance and data.distance > 0.0){
|
||||
accum.distance = data.distance
|
||||
accum.angle = data.azimuth
|
||||
return accum
|
||||
}
|
||||
return accum
|
||||
}, {.distance= safety_radius * 2.0, .angle= 0.0})
|
||||
|
||||
d_i = result.distance
|
||||
data_alpha_i = result.angle
|
||||
|
||||
penalty = math.exp(d_i - safety_radius)
|
||||
if( math.cos(math.vec2.angle(vel_vec)) < 0.0){
|
||||
penalty = math.exp(-(d_i - safety_radius))
|
||||
}
|
||||
|
||||
V = math.vec2.length(vel_vec) - math.cos(math.vec2.angle(vel_vec)) * penalty * k_v
|
||||
W = -k_w * penalty * math.sin( math.vec2.angle(vel_vec) )
|
||||
|
||||
return math.vec2.newp(V, W)
|
||||
} else
|
||||
return vel_vec
|
||||
}
|
||||
|
|
@ -0,0 +1,26 @@
|
|||
GOTO_MAXVEL = 1.5 # m/steps
|
||||
GOTO_MAXDIST = 150 # m.
|
||||
GOTODIST_TOL = 0.4 # m.
|
||||
GOTOANG_TOL = 0.1 # rad.
|
||||
|
||||
# Core naviguation function to travel to a GPS target location.
|
||||
function goto_gps(transf) {
|
||||
m_navigation = vec_from_gps(cur_goal.latitude, cur_goal.longitude, 0)
|
||||
#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 (", math.vec2.length(m_navigation), " / ", GOTO_MAXDIST, " )")
|
||||
else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL){ # reached destination
|
||||
if(transf!=nil)
|
||||
transf()
|
||||
} else {
|
||||
m_navigation = LimitSpeed(m_navigation, 1.0)
|
||||
#m_navigation = LCA(m_navigation)
|
||||
goto_abs(m_navigation.x, m_navigation.y, cur_goal.altitude - pose.position.altitude, 0.0)
|
||||
}
|
||||
}
|
||||
|
||||
function LimitSpeed(vel_vec, factor){
|
||||
if(math.vec2.length(vel_vec)>GOTO_MAXVEL*factor)
|
||||
vel_vec = math.vec2.scale(vel_vec, GOTO_MAXVEL*factor/math.vec2.length(vel_vec))
|
||||
return vel_vec
|
||||
}
|
||||
|
|
@ -0,0 +1,129 @@
|
|||
# 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, "AGGREGATE", 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)
|
||||
barrier_ready(903)
|
||||
neighbors.broadcast("cmd", 903)
|
||||
}
|
||||
}
|
||||
|
||||
# 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){ # 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
|
||||
# })
|
||||
}
|
||||
#}
|
||||
})
|
||||
}
|
||||
|
||||
# broadcast GPS goals
|
||||
function bd_goal() {
|
||||
neighbors.broadcast("goal", {.id=rc_goto.id, .la=rc_goto.latitude, .lo=rc_goto.longitude, .al=rc_goto.altitude})
|
||||
}
|
||||
|
||||
# listens to neighbors broadcasting gps goals
|
||||
function nei_goal_listen() {
|
||||
neighbors.listen("goal",
|
||||
function(vid, value, rid) {
|
||||
print("Got (", vid, ",", value.id, value.la, value.lo, ") #", rid)
|
||||
if(value.id==id) {
|
||||
print("Got (", vid, ",", value, ") #", rid)
|
||||
storegoal(value.la, value.lo, pose.position.altitude)
|
||||
}
|
||||
})
|
||||
}
|
||||
|
|
@ -5,45 +5,30 @@
|
|||
########################################
|
||||
include "utils/vec2.bzz"
|
||||
include "act/barrier.bzz"
|
||||
#include "act/old_barrier.bzz"
|
||||
include "utils/conversions.bzz"
|
||||
include "act/naviguation.bzz"
|
||||
include "act/CA.bzz"
|
||||
include "act/neighborcomm.bzz"
|
||||
|
||||
TARGET_ALTITUDE = 15.0 # m.
|
||||
BVMSTATE = "TURNEDOFF"
|
||||
PICTURE_WAIT = 20 # steps
|
||||
|
||||
GOTO_MAXVEL = 1.5 # m/steps
|
||||
GOTO_MAXDIST = 150 # m.
|
||||
GOTODIST_TOL = 0.4 # m.
|
||||
GOTOANG_TOL = 0.1 # rad.
|
||||
path_it = 0
|
||||
pic_time = 0
|
||||
g_it = 0
|
||||
|
||||
# Core state function when on the ground
|
||||
function turnedoff() {
|
||||
BVMSTATE = "TURNEDOFF"
|
||||
}
|
||||
|
||||
# Core state function when hovering
|
||||
function idle() {
|
||||
BVMSTATE = "IDLE"
|
||||
}
|
||||
|
||||
|
||||
# Custom function for the user.
|
||||
function cusfun(){
|
||||
BVMSTATE="CUSFUN"
|
||||
log("cusfun: yay!!!")
|
||||
LOCAL_TARGET = math.vec2.new(5 ,0 )
|
||||
local_set_point = math.vec2.new(LOCAL_TARGET.x - pose.position.x ,LOCAL_TARGET.y - pose.position.y ) # has to move 5 meters in x from the home location
|
||||
if(math.vec2.length(local_set_point) > GOTODIST_TOL){
|
||||
local_set_point = LimitSpeed(local_set_point, 1.0)
|
||||
goto_abs(local_set_point.x, local_set_point.y, 0.0, 0.0)
|
||||
}
|
||||
else{
|
||||
log("TARGET REACHED")
|
||||
}
|
||||
}
|
||||
|
||||
# Core state function to launch the robot: takeoff and wait for others, or stop (land)
|
||||
function launch() {
|
||||
BVMSTATE = "LAUNCH"
|
||||
if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so TAKE_OFF
|
||||
|
|
@ -62,6 +47,7 @@ function launch() {
|
|||
}
|
||||
}
|
||||
|
||||
# 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
|
||||
|
|
@ -81,6 +67,7 @@ function launch_switch() {
|
|||
}
|
||||
|
||||
gohomeT=100
|
||||
# State function to go back to ROSBuzz recorded home GPS position (at takeoff)
|
||||
function goinghome() {
|
||||
BVMSTATE = "GOHOME"
|
||||
if(gohomeT > 0) { # TODO: Make a real check if home is reached
|
||||
|
|
@ -90,6 +77,7 @@ function goinghome() {
|
|||
BVMSTATE = AUTO_LAUNCH_STATE
|
||||
}
|
||||
|
||||
# Core state function to stop and land.
|
||||
function stop() {
|
||||
BVMSTATE = "STOP"
|
||||
if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so LAND
|
||||
|
|
@ -107,6 +95,28 @@ function stop() {
|
|||
}
|
||||
}
|
||||
|
||||
# State function for individual waypoint control
|
||||
firsttimeinwp = 1
|
||||
function indiWP() {
|
||||
if(firsttimeinwp) {
|
||||
nei_goal_listen()
|
||||
storegoal(pose.position.latitude, pose.position.longitude, pose.position.altitude)
|
||||
firsttimeinwp = 0
|
||||
}
|
||||
BVMSTATE = "INDIWP"
|
||||
if(rc_goto.id != -1) {
|
||||
log(rc_goto.id)
|
||||
if(rc_goto.id == id) {
|
||||
storegoal(rc_goto.latitude, rc_goto.longitude, pose.position.altitude)
|
||||
} else
|
||||
bd_goal()
|
||||
}
|
||||
|
||||
goto_gps(nil)
|
||||
|
||||
}
|
||||
|
||||
# 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)
|
||||
|
|
@ -119,66 +129,7 @@ function take_picture() {
|
|||
pic_time=pic_time+1
|
||||
}
|
||||
|
||||
function goto_gps(transf) {
|
||||
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))
|
||||
if(math.vec2.length(m_navigation)>GOTO_MAXDIST)
|
||||
log("Sorry this is too far (", math.vec2.length(m_navigation), " / ", GOTO_MAXDIST, " )")
|
||||
else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination
|
||||
transf()
|
||||
else {
|
||||
m_navigation = LimitSpeed(m_navigation, 1.0)
|
||||
#m_navigation = LCA(m_navigation)
|
||||
goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0)
|
||||
}
|
||||
}
|
||||
|
||||
function LCA(vel_vec) {
|
||||
var safety_radius = 3.0
|
||||
var default_velocity = 2.0
|
||||
collide = 0
|
||||
|
||||
var k_v = 5 # velocity gain
|
||||
var k_w = 10 # angular velocity gain
|
||||
|
||||
cart = neighbors.map(
|
||||
function(rid, data) {
|
||||
var c = {}
|
||||
c.distance = data.distance
|
||||
c.azimuth = data.azimuth
|
||||
if (c.distance < (safety_radius * 2.0) )
|
||||
collide = 1
|
||||
return c
|
||||
})
|
||||
if (collide) {
|
||||
log("")
|
||||
log("------> AVOIDING NEIGHBOR! <------")
|
||||
log("")
|
||||
result = cart.reduce(function(rid, data, accum) {
|
||||
if(data.distance < accum.distance and data.distance > 0.0){
|
||||
accum.distance = data.distance
|
||||
accum.angle = data.azimuth
|
||||
return accum
|
||||
}
|
||||
return accum
|
||||
}, {.distance= safety_radius * 2.0, .angle= 0.0})
|
||||
|
||||
d_i = result.distance
|
||||
data_alpha_i = result.angle
|
||||
|
||||
penalty = math.exp(d_i - safety_radius)
|
||||
if( math.cos(math.vec2.angle(vel_vec)) < 0.0){
|
||||
penalty = math.exp(-(d_i - safety_radius))
|
||||
}
|
||||
|
||||
V = math.vec2.length(vel_vec) - math.cos(math.vec2.angle(vel_vec)) * penalty * k_v
|
||||
W = -k_w * penalty * math.sin( math.vec2.angle(vel_vec) )
|
||||
|
||||
return math.vec2.newp(V, W)
|
||||
} else
|
||||
return vel_vec
|
||||
}
|
||||
|
||||
# State function to follow a moving attractor (GPS sent from a user phone)
|
||||
function follow() {
|
||||
if(size(targets)>0) {
|
||||
BVMSTATE = "FOLLOW"
|
||||
|
|
@ -194,7 +145,7 @@ function follow() {
|
|||
}
|
||||
}
|
||||
|
||||
# converge to centroid
|
||||
# State function to converge to centroid
|
||||
function aggregate() {
|
||||
BVMSTATE="AGGREGATE"
|
||||
centroid = neighbors.reduce(function(rid, data, centroid) {
|
||||
|
|
@ -208,7 +159,7 @@ function aggregate() {
|
|||
goto_abs(cmdbin.x, cmdbin.y, 0.0, 0.0)
|
||||
}
|
||||
|
||||
# circle all together around centroid
|
||||
# State fucntion to circle all together around centroid
|
||||
function pursuit() {
|
||||
BVMSTATE="PURSUIT"
|
||||
rd = 20.0
|
||||
|
|
@ -248,7 +199,7 @@ function lj_sum(rid, data, accum) {
|
|||
return math.vec2.add(data, accum)
|
||||
}
|
||||
|
||||
# Calculates and actuates the flocking interaction
|
||||
# Sate function that calculates and actuates the flocking interaction
|
||||
function formation() {
|
||||
BVMSTATE="FORMATION"
|
||||
# Calculate accumulator
|
||||
|
|
@ -259,115 +210,17 @@ function formation() {
|
|||
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) {
|
||||
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, "AGGREGATE", 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)
|
||||
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(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){ # 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
|
||||
# })
|
||||
}
|
||||
#}
|
||||
})
|
||||
# Custom state function for the developer to play with
|
||||
function cusfun(){
|
||||
BVMSTATE="CUSFUN"
|
||||
log("cusfun: yay!!!")
|
||||
LOCAL_TARGET = math.vec2.new(5 ,0 )
|
||||
local_set_point = math.vec2.new(LOCAL_TARGET.x - pose.position.x ,LOCAL_TARGET.y - pose.position.y ) # has to move 5 meters in x from the home location
|
||||
if(math.vec2.length(local_set_point) > GOTODIST_TOL){
|
||||
local_set_point = LimitSpeed(local_set_point, 1.0)
|
||||
goto_abs(local_set_point.x, local_set_point.y, 0.0, 0.0)
|
||||
}
|
||||
else{
|
||||
log("TARGET REACHED")
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,639 @@
|
|||
|
||||
|
||||
########################################
|
||||
#
|
||||
# GLOBAL VARIABLES / PARAMETERS
|
||||
#
|
||||
########################################
|
||||
|
||||
CSV_FILENAME_AND_PATH = "/home/amber/rosbuzz-coverage/rosbuzz-coverage/waypoints/waypoints_15.csv"
|
||||
OUTPUT_FILENAME_AND_PATH = "/home/amber/bidding_output/output-" # automatically completed with 'ID.csv'
|
||||
BID_WAIT = 40
|
||||
PICTURE_WAIT = 40
|
||||
BASE_ALTITUDE = 5.0
|
||||
OFFSET_LAT = 0.0 # Switzerland (CSV) to MTL (ROSBuzz)
|
||||
OFFSET_LON = 0.0 # Switzerland (CSV) to MTL (ROSBuzz)
|
||||
#
|
||||
waypoints = {}
|
||||
#
|
||||
highest_bid = -2
|
||||
highest_area = -2
|
||||
#
|
||||
bid_made = 0
|
||||
bidded_area = -1
|
||||
bid_time = 999999
|
||||
picture_time = 999999
|
||||
#
|
||||
current_area_wp_order = {}
|
||||
current_area_wp_number = 0
|
||||
current_area_wp_index = 0
|
||||
|
||||
########################################
|
||||
#
|
||||
# UTILITY FUNCTIONS
|
||||
#
|
||||
########################################
|
||||
|
||||
# function name: read_from_csv
|
||||
# description: read a csv file (with header 'area,type,latitude,longitude,altitude,IMG_XXXX.JPG') containing the list of waypoints
|
||||
# inputs: a string with the full path and filename of the csv to read
|
||||
# output: n/a, the function writes in the global variable 'waypoints'
|
||||
function read_from_csv(s) {
|
||||
var csv_file=io.fopen(s, "r")
|
||||
csv_entry = 0
|
||||
csv_area_counter = 0
|
||||
csv_area_id = -1
|
||||
csv_wp_counter = 0
|
||||
io.fforeach(csv_file, function(line) {
|
||||
csv_wp_counter = csv_wp_counter + 1
|
||||
var csv_line_length = string.length(line)
|
||||
var csv_value_begin_i = 0
|
||||
var csv_scanner_i = 0
|
||||
var csv_column = 0
|
||||
|
||||
while (csv_scanner_i < csv_line_length) {
|
||||
if (string.sub(line, csv_scanner_i, csv_scanner_i+1) == ',') {
|
||||
waypoints[csv_entry] = string.tofloat(string.sub(line, csv_value_begin_i, csv_scanner_i))
|
||||
if (csv_column == 0) {
|
||||
if (waypoints[csv_entry] != csv_area_id) {
|
||||
csv_area_id = waypoints[csv_entry]
|
||||
csv_area_counter = csv_area_counter + 1
|
||||
}
|
||||
}
|
||||
csv_entry = csv_entry + 1
|
||||
csv_value_begin_i = csv_scanner_i + 1
|
||||
csv_column = csv_column + 1
|
||||
}
|
||||
csv_scanner_i = csv_scanner_i + 1
|
||||
}
|
||||
waypoints[csv_entry] = string.sub(line, csv_value_begin_i, csv_scanner_i)
|
||||
csv_entry = csv_entry + 1
|
||||
})
|
||||
io.fclose(csv_file)
|
||||
NUM_AREAS = csv_area_counter
|
||||
NUM_WP = csv_wp_counter
|
||||
}
|
||||
|
||||
# function name: table_print
|
||||
# description: printout the content of a dictionary
|
||||
# inputs: the dictionary to print out
|
||||
# output: n/a, the function print to terminal
|
||||
function table_print(t) {
|
||||
foreach(t, function(key, value) {
|
||||
log(key, " -> ", value)
|
||||
})
|
||||
}
|
||||
|
||||
# function name: wp_area
|
||||
# description: obtain the area associated to a waypoint
|
||||
# inputs: the waypoint number
|
||||
# output: the area that the waypoint belongs to
|
||||
function wp_area(wp_number) {
|
||||
return waypoints[wp_number * 6 + 0]
|
||||
}
|
||||
|
||||
# function name: wp_type
|
||||
# description: obtain the type associated to a waypoint
|
||||
# inputs: the waypoint number
|
||||
# output: the type of the waypoint
|
||||
function wp_type(wp_number) {
|
||||
return waypoints[wp_number * 6 + 1]
|
||||
}
|
||||
|
||||
# function name: wp_lat
|
||||
# description: obtain the latitude of a waypoint
|
||||
# inputs: the waypoint number
|
||||
# output: the latitude of the waypoint
|
||||
function wp_lat(wp_number) {
|
||||
return waypoints[wp_number * 6 + 2] - OFFSET_LAT # transform to MISTLab's ROSBuzz coordinates
|
||||
}
|
||||
|
||||
# function name: wp_lon
|
||||
# description: obtain the longitude of a waypoint
|
||||
# inputs: the waypoint number
|
||||
# output: the longitude of the waypoint
|
||||
function wp_lon(wp_number) {
|
||||
return waypoints[wp_number * 6 + 3] - OFFSET_LON # transform to MISTLab's ROSBuzz coordinates
|
||||
}
|
||||
|
||||
# function name: wp_alt
|
||||
# description: obtain the altitude of a waypoint
|
||||
# inputs: the waypoint number
|
||||
# output: the altitude of the waypoint
|
||||
function wp_alt(wp_number) {
|
||||
return waypoints[wp_number * 6 + 4]
|
||||
}
|
||||
|
||||
# function name: wp_filename
|
||||
# description: obtain the filname of a waypoint's image
|
||||
# inputs: the waypoint number
|
||||
# output: the filename of the image taken at the waypoint
|
||||
function wp_filename(wp_number) {
|
||||
return waypoints[wp_number * 6 + 5]
|
||||
}
|
||||
|
||||
# function name: distance_from_gps
|
||||
# description: compute the distance, in meters, between the drone and a (latitude, longitude) pair of GPS coordinates
|
||||
# inputs: the latitude and longitude of the point whose distance we want to compute
|
||||
# output: the distance, in meters, between the drone and the input (latitude, longitude) pair
|
||||
function distance_from_gps(lat, lon) {
|
||||
var x_lon = lon - pose.position.longitude
|
||||
var x_lat = lat - pose.position.latitude
|
||||
var ned_xx = x_lat/180*math.pi * 6371000.0
|
||||
var ned_yy = x_lon/180*math.pi * 6371000.0 * math.cos(lat/180*math.pi)
|
||||
return math.sqrt(ned_xx*ned_xx+ned_yy*ned_yy)
|
||||
}
|
||||
|
||||
# function name: distance_between_coord
|
||||
# description: compute the distance, in meters, between the twop (latitude, longitude) pairs of GPS coordinates
|
||||
# inputs: the latitude and longitude of the first and second point whose distance we want to compute
|
||||
# output: the distance, in meters, between the two (latitude, longitude) pairs
|
||||
function distance_between_coord(lat1, lon1, lat2, lon2) {
|
||||
var x_lon = lon2 - lon1
|
||||
var x_lat = lat2 - lat1
|
||||
var ned_xx = x_lat/180*math.pi * 6371000.0
|
||||
var ned_yy = x_lon/180*math.pi * 6371000.0 * math.cos(lat2/180*math.pi)
|
||||
return math.sqrt(ned_xx*ned_xx+ned_yy*ned_yy)
|
||||
}
|
||||
|
||||
# function name: x_from_gps
|
||||
# description: compute the X AXIS SEPARATION, in meters, between the drone and a (latitude, longitude) pair of GPS coordinates
|
||||
# inputs: the latitude and longitude of the point whose distance we want to compute
|
||||
# output: the X AXIS SEPARATION, in meters, between the drone and the input (latitude, longitude) pair
|
||||
function x_from_gps(lat, lon) {
|
||||
var x_lat = lat - pose.position.latitude
|
||||
return x_lat/180*math.pi * 6371000.0
|
||||
}
|
||||
|
||||
# function name: y_from_gps
|
||||
# description: compute the Y AXIS SEPARATION, in meters, between the drone and a (latitude, longitude) pair of GPS coordinates
|
||||
# inputs: the latitude and longitude of the point whose distance we want to compute
|
||||
# output: the Y AXIS SEPARATION, in meters, between the drone and the input (latitude, longitude) pair
|
||||
function y_from_gps(lat, lon) {
|
||||
var x_lon = lon - pose.position.longitude
|
||||
return x_lon/180*math.pi * 6371000.0 * math.cos(lat/180*math.pi)
|
||||
}
|
||||
|
||||
# function name: sc_move_gps
|
||||
# description: shortcut function to use the primitive 'uav_moveto' to move the drone towards a (latitude, longitude) pair of GPS coordinates
|
||||
# inputs: the latitude and longitude of the point that we want to approach
|
||||
# output: n/a, the drone will move
|
||||
function sc_move_gps(lat, lon) {
|
||||
#var a_coeff = 14.5
|
||||
#var x_setpoint = x_from_gps(lat, lon)
|
||||
#var y_setpoint = y_from_gps(lat, lon)
|
||||
#var d_setpoint = distance_from_gps(lat, lon)
|
||||
|
||||
#if (d_setpoint > 15.0) {
|
||||
|
||||
# x_setpoint = a_coeff * x_setpoint/d_setpoint
|
||||
# y_setpoint = a_coeff * y_setpoint/d_setpoint
|
||||
#} else {
|
||||
# ;
|
||||
#}
|
||||
|
||||
#log("Distance from", current_area_wp_index, "-th wp in current area", bidded_area," ->", d_setpoint) # USEFUL FOR DEBUGGING
|
||||
#goto_abs(x_setpoint, y_setpoint, 0.0, 0.0)
|
||||
m_navigation = vec_from_gps(lat,lon, 0)
|
||||
#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 (", math.vec2.length(m_navigation), " / ", GOTO_MAXDIST, " )")
|
||||
else {
|
||||
log("Distance from", current_area_wp_index, "-th wp in current area", bidded_area," ->", math.vec2.length(m_navigation)) # USEFUL FOR DEBUGGING
|
||||
m_navigation = LimitSpeed(m_navigation, 1.0)
|
||||
#m_navigation = LCA(m_navigation)
|
||||
goto_abs(m_navigation.x, m_navigation.y, 0.0, 0.0)
|
||||
}
|
||||
}
|
||||
|
||||
# function name: sc_move_wp
|
||||
# description: shortcut function to use the primitive 'uav_moveto' to move the drone towards a waypoint
|
||||
# inputs: the index in the dictionary 'waypoints' of the waypoint that we want to approach
|
||||
# NOTE: '-1' is a special input that will drive the drone back to its homepoint
|
||||
# output: n/a, the drone will move
|
||||
function sc_move_wp(m_wp_i) {
|
||||
if (m_wp_i == -1) {
|
||||
#log("WARNING: moving to homepoint, if homewpoint was not initialized the script will crash")
|
||||
sc_move_gps(HOME_LAT, HOME_LON) # CAREFUL: homepoint initalized at the end of the first take off
|
||||
} else {
|
||||
sc_move_gps(wp_lat(m_wp_i), wp_lon(m_wp_i))
|
||||
}
|
||||
}
|
||||
|
||||
# function name: sc_cover_assigned_area
|
||||
# description: shortcut function to make a drone reach all the waypoints in an area
|
||||
# inputs: n/a
|
||||
# NOTE: the functions exploits the global variables 'current_area_wp_order', 'current_area_wp_number', 'current_area_wp_index'
|
||||
# NOTE: these MUST be set before its use
|
||||
# output: n/a, the drone will move
|
||||
function sc_cover_assigned_area() {
|
||||
var pursuing_wp = current_area_wp_order[current_area_wp_index]
|
||||
sc_move_wp(pursuing_wp)
|
||||
if (distance_from_gps(wp_lat(pursuing_wp), wp_lon(pursuing_wp))<0.1) {
|
||||
io.fwrite(output_file, string.concat( string.tostring(wp_area(pursuing_wp)), ",",
|
||||
string.tostring(wp_type(pursuing_wp)), ",",
|
||||
string.tostring(wp_lat(pursuing_wp)), ",",
|
||||
string.tostring(wp_lon(pursuing_wp)), ",",
|
||||
string.tostring(wp_alt(pursuing_wp)), ",",
|
||||
wp_filename(pursuing_wp), ",",
|
||||
string.tostring(id) ))
|
||||
if (current_area_wp_index < (current_area_wp_number - 1)) {
|
||||
current_area_wp_index = current_area_wp_index + 1
|
||||
} else {
|
||||
return 1
|
||||
}
|
||||
}
|
||||
return 0
|
||||
}
|
||||
|
||||
########################################
|
||||
#
|
||||
# BIDDING
|
||||
#
|
||||
########################################
|
||||
|
||||
# function name: drone2area_closest_wp
|
||||
# description: compute the closest waypoint in an area from the current position of the drone
|
||||
# inputs: the area id
|
||||
# output: the id of the closest waypoint in an area
|
||||
function drone2area_closest_wp(area_id) {
|
||||
var wp_i = 0
|
||||
var dist = 6371000.0
|
||||
var closest_wp = -1
|
||||
while (wp_i<NUM_WP) {
|
||||
if (wp_area(wp_i) == area_id) {
|
||||
var temp = distance_from_gps(wp_lat(wp_i), wp_lon(wp_i))
|
||||
if (temp < dist) {
|
||||
dist = temp
|
||||
closest_wp = wp_i
|
||||
}
|
||||
}
|
||||
wp_i = wp_i + 1
|
||||
}
|
||||
return closest_wp
|
||||
}
|
||||
|
||||
# function name: drone2area_dist
|
||||
# description: compute the distance of the closest waypoint in an area from the current position of the drone
|
||||
# inputs: the area id
|
||||
# output: the distance of the closest waypoint in an area
|
||||
function drone2area_dist(area_id) {
|
||||
return distance_from_gps(wp_lat(drone2area_closest_wp(area_id)), wp_lon(drone2area_closest_wp(area_id)))
|
||||
}
|
||||
|
||||
# function name: drone2area_path
|
||||
# description: compute the length of the greedy traversal of all point in an area from the current position of the drone
|
||||
# inputs: the area id
|
||||
# output: the lenght of the path over all the waypoints in the area, the function writes into the global variables 'current_area_wp_order', 'current_area_wp_number'
|
||||
# NOTE: the writing of 'current_area_wp_order', 'current_area_wp_number' is affected by whether 'drone_assigned2area' is set or not
|
||||
function drone2area_path(area_id) {
|
||||
var path = 0.0
|
||||
var temp_wp_list = {}
|
||||
var wp_i = 0
|
||||
var copy_i = 0
|
||||
while (wp_i<NUM_WP) {
|
||||
if (wp_area(wp_i) == area_id) {
|
||||
temp_wp_list[copy_i] = wp_i
|
||||
copy_i = copy_i + 1
|
||||
}
|
||||
wp_i = wp_i + 1
|
||||
}
|
||||
var wp_in_area = copy_i
|
||||
if (drone_assigned2area == 0) {
|
||||
current_area_wp_number = wp_in_area
|
||||
}
|
||||
var current_wp = drone2area_closest_wp(area_id)
|
||||
var seg_i = 0
|
||||
if (drone_assigned2area == 0) {
|
||||
current_area_wp_order[seg_i] = current_wp
|
||||
}
|
||||
while (seg_i<(wp_in_area-1)) {
|
||||
var wp_i_2 = 0
|
||||
var next_wp_dist = 6371000.0
|
||||
var next_wp = -1
|
||||
while (wp_i_2 < wp_in_area) {
|
||||
if (temp_wp_list[wp_i_2] != -1) {
|
||||
if (temp_wp_list[wp_i_2] == current_wp) {
|
||||
temp_wp_list[wp_i_2] = -1
|
||||
} else {
|
||||
var temp_seg = distance_between_coord(wp_lat(current_wp), wp_lon(current_wp), wp_lat(temp_wp_list[wp_i_2]), wp_lon(temp_wp_list[wp_i_2]))
|
||||
if (temp_seg < next_wp_dist) {
|
||||
next_wp_dist = temp_seg
|
||||
next_wp = temp_wp_list[wp_i_2]
|
||||
}
|
||||
}
|
||||
}
|
||||
wp_i_2 = wp_i_2 + 1
|
||||
}
|
||||
#if (next_wp == -1) { log("WARNING: couldn't find next wp") }
|
||||
path = path + next_wp_dist
|
||||
current_wp = next_wp
|
||||
seg_i = seg_i + 1
|
||||
if (drone_assigned2area == 0) {
|
||||
current_area_wp_order[seg_i] = current_wp
|
||||
}
|
||||
}
|
||||
return path
|
||||
}
|
||||
|
||||
# function name: stig_set_bid
|
||||
# description: set the bid for an area in 'bidding_stigmergy', the bidder is automatically set to 'id'
|
||||
# inputs: the area id and the bid to be set
|
||||
# output: n/a
|
||||
function stig_set_bid(area_id, m_bid) {
|
||||
bidding_stigmergy.put(area_id * 3 + 0, m_bid)
|
||||
bidding_stigmergy.put(area_id * 3 + 1, id)
|
||||
}
|
||||
|
||||
# function name: stig_set_status
|
||||
# description: set the status of an area in 'bidding_stigmergy'
|
||||
# inputs: the area id and the status to be set
|
||||
# output: n/a
|
||||
function stig_set_status(area_id, stat) {
|
||||
bidding_stigmergy.put(area_id * 3 + 2, stat)
|
||||
}
|
||||
|
||||
# function name: stig_remove_bid
|
||||
# description: resets the bid and bidder of an area in 'bidding_stigmergy'
|
||||
# inputs: the area id
|
||||
# output: n/a
|
||||
function stig_remove_bid(area_id) {
|
||||
bidding_stigmergy.put(area_id * 3 + 0, -1)
|
||||
bidding_stigmergy.put(area_id * 3 + 1, -1)
|
||||
}
|
||||
|
||||
# function name: stig_get_bid
|
||||
# description: read the local copy of 'bidding_stigmergy' for the value of an area's current bid
|
||||
# inputs: the area id
|
||||
# output: the status of area, if initialized, -1 otherwise
|
||||
function stig_get_bid(area_id) {
|
||||
var return_val = bidding_stigmergy.get(area_id * 3 + 0)
|
||||
if (return_val != nil) {
|
||||
return return_val
|
||||
} else {
|
||||
return -1
|
||||
}
|
||||
}
|
||||
|
||||
# function name: stig_get_bidder
|
||||
# description: read the local copy of 'bidding_stigmergy' for the value of an area's current bidder
|
||||
# inputs: the area id
|
||||
# output: the status of area, if initialized, -1 otherwise
|
||||
function stig_get_bidder(area_id) {
|
||||
var return_val = bidding_stigmergy.get(area_id * 3 + 1)
|
||||
if (return_val != nil) {
|
||||
return return_val
|
||||
} else {
|
||||
return -1
|
||||
}
|
||||
}
|
||||
|
||||
# function name: stig_get_status
|
||||
# description: read the local copy of 'bidding_stigmergy' for the value of an area's current status
|
||||
# inputs: the area id
|
||||
# output: the status of area, if initialized, 0 otherwise
|
||||
function stig_get_status(area_id) {
|
||||
var return_val = bidding_stigmergy.get(area_id * 3 + 2)
|
||||
if (return_val != nil) {
|
||||
return return_val
|
||||
} else {
|
||||
return 0
|
||||
}
|
||||
}
|
||||
|
||||
# function name: print_out_bidding_stigmergy
|
||||
# description: prints all the entries in the local copy of 'bidding_stigmergy'
|
||||
# inputs: n/a
|
||||
# output: n/a, the function print to terminal
|
||||
function print_out_bidding_stigmergy() {
|
||||
var k = 0
|
||||
while (k < NUM_AREAS) {
|
||||
log("Area", k, "(", stig_get_bid(k), ",", stig_get_bidder(k), ",", stig_get_status(k), ")")
|
||||
k = k + 1
|
||||
}
|
||||
}
|
||||
|
||||
########################################
|
||||
#
|
||||
# MAIN FUNCTIONS
|
||||
#
|
||||
########################################
|
||||
|
||||
# executed once at init time
|
||||
function init_bidding() {
|
||||
|
||||
# read the csv file with the waypoints information
|
||||
read_from_csv(CSV_FILENAME_AND_PATH)
|
||||
|
||||
# create bidding stigmergy
|
||||
bidding_stigmergy = stigmergy.create(1)
|
||||
|
||||
# inital take off status
|
||||
taken_off = 0
|
||||
|
||||
# flag to log the initial latitude and longitude
|
||||
logged_homepoint = 0
|
||||
|
||||
# flag to make the drone fly back home
|
||||
go_home = 0
|
||||
|
||||
#flag to state whether a drone is assigned to an area or not
|
||||
drone_assigned2area = 0
|
||||
|
||||
# initalize iteration counter
|
||||
experiment_iteration = 0
|
||||
|
||||
#open the output file
|
||||
output_file=io.fopen(string.concat(OUTPUT_FILENAME_AND_PATH, string.tostring(id), ".csv"), "w")
|
||||
|
||||
log("Drone", id, "initialized")
|
||||
|
||||
}
|
||||
|
||||
# executed at each time step
|
||||
function bidding() {
|
||||
|
||||
log("experiment_iteration: ", experiment_iteration)
|
||||
|
||||
if (experiment_iteration > 5){
|
||||
# save homepoint
|
||||
if (logged_homepoint == 0) {
|
||||
HOME_LAT = pose.position.latitude
|
||||
HOME_LON = pose.position.longitude
|
||||
logged_homepoint = 1
|
||||
}
|
||||
}
|
||||
|
||||
################################################
|
||||
################# TAKE OFF #####################
|
||||
################################################
|
||||
################################################
|
||||
|
||||
# takeoff
|
||||
#if (pose.position.altitude < (2 * id + BASE_ALTITUDE) and taken_off == 0){
|
||||
# uav_takeoff(2 * id + BASE_ALTITUDE + 0.1)
|
||||
# log("Drone", id, "is taking-off") # CAREFUL: take off might be unresponsive at times (ROSBuzz fix required?) notes: 'Got command: 22', 'Reply: 1' (apparent success)
|
||||
#} else {
|
||||
# taken_off = 1
|
||||
#}
|
||||
#}
|
||||
#################################################
|
||||
################################################
|
||||
|
||||
taken_off = 1
|
||||
|
||||
# in the air, switch between a 'bid and evaluate bids' mode and a 'cover assigned area' mode
|
||||
if (taken_off == 1 and experiment_iteration > 10){
|
||||
if (drone_assigned2area == 0) {
|
||||
# go home is the appropriate flag was set (no unassigned areas left)
|
||||
if (go_home) {
|
||||
log("Drone", id, "is going home")
|
||||
sc_move_wp(-1) # requires a set homepoint
|
||||
} else {
|
||||
########################################
|
||||
# BIDDING BLOCK START ##################
|
||||
########################################
|
||||
|
||||
########################################
|
||||
# EVALUATE BID RESULT ##################
|
||||
########################################
|
||||
# if bid was won, assign area. otherwiese reset 'bid_made', 'bidded_area'
|
||||
if (experiment_iteration>(bid_time+BID_WAIT)) { # long wait between bids evaluations to allow other drones to bid (relatively high comp. time)
|
||||
|
||||
if (bid_made == 1) {
|
||||
#check if the bid was won
|
||||
var winner = stig_get_bidder(bidded_area)
|
||||
if (winner == id) {
|
||||
log("Drone", id, "decided it has won area", bidded_area, "at iter", experiment_iteration)
|
||||
stig_set_status(bidded_area, 1)
|
||||
drone2area_path(bidded_area) # IMPORTANT: COMPUTE AREA WP ORDER AND NUMBER
|
||||
drone_assigned2area = 1 # IMPORTANT: MAKE THE DRONE SWITCH TO COVERAGE (ALSO BLOCK THE OVER-WRITING OF THE AREA WP ORDER)
|
||||
} else {
|
||||
# free bid flags
|
||||
log("Drone", id, "decided it has lost area", bidded_area, "at iter", experiment_iteration)
|
||||
bid_made = 0
|
||||
bidded_area = -1
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
# if I haven't a currently set bid (NOTE: IT IS IMPORTANT TO COMPUTE BIDS AND TRY TO WRITE STIGMERGY IN DIFFERENT CONTROL STEPS)
|
||||
if (bid_made == 0) {
|
||||
picture_time_set = 0
|
||||
########################################
|
||||
# COMPUTE BID ##########################
|
||||
########################################
|
||||
if (experiment_iteration%2==0) {
|
||||
# find my own highest bid, if any exists
|
||||
highest_bid = -1
|
||||
highest_area = -1
|
||||
var i = 0
|
||||
while (i<NUM_AREAS) {
|
||||
# only bid on unassigned/uncovered areas
|
||||
if (stig_get_status(i) == 0) {
|
||||
var temp_bid = 1000/(drone2area_dist(i) + drone2area_path(i))
|
||||
if (temp_bid > highest_bid) {
|
||||
highest_bid = temp_bid
|
||||
highest_area = i
|
||||
}
|
||||
}
|
||||
i = i + 1
|
||||
}
|
||||
log("Drone", id, "found its highest bid to be", highest_bid, "on", highest_area)
|
||||
}
|
||||
########################################
|
||||
# PLACE BID ############################
|
||||
########################################
|
||||
if (experiment_iteration%2==1) {
|
||||
if (highest_area == -2) {
|
||||
;
|
||||
} else {
|
||||
# if no areas are available, set a flag to return home
|
||||
if (highest_area == -1) {
|
||||
go_home = 1
|
||||
} else {
|
||||
# bid (own highest only, if own bid is higher than existing and the wp status is not take)
|
||||
if ( (highest_bid > stig_get_bid(highest_area)) and (stig_get_status(highest_area) == 0) ) {
|
||||
stig_set_bid(highest_area, highest_bid)
|
||||
bid_made = 1
|
||||
bid_time = experiment_iteration
|
||||
bidded_area = highest_area
|
||||
log("Drone", id, "SET its bid", highest_bid, "on", highest_area, "at iter", experiment_iteration, "status was", stig_get_status(highest_area))
|
||||
} else {
|
||||
bid_made = 0
|
||||
bidded_area = -1
|
||||
log("Drone", id, "GAVE UP bid", highest_bid, "on", highest_area, "at iter", experiment_iteration, "status was", stig_get_status(highest_area))
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
########################################
|
||||
# BIDDING BLOCK END ####################
|
||||
########################################
|
||||
}
|
||||
} else {
|
||||
log("Drone", id, "is covering area", bidded_area)
|
||||
#######################################################
|
||||
# DOUBLE CHECK NO OTHER DRONE IS COVERING THE SAME AREA
|
||||
#######################################################
|
||||
var leave_area = 0
|
||||
var stig_assigned = stig_get_bidder(bidded_area)
|
||||
if (stig_assigned != id) {
|
||||
log("Drone", id, "figured out that", bidded_area, "is actually assigned to", stig_assigned, "on stigmergy")
|
||||
leave_area = 1
|
||||
}
|
||||
|
||||
var completed = sc_cover_assigned_area() # CAREFUL: this MUST follow a call to 'drone2area_path(area_id)' when 'drone_assigned2area == 0'
|
||||
if ((completed == 1) and (picture_time_set == 0)){
|
||||
picture_time_set = 1
|
||||
picture_time = experiment_iteration
|
||||
}
|
||||
|
||||
if ( (completed == 1) or (leave_area == 1) ) {
|
||||
# free assignment and bidding flags
|
||||
if (experiment_iteration > (picture_time+PICTURE_WAIT)) {
|
||||
drone_assigned2area = 0 # IMPORTANT: MAKE THE DRONE SWITCH TO BIDDING (ALSO RE-ENABLE THE OVER-WRITING OF THE AREA WP ORDER)
|
||||
current_area_wp_index = 0 # IMPORTANT: RESET THE AREA WP INDEX
|
||||
bid_made = 0
|
||||
bidded_area = -1
|
||||
}
|
||||
else {
|
||||
log("Drone is taking pictures")
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
########################################
|
||||
# TEMP DEBUG BLOCK START ###############
|
||||
########################################
|
||||
if (id == 2) {
|
||||
if (experiment_iteration%20==0){
|
||||
print_out_bidding_stigmergy()
|
||||
}
|
||||
log("===============")
|
||||
}
|
||||
########################################
|
||||
# TEMP DEBUG BLOCK END #################
|
||||
########################################
|
||||
|
||||
# increase iteration counter
|
||||
experiment_iteration = experiment_iteration + 1
|
||||
|
||||
# log of the drone position
|
||||
#log("P", position.latitude, position.longitude, position.altitude, "TO", taken_off, "v03") # may want to log flight.status too
|
||||
|
||||
}
|
||||
|
||||
# executed once when the robot (or the simulator) is reset
|
||||
#function reset() {
|
||||
#log("Drone", id, "was reset")
|
||||
#}
|
||||
|
||||
# executed once at the end of experiment
|
||||
function close_bidding() {
|
||||
io.fclose(output_file)
|
||||
log("Drone", id, "bids farewell")
|
||||
}
|
||||
|
|
@ -7,12 +7,6 @@ function LimitAngle(angle){
|
|||
return angle
|
||||
}
|
||||
|
||||
function LimitSpeed(vel_vec, factor){
|
||||
if(math.vec2.length(vel_vec)>GOTO_MAXVEL*factor)
|
||||
vel_vec = math.vec2.scale(vel_vec, GOTO_MAXVEL*factor/math.vec2.length(vel_vec))
|
||||
return vel_vec
|
||||
}
|
||||
|
||||
# TODO: add other conversions....
|
||||
function convert_path(P) {
|
||||
var pathR={}
|
||||
|
|
|
|||
|
|
@ -2,10 +2,11 @@ takeoff_heights ={
|
|||
.1 = 21.0,
|
||||
.2 = 18.0,
|
||||
.3 = 12.0,
|
||||
.5 = 24.0,
|
||||
.5 = 12.0,
|
||||
.6 = 3.0,
|
||||
.9 = 15.0,
|
||||
.11 = 6.0,
|
||||
.14 = 18.0,
|
||||
.16 = 9.0,
|
||||
.17 = 3.0,
|
||||
.18 = 6.0,
|
||||
|
|
|
|||
|
|
@ -4,14 +4,14 @@ include "update.bzz"
|
|||
include "act/states.bzz"
|
||||
include "plan/rrtstar.bzz"
|
||||
include "taskallocate/graphformGPS.bzz"
|
||||
include "bidding/bidding.bzz"
|
||||
#include "taskallocate/bidding.bzz"
|
||||
include "vstigenv.bzz"
|
||||
#include "timesync.bzz"
|
||||
include "utils/takeoff_heights.bzz"
|
||||
|
||||
#State launched after takeoff
|
||||
|
||||
AUTO_LAUNCH_STATE = "BIDDING"
|
||||
AUTO_LAUNCH_STATE = "INDIWP"
|
||||
TARGET = 9.0
|
||||
EPSILON = 30.0
|
||||
ROOT_ID = 3
|
||||
|
|
@ -35,9 +35,7 @@ goal_list = {
|
|||
function init() {
|
||||
init_stig()
|
||||
init_swarm()
|
||||
init_bidding()
|
||||
|
||||
# initGraph()
|
||||
#init_bidding()
|
||||
|
||||
TARGET_ALTITUDE = takeoff_heights[id]
|
||||
|
||||
|
|
@ -73,6 +71,8 @@ function step() {
|
|||
statef=launch_switch
|
||||
else if(BVMSTATE=="GOHOME") # ends on AUTO_LAUNCH_STATE
|
||||
statef=goinghome
|
||||
else if(BVMSTATE=="INDIWP")
|
||||
statef=indiWP
|
||||
else if(BVMSTATE=="IDLE")
|
||||
statef=idle
|
||||
else if(BVMSTATE=="AGGREGATE")
|
||||
|
|
|
|||
|
|
@ -50,6 +50,10 @@ int buzzuav_takepicture(buzzvm_t vm);
|
|||
* Returns the current command from local variable
|
||||
*/
|
||||
int getcmd();
|
||||
/*
|
||||
* update GPS goal value
|
||||
*/
|
||||
void set_gpsgoal(double goal[3]);
|
||||
/*
|
||||
* Sets goto position from rc client
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -14,23 +14,29 @@ namespace buzzuav_closures
|
|||
// TODO: Minimize the required global variables and put them in the header
|
||||
// static const rosbzz_node::roscontroller* roscontroller_ptr;
|
||||
static double goto_pos[4];
|
||||
static double rc_goto_pos[3];
|
||||
static float rc_gimbal[4];
|
||||
static float batt[3];
|
||||
static float obst[5] = { 0, 0, 0, 0, 0 };
|
||||
static double goto_gpsgoal[3];
|
||||
static double cur_pos[4];
|
||||
static double cur_NEDpos[2];
|
||||
static uint8_t status;
|
||||
static int cur_cmd = 0;
|
||||
static int rc_cmd = 0;
|
||||
|
||||
static int rc_id = -1;
|
||||
static int rc_cmd = 0;
|
||||
static double rc_gpsgoal[3];
|
||||
static float rc_gimbal[4];
|
||||
|
||||
static float batt[3];
|
||||
static float obst[5] = { 0, 0, 0, 0, 0 };
|
||||
static uint8_t status;
|
||||
|
||||
static int cur_cmd = 0;
|
||||
static int buzz_cmd = 0;
|
||||
static float height = 0;
|
||||
|
||||
static bool deque_full = false;
|
||||
static int rssi = 0;
|
||||
static float raw_packet_loss = 0.0;
|
||||
static int filtered_packet_loss = 0;
|
||||
static float api_rssi = 0.0;
|
||||
|
||||
string WPlistname = "";
|
||||
|
||||
std::map<int, buzz_utility::RB_struct> targets_map;
|
||||
|
|
@ -231,7 +237,7 @@ int buzzuav_moveto(buzzvm_t vm)
|
|||
// DEBUG
|
||||
// ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0],
|
||||
// goto_pos[1], goto_pos[2]);
|
||||
buzz_cmd = NAV_SPLINE_WAYPOINT; // TODO: standard mavros?
|
||||
buzz_cmd = NAV_SPLINE_WAYPOINT;
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
|
||||
|
|
@ -386,14 +392,24 @@ int buzzuav_storegoal(buzzvm_t vm)
|
|||
wplist_map.erase(wplist_map.begin()->first);
|
||||
}
|
||||
|
||||
double rb[3];
|
||||
set_gpsgoal(goal);
|
||||
// prevent an overwrite
|
||||
rc_id = -1;
|
||||
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
|
||||
void set_gpsgoal(double goal[3])
|
||||
/*
|
||||
/ update GPS goal value
|
||||
-----------------------------------*/
|
||||
{
|
||||
double rb[3];
|
||||
rb_from_gps(goal, rb, cur_pos);
|
||||
if (fabs(rb[0]) < 150.0) {
|
||||
rc_set_goto((int)buzz_utility::get_robotid(), goal[0], goal[1], goal[2]);
|
||||
ROS_INFO("Set RC_GOTO ---- %f %f %f (%f %f, %f %f)", goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]);
|
||||
goto_gpsgoal[0] = goal[0];goto_gpsgoal[1] = goal[1];goto_gpsgoal[2] = goal[2];
|
||||
ROS_INFO("Set GPS GOAL TO ---- %f %f %f (%f %f, %f %f)", goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]);
|
||||
}
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
|
||||
int buzzuav_arm(buzzvm_t vm)
|
||||
|
|
@ -504,9 +520,9 @@ void rc_set_goto(int id, double latitude, double longitude, double altitude)
|
|||
-----------------------------------*/
|
||||
{
|
||||
rc_id = id;
|
||||
rc_goto_pos[0] = latitude;
|
||||
rc_goto_pos[1] = longitude;
|
||||
rc_goto_pos[2] = altitude;
|
||||
rc_gpsgoal[0] = latitude;
|
||||
rc_gpsgoal[1] = longitude;
|
||||
rc_gpsgoal[2] = altitude;
|
||||
}
|
||||
|
||||
void rc_set_gimbal(int id, float yaw, float roll, float pitch, float t)
|
||||
|
|
@ -773,15 +789,30 @@ int buzzuav_update_flight_status(buzzvm_t vm)
|
|||
buzzvm_tput(vm);
|
||||
buzzvm_dup(vm);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1));
|
||||
buzzvm_pushf(vm, rc_goto_pos[0]);
|
||||
buzzvm_pushf(vm, rc_gpsgoal[0]);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_dup(vm);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 1));
|
||||
buzzvm_pushf(vm, rc_goto_pos[1]);
|
||||
buzzvm_pushf(vm, rc_gpsgoal[1]);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_dup(vm);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 1));
|
||||
buzzvm_pushf(vm, rc_goto_pos[2]);
|
||||
buzzvm_pushf(vm, rc_gpsgoal[2]);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_gstore(vm);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "cur_goal", 1));
|
||||
buzzvm_pusht(vm);
|
||||
buzzvm_dup(vm);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1));
|
||||
buzzvm_pushf(vm, goto_gpsgoal[0]);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_dup(vm);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 1));
|
||||
buzzvm_pushf(vm, goto_gpsgoal[1]);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_dup(vm);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 1));
|
||||
buzzvm_pushf(vm, goto_gpsgoal[2]);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_gstore(vm);
|
||||
return vm->state;
|
||||
|
|
|
|||
|
|
@ -762,6 +762,10 @@ script
|
|||
Arm();
|
||||
// Registering HOME POINT.
|
||||
home = cur_pos;
|
||||
// Initialize GPS goal for safety.
|
||||
double gpspgoal[3] = {cur_pos.latitude,cur_pos.longitude,cur_pos.altitude};
|
||||
buzzuav_closures::set_gpsgoal(gpspgoal);
|
||||
BClpose = true;
|
||||
}
|
||||
if (current_mode != "GUIDED" && setmode)
|
||||
SetMode("GUIDED", 2000); // added for compatibility with 3DR Solo
|
||||
|
|
@ -944,7 +948,7 @@ void roscontroller::gps_rb(POSE nei_pos, double out[])
|
|||
|
||||
void roscontroller::gps_ned_cur(float& ned_x, float& ned_y, POSE t)
|
||||
/*
|
||||
/ Get GPS from NED and a reference GPS point (struct input)
|
||||
/ Get NED coordinates from a reference GPS point (struct input)
|
||||
----------------------------------------------------------- */
|
||||
{
|
||||
gps_convert_ned(ned_x, ned_y, t.longitude, t.latitude, cur_pos.longitude, cur_pos.latitude);
|
||||
|
|
@ -953,7 +957,7 @@ void roscontroller::gps_ned_cur(float& ned_x, float& ned_y, POSE t)
|
|||
void roscontroller::gps_convert_ned(float& ned_x, float& ned_y, double gps_t_lon, double gps_t_lat, double gps_r_lon,
|
||||
double gps_r_lat)
|
||||
/*
|
||||
/ Get GPS from NED and a reference GPS point
|
||||
/ Get NED coordinates from a reference GPS point and current GPS location
|
||||
----------------------------------------------------------- */
|
||||
{
|
||||
double d_lon = gps_t_lon - gps_r_lon;
|
||||
|
|
|
|||
Loading…
Reference in New Issue