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

Fixed the crashing at init_bidding, add the waypoint csv file 

deleted:    src/rosbuzz/buzz_scripts/include/bidding/bidding.bzz
	modified:   src/rosbuzz/buzz_scripts/include/taskallocate/bidding.bzz
	new file:   src/rosbuzz/buzz_scripts/include/taskallocate/waypoints_12.csv
	modified:   src/rosbuzz/buzz_scripts/main.bzz
This commit is contained in:
Fang Wu 2018-10-22 14:35:09 -04:00
parent fd8e802274
commit 5cea95ce55
4 changed files with 27 additions and 646 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

639
buzz_scripts/include/bidding/bidding.bzz
View File

@ -1,639 +0,0 @@
########################################
#
# 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")
}

11
buzz_scripts/include/taskallocate/bidding.bzz
View File

@ -6,7 +6,7 @@
#
########################################
CSV_FILENAME_AND_PATH = "/home/amber/rosbuzz-coverage/rosbuzz-coverage/waypoints/waypoints_15.csv"
CSV_FILENAME_AND_PATH = "/home/amber/ROS_WS/src/rosbuzz/buzz_scripts/include/taskallocate/waypoints_12.csv"
OUTPUT_FILENAME_AND_PATH = "/home/amber/bidding_output/output-" # automatically completed with 'ID.csv'
BID_WAIT = 40
PICTURE_WAIT = 40
@ -69,6 +69,7 @@ function read_from_csv(s) {
waypoints[csv_entry] = string.sub(line, csv_value_begin_i, csv_scanner_i)
csv_entry = csv_entry + 1
})
#table_print(waypoints)
io.fclose(csv_file)
NUM_AREAS = csv_area_counter
NUM_WP = csv_wp_counter
@ -423,7 +424,10 @@ function print_out_bidding_stigmergy() {
function init_bidding() {
# read the csv file with the waypoints information
read_from_csv(CSV_FILENAME_AND_PATH)
#read_from_csv(CSV_FILENAME_AND_PATH)
# read the csv file using hook in rosbuzz
#storegoal(-1.0, -1.0, -1.0)
# create bidding stigmergy
bidding_stigmergy = stigmergy.create(1)
@ -453,6 +457,9 @@ function init_bidding() {
# executed at each time step
function bidding() {
# read the csv file with the waypoints information
read_from_csv(CSV_FILENAME_AND_PATH)
log("experiment_iteration: ", experiment_iteration)
if (experiment_iteration > 5){

13
buzz_scripts/include/taskallocate/waypoints_12.csv Executable file
View File

@ -0,0 +1,13 @@
0,0,45.510378000000000,-73.609516000000000,659.526,IMG_1102.JPG
1,0,45.510322000000000,-73.609484000000000,657.343,IMG_1142.JPG
2,0,45.510259000000000,-73.609444000000000,660.146,IMG_1056.JPG
3,0,45.510415000000000,-73.609412000000000,660.287,IMG_1049.JPG
4,0,45.510357000000000,-73.609377000000000,660.123,IMG_1072.JPG
5,0,45.510295000000000,-73.609336000000000,663.132,IMG_1078.JPG
6,0,45.510550000000000,-73.609176000000000,665.142,IMG_1092.JPG
7,0,45.510500000000000,-73.609142000000000,666.124,IMG_1095.JPG
8,0,45.510433000000000,-73.609104000000000,661.123,IMG_1098.JPG
9,0,45.510374000000000,-73.609069000000000,663.123,IMG_1100.JPG
10,0,45.510330000000000,-73.609041000000000,664.110,IMG_1101.JPG
11,0,45.510345000000000,-73.608986000000000,665.123,IMG_1103.JPG
1 0 0 45.510378000000000 -73.609516000000000 659.526 IMG_1102.JPG
2 1 0 45.510322000000000 -73.609484000000000 657.343 IMG_1142.JPG
3 2 0 45.510259000000000 -73.609444000000000 660.146 IMG_1056.JPG
4 3 0 45.510415000000000 -73.609412000000000 660.287 IMG_1049.JPG
5 4 0 45.510357000000000 -73.609377000000000 660.123 IMG_1072.JPG
6 5 0 45.510295000000000 -73.609336000000000 663.132 IMG_1078.JPG
7 6 0 45.510550000000000 -73.609176000000000 665.142 IMG_1092.JPG
8 7 0 45.510500000000000 -73.609142000000000 666.124 IMG_1095.JPG
9 8 0 45.510433000000000 -73.609104000000000 661.123 IMG_1098.JPG
10 9 0 45.510374000000000 -73.609069000000000 663.123 IMG_1100.JPG
11 10 0 45.510330000000000 -73.609041000000000 664.110 IMG_1101.JPG
12 11 0 45.510345000000000 -73.608986000000000 665.123 IMG_1103.JPG

10
buzz_scripts/main.bzz
View File

@ -4,20 +4,20 @@ include "update.bzz"
include "act/states.bzz"
include "plan/rrtstar.bzz"
include "taskallocate/graphformGPS.bzz"
#include "taskallocate/bidding.bzz"
include "taskallocate/bidding.bzz"
include "vstigenv.bzz"
#include "timesync.bzz"
include "utils/takeoff_heights.bzz"
#State launched after takeoff
AUTO_LAUNCH_STATE = "INDIWP"
AUTO_LAUNCH_STATE = "BIDDING"
TARGET = 9.0
EPSILON = 30.0
ROOT_ID = 3
graph_id = 3
graph_loop = 0
LAND_AFTER_BARRIER_EXPIRE = 1 # if set to be 1 , the robots will land after barrier expire; if set to be 0, the robots will carry on to AUTO_LAUNCH_STATE.
LAND_AFTER_BARRIER_EXPIRE = 0 # if set to be 1 , the robots will land after barrier expire; if set to be 0, the robots will carry on to AUTO_LAUNCH_STATE.
#####
# Vehicule type:
@ -35,7 +35,7 @@ goal_list = {
function init() {
init_stig()
init_swarm()
#init_bidding()
init_bidding()
TARGET_ALTITUDE = takeoff_heights[id]
@ -83,7 +83,7 @@ function step() {
statef=pursuit
else if(BVMSTATE=="TASK_ALLOCATE") # or bidding ?
statef=resetGraph
else if(BVMSTATE=="BIDDING")
else if(BVMSTATE=="BIDDING") # check the absolute path of the waypointlist csv file in bidding.bzz
statef=bidding
else if(BVMSTATE=="GRAPH_FREE")
statef=DoFree