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

Merge commit 'fd8e8022744c30cb9d59b1dd4cb6cf90f1c728f8' into ros_drones_ws

This commit is contained in:
dave 2018-10-19 02:40:18 -04:00
parent 3a0239b2ae fd8e802274
commit 4b293afac5
11 changed files with 954 additions and 228 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
buzz_scripts/include/act/CA.bzz Normal file
View File

@ -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
}

26
buzz_scripts/include/act/naviguation.bzz Normal file
View File

@ -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
}

129
buzz_scripts/include/act/neighborcomm.bzz Normal file
View File

@ -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)
}
})
}

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

@ -5,45 +5,30 @@
######################################## ########################################
include "utils/vec2.bzz" include "utils/vec2.bzz"
include "act/barrier.bzz" include "act/barrier.bzz"
#include "act/old_barrier.bzz"
include "utils/conversions.bzz" include "utils/conversions.bzz"
include "act/naviguation.bzz"
include "act/CA.bzz"
include "act/neighborcomm.bzz"
TARGET_ALTITUDE = 15.0 # m. TARGET_ALTITUDE = 15.0 # m.
BVMSTATE = "TURNEDOFF" BVMSTATE = "TURNEDOFF"
PICTURE_WAIT = 20 # steps 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 path_it = 0
pic_time = 0 pic_time = 0
g_it = 0 g_it = 0
# Core state function when on the ground
function turnedoff() { function turnedoff() {
BVMSTATE = "TURNEDOFF" BVMSTATE = "TURNEDOFF"
} }
# Core state function when hovering
function idle() { function idle() {
BVMSTATE = "IDLE" BVMSTATE = "IDLE"
} }
# Core state function to launch the robot: takeoff and wait for others, or stop (land)
# 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")
}
}
function launch() { function launch() {
BVMSTATE = "LAUNCH" BVMSTATE = "LAUNCH"
if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so TAKE_OFF 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() { function launch_switch() {
BVMSTATE = "LAUNCH_SWITCH" BVMSTATE = "LAUNCH_SWITCH"
if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so TAKE_OFF if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so TAKE_OFF
@ -81,6 +67,7 @@ function launch_switch() {
} }
gohomeT=100 gohomeT=100
# State function to go back to ROSBuzz recorded home GPS position (at takeoff)
function goinghome() { function goinghome() {
BVMSTATE = "GOHOME" BVMSTATE = "GOHOME"
if(gohomeT > 0) { # TODO: Make a real check if home is reached if(gohomeT > 0) { # TODO: Make a real check if home is reached
@ -90,6 +77,7 @@ function goinghome() {
BVMSTATE = AUTO_LAUNCH_STATE BVMSTATE = AUTO_LAUNCH_STATE
} }
# Core state function to stop and land.
function stop() { function stop() {
BVMSTATE = "STOP" BVMSTATE = "STOP"
if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so LAND 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() { function take_picture() {
BVMSTATE="PICTURE" BVMSTATE="PICTURE"
setgimbal(0.0, 0.0, -90.0, 20.0) setgimbal(0.0, 0.0, -90.0, 20.0)
@ -119,66 +129,7 @@ function take_picture() {
pic_time=pic_time+1 pic_time=pic_time+1
} }
function goto_gps(transf) { # State function to follow a moving attractor (GPS sent from a user phone)
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
}
function follow() { function follow() {
if(size(targets)>0) { if(size(targets)>0) {
BVMSTATE = "FOLLOW" BVMSTATE = "FOLLOW"
@ -194,7 +145,7 @@ function follow() {
} }
} }
# converge to centroid # State function to converge to centroid
function aggregate() { function aggregate() {
BVMSTATE="AGGREGATE" BVMSTATE="AGGREGATE"
centroid = neighbors.reduce(function(rid, data, centroid) { centroid = neighbors.reduce(function(rid, data, centroid) {
@ -208,7 +159,7 @@ function aggregate() {
goto_abs(cmdbin.x, cmdbin.y, 0.0, 0.0) 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() { function pursuit() {
BVMSTATE="PURSUIT" BVMSTATE="PURSUIT"
rd = 20.0 rd = 20.0
@ -248,7 +199,7 @@ function lj_sum(rid, data, accum) {
return math.vec2.add(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() { function formation() {
BVMSTATE="FORMATION" BVMSTATE="FORMATION"
# Calculate accumulator # Calculate accumulator
@ -259,115 +210,17 @@ function formation() {
goto_abs(accum_lj.x, accum_lj.y, 0.0, 0.0) goto_abs(accum_lj.x, accum_lj.y, 0.0, 0.0)
} }
# listens to commands from the remote control (web, commandline, etc) # Custom state function for the developer to play with
function rc_cmd_listen() { function cusfun(){
if(flight.rc_cmd==22) { BVMSTATE="CUSFUN"
log("cmd 22") log("cusfun: yay!!!")
flight.rc_cmd=0 LOCAL_TARGET = math.vec2.new(5 ,0 )
BVMSTATE = "LAUNCH" 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
neighbors.broadcast("cmd", 22) if(math.vec2.length(local_set_point) > GOTODIST_TOL){
} else if(flight.rc_cmd==21) { local_set_point = LimitSpeed(local_set_point, 1.0)
flight.rc_cmd=0 goto_abs(local_set_point.x, local_set_point.y, 0.0, 0.0)
AUTO_LAUNCH_STATE = "TURNEDOFF" }
#barrier_set(ROBOTS, "GOHOME", BVMSTATE, 21) else{
#barrier_ready(21) log("TARGET REACHED")
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
# })
}
#}
})
}

639
buzz_scripts/include/taskallocate/bidding.bzz Normal file
View File

@ -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")
}

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

@ -7,12 +7,6 @@ function LimitAngle(angle){
return 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.... # TODO: add other conversions....
function convert_path(P) { function convert_path(P) {
var pathR={} var pathR={}

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

@ -2,10 +2,11 @@ takeoff_heights ={
.1 = 21.0, .1 = 21.0,
.2 = 18.0, .2 = 18.0,
.3 = 12.0, .3 = 12.0,
.5 = 24.0, .5 = 12.0,
.6 = 3.0, .6 = 3.0,
.9 = 15.0, .9 = 15.0,
.11 = 6.0, .11 = 6.0,
.14 = 18.0,
.16 = 9.0, .16 = 9.0,
.17 = 3.0, .17 = 3.0,
.18 = 6.0, .18 = 6.0,

10
buzz_scripts/main.bzz
View File

@ -4,14 +4,14 @@ include "update.bzz"
include "act/states.bzz" include "act/states.bzz"
include "plan/rrtstar.bzz" include "plan/rrtstar.bzz"
include "taskallocate/graphformGPS.bzz" include "taskallocate/graphformGPS.bzz"
include "bidding/bidding.bzz" #include "taskallocate/bidding.bzz"
include "vstigenv.bzz" include "vstigenv.bzz"
#include "timesync.bzz" #include "timesync.bzz"
include "utils/takeoff_heights.bzz" include "utils/takeoff_heights.bzz"
#State launched after takeoff #State launched after takeoff
AUTO_LAUNCH_STATE = "BIDDING" AUTO_LAUNCH_STATE = "INDIWP"
TARGET = 9.0 TARGET = 9.0
EPSILON = 30.0 EPSILON = 30.0
ROOT_ID = 3 ROOT_ID = 3
@ -35,9 +35,7 @@ goal_list = {
function init() { function init() {
init_stig() init_stig()
init_swarm() init_swarm()
init_bidding() #init_bidding()
# initGraph()
TARGET_ALTITUDE = takeoff_heights[id] TARGET_ALTITUDE = takeoff_heights[id]
@ -73,6 +71,8 @@ function step() {
statef=launch_switch statef=launch_switch
else if(BVMSTATE=="GOHOME") # ends on AUTO_LAUNCH_STATE else if(BVMSTATE=="GOHOME") # ends on AUTO_LAUNCH_STATE
statef=goinghome statef=goinghome
else if(BVMSTATE=="INDIWP")
statef=indiWP
else if(BVMSTATE=="IDLE") else if(BVMSTATE=="IDLE")
statef=idle statef=idle
else if(BVMSTATE=="AGGREGATE") else if(BVMSTATE=="AGGREGATE")

4
include/buzzuav_closures.h
View File

@ -50,6 +50,10 @@ int buzzuav_takepicture(buzzvm_t vm);
* Returns the current command from local variable * Returns the current command from local variable
*/ */
int getcmd(); int getcmd();
/*
* update GPS goal value
*/
void set_gpsgoal(double goal[3]);
/* /*
* Sets goto position from rc client * Sets goto position from rc client
*/ */

67
src/buzzuav_closures.cpp
View File

@ -14,23 +14,29 @@ namespace buzzuav_closures
// TODO: Minimize the required global variables and put them in the header // TODO: Minimize the required global variables and put them in the header
// static const rosbzz_node::roscontroller* roscontroller_ptr; // static const rosbzz_node::roscontroller* roscontroller_ptr;
static double goto_pos[4]; static double goto_pos[4];
static double rc_goto_pos[3]; static double goto_gpsgoal[3];
static float rc_gimbal[4];
static float batt[3];
static float obst[5] = { 0, 0, 0, 0, 0 };
static double cur_pos[4]; static double cur_pos[4];
static double cur_NEDpos[2]; 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_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 int buzz_cmd = 0;
static float height = 0; static float height = 0;
static bool deque_full = false; static bool deque_full = false;
static int rssi = 0; static int rssi = 0;
static float raw_packet_loss = 0.0; static float raw_packet_loss = 0.0;
static int filtered_packet_loss = 0; static int filtered_packet_loss = 0;
static float api_rssi = 0.0; static float api_rssi = 0.0;
string WPlistname = ""; string WPlistname = "";
std::map<int, buzz_utility::RB_struct> targets_map; std::map<int, buzz_utility::RB_struct> targets_map;
@ -231,7 +237,7 @@ int buzzuav_moveto(buzzvm_t vm)
// DEBUG // DEBUG
// ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0], // 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]); // goto_pos[1], goto_pos[2]);
buzz_cmd = NAV_SPLINE_WAYPOINT; // TODO: standard mavros? buzz_cmd = NAV_SPLINE_WAYPOINT;
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }
@ -386,14 +392,24 @@ int buzzuav_storegoal(buzzvm_t vm)
wplist_map.erase(wplist_map.begin()->first); 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); rb_from_gps(goal, rb, cur_pos);
if (fabs(rb[0]) < 150.0) { if (fabs(rb[0]) < 150.0) {
rc_set_goto((int)buzz_utility::get_robotid(), goal[0], goal[1], goal[2]); goto_gpsgoal[0] = goal[0];goto_gpsgoal[1] = goal[1];goto_gpsgoal[2] = 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]); 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) 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_id = id;
rc_goto_pos[0] = latitude; rc_gpsgoal[0] = latitude;
rc_goto_pos[1] = longitude; rc_gpsgoal[1] = longitude;
rc_goto_pos[2] = altitude; rc_gpsgoal[2] = altitude;
} }
void rc_set_gimbal(int id, float yaw, float roll, float pitch, float t) 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_tput(vm);
buzzvm_dup(vm); buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1)); 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_tput(vm);
buzzvm_dup(vm); buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 1)); 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_tput(vm);
buzzvm_dup(vm); buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 1)); 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_tput(vm);
buzzvm_gstore(vm); buzzvm_gstore(vm);
return vm->state; return vm->state;

8
src/roscontroller.cpp
View File

@ -762,6 +762,10 @@ script
Arm(); Arm();
// Registering HOME POINT. // Registering HOME POINT.
home = cur_pos; 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) if (current_mode != "GUIDED" && setmode)
SetMode("GUIDED", 2000); // added for compatibility with 3DR Solo 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) 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); 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, 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) 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; double d_lon = gps_t_lon - gps_r_lon;