merge from drone_ws
This commit is contained in:
dave
commit
fd8e802274
|
|
@ -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")
|
||||
}
|
||||
|
|
@ -164,8 +164,8 @@ void roscontroller::RosControllerRun()
|
|||
// Call the flight controler service
|
||||
flight_controller_service_call();
|
||||
// Broadcast local position to FCU
|
||||
if(BClpose)
|
||||
SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]);
|
||||
if(BClpose && !setmode)
|
||||
SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]);
|
||||
// Set ROBOTS variable (swarm size)
|
||||
get_number_of_robots();
|
||||
buzz_utility::set_robot_var(no_of_robots);
|
||||
|
|
@ -795,7 +795,6 @@ script
|
|||
{
|
||||
armstate = 0;
|
||||
Arm();
|
||||
BClpose = false;
|
||||
}
|
||||
if (mav_client.call(cmd_srv))
|
||||
{
|
||||
|
|
@ -843,6 +842,8 @@ script
|
|||
|
||||
case NAV_SPLINE_WAYPOINT:
|
||||
goto_pos = buzzuav_closures::getgoto();
|
||||
if(setmode)
|
||||
SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]);
|
||||
break;
|
||||
|
||||
case DO_MOUNT_CONTROL:
|
||||
|
|
|
|||
Loading…
Reference in New Issue