Merge commit 'b3f9b7cf4e39329abcb820ae0892725b25b2f61a' into ros_drones_ws

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,14 +2,13 @@ GOTO_MAXVEL = 1.5 # m/steps
 GOTO_MAXDIST = 150 # m.
 GOTODIST_TOL = 0.4  # m.
 GOTOANG_TOL = 0.1 # rad.
-GPSlimit = {.1={.lat=45.510386, .lng=-73.610400},
+GPSlimit = {.1={.lat=45.510523, .lng=-73.611118},#{.lat=45.510400, .lng=-73.610421},
-			.2={.lat=45.509839, .lng=-73.610047},
+			.2={.lat=45.510896, .lng=-73.608731},
-			.3={.lat=45.510859, .lng=-73.608714},
+			.3={.lat=45.510355, .lng=-73.608404},
-			.4={.lat=45.510327, .lng=-73.608393}}
+			.4={.lat=45.509840, .lng=-73.610072}}
 
 # Core naviguation function to travel to a GPS target location.
 function goto_gps(transf) {
-	if(Geofence()) {
 	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)
@@ -18,11 +17,11 @@ function goto_gps(transf) {
 			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)
 		#m_navigation = LCA(m_navigation)
 		goto_abs(m_navigation.x, m_navigation.y, cur_goal.altitude - pose.position.altitude, 0.0)
 	}
-	} else
-		log("Geofencing prevents from going to that location!")
 }
 
 function LimitSpeed(vel_vec, factor){
@@ -30,16 +29,3 @@ function LimitSpeed(vel_vec, factor){
   	vel_vec = math.vec2.scale(vel_vec, GOTO_MAXVEL*factor/math.vec2.length(vel_vec))
 	return vel_vec
 }
-
-function Geofence(){ #TODO: rotate the fence box to really fit the coordinates
-	if(cur_goal.latitude > GPSlimit[1].lat and cur_goal.latitude > GPSlimit[2].lat and cur_goal.latitude > GPSlimit[3].lat and cur_goal.latitude > GPSlimit[4].lat)
-		return 0;
-	if(cur_goal.latitude < GPSlimit[1].lat and cur_goal.latitude < GPSlimit[2].lat and cur_goal.latitude < GPSlimit[3].lat and cur_goal.latitude < GPSlimit[4].lat)
-		return 0;
-	if(cur_goal.longitude > GPSlimit[1].lng and cur_goal.longitude > GPSlimit[2].lng and cur_goal.longitude > GPSlimit[3].lng and cur_goal.longitude > GPSlimit[4].lng)
-		return 0;
-	if(cur_goal.longitude < GPSlimit[1].lng and cur_goal.longitude < GPSlimit[2].lng and cur_goal.longitude < GPSlimit[3].lng and cur_goal.longitude < GPSlimit[4].lng)
-		return 0;
-
-	return 1
-}

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,18 +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())
+	if(V_TYPE == 2)		# NOT MOVING!
+		return
+	if(not(v_wp.size() > 0))
+		return
 	it_pts = 0
-		#table_print(pts)
+	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) {
-				pts[it_pts]=math.vec2.newp(data.distance,data.azimuth+pts.oa)
+				if(rid!=0){	#remove GS (?)
-				it_pts = it_pts + 1})
+					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
 
@@ -265,45 +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
-	minYid = 1
-	maxY = RBlimit[1].y
-	maxYid = 1
-	foreach(RBlimit, function(key, value) {
-		if(value.y<minY){
-			minY = value.y
-			minYid = key
-		}
-		if(value.y>maxY){
-			maxY = value.y
-			maxYid = key
-		}
-	})
-	minY2 = maxY
-	minY2id = maxYid
-	foreach(RBlimit, function(key, value) {
-		if(value.y<minY2 and key!=minYid){
-			minY2 = value.y
-			minY2id = key
-		}
-	})
-	angle_offset = math.atan(RBlimit[minY2id].y-RBlimit[minYid].y,RBlimit[minY2id].x-RBlimit[minYid].x)
-	if(angle_offset > math.pi/2.0)
-		angle_offset = math.pi - angle_offset
-	#log(angle_offset, minYid, minY2id, maxYid)
-	dvec = math.vec2.sub(RBlimit[maxYid],RBlimit[minYid])
-	new_maxY = math.vec2.add(RBlimit[minYid], math.vec2.newp(math.vec2.length(dvec),math.vec2.angle(dvec)+angle_offset))
-	if(new_maxY.x > RBlimit[minYid].x)
-		return {.miny=minY, .minx=RBlimit[minYid].x, .maxy=new_maxY.y, .maxx=new_maxY.x}
-	else
-		return {.miny=minY, .minx=new_maxY.x, .maxy=new_maxY.y, .maxx=RBlimit[minYid].x, .oa=angle_offset}
-}
-
 # Custom state function for the developer to play with
 function cusfun(){
 	BVMSTATE="CUSFUN"

buzz_scripts/include/taskallocate/bidding.bzz

@@ -457,6 +457,9 @@ function init_bidding() {
 # executed at each time step
 function bidding() {
 
+	# enable debug will increase the message size
+        debug = 0
+
     	# read the csv file with the waypoints information
         read_from_csv(CSV_FILENAME_AND_PATH)
 
@@ -565,11 +568,11 @@ function bidding() {
                                     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))
+                                    log("Drone", id, "SET its bid", highest_bid, "on", highest_area, "at iter", experiment_iteration)
                                 } 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))
+                                    log("Drone", id, "GAVE UP bid", highest_bid, "on", highest_area, "at iter", experiment_iteration)
                                 }                    
                             }
                         }    
@@ -616,7 +619,7 @@ function bidding() {
     ######################################## 
     # TEMP DEBUG BLOCK START ###############
     ########################################   
-    if (id == 2) {
+    if (id == 2 and debug == 1) {
         if (experiment_iteration%20==0){
             print_out_bidding_stigmergy()
         }  

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

@@ -0,0 +1,30 @@
+#!/usr/bin/python
+import matplotlib.pyplot as plt
+plt.style.use('seaborn-whitegrid')
+import matplotlib.animation as animation
+import numpy as np
+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_3.csv', 'r')
+Vorreader = csv.reader(datafile, delimiter=',')
+
+def animate(i):
+    for row in Vorreader:
+        ax.clear()
+#        ax.plot([-50, -50, 50, 50, -50],[-50, 50, 50, -50, -50],'b--')
+        j = 1
+        while j < len(row)-2:
+            ax.plot([float(row[j]), float(row[j+2])], [float(row[j+1]), float(row[j+3])])
+            j += 6
+        ax.plot(float(row[len(row)-2]),float(row[len(row)-1]),'x')
+        return
+
+ani = animation.FuncAnimation(fig, animate, interval=250)
+ani.save('Voronoi.gif', dpi=80, writer='imagemagick')
+plt.show()

buzz_scripts/main.bzz

@@ -3,9 +3,9 @@ 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"
@@ -13,11 +13,6 @@ include "utils/takeoff_heights.bzz"
 #State launched after takeoff
 
 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]
 	

include/VoronoiDiagramGenerator.h

@@ -33,6 +33,8 @@
 #include <math.h>
 #include <stdlib.h>
 #include <string.h>
+#include <stdio.h>
+#include <algorithm>
 
 
 #ifndef NULL
@@ -66,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 
@@ -84,12 +88,14 @@ struct Edge
 	struct	Site 	*ep[2];
 	struct	Site	*reg[2];
 	int		edgenbr;
+	int		sites[2];
 
 };
 
 struct GraphEdge
 {
 	float x1,y1,x2,y2;
+	int sites[2];
 	struct GraphEdge* next;
 };
 
@@ -123,7 +129,7 @@ public:
 		iteratorEdges = allEdges;
 	}
 
-	bool getNext(float& x1, float& y1, float& x2, float& y2)
+	bool getNext(float& x1, float& y1, float& x2, float& y2, int *s)
 	{
 		if(iteratorEdges == 0)
 			return false;
@@ -132,6 +138,7 @@ public:
 		x2 = iteratorEdges->x2;
 		y1 = iteratorEdges->y1;
 		y2 = iteratorEdges->y2;
+		std::copy(iteratorEdges->sites, iteratorEdges->sites+2, s);
 
 		iteratorEdges = iteratorEdges->next;
 
@@ -194,14 +201,13 @@ private:
 	void out_vertex(struct Site *v);
 	struct Site *nextone();
 
-	void pushGraphEdge(float x1, float y1, float x2, float y2);
+	void pushGraphEdge(float x1, float y1, float x2, float y2, int s[2]);
 
 	void openpl();
-	void line(float x1, float y1, float x2, float y2);
+	void line(float x1, float y1, float x2, float y2, int s[2]);
 	void circle(float x, float y, float radius);
 	void range(float minX, float minY, float maxX, float maxY);
 
-
 	struct  Freelist	hfl;
 	struct	Halfedge *ELleftend, *ELrightend;
 	int 	ELhashsize;

include/buzzuav_closures.h

@@ -21,6 +21,7 @@ namespace buzzuav_closures
  */
 int buzzros_print(buzzvm_t vm);
 void setWPlist(std::string file);
+void setVorlog(std::string path);
 void check_targets_sim(double lat, double lon, double *res);
 
 /*
@@ -154,6 +155,7 @@ int buzzuav_land(buzzvm_t vm);
  * Command the UAV to go to home location
  */
 int buzzuav_gohome(buzzvm_t vm);
+int buzzuav_geofence(buzzvm_t vm);
 
 /*
  * Updates battery information in Buzz

src/VoronoiDiagramGenerator.cpp

@@ -325,8 +325,10 @@ struct Edge * VoronoiDiagramGenerator::bisect(struct Site *s1,struct	Site *s2)
 	};
 	
 	newedge -> edgenbr = nedges;
+	newedge -> sites[0] = s1->sitenbr;
+	newedge -> sites[1] = s2->sitenbr;
 
-	//printf("\nbisect(%d) ((%f,%f) and (%f,%f)",nedges,s1->coord.x,s1->coord.y,s2->coord.x,s2->coord.y);
+	//printf("\nbisect(%d) (%d(%f,%f) and %d(%f,%f)",nedges,s1->sitenbr,s1->coord.x,s1->coord.y,s2->sitenbr,s2->coord.x,s2->coord.y);
 	
 	nedges += 1;
 	return(newedge);
@@ -655,7 +657,7 @@ void VoronoiDiagramGenerator::cleanupEdges()
 
 }
 
-void VoronoiDiagramGenerator::pushGraphEdge(float x1, float y1, float x2, float y2)
+void VoronoiDiagramGenerator::pushGraphEdge(float x1, float y1, float x2, float y2, int s[2])
 {
 	GraphEdge* newEdge = new GraphEdge;
 	newEdge->next = allEdges;
@@ -664,6 +666,7 @@ void VoronoiDiagramGenerator::pushGraphEdge(float x1, float y1, float x2, float
 	newEdge->y1 = y1;
 	newEdge->x2 = x2;
 	newEdge->y2 = y2;
+	std::copy(s, s+2, newEdge->sites);
 }
 
 
@@ -679,9 +682,9 @@ char * VoronoiDiagramGenerator::myalloc(unsigned n)
 /* for those who don't have Cherry's plot */
 /* #include <plot.h> */
 void VoronoiDiagramGenerator::openpl(){}
-void VoronoiDiagramGenerator::line(float x1, float y1, float x2, float y2)
+void VoronoiDiagramGenerator::line(float x1, float y1, float x2, float y2, int s[2])
 {	
-	pushGraphEdge(x1,y1,x2,y2);
+	pushGraphEdge(x1,y1,x2,y2,s);
 
 }
 void VoronoiDiagramGenerator::circle(float x, float y, float radius){}
@@ -739,7 +742,6 @@ void VoronoiDiagramGenerator::plotinit()
 	range(pxmin, pymin, pxmax, pymax);
 }
 
-
 void VoronoiDiagramGenerator::clip_line(struct Edge *e)
 {
 	struct Site *s1, *s2;
@@ -848,8 +850,8 @@ 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);
+	line(x1,y1,x2,y2,e->sites);
 }
 
 
@@ -895,6 +897,7 @@ bool VoronoiDiagramGenerator::voronoi(int triangulate)
 			e = bisect(bot, newsite);					//create a new edge that bisects 
 			bisector = HEcreate(e, le);					//create a new HalfEdge, setting its ELpm field to 0			
 			ELinsert(lbnd, bisector);					//insert this new bisector edge between the left and right vectors in a linked list	
+			//printf("Newsite %d: %f,%f\n",newsite->sitenbr,newsite->coord.x,newsite->coord.y);
 
 			if ((p = intersect(lbnd, bisector)) != (struct Site *) NULL) 	//if the new bisector intersects with the left edge, remove the left edge's vertex, and put in the new one
 			{	

src/buzz_utility.cpp

@@ -243,6 +243,9 @@ static int buzz_register_hooks()
   buzzvm_pushs(VM, buzzvm_string_register(VM, "voronoi", 1));
   buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::voronoi_center));
   buzzvm_gstore(VM);
+  buzzvm_pushs(VM, buzzvm_string_register(VM, "geofence", 1));
+  buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_geofence));
+  buzzvm_gstore(VM);
 
   return VM->state;
 }

src/buzzuav_closures.cpp

@@ -37,7 +37,17 @@ static int rssi = 0;
 static float raw_packet_loss = 0.0;
 static int filtered_packet_loss = 0;
 static float api_rssi = 0.0;
+static bool logVoronoi = false;
 
+std::ofstream voronoicsv;
+
+struct Point
+{ 
+    float x; 
+    float y;
+    Point(): x( 0.0 ), y( 0.0 ) { }
+    Point( float x, float y ): x( x ), y( y ) { }
+};
 string WPlistname = "";
 
 std::map<int, buzz_utility::RB_struct> targets_map;
@@ -55,7 +65,7 @@ int buzzros_print(buzzvm_t vm)
 ----------------------------------------------------------- */
 {
   std::ostringstream buffer(std::ostringstream::ate);
-  buffer << std::fixed << std::setprecision(6) << "[" << buzz_utility::get_robotid() << "] ";
+  buffer << "[" << buzz_utility::get_robotid() << "] ";
   for (uint32_t index = 1; index < buzzdarray_size(vm->lsyms->syms); ++index)
   {
     buzzvm_lload(vm, index);
@@ -104,10 +114,19 @@ void setWPlist(string file)
 / set the absolute path for a csv list of waypoints
 ----------------------------------------------------------- */
 {
-  WPlistname = file;//path + "include/taskallocate/waypointlist.csv";
+  WPlistname = file;
   parse_gpslist();
 }
 
+void setVorlog(string path)
+/*
+/ set the absolute path for a csv list of waypoints
+----------------------------------------------------------- */
+{
+  voronoicsv.open(path + "/log/voronoi_"+std::to_string(buzz_utility::get_robotid())+".csv", std::ios_base::trunc | std::ios_base::out);
+  logVoronoi = true;
+}
+
 float constrainAngle(float x)
 /*
 / Wrap the angle between -pi, pi
@@ -200,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)
   {
@@ -207,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;
@@ -248,10 +268,266 @@ int buzz_exportmap(buzzvm_t vm)
   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/  
+ */
+
+// Given three colinear points p, q, r, the function checks if 
+// point q lies on line segment 'pr' 
+bool onSegment(Point p, Point q, Point r) 
+{ 
+    if (q.x <= max(p.x, r.x) && q.x >= min(p.x, r.x) && 
+            q.y <= max(p.y, r.y) && q.y >= 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 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 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 
+}
+
+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;
+    Point P;
+    Point V[4];
+    int tmp;
+    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);
+
+    if(buzzdict_size(t->t.value) != 5) {
+      ROS_ERROR("Wrong Geofence input size (%i).", buzzdict_size(t->t.value));
+      return buzzvm_ret0(vm);
+    }
+    for(int32_t i = 0; i < buzzdict_size(t->t.value); ++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);
+      tmp = buzzvm_stack_at(vm, 1)->f.value;
+      //ROS_INFO("[%i]---x-->%i",buzz_utility::get_robotid(), tmp);
+      if(i==0)
+        P.x = tmp;
+      else
+        V[i-1].x = tmp;
+      buzzvm_pop(vm);
+      buzzvm_dup(vm);
+      buzzvm_pushs(vm, buzzvm_string_register(vm, "y", 1));
+      buzzvm_tget(vm);
+      tmp = buzzvm_stack_at(vm, 1)->f.value;
+      //ROS_INFO("[%i]---y-->%i",buzz_utility::get_robotid(), tmp);
+      if(i==0)
+        P.y = tmp;
+      else
+        V[i-1].y = tmp;
+      buzzvm_pop(vm);
+
+      buzzvm_pop(vm);
+    }
+    // TODO: use vector
+    //sortclose_polygon(&V);
+
+    // simple polygon: rectangle, 4 points
+    int n = 4;
+    // Create a point for line segment from p to infinite 
+    Point extreme = {10000, P.y}; 
+  
+    // Count intersections of the above line with sides of polygon 
+    int count = 0, i = 0; 
+    do
+    {
+        int next = (i+1)%n;
+  
+        // Check if the line segment from 'p' to 'extreme' intersects 
+        // with the line segment from 'polygon[i]' to 'polygon[next]' 
+        if (doIntersect(V[i], V[next], P, 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(V[i], P, V[next]) == 0) {
+              onedge = onSegment(V[i], P, V[next]);
+              if(onedge)
+                break;
+            }
+  
+            count++; 
+        }
+      i = next; 
+    } while (i != 0); 
+  
+  //ROS_INFO("[%i] Geofence: %i, %i",buzz_utility::get_robotid(),count, onedge);
+
+  if((count%2 == 0) || onedge) {
+    goto_gpsgoal[0] = cur_pos[0];
+    goto_gpsgoal[1] = cur_pos[1];
+    ROS_WARN("Geofencing trigered, not going any further!");
+  }
+
+  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 *xValues;//[4] = {-22, -17, 4,22};
+
-	float *yValues;//[4] = {-9, 31,13,-5};
+  float dist_max = 300;
-  float minx, miny, maxx, maxy;
+
 	buzzvm_lnum_assert(vm, 1);
   // Get the parameter
   buzzvm_lload(vm, 1);
@@ -259,39 +535,63 @@ int voronoi_center(buzzvm_t vm) {
   buzzobj_t t = buzzvm_stack_at(vm, 1);
 
   buzzvm_dup(vm);
-  buzzvm_pushs(vm, buzzvm_string_register(vm, "maxx", 1));
+  buzzvm_pushs(vm, buzzvm_string_register(vm, "np", 1));
   buzzvm_tget(vm);
-  maxx = buzzvm_stack_at(vm, 1)->f.value;
+  int Poly_vert = buzzvm_stack_at(vm, 1)->i.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;
+	std::vector <Point> polygon_bound;
-  xValues = new float[count];
+  for(int32_t i = 0; i < Poly_vert; ++i) {
-  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);
+    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);
@@ -309,68 +609,90 @@ int voronoi_center(buzzvm_t vm) {
   }
 
 	VoronoiDiagramGenerator vdg;
-	vdg.generateVoronoi(xValues,yValues,count, minx, maxx, miny, maxy,3);
+  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;
-  map<float, std::array<float, 4>> edges;
+  int s[2];
-	while(vdg.getNext(x1,y1,x2,y2))
+  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)\n",x1,y1,x2, y2);
+		//ROS_INFO("GOT Line (%f,%f)->(%f,%f) between sites %d,%d",x1,y1,x2,y2,s[0],s[1]);
-    edges.insert(make_pair(sqrt((x2+x1)*(x2+x1)/4+(y2+y1)*(y2+y1)/4), std::array<float, 4>{x1,y1,x2,y2}));
+    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);
     }
-  double center_dist[2] = {0,0};
+    if(logVoronoi)  voronoicsv << x1 << "," << y1 << "," << x2 << "," << y2 << "," << s[0] << "," << s[1] << ",";
-  float closest_points[8];
+    i++;
-  int nit = 1;
+    if((s[0]==0 || s[1]==0)) {
-  float prev;
+      if(cell_vert.empty()){
-  map<float, std::array<float, 4>>::iterator it;
+        cell_vert.push_back(Point(x1,y1));
-  int got3 = 0;
+        cell_vert.push_back(Point(x2,y2));
-  for (it = edges.begin(); it != edges.end(); ++it)
-  {
-     if(nit == 1) {
-      //center_dist[0] += it->second[0] + it->second[2];
-      //center_dist[1] += it->second[1] + it->second[3];
-      closest_points[0] = it->second[0]; closest_points[1] = it->second[1]; closest_points[2] = it->second[2]; closest_points[3] = it->second[3];
-      ROS_INFO("USE Line (%f,%f)->(%f,%f): %f\n",it->second[0],it->second[1],it->second[2], it->second[3], it->first);
-      prev = it->first;
-     } else if(nit == 2) {
-       map<float, std::array<float, 4>>::iterator it_prev = edges.find(prev);
-      if(it->second[0]!=it_prev->second[0] && it->second[1]!=it_prev->second[1] && it->second[0]!=it_prev->second[2] && it->second[1]!=it_prev->second[3]){
-        //center_dist[0] += it->second[0];
-        //center_dist[1] += it->second[1];
-        closest_points[4] = it->second[0]; closest_points[5] = it->second[1];
-        ROS_INFO("USE Point (%f,%f): %f\n",it->second[0],it->second[1], it->first);
-        got3 = 1;
-      }
-      if(it->second[2]!=it_prev->second[0] && it->second[3]!=it_prev->second[1] && it->second[2]!=it_prev->second[2] && it->second[3]!=it_prev->second[3]){
-        //center_dist[0] += it->second[2];
-        //center_dist[1] += it->second[3];
-        if(!got3) {
-          closest_points[4] = it->second[2]; closest_points[5] = it->second[3];
       } else {
-          closest_points[6] = it->second[2]; closest_points[7] = it->second[3];
+        bool alreadyin = false;
-          got3=2;
+        vector <Point>::iterator itc;
-        }
+        for (itc = cell_vert.begin(); itc != cell_vert.end(); ++itc) {
-        ROS_INFO("USE Point (%f,%f): %f (%i)\n",it->second[2],it->second[3], it->first, got3);
+          double dist = sqrt((itc->x - x1) * (itc->x - x1) + (itc->y - y1) * (itc->y - y1));
-      }
+          if(dist < 0.1) {
-     } else
+            alreadyin = true;
             break;
-     nit++;
           }
-  if(got3==2)
+        }
-    center_dist[0]=(closest_points[0]+closest_points[2]+closest_points[4]+closest_points[6])/4;
+        if(!alreadyin)
-  else
+          cell_vert.push_back(Point(x1, y1));
-    center_dist[0]=(closest_points[0]+closest_points[2]+closest_points[4])/3;
+        alreadyin = false;
-  if(got3==2)
+        for (itc = cell_vert.begin(); itc != cell_vert.end(); ++itc) {
-    center_dist[1]=(closest_points[1]+closest_points[3]+closest_points[5]+closest_points[7])/4;
+          double dist = sqrt((itc->x - x2) * (itc->x - x2) + (itc->y - y2) * (itc->y - y2));
-  else
+          if(dist < 0.1) {
-    center_dist[1]=(closest_points[1]+closest_points[3]+closest_points[5])/3;
+            alreadyin = true;
-  center_dist[0]=center_dist[0]*cos(offset_angle)-center_dist[1]*sin(offset_angle);
+            break;
-  center_dist[1]=center_dist[0]*sin(offset_angle)+center_dist[1]*cos(offset_angle);
+          }
+        }
+        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]);
+  //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);
@@ -457,8 +779,8 @@ int buzzuav_addNeiStatus(buzzvm_t vm)
   buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE);
   buzzobj_t t = buzzvm_stack_at(vm, 1);
   if(buzzdict_size(t->t.value) != 5) {
-    ROS_WARN("Wrong neighbor status size.");
+    ROS_ERROR("Wrong neighbor status size.");
-    return vm->state;
+    return buzzvm_ret0(vm);
   }
 
   buzz_utility::neighbors_status newRS;

src/roscontroller.cpp

@@ -36,7 +36,6 @@ logical_clock(ros::Time()), previous_step_time(ros::Time())
   bcfname = fname + ".bo";
   dbgfname = fname + ".bdb";
   buzz_update::set_bzz_file(bzzfile_name.c_str(),debug);
-  buzzuav_closures::setWPlist(WPfile);
   //  Initialize variables
   if(setmode)
     SetMode("LOITER", 0);
@@ -71,6 +70,7 @@ logical_clock(ros::Time()), previous_step_time(ros::Time())
   out_msg_time=0;
   // Create log dir and open log file
   initcsvlog();
+  buzzuav_closures::setWPlist(WPfile);
 }
 
 roscontroller::~roscontroller()
@@ -114,6 +114,7 @@ void roscontroller::RosControllerRun()
   //  Set the Buzz bytecode
   if (buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(), robot_id))
   {
+    buzzuav_closures::setVorlog(bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")));
     ROS_INFO("[%i] INIT DONE!!!", robot_id);
     int packet_loss_tmp, time_step = 0;
     double cur_packet_loss = 0;