diff --git a/buzz_scripts/include/bidding/bidding.bzz b/buzz_scripts/include/bidding/bidding.bzz deleted file mode 100644 index 8f28c01..0000000 --- a/buzz_scripts/include/bidding/bidding.bzz +++ /dev/null @@ -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 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 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") -} diff --git a/buzz_scripts/include/taskallocate/bidding.bzz b/buzz_scripts/include/taskallocate/bidding.bzz index 8f28c01..01fd468 100644 --- a/buzz_scripts/include/taskallocate/bidding.bzz +++ b/buzz_scripts/include/taskallocate/bidding.bzz @@ -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,8 +424,11 @@ 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){ diff --git a/buzz_scripts/include/taskallocate/waypoints_12.csv b/buzz_scripts/include/taskallocate/waypoints_12.csv new file mode 100755 index 0000000..ec4cfdf --- /dev/null +++ b/buzz_scripts/include/taskallocate/waypoints_12.csv @@ -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 + diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index f499263..787ed84 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -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