added Hull polygon from user inputs and minor enhancement to GIS and state machine

buzz_scripts/include/act/barrier.bzz

@@ -9,7 +9,7 @@
 #
 BARRIER_TIMEOUT = 200 # in steps
 BARRIER_VSTIG = 80
-timeW = 0
+timeW = 1
 barrier = nil
 hvs = 0;
 
@@ -19,7 +19,7 @@ hvs = 0;
 #
 function barrier_create() {
   # reset
-  timeW = 0
+  timeW = 1
   # create barrier vstig
   #log("---> Prev. br. ", barrier, " ", BARRIER_VSTIG)
   if(barrier!=nil) {
@@ -61,7 +61,6 @@ function barrier_wait(threshold, transf, resumef, bc) {
     barrier_create()
   
   barrier.put(id, bc)
-  barrier.get(id)
   log("--->BS: ", barrier.size(), " / ", threshold, " (", BARRIER_VSTIG, " - ", barrier.get("d"), ") t= ", timeW)
   #if(barrier.size()-1 >= threshold or barrier.get("d") == 1) {
     if(barrier_allgood(barrier,bc)) {

buzz_scripts/include/act/naviguation.bzz

@@ -2,7 +2,7 @@ GOTO_MAXVEL = 1.5 # m/steps
 GOTO_MAXDIST = 150 # m.
 GOTODIST_TOL = 0.4  # m.
 GOTOANG_TOL = 0.1 # rad.
-GPSlimit = {.1={.lat=45.510400, .lng=-73.610421},
+GPSlimit = {.1={.lat=45.510523, .lng=-73.611118},#{.lat=45.510400, .lng=-73.610421},
 			.2={.lat=45.510896, .lng=-73.608731},
 			.3={.lat=45.510355, .lng=-73.608404},
 			.4={.lat=45.509840, .lng=-73.610072}}
@@ -18,7 +18,7 @@ function goto_gps(transf) {
 	} else {
 		m_navigation = LimitSpeed(m_navigation, 1.0)
 		gf = {.0=m_navigation, .1=vec_from_gps(GPSlimit[1].lat, GPSlimit[1].lng, 0), .2=vec_from_gps(GPSlimit[2].lat, GPSlimit[2].lng, 0), .3=vec_from_gps(GPSlimit[3].lat, GPSlimit[3].lng, 0), .4=vec_from_gps(GPSlimit[4].lat, GPSlimit[4].lng, 0)}
-		geofence(gf)
+		#geofence(gf)
 		#m_navigation = LCA(m_navigation)
 		goto_abs(m_navigation.x, m_navigation.y, cur_goal.altitude - pose.position.altitude, 0.0)
 	}

buzz_scripts/include/act/neighborcomm.bzz

@@ -1,13 +1,26 @@
 # listens to commands from the remote control (web, commandline, rcclient node, etc)
 function rc_cmd_listen() {
-  if(flight.rc_cmd==22) {
+	if(BVMSTATE=="TURNEDOFF") {
-		log("cmd 22")
+		if(flight.rc_cmd==400) {	#ARM
+			flight.rc_cmd=0
+			arm()
+			neighbors.broadcast("cmd", 400)
+		} else if (flight.rc_cmd==401){	#DISARM
+			flight.rc_cmd=0
+			disarm()
+			neighbors.broadcast("cmd", 401)
+		} else if(flight.rc_cmd==22) {	#TAKEOFF\LAUNCH
 			flight.rc_cmd=0
 			BVMSTATE = "LAUNCH"
 			neighbors.broadcast("cmd", 22)
-	} else if(flight.rc_cmd==21) {
+		} else if (flight.rc_cmd==777){	#SYNC
+			flight.rc_cmd=0
+			reinit_time_sync()
+			neighbors.broadcast("cmd", 777)
+		}
+	} 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"
@@ -18,24 +31,10 @@ function rc_cmd_listen() {
 			barrier_set(ROBOTS, "GOHOME", BVMSTATE, 20)
 			barrier_ready(20)
 			neighbors.broadcast("cmd", 20)
-#	} else if(flight.rc_cmd==16) {
+		} else if(BVMSTATE!="BARRIERWAIT" and BVMSTATE!="LAUNCH") {
-#		flight.rc_cmd=0
+			if (flight.rc_cmd==666){
-#		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)
@@ -44,12 +43,14 @@ function rc_cmd_listen() {
 			} else if (flight.rc_cmd==901){
 				flight.rc_cmd=0
 				destroyGraph()
-		barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 901)
+				resetWP()
+				barrier_set(ROBOTS, "DEPLOY", BVMSTATE, 901)
 				barrier_ready(901)
 				neighbors.broadcast("cmd", 901)
 			} else if (flight.rc_cmd==902){
 				flight.rc_cmd=0
 				destroyGraph()
+				resetWP()
 				barrier_set(ROBOTS, "WAYPOINT", BVMSTATE, 902)
 				barrier_ready(902)
 				neighbors.broadcast("cmd", 902)
@@ -68,59 +69,60 @@ function rc_cmd_listen() {
 				neighbors.broadcast("cmd", 904)
 			}
 		}
+	}
+}
 
 # 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(BVMSTATE=="TURNEDOFF") {
-					if(value==22 and BVMSTATE=="TURNEDOFF") {
+					if(value==22) {
 						BVMSTATE = "LAUNCH"
-					}else if(value==20) {
+					} else if(value==400) {
+						arm()
+					} else if(value==401){
+						disarm()
+					} else if(value==777){
+						reinit_time_sync()
+					}
+				} else {
+					if(value==20) {
 						AUTO_LAUNCH_STATE = "IDLE"
 						BVMSTATE = "GOHOME"
-					} else if(value==21 and BVMSTATE!="TURNEDOFF") {
+					} else if(value==21 ) {
 						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
+					}else if(BVMSTATE!="BARRIERWAIT" and BVMSTATE!="LAUNCH") {
+						if(value==900){	# Shapes
 							barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900)
 							#barrier_ready(900)
 							#neighbors.broadcast("cmd", 900)
-					} else if(value==901 and BVMSTATE!="BARRIERWAIT"){	# Pursuit
+						} else if(value==901 and BVMSTATE!="DEPLOY" ){	# Pursuit
 							destroyGraph()
-						barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 901)
+							barrier_set(ROBOTS, "DEPLOY", BVMSTATE, 901)
 							#barrier_ready(901)
 							#neighbors.broadcast("cmd", 901)
-					} else if(value==902 and BVMSTATE!="BARRIERWAIT" and BVMSTATE!="WAYPOINT"){	# Waypoint
+						} else if(value==902 and BVMSTATE!="WAYPOINT" and BVMSTATE!="TURNEDOFF"){	# Waypoint
 							destroyGraph()
 							barrier_set(ROBOTS, "WAYPOINT", BVMSTATE, 902)
 							#barrier_ready(902)
 							#neighbors.broadcast("cmd", 902)
-					} else if(value==903 and BVMSTATE!="BARRIERWAIT" and BVMSTATE!="POTENTIAL"){	# Formation
+						} else if(value==903 and BVMSTATE!="POTENTIAL" and BVMSTATE!="TURNEDOFF"){	# Formation
 							destroyGraph()
-						resetWP()
 							barrier_set(ROBOTS, "POTENTIAL", BVMSTATE, 903)
 							#barrier_ready(903)
 							#neighbors.broadcast("cmd", 903)
-					} else if(value==904 and BVMSTATE!="BARRIERWAIT" and BVMSTATE!="IDLE"){	# idle
+						} else if(value==904 and BVMSTATE!="IDLE" and BVMSTATE!="TURNEDOFF"){	# idle
 							destroyGraph()
+							resetWP()
 							barrier_set(ROBOTS, "IDLE", BVMSTATE, 904)
 							#barrier_ready(904)
 							#neighbors.broadcast("cmd", 904)
-					} 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
-					#  })
 						}
-				#}
+					}
+				}
     })
 }
 
@@ -141,7 +143,8 @@ function check_rc_wp() {
 			v_wp.put(rc_goto.id,ls)
 			reset_rc()
 		}
-	}
+	} else
+		v_wp.get(0)
 }
 
 function resetWP() {

buzz_scripts/include/act/states.bzz

@@ -6,6 +6,7 @@
 include "utils/vec2.bzz"
 include "act/barrier.bzz"
 include "utils/conversions.bzz"
+include "utils/quickhull.bzz"
 include "act/naviguation.bzz"
 include "act/CA.bzz"
 include "act/neighborcomm.bzz"
@@ -32,6 +33,7 @@ function idle() {
 # Core state function to launch the robot: takeoff and wait for others, or stop (land)
 function launch() {
 	BVMSTATE = "LAUNCH"
+	log("AUTO_LAUNCH_STATE: ", AUTO_LAUNCH_STATE)
 	if(V_TYPE == 0 or V_TYPE == 1) {	# flying vehicle so TAKE_OFF
 		homegps = {.lat=pose.position.latitude, .long=pose.position.longitude}
 		if(pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
@@ -71,12 +73,14 @@ gohomeT=100
 # State function to go back to ROSBuzz recorded home GPS position (at takeoff)
 function goinghome() {
   BVMSTATE = "GOHOME"
+	if(V_TYPE == 0 or V_TYPE == 1) {	# flying vehicle so TAKE_OFF
 		if(gohomeT > 0) {	# TODO: Make a real check if home is reached
 			gohome()
 			gohomeT = gohomeT - 1
 		} else
 			BVMSTATE = AUTO_LAUNCH_STATE
 	}
+}
 
 # Core state function to stop and land.
 function stop() {
@@ -85,14 +89,14 @@ function stop() {
 		neighbors.broadcast("cmd", 21)
 		uav_land()
 		if(pose.position.altitude <= 0.2) {
-			BVMSTATE = "TURNEDOFF"
+			BVMSTATE = "STOP"
-			#barrier_set(ROBOTS,"TURNEDOFF","STOP", 21)
+			barrier_set(ROBOTS,"TURNEDOFF","STOP", 21)
-			#barrier_ready(21)
+			barrier_ready(21)
 		}
 	} else {
-			BVMSTATE = "TURNEDOFF"
+			BVMSTATE = "STOP"
-			#barrier_set(ROBOTS,"TURNEDOFF","STOP", 21)
+			barrier_set(ROBOTS,"TURNEDOFF","STOP", 21)
-			#barrier_ready(21)
+			barrier_ready(21)
 	}
 }
 
@@ -214,6 +218,8 @@ function lj_sum(rid, data, accum) {
 function lennardjones() {
   BVMSTATE="POTENTIAL"
 	check_rc_wp()
+	if(V_TYPE == 2)		# NOT MOVING!
+		return
   # Calculate accumulator
   accum_lj = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
   if(neighbors.count() > 0)
@@ -243,22 +249,54 @@ function lennardjones() {
 counter = 0
 function voronoicentroid() {
   BVMSTATE="DEPLOY"
-	if(counter==0 and id!=0) {
+	check_rc_wp()
-		pts = getbounds()
+	log("V_wp size:",v_wp.size())
-		pts[0] = {.x=0, .y=0}	#add itself
+	if(V_TYPE == 2)		# NOT MOVING!
-		it_pts = 1
+		return
-		#table_print(pts)
+	if(not(v_wp.size() > 0))
+		return
+	it_pts = 0
+	att = {}
+	v_wp.foreach(
+    function(key, value, robot){
+			wp = unpackWP2i(value)
+			if(key > 99)
+				log("Nothing planed for the repulsors yet....")
+			else if(key > 49)
+				att[it_pts]=vec_from_gps(wp.lat, wp.lon, 0)
+			it_pts = it_pts + 1
+	})
+	# Boundaries from Geofence
+	#it_pts = 0
+	#foreach(GPSlimit, function(key, value) {
+	#		bounds[it_pts]=vec_from_gps(value.lat, value.lng, 0)
+	#		it_pts = it_pts + 1
+	#})
+	# Boundaries from user attractors
+	#att = {.0=vec_from_gps(45.510283, -73.609633, 0), .1=vec_from_gps(45.510398, -73.609281, 0)}
+	bounds = QuickHull(att)
+	if(size(bounds)<3 )
+		return
+	if(counter==0) {
+		pts = {.np=size(bounds)}
+		it_pts = 0
+		foreach(bounds, function(key, value) {
+			pts[it_pts]=value
+			it_pts = it_pts + 1
+		})
+		pts[it_pts] = {.x=0, .y=0}	#add itself
+		it_pts = it_pts + 1
 		if(neighbors.count() > 0) {
 			neighbors.foreach(function(rid, data) {
 				if(rid!=0){	#remove GS (?)
-					pts[it_pts]=math.vec2.newp(data.distance,data.azimuth+pts.oa)
+					pts[it_pts]=math.vec2.newp(data.distance,data.azimuth)
 					it_pts = it_pts + 1
 				}
 			})
 			#table_print(pts)
 			voronoi(pts)
 		}
-		counter=10
+		counter=4
 	}
 	counter=counter-1
 
@@ -269,28 +307,6 @@ function voronoicentroid_done() {
   BVMSTATE="DEPLOY"
 }
 
-function getbounds(){
-	var RBlimit = {}
-	foreach(GPSlimit, function(key, value) {
-		RBlimit[key] = vec_from_gps(value.lat, value.lng, 0)
-	})
-	minY = RBlimit[1].y
-	maxY = RBlimit[1].y
-	minX = RBlimit[1].x
-	maxX = RBlimit[1].x
-	foreach(RBlimit, function(key, value) {
-		if(value.y<minY)
-			minY = value.y
-		if(value.y>maxY)
-			maxY = value.y
-		if(value.x>maxX)
-			maxX = value.x
-		if(value.x<minX)
-			minX = value.x
-	})
-	return {.miny=minY, .minx=minX, .maxy=maxY, .maxx=maxX, .oa=0.0}
-}
-
 # Custom state function for the developer to play with
 function cusfun(){
 	BVMSTATE="CUSFUN"

buzz_scripts/include/taskallocate/graphformGPS.bzz

@@ -110,7 +110,7 @@ function r2i(value){
 #
 function find(table,value){
 	var ind=nil
-	var i=0
+	i=0
 	while(i<size(table)){
 		if(table[i]==value)
 			ind=i
@@ -187,7 +187,7 @@ function unpack_guide_msg(recv_value){
 #
 function target4label(nei_id){
     var return_val="miss"
-    var i=0
+    i=0
     while(i<size(lock_neighbor_id)){
 		if(lock_neighbor_id[i]==nei_id){
 			return_val=lock_neighbor_dis[i]
@@ -216,7 +216,7 @@ function LJ_vec(i){
 #calculate the motion vector
 #
 function motion_vector(){
-    var i=0
+    i=0
     var m_vector={.x=0.0,.y=0.0}
     while(i<m_neighbourCount){
 		#calculate and add the motion vector
@@ -272,7 +272,7 @@ function Get_DisAndAzi(t_id){
 function UpdateNodeInfo(){
 	#Collect informaiton
 	#Update information
-	var i=0
+	i=0
 
 	while(i<m_neighbourCount){
 		if(m_MessageState[i]=="GRAPH_JOINED"){
@@ -316,8 +316,8 @@ function DoFree() {
 	#find a set of joined robots
 	var setJoinedLabels={}
 	var setJoinedIndexes={}
-	var i=0
+	i=0
-	var j=0
+	j=0
 	while(i<m_neighbourCount){
 		if(m_MessageState[i]=="GRAPH_JOINED"){
 		setJoinedLabels[j]=m_MessageLabel[i]
@@ -367,7 +367,7 @@ function DoAsking(){
 	}else {
 		UpdateGraph()
 		#look for response from predecessor
-		var i=0
+		i=0
 		var psResponse=-1
 		while(i<m_neighbourCount and psResponse==-1){
 			#the respond robot in joined state
@@ -448,7 +448,7 @@ function DoJoining(){
 
 function set_rc_goto() {
   #get information of pred
-	var i=0
+	i=0
 	var IDofPred=-1
 	while(i<m_neighbourCount and IDofPred==-1){
 		if(m_MessageLabel[i]==m_vecNodes[m_nLabel].Pred and m_MessageState[i]=="GRAPH_JOINED")
@@ -497,8 +497,8 @@ function DoJoined(){
 
 		#collect all requests
 		var mapRequests={}
-		var i=0
+		i=0
-		var j=0
+		j=0
 		var ReqLabel
 		var JoiningLabel
 		var seenPred=0
@@ -588,7 +588,7 @@ function DoLock() {
 	#record neighbor distance
 	lock_neighbor_id={}
 	lock_neighbor_dis={}
-	var i=0
+	i=0
 	while(i<m_neighbourCount){
 		lock_neighbor_id[i]=m_messageID[i]
 		lock_neighbor_dis[i]=m_MessageRange[i]

buzz_scripts/include/utils/quickhull.bzz

@@ -0,0 +1,122 @@
+hullPts = {}
+hullC = 0
+
+# When called returns a list of points that forms a convex hull around 
+#   the listPts Given (http://adultstudylife.blogspot.com/2016/06/quick-hull-in-python.html)
+function QuickHull(focalpts) {
+    listPts = {}
+    hullPts = {}
+    jp = 0
+    foreach(focalpts, function(key, pt) {
+        ip = 0
+        while(ip<6){
+            listPts[jp*6+ip]=math.vec2.add(pt,math.vec2.newp(20, ip*math.pi/3))
+            #log("Created new pt: ", listPts[j*6+i].x, listPts[j*6+i].y, "(", pt.x, pt.y, ")")
+            ip=ip+1
+        }
+        jp=jp+1
+    })
+
+    # get the min, and max from the list of points
+    min_max = get_min_max_x(listPts)
+
+    hullPts = quickhull_rec(listPts, min_max.min, min_max.max)
+
+    hullPts = quickhull_rec(listPts, min_max.max, min_max.min)
+
+    return hullPts
+} 
+
+# Does the sorting for the quick hull sorting algorithm
+function quickhull_rec(listPts, minp, maxp) {
+    left_of_line_pts = get_points_left_of_line(minp, maxp, listPts)
+
+    ptC = point_max_from_line(minp, maxp, left_of_line_pts)
+
+    if(size(ptC) < 1){
+        hullPts[hullC] = maxp    # max
+        hullC = hullC +1
+        return hullPts
+        #log("Add pt:", maxp.x, maxp.y)
+    }
+
+    hullPts = quickhull_rec(left_of_line_pts, minp, ptC)
+
+    hullPts = quickhull_rec(left_of_line_pts, ptC, maxp)
+
+    return hullPts
+}
+
+# Returns all points that are LEFT of a line start->end
+function get_points_left_of_line(minp, maxp, listPts) {
+    pts = {}
+    ih = 0
+
+    foreach(listPts, function(key, pt) {
+        if(isCCW(minp, maxp, pt)){
+            pts[ih]=pt
+            ih=ih+1
+        }   
+    })
+
+    return pts
+}
+
+# Returns the maximum point from a line start->end
+function point_max_from_line(minp, maxp, points) {
+    max_dist = 0
+    max_point = {}
+
+    foreach(points, function(key, point) {
+        if((not(math.vec2.equal(point, minp))) and (not(math.vec2.equal(point, maxp)))) {
+            #log("Get distance of pt: ", point.x, point.y)
+            dist = distance_toline(minp, maxp, point)
+            if(dist > max_dist) {
+                max_dist = dist
+                max_point = point
+            }
+        }
+    })
+
+    return max_point
+}
+
+function get_min_max_x(list_pts) {
+    min_x = 100000
+    max_x = 0
+    min_y = 0
+    max_y = 0
+
+    foreach(list_pts, function(key, point) {
+        if(point.x < min_x){
+            min_x = point.x
+            min_y = point.y
+        }
+        if(point.x > max_x){
+            max_x = point.x
+            max_y = point.y
+        }
+    })
+
+    return {.min=math.vec2.new(min_x, min_y), .max=math.vec2.new(max_x, max_y)}
+}
+
+# Given a line of start->end, will return the distance that
+#    point, pt, is from the line.
+function distance_toline(start, end, pt) { # pt is the point
+    x1 = start.x
+    y1 = start.y
+    x2 = end.x
+    y2 = end.y
+    x0 = pt.x
+    y0 = pt.y
+    nom = math.abs((y2 - y1) * x0 - (x2 - x1) * y0 + x2 * y1 - y2 * x1)
+    denom = math.vec2.length(math.vec2.sub(end,start))
+    result = nom / denom
+    return result
+}
+
+# Tests whether the turn formed by A, B, and C is ccw
+function isCCW(A, B, C){
+    return (B.x - A.x) * (C.y - A.y) > (B.y - A.y) * (C.x - A.x)
+}

buzz_scripts/include/utils/table.bzz

@@ -1,8 +1,12 @@
 function table_print(t) {
+	if(t==nil)
+		log("Table do not exist!")
+	else {
 		foreach(t, function(key, value) {
       log(key, " -> ", value)
     })
 	}
+}
 
 function table_copy(t) {
   var t2 = {}
@@ -16,8 +20,8 @@ function table_copy(t) {
 #return the number of value in table
 #
 function count(table,value){
-	var number=0
+	number=0
-	var i=0
+	i=0
 	while(i<size(table)){
 		if(table[i]==value){
 			number=number+1
@@ -27,17 +31,17 @@ function count(table,value){
 	return number
 }
 #
-#map from int to state
+# map from int to state - vstig serialization limits to 9....
 #
 function i2s(value){
 	if(value==1){
-	return "GRAPH_ASKING"
+	return "IDLE"
 	}
 	else if(value==2){
-	return "GRAPH_JOINING"
+	return "DEPLOY"
 	}
 	else if(value==3){
-	return "GRAPH_JOINED"
+	return "STOP"
 	}
 	else if(value==4){
 	return "TURNEDOFF"
@@ -62,16 +66,16 @@ function i2s(value){
 	}
 }
 #
-#map from state to int
+# map from state to int - vstig serialization limits to 9....
 #
 function s2i(value){
-	if(value=="GRAPH_ASKING"){
+	if(value=="IDLE"){
 	return 1
 	}
-	else if(value=="GRAPH_JOINING"){
+	else if(value=="DEPLOY"){
 	return 2
 	}
-	else if(value=="GRAPH_JOINED"){
+	else if(value=="STOP"){
 	return 3
 	}
 	else if(value=="TURNEDOFF"){

buzz_scripts/include/utils/vec2.bzz

@@ -105,3 +105,7 @@ math.vec2.scale = function(v, s) {
 math.vec2.dot = function(v1, v2) {
   return v1.x * v2.x + v1.y * v2.y
 }
+
+math.vec2.equal = function(v1, v2) {
+  return (v1.x == v2.x) and (v1.y == v2.y)
+}

buzz_scripts/log/anim_voronoi_cells.py

@@ -7,10 +7,11 @@ import csv
 
 fig = plt.figure()
 ax = fig.add_subplot(1,1,1)
+fig.set_tight_layout(True)
 axes = plt.gca()
 axes.set_xlim([-70,70])
 axes.set_ylim([-70,70])
-datafile = open('src/rosbuzz/buzz_scripts/log/voronoi_4.csv', 'r')
+datafile = open('src/rosbuzz/buzz_scripts/log/voronoi_3.csv', 'r')
 Vorreader = csv.reader(datafile, delimiter=',')
 
 def animate(i):
@@ -25,4 +26,5 @@ def animate(i):
         return
 
 ani = animation.FuncAnimation(fig, animate, interval=250)
+ani.save('Voronoi.gif', dpi=80, writer='imagemagick')
 plt.show()

buzz_scripts/main.bzz

@@ -3,21 +3,16 @@ include "update.bzz"
 # it requires an 'action' function to be defined here.
 include "act/states.bzz"
 include "utils/table.bzz"
-include "plan/rrtstar.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 = "DEPLOY"
+AUTO_LAUNCH_STATE = "IDLE"
-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.
 
 #####
@@ -36,7 +31,7 @@ else
 function init() {
   	init_stig()
 	init_swarm()
-	init_bidding()
+	#init_bidding()
 	
 	TARGET_ALTITUDE = takeoff_heights[id]
 	
@@ -44,7 +39,7 @@ function init() {
 	nei_cmd_listen()
 
     # Starting state: TURNEDOFF to wait for user input.
-	BVMSTATE = "LAUNCH"
+	BVMSTATE = "TURNEDOFF"
 }
 
 # Executed at each time step.

include/VoronoiDiagramGenerator.h

@@ -68,6 +68,8 @@ struct	Freelist
 struct Point	
 {
 	float x,y;
+    Point(): x( 0.0 ), y( 0.0 ) { }
+    Point( float x, float y ): x( x ), y( y ) { }
 };
 
 // structure used both for sites and for vertices 
@@ -206,12 +208,6 @@ private:
 	void circle(float x, float y, float radius);
 	void range(float minX, float minY, float maxX, float maxY);
 
-
-	bool onSegment(Point p, Point q, Point r);
-	int orientation(Point p, Point q, Point r);
-	bool doIntersect(Point p1, Point q1, Point p2, Point q2);
-
-
 	struct  Freelist	hfl;
 	struct	Halfedge *ELleftend, *ELrightend;
 	int 	ELhashsize;

src/VoronoiDiagramGenerator.cpp

@@ -742,60 +742,6 @@ void VoronoiDiagramGenerator::plotinit()
 	range(pxmin, pymin, pxmax, pymax);
 }
 
-
-// Given three colinear points p, q, r, the function checks if 
-// point q lies on line segment 'pr' 
-bool VoronoiDiagramGenerator::onSegment(Point p, Point q, Point r) 
-{ 
-    if (q.x <= std::max(p.x, r.x) && q.x >= std::min(p.x, r.x) && 
-            q.y <= std::max(p.y, r.y) && q.y >= std::min(p.y, r.y)) 
-        return true; 
-    return false; 
-}
-// To find orientation of ordered triplet (p, q, r). 
-// The function returns following values 
-// 0 --> p, q and r are colinear 
-// 1 --> Clockwise 
-// 2 --> Counterclockwise 
-int VoronoiDiagramGenerator::orientation(Point p, Point q, Point r) 
-{ 
-    int val = (q.y - p.y) * (r.x - q.x) - 
-              (q.x - p.x) * (r.y - q.y); 
-  
-    if (val == 0) return 0;  // colinear 
-    return (val > 0)? 1: 2; // clock or counterclock wise 
-}
-// The function that returns true if line segment 'p1q1' 
-// and 'p2q2' intersect. 
-bool VoronoiDiagramGenerator::doIntersect(Point p1, Point q1, Point p2, Point q2) 
-{ 
-    // Find the four orientations needed for general and 
-    // special cases 
-    int o1 = orientation(p1, q1, p2); 
-    int o2 = orientation(p1, q1, q2); 
-    int o3 = orientation(p2, q2, p1); 
-    int o4 = orientation(p2, q2, q1); 
-  
-    // General case 
-    if (o1 != o2 && o3 != o4) 
-        return true; 
-  
-    // Special Cases 
-    // p1, q1 and p2 are colinear and p2 lies on segment p1q1 
-    if (o1 == 0 && onSegment(p1, p2, q1)) return true; 
-  
-    // p1, q1 and p2 are colinear and q2 lies on segment p1q1 
-    if (o2 == 0 && onSegment(p1, q2, q1)) return true; 
-  
-    // p2, q2 and p1 are colinear and p1 lies on segment p2q2 
-    if (o3 == 0 && onSegment(p2, p1, q2)) return true; 
-  
-     // p2, q2 and q1 are colinear and q1 lies on segment p2q2 
-    if (o4 == 0 && onSegment(p2, q1, q2)) return true; 
-  
-    return false; // Doesn't fall in any of the above cases 
-}
-
 void VoronoiDiagramGenerator::clip_line(struct Edge *e)
 {
 	struct Site *s1, *s2;
@@ -805,7 +751,6 @@ void VoronoiDiagramGenerator::clip_line(struct Edge *e)
 	x2 = e->reg[1]->coord.x;
 	y1 = e->reg[0]->coord.y;
 	y2 = e->reg[1]->coord.y;
-	//printf("Clip line for edges: %d", e->edgenbr);
 
 	//if the distance between the two points this line was created from is less than 
 	//the square root of 2, then ignore it
@@ -905,7 +850,7 @@ void VoronoiDiagramGenerator::clip_line(struct Edge *e)
 		{	y2 = pymin; x2 = (e -> c - y2)/e -> a;};
 	};
 	
-	//printf("\nPushing line (%f,%f,%f,%f)",x1,y1,x2,y2);
+	//printf("Pushing line (%f,%f,%f,%f)\n",x1,y1,x2,y2);
 	line(x1,y1,x2,y2,e->sites);
 }
 

src/buzzuav_closures.cpp

@@ -42,16 +42,11 @@ static bool logVoronoi = false;
 std::ofstream voronoicsv;
 
 struct Point
-{ 
-    int x; 
-    int y; 
-};
-struct Pointf 
 { 
     float x; 
     float y;
-    Pointf(): x( 0.0 ), y( 0.0 ) { }
+    Point(): x( 0.0 ), y( 0.0 ) { }
-    Pointf( float x, float y ): x( x ), y( y ) { }
+    Point( float x, float y ): x( x ), y( y ) { }
 };
 string WPlistname = "";
 
@@ -224,6 +219,7 @@ void check_targets_sim(double lat, double lon, double *res)
 / check if a listed target is close
 ----------------------------------------------------------- */
 {
+  float visibility_radius = 5.0;
   map<int, buzz_utility::RB_struct>::iterator it;
   for (it = wplist_map.begin(); it != wplist_map.end(); ++it)
   {
@@ -231,7 +227,7 @@ void check_targets_sim(double lat, double lon, double *res)
     double ref[2]={lat, lon};
     double tar[2]={it->second.latitude, it->second.longitude};
     rb_from_gps(tar, rb, ref);
-    if(rb[0]<3.0){
+    if(rb[0] < visibility_radius){
       ROS_WARN("FOUND A TARGET!!! [%i]", it->first);
       res[0] = it->first;
       res[1] = it->second.latitude;
@@ -272,164 +268,6 @@ int buzz_exportmap(buzzvm_t vm)
   return buzzvm_ret0(vm);
 }
 
-float pol_area(vector <Pointf> vert) {
-  float a = 0.0;
-  vector <Pointf>::iterator it;
-  vector <Pointf>::iterator next;
-  for (it = vert.begin(); it != vert.end()-1; ++it){
-    next = it+1;
-    a += it->x * next->y - next->x * it->y;
-  }
-  a *= 0.5;
-  //ROS_INFO("Polygon area: %f",a);
-  return a;
-}
-
-double* polygone_center(vector <Pointf> vert, double *c) {
-  float A = pol_area(vert);
-  int i1 = 1;
-  vector <Pointf>::iterator it;
-  vector <Pointf>::iterator next;
-  for (it = vert.begin(); it != vert.end()-1; ++it){
-    next = it+1;
-    float t = it->x*next->y - next->x*it->y;
-    c[0] += (it->x+next->x) * t;
-    c[1] += (it->y+next->y) * t;
-  }
-  c[0] = c[0] / (6.0 * A);
-  c[1] = c[1] / (6.0 * A);
-  return c;
-}
-
-float clockwise_angle_of( const Pointf& p )
-{
-  return atan2(p.y,p.x);
-}
-
-bool clockwise_compare_points( const Pointf& a, const Pointf& b )
-{
-  return clockwise_angle_of( a ) < clockwise_angle_of( b );
-}
-
-int voronoi_center(buzzvm_t vm) {
-  float *xValues;//[4] = {-22, -17, 4,22};
-	float *yValues;//[4] = {-9, 31,13,-5};
-  float minx, miny, maxx, maxy;
-	buzzvm_lnum_assert(vm, 1);
-  // Get the parameter
-  buzzvm_lload(vm, 1);
-  buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE);    // dictionary
-  buzzobj_t t = buzzvm_stack_at(vm, 1);
-   
-  buzzvm_dup(vm);
-  buzzvm_pushs(vm, buzzvm_string_register(vm, "maxx", 1));
-  buzzvm_tget(vm);
-  maxx = buzzvm_stack_at(vm, 1)->f.value;
-  buzzvm_pop(vm);
-  buzzvm_dup(vm);
-  buzzvm_pushs(vm, buzzvm_string_register(vm, "maxy", 1));
-  buzzvm_tget(vm);
-  maxy = buzzvm_stack_at(vm, 1)->f.value;
-  buzzvm_pop(vm);
-  buzzvm_dup(vm);
-  buzzvm_pushs(vm, buzzvm_string_register(vm, "minx", 1));
-  buzzvm_tget(vm);
-  minx = buzzvm_stack_at(vm, 1)->f.value;
-  buzzvm_pop(vm);
-  buzzvm_dup(vm);
-  buzzvm_pushs(vm, buzzvm_string_register(vm, "miny", 1));
-  buzzvm_tget(vm);
-  miny = buzzvm_stack_at(vm, 1)->f.value;
-  buzzvm_pop(vm);
-  buzzvm_dup(vm);
-  buzzvm_pushs(vm, buzzvm_string_register(vm, "oa", 1));
-  buzzvm_tget(vm);
-  float offset_angle = buzzvm_stack_at(vm, 1)->f.value;
-  buzzvm_pop(vm);
-
-  long count = buzzdict_size(t->t.value)-5;
-  xValues = new float[count];
-  yValues = new float[count];
-  for(int32_t i = 0; i < count; ++i) {
-    buzzvm_dup(vm);
-    buzzvm_pushi(vm, i);
-    buzzvm_tget(vm);
-   
-    buzzvm_dup(vm);
-    buzzvm_pushs(vm, buzzvm_string_register(vm, "x", 1));
-    buzzvm_tget(vm);
-    //ROS_INFO("---x-->%f",buzzvm_stack_at(vm, 1)->f.value);
-    xValues[i] = buzzvm_stack_at(vm, 1)->f.value;
-    buzzvm_pop(vm);
-    buzzvm_dup(vm);
-    buzzvm_pushs(vm, buzzvm_string_register(vm, "y", 1));
-    buzzvm_tget(vm);
-    //ROS_INFO("---y-->%f",buzzvm_stack_at(vm, 1)->f.value);
-    yValues[i] = buzzvm_stack_at(vm, 1)->f.value;
-    buzzvm_pop(vm);
-
-    buzzvm_pop(vm);
-  }
-
-	VoronoiDiagramGenerator vdg;
-	vdg.generateVoronoi(xValues,yValues,count, minx, maxx, miny, maxy,3);
-  if(logVoronoi)  voronoicsv << ros::Time::now().toNSec() << ",";
-	vdg.resetIterator();
-
-	float x1,y1,x2,y2;
-  int s[2];
-  vector <Pointf> cell_vert;
-  int i=0;
-	while(vdg.getNext(x1,y1,x2,y2,s))
-	{
-		//ROS_INFO("GOT Line (%f,%f)->(%f,%f) between sites %d,%d\n",x1,y1,x2,y2,s[0],s[1]);
-    if(logVoronoi)  voronoicsv << x1 << "," << y1 << "," << x2 << "," << y2 << "," << s[0] << "," << s[1] << ",";
-    i++;
-    if((s[0]==0 || s[1]==0) && sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1))>0.1) {
-      if(cell_vert.empty()){
-        cell_vert.push_back(Pointf(x1,y1));
-        cell_vert.push_back(Pointf(x2,y2));
-      } else {
-        bool alreadyin = false;
-        vector <Pointf>::iterator itc;
-        for (itc = cell_vert.begin(); itc != cell_vert.end(); ++itc) {
-          double dist = sqrt((itc->x - x1) * (itc->x - x1) + (itc->y - y1) * (itc->y - y1));
-          if(dist < 0.1) {
-            alreadyin = true;
-            break;
-          }
-        }
-        if(!alreadyin)
-          cell_vert.push_back(Pointf(x1, y1));
-        alreadyin = false;
-        for (itc = cell_vert.begin(); itc != cell_vert.end(); ++itc) {
-          double dist = sqrt((itc->x - x2) * (itc->x - x2) + (itc->y - y2) * (itc->y - y2));
-          if(dist < 0.1) {
-            alreadyin = true;
-            break;
-          }
-        }
-        if(!alreadyin)
-          cell_vert.push_back(Pointf(x2, y2));
-      }
-    }
-	}
-  std::sort( cell_vert.begin(), cell_vert.end(), clockwise_compare_points );
-  cell_vert.push_back(cell_vert[0]);
-  
-  double center_dist[2] = {0.0, 0.0};
-  polygone_center(cell_vert, center_dist);
-  if(logVoronoi)  voronoicsv << center_dist[0] << "," << center_dist[1] << std::endl;
-  center_dist[0]/=2;
-  center_dist[1]/=2;
-  double gps[3];
-  gps_from_vec(center_dist, gps);
-  //ROS_INFO("[%i] Voronoi cell center: %f, %f, %f, %f", buzz_utility::get_robotid(), center_dist[0], center_dist[1], gps[0], gps[1]);
-  set_gpsgoal(gps);
-
-  return buzzvm_ret0(vm);
-}
-
 /*
  *  Geofence(): test for a point in a polygon
  *     TAKEN from https://www.geeksforgeeks.org/how-to-check-if-a-given-point-lies-inside-a-polygon/  
@@ -488,6 +326,21 @@ bool doIntersect(Point p1, Point q1, Point p2, Point q2)
     return false; // Doesn't fall in any of the above cases 
 }
 
+float clockwise_angle_of( const Point& p )
+{
+  return atan2(p.y,p.x);
+}
+
+bool clockwise_compare_points( const Point& a, const Point& b )
+{
+  return clockwise_angle_of( a ) < clockwise_angle_of( b );
+}
+
+void sortclose_polygon(vector <Point> *P){
+  std::sort( P->begin(), P->end(), clockwise_compare_points );
+  P->push_back((*P)[0]);
+}
+
 int buzzuav_geofence(buzzvm_t vm)
 {
     bool onedge = false;
@@ -512,7 +365,7 @@ int buzzuav_geofence(buzzvm_t vm)
       buzzvm_dup(vm);
       buzzvm_pushs(vm, buzzvm_string_register(vm, "x", 1));
       buzzvm_tget(vm);
-      tmp = round(buzzvm_stack_at(vm, 1)->f.value*10);
+      tmp = buzzvm_stack_at(vm, 1)->f.value;
       //ROS_INFO("[%i]---x-->%i",buzz_utility::get_robotid(), tmp);
       if(i==0)
         P.x = tmp;
@@ -522,7 +375,7 @@ int buzzuav_geofence(buzzvm_t vm)
       buzzvm_dup(vm);
       buzzvm_pushs(vm, buzzvm_string_register(vm, "y", 1));
       buzzvm_tget(vm);
-      tmp = round(buzzvm_stack_at(vm, 1)->f.value*10);
+      tmp = buzzvm_stack_at(vm, 1)->f.value;
       //ROS_INFO("[%i]---y-->%i",buzz_utility::get_robotid(), tmp);
       if(i==0)
         P.y = tmp;
@@ -532,6 +385,8 @@ int buzzuav_geofence(buzzvm_t vm)
 
       buzzvm_pop(vm);
     }
+    // TODO: use vector
+    //sortclose_polygon(&V);
 
     // simple polygon: rectangle, 4 points
     int n = 4;
@@ -573,6 +428,276 @@ int buzzuav_geofence(buzzvm_t vm)
   return buzzvm_ret0(vm);
 }
 
+float pol_area(vector <Point> vert) {
+  float a = 0.0;
+  //ROS_INFO("Polygone %d edges area.",vert.size());
+  vector <Point>::iterator it;
+  vector <Point>::iterator next;
+  for (it = vert.begin(); it != vert.end()-1; ++it){
+    next = it+1;
+    a += it->x * next->y - next->x * it->y;
+  }
+  a *= 0.5;
+  //ROS_INFO("Polygon area: %f",a);
+  return a;
+}
+
+double* polygone_center(vector <Point> vert, double *c) {
+  float A = pol_area(vert);
+  int i1 = 1;
+  vector <Point>::iterator it;
+  vector <Point>::iterator next;
+  for (it = vert.begin(); it != vert.end()-1; ++it){
+    next = it+1;
+    float t = it->x*next->y - next->x*it->y;
+    c[0] += (it->x+next->x) * t;
+    c[1] += (it->y+next->y) * t;
+  }
+  c[0] = c[0] / (6.0 * A);
+  c[1] = c[1] / (6.0 * A);
+  return c;
+}
+
+double numerator  ( Point A, Point C, Point E, Point F ) { return (A.y - C.y) * (F.x - E.x) - (A.x - C.x) * (F.y - E.y); }
+double denominator( Point A, Point B, Point C, Point D ) { return (B.x - A.x) * (D.y - C.y) - (B.y - A.y) * (D.x - C.x); }
+
+void getintersection(Point S, Point D, std::vector <Point> Poly, Point *I) {
+	//printf("Points for intersection 1(%f,%f->%f,%f) and 2(%f,%f->%f,%f)\n",q1.x,q1.y,p1.x,p1.y,q2.x,q2.y,p2.x,p2.y);
+  bool parallel = false;
+  bool collinear = false;
+	std::vector <Point>::iterator itc;
+	std::vector <Point>::iterator next;
+  for (itc = Poly.begin(); itc != Poly.end()-1; ++itc) {
+    next = itc+1;
+    if (doIntersect((*itc), (*next), S, D)) 
+    {
+      // Uses the determinant of the two lines. For more information, refer to one of the following:
+      // https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection#Given_two_points_on_each_line
+      // http://www.faqs.org/faqs/graphics/algorithms-faq/ (Subject 1.03)
+      
+      double d = denominator( S, D, (*itc), (*next) );
+      
+      if (std::abs( d ) < 0.000000001)
+      {
+        parallel  = true;
+        collinear = abs(numerator( S, D, (*itc), (*next) )) < 0.000000001;
+        return;
+      }
+      
+      double r = numerator( S, (*itc), (*itc), (*next) ) / d;
+      double s = numerator( S, (*itc), S, D ) / d;
+
+      (*I)=Point(S.x + r * (D.x - S.x), S.y + r * (D.y - S.y));
+    }
+  }
+}
+
+bool isSiteout(Point S, std::vector <Point> Poly) {
+  bool onedge = false;
+
+	// Create a point for line segment from p to infinite 
+  Point extreme = {10000, S.y};
+
+  // Count intersections of the above line with sides of polygon 
+  int count = 0; 
+	std::vector <Point>::iterator itc;
+	std::vector <Point>::iterator next;
+	for (itc = Poly.begin(); itc != Poly.end()-1; ++itc) {
+    next = itc+1;
+
+    // Check if the line segment from 'p' to 'extreme' intersects 
+    // with the line segment from 'polygon[i]' to 'polygon[next]' 
+    if (doIntersect((*itc), (*next), S, extreme)) 
+    { 
+        // If the point 'p' is colinear with line segment 'i-next', 
+        // then check if it lies on segment. If it lies, return true, 
+        // otherwise false 
+        if (orientation((*itc), S, (*next)) == 0) {
+          onedge = onSegment((*itc), S, (*next));
+          if(onedge)
+            break;
+        }
+        count++; 
+    }
+  }
+
+  return ((count%2 == 0) && !onedge);
+}
+
+int voronoi_center(buzzvm_t vm) {
+
+  float dist_max = 300;
+
+	buzzvm_lnum_assert(vm, 1);
+  // Get the parameter
+  buzzvm_lload(vm, 1);
+  buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE);    // dictionary
+  buzzobj_t t = buzzvm_stack_at(vm, 1);
+
+  buzzvm_dup(vm);
+  buzzvm_pushs(vm, buzzvm_string_register(vm, "np", 1));
+  buzzvm_tget(vm);
+  int Poly_vert = buzzvm_stack_at(vm, 1)->i.value;
+  buzzvm_pop(vm);
+
+	std::vector <Point> polygon_bound;
+  for(int32_t i = 0; i < Poly_vert; ++i) {
+    buzzvm_dup(vm);
+    buzzvm_pushi(vm, i);
+    buzzvm_tget(vm);
+   
+    buzzvm_dup(vm);
+    buzzvm_pushs(vm, buzzvm_string_register(vm, "x", 1));
+    buzzvm_tget(vm);
+    //ROS_INFO("---x-->%f",buzzvm_stack_at(vm, 1)->f.value);
+    float tmpx = buzzvm_stack_at(vm, 1)->f.value;
+    buzzvm_pop(vm);
+    buzzvm_dup(vm);
+    buzzvm_pushs(vm, buzzvm_string_register(vm, "y", 1));
+    buzzvm_tget(vm);
+    //ROS_INFO("---y-->%f",buzzvm_stack_at(vm, 1)->f.value);
+    float tmpy = buzzvm_stack_at(vm, 1)->f.value;
+    buzzvm_pop(vm);
+
+    polygon_bound.push_back(Point(tmpx, tmpy));
+    //ROS_INFO("[%i] Polygon vertex: %f, %f", buzz_utility::get_robotid(),tmpx,tmpy);
+
+    buzzvm_pop(vm);
+  }
+  sortclose_polygon(&polygon_bound);
+  // Check if we are in the zone
+  if(isSiteout(Point(0,0), polygon_bound)){
+    //ROS_WARN("Not in the Zone!!!");
+    double goal_tmp[2];
+    do{
+      goal_tmp[0] = polygon_bound[0].x + (rand()%100)/100.0*(polygon_bound[2].x- polygon_bound[0].x);
+      goal_tmp[1] = polygon_bound[0].y + (rand()%100)/100.0*(polygon_bound[2].y- polygon_bound[0].y);
+      //ROS_WARN(" in the Zone (%f,%f)!",goal_tmp[0],goal_tmp[1]);
+    } while(isSiteout(Point(goal_tmp[0],goal_tmp[1]), polygon_bound));
+    ROS_WARN("Sending at a random location in the Zone (%f,%f)!",goal_tmp[0],goal_tmp[1]);
+    double gps[3];
+    gps_from_vec(goal_tmp, gps);
+    set_gpsgoal(gps);
+    return buzzvm_ret0(vm);
+  }
+  
+
+  int count = buzzdict_size(t->t.value)-(Poly_vert+1);
+  ROS_WARN("NP: %d, Sites: %d", Poly_vert, count);
+  float *xValues = new float[count];
+  float *yValues = new float[count];
+  for(int32_t i = 0; i < count; ++i) {
+    int index = i + Poly_vert;
+    buzzvm_dup(vm);
+    buzzvm_pushi(vm, index);
+    buzzvm_tget(vm);
+   
+    buzzvm_dup(vm);
+    buzzvm_pushs(vm, buzzvm_string_register(vm, "x", 1));
+    buzzvm_tget(vm);
+    //ROS_INFO("---x-->%f",buzzvm_stack_at(vm, 1)->f.value);
+    xValues[i] = buzzvm_stack_at(vm, 1)->f.value;
+    buzzvm_pop(vm);
+    buzzvm_dup(vm);
+    buzzvm_pushs(vm, buzzvm_string_register(vm, "y", 1));
+    buzzvm_tget(vm);
+    //ROS_INFO("---y-->%f",buzzvm_stack_at(vm, 1)->f.value);
+    yValues[i] = buzzvm_stack_at(vm, 1)->f.value;
+    buzzvm_pop(vm);
+
+    buzzvm_pop(vm);
+  }
+
+	VoronoiDiagramGenerator vdg;
+  ROS_WARN("[%i] Voronoi Bounded tessellation starting with %i sites...", buzz_utility::get_robotid(),count);
+	vdg.generateVoronoi(xValues, yValues, count, -dist_max, dist_max, -dist_max, dist_max, 3.0);
+  if(logVoronoi)  voronoicsv << ros::Time::now().toNSec() << ",";
+	vdg.resetIterator();
+  //ROS_WARN("[%i] Voronoi Bounded tessellation done!", buzz_utility::get_robotid());
+
+  std::vector <Point>::iterator itc, next;
+	for (itc = polygon_bound.begin(); itc != polygon_bound.end()-1; ++itc) {
+    next = itc+1;
+    if(logVoronoi)  voronoicsv << itc->x << "," << itc->y << "," << next->x << "," << next->y << "," << 0 << "," << 0 << ",";
+  }
+
+	float x1,y1,x2,y2;
+  int s[2];
+  vector <Point> cell_vert;
+  Point Intersection;
+  int i=0;
+	while(vdg.getNext(x1,y1,x2,y2,s))
+	{
+		//ROS_INFO("GOT Line (%f,%f)->(%f,%f) between sites %d,%d",x1,y1,x2,y2,s[0],s[1]);
+    if(sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1))<0.1)
+      continue;
+    bool isout1 = isSiteout(Point(x1,y1), polygon_bound);
+    bool isout2 = isSiteout(Point(x2,y2), polygon_bound);
+    if(isout1 && isout2){
+      //ROS_INFO("Line out of area!");
+      continue;
+    }else if(isout1) {
+      getintersection(Point(x2,y2), Point(x1,y1), polygon_bound, &Intersection);
+      x1 = Intersection.x;
+      y1 = Intersection.y;
+      //ROS_INFO("Site out 1 -> (%f,%f)", x1, y1);
+	  }else if(isout2) {
+      getintersection(Point(x1,y1), Point(x2,y2), polygon_bound, &Intersection);
+      x2 = Intersection.x;
+      y2 = Intersection.y;
+      //ROS_INFO("Site out 2 -> (%f,%f)", x2, y2);
+    }
+    if(logVoronoi)  voronoicsv << x1 << "," << y1 << "," << x2 << "," << y2 << "," << s[0] << "," << s[1] << ",";
+    i++;
+    if((s[0]==0 || s[1]==0)) {
+      if(cell_vert.empty()){
+        cell_vert.push_back(Point(x1,y1));
+        cell_vert.push_back(Point(x2,y2));
+      } else {
+        bool alreadyin = false;
+        vector <Point>::iterator itc;
+        for (itc = cell_vert.begin(); itc != cell_vert.end(); ++itc) {
+          double dist = sqrt((itc->x - x1) * (itc->x - x1) + (itc->y - y1) * (itc->y - y1));
+          if(dist < 0.1) {
+            alreadyin = true;
+            break;
+          }
+        }
+        if(!alreadyin)
+          cell_vert.push_back(Point(x1, y1));
+        alreadyin = false;
+        for (itc = cell_vert.begin(); itc != cell_vert.end(); ++itc) {
+          double dist = sqrt((itc->x - x2) * (itc->x - x2) + (itc->y - y2) * (itc->y - y2));
+          if(dist < 0.1) {
+            alreadyin = true;
+            break;
+          }
+        }
+        if(!alreadyin)
+          cell_vert.push_back(Point(x2, y2));
+      }
+    }
+	}
+  if(cell_vert.size()<3){
+    ROS_WARN("[%i] Voronoi Bounded tessellation failed (%d)!", buzz_utility::get_robotid(),cell_vert.size());
+    return buzzvm_ret0(vm);
+  }
+  std::sort( cell_vert.begin(), cell_vert.end(), clockwise_compare_points );
+  cell_vert.push_back(cell_vert[0]);
+  
+  double center_dist[2] = {0.0, 0.0};
+  polygone_center(cell_vert, center_dist);
+  if(logVoronoi)  voronoicsv << center_dist[0] << "," << center_dist[1] << std::endl;
+  center_dist[0]/=2;
+  center_dist[1]/=2;
+  double gps[3];
+  gps_from_vec(center_dist, gps);
+  //ROS_INFO("[%i] Voronoi cell center: %f, %f, %f, %f", buzz_utility::get_robotid(), center_dist[0], center_dist[1], gps[0], gps[1]);
+  set_gpsgoal(gps);
+
+  return buzzvm_ret0(vm);
+}
+
 int buzzuav_moveto(buzzvm_t vm)
 /*
 / Buzz closure to move following a 3D vector + Yaw