diff --git a/buzz_scripts/include/bidding/bidding.bzz b/buzz_scripts/include/bidding/bidding.bzz new file mode 100644 index 0000000..8f28c01 --- /dev/null +++ b/buzz_scripts/include/bidding/bidding.bzz @@ -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 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/src/roscontroller.cpp b/src/roscontroller.cpp index ca27a9d..362d477 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -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: