Merge commit 'fd8e8022744c30cb9d59b1dd4cb6cf90f1c728f8' into ros_drones_ws

buzz_scripts/include/act/CA.bzz

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

buzz_scripts/include/act/naviguation.bzz

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

buzz_scripts/include/act/neighborcomm.bzz

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

buzz_scripts/include/act/states.bzz

@@ -5,45 +5,30 @@
 ########################################
 include "utils/vec2.bzz"
 include "act/barrier.bzz"
-#include "act/old_barrier.bzz"
 include "utils/conversions.bzz"
+include "act/naviguation.bzz"
+include "act/CA.bzz"
+include "act/neighborcomm.bzz"
 
 TARGET_ALTITUDE = 15.0 # m.
 BVMSTATE = "TURNEDOFF"
 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
 pic_time = 0
 g_it = 0
 
+# Core state function when on the ground
 function turnedoff() {
     BVMSTATE = "TURNEDOFF"
 }
 
+# Core state function when hovering
 function 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() {
 	BVMSTATE = "LAUNCH"
 	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() {
 	BVMSTATE = "LAUNCH_SWITCH"
 	if(V_TYPE == 0 or V_TYPE == 1) {	# flying vehicle so TAKE_OFF
@@ -81,6 +67,7 @@ function launch_switch() {
 }
 
 gohomeT=100
+# State function to go back to ROSBuzz recorded home GPS position (at takeoff)
 function goinghome() {
   BVMSTATE = "GOHOME"
 	if(gohomeT > 0) {	# TODO: Make a real check if home is reached
@@ -90,6 +77,7 @@ function goinghome() {
   	BVMSTATE = AUTO_LAUNCH_STATE
 }
 
+# Core state function to stop and land.
 function stop() {
   BVMSTATE = "STOP"
 	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() {
   BVMSTATE="PICTURE"
   setgimbal(0.0, 0.0, -90.0, 20.0)
@@ -119,66 +129,7 @@ function take_picture() {
   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() {
 	if(size(targets)>0) {
 		BVMSTATE = "FOLLOW"
@@ -194,7 +145,7 @@ function follow() {
 	}
 }
 
-# converge to centroid
+# State function to converge to centroid
 function aggregate() {
   BVMSTATE="AGGREGATE"
   centroid = neighbors.reduce(function(rid, data, centroid) {
@@ -208,7 +159,7 @@ function aggregate() {
   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() {
   BVMSTATE="PURSUIT"
 	rd = 20.0
@@ -248,7 +199,7 @@ function lj_sum(rid, 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() {
   BVMSTATE="FORMATION"
   # Calculate accumulator
@@ -259,115 +210,17 @@ function formation() {
   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
-					#  })
-					}
-				#}
-    })
-}

buzz_scripts/include/taskallocate/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<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")
+}

buzz_scripts/include/utils/conversions.bzz

@@ -7,12 +7,6 @@ function LimitAngle(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....
 function convert_path(P) {
     var pathR={}

buzz_scripts/include/utils/takeoff_heights.bzz

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

buzz_scripts/main.bzz

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

include/buzzuav_closures.h

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

src/buzzuav_closures.cpp

@@ -14,23 +14,29 @@ namespace buzzuav_closures
 // TODO: Minimize the required global variables and put them in the header
 // static const rosbzz_node::roscontroller* roscontroller_ptr;
 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_NEDpos[2];
-static uint8_t status;
+
-static int cur_cmd = 0;
-static int rc_cmd = 0;
 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 float height = 0;
+
 static bool deque_full = false;
 static int rssi = 0;
 static float raw_packet_loss = 0.0;
 static int filtered_packet_loss = 0;
 static float api_rssi = 0.0;
+
 string WPlistname = "";
 
 std::map<int, buzz_utility::RB_struct> targets_map;
@@ -231,7 +237,7 @@ int buzzuav_moveto(buzzvm_t vm)
   //  DEBUG
   // 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]);
-  buzz_cmd = NAV_SPLINE_WAYPOINT;  // TODO: standard mavros?
+  buzz_cmd = NAV_SPLINE_WAYPOINT;
   return buzzvm_ret0(vm);
 }
 
@@ -386,14 +392,24 @@ int buzzuav_storegoal(buzzvm_t vm)
     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);
   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)
@@ -504,9 +520,9 @@ void rc_set_goto(int id, double latitude, double longitude, double altitude)
 -----------------------------------*/
 {
   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)
@@ -773,15 +789,30 @@ int buzzuav_update_flight_status(buzzvm_t vm)
   buzzvm_tput(vm);
   buzzvm_dup(vm);
   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_dup(vm);
   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_dup(vm);
   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_gstore(vm);
   return vm->state;

src/roscontroller.cpp

@@ -762,6 +762,10 @@ script
         Arm();
         // Registering HOME POINT.
         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)
         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)
 /*
-/ 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);
@@ -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,
                                     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;