merge rosbuzz sim

buzz_scripts/include/act/barrier.bzz

@@ -22,17 +22,18 @@ function barrier_create() {
   timeW = 1
   # create barrier vstig
   #log("---> Prev. br. ", barrier, " ", BARRIER_VSTIG)
-  if(barrier!=nil) {
+  #if(barrier!=nil) {
-    barrier=nil
+  #  barrier=nil
-    if(hvs) {
+    #if(hvs) {
-      BARRIER_VSTIG = BARRIER_VSTIG -1
+    #  BARRIER_VSTIG = BARRIER_VSTIG -1
-      hvs = 0
+    #  hvs = 0
-    }else{
+    #}else{
-      BARRIER_VSTIG = BARRIER_VSTIG +1
+    #  BARRIER_VSTIG = BARRIER_VSTIG +1
-      hvs = 1
+    #  hvs = 1
-    }
+    #}
-  }
+  #}
   #log("---> New. br. ", barrier, " ", BARRIER_VSTIG)
+  if(barrier==nil)
     barrier = stigmergy.create(BARRIER_VSTIG)
 }
 
@@ -51,6 +52,7 @@ function barrier_ready(bc) {
   #log("BARRIER READY -------")
   barrier.put(id, bc)
   barrier.put("d", 0)
+  barrier.put("n", 1)
 }
 
 #
@@ -61,22 +63,33 @@ function barrier_wait(threshold, transf, resumef, bc) {
     barrier_create()
   
   barrier.put(id, bc)
-  log("--->BS: ", barrier.size(), " / ", threshold, " (", BARRIER_VSTIG, " - ", barrier.get("d"), ") t= ", timeW)
+  if(barrier.get("n")<threshold)
-  #if(barrier.size()-1 >= threshold or barrier.get("d") == 1) {
+    barrier.put("n", threshold)
+  if(threshold>1) {
+    neighbors.foreach(function (nei,data){
+      barrier.get(nei)
+    })
+  }
+
+  # Not to miss any RC inputs, for waypoints, potential and deploy states
+	check_rc_wp()
+
+  #log("--->BS: ", barrier.size(), " / ", threshold, " (", BARRIER_VSTIG, " - ", barrier.get("d"), ") t= ", timeW)
+  if(barrier.size()-2 >= barrier.get("n")) {
     if(barrier_allgood(barrier,bc)) {
       barrier.put("d", 1)
       timeW = 0
       BVMSTATE = transf
     } else 
       barrier.put("d", 0)
-  #}
+  }
 
   if(timeW >= BARRIER_TIMEOUT) {
     log("------> Barrier Timeout !!!!")
-    barrier = nil
+    #barrier = nil
     timeW = 0
     BVMSTATE = resumef
-  } else if(timeW % 100 == 0 and bc > 0)
+  } else if(timeW % 10 == 0 and bc > 0)
     neighbors.broadcast("cmd", bc)
 
   timeW = timeW+1
@@ -86,11 +99,12 @@ function barrier_wait(threshold, transf, resumef, bc) {
 function barrier_allgood(barrier, bc) {
   barriergood = 1
   barrier.foreach(function(key, value, robot){
+      barrier.get(key)
       #log("VS entry : ", key, " ", value, " ", robot)
       if(key == "d"){
         if(value == 1)
           return 1
-      } else if(value != bc)
+      } else if(key!="n" and value != bc)
         barriergood = 0
     })
   return barriergood

buzz_scripts/include/act/naviguation.bzz

@@ -1,12 +1,17 @@
 GOTO_MAXVEL = 1.5 # m/steps
-GOTO_MAXDIST = 150 # m.
+GOTO_MAXDIST = 250 # m.
 GOTODIST_TOL = 0.4  # m.
 GOTOANG_TOL = 0.1 # rad.
-GPSlimit = {.1={.lat=45.510400, .lng=-73.610421},
+GPSlimit_CEPSUM = {.1={.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}}
 
+GPSlimit_PANGEAE = {.1={.lat=29.020871, .lng=-13.712477},
+			.2={.lat=29.019850, .lng=-13.712378},
+			.3={.lat=29.019875, .lng=-13.710096},
+			.4={.lat=29.021245, .lng=-13.710184}}
+
 # 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)
@@ -17,7 +22,7 @@ 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)}
+		gf = {.0=m_navigation, .1=vec_from_gps(GPSlimit_PANGEAE[1].lat, GPSlimit_PANGEAE[1].lng, 0), .2=vec_from_gps(GPSlimit_PANGEAE[2].lat, GPSlimit_PANGEAE[2].lng, 0), .3=vec_from_gps(GPSlimit_PANGEAE[3].lat, GPSlimit_PANGEAE[3].lng, 0), .4=vec_from_gps(GPSlimit_PANGEAE[4].lat, GPSlimit_PANGEAE[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)

buzz_scripts/include/act/neighborcomm.bzz

@@ -43,14 +43,14 @@ function rc_cmd_listen() {
 			} else if (flight.rc_cmd==901){
 				flight.rc_cmd=0
 				destroyGraph()
-				resetWP()
+				check_rc_wp()
 				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()
+				check_rc_wp()
 				barrier_set(ROBOTS, "WAYPOINT", BVMSTATE, 902)
 				barrier_ready(902)
 				neighbors.broadcast("cmd", 902)
@@ -58,12 +58,14 @@ function rc_cmd_listen() {
 				flight.rc_cmd=0
 				destroyGraph()
 				resetWP()
+				check_rc_wp()
 				barrier_set(ROBOTS, "POTENTIAL", BVMSTATE, 903)
 				barrier_ready(903)
 				neighbors.broadcast("cmd", 903)
 			} else if (flight.rc_cmd==904){
 				flight.rc_cmd=0
 				destroyGraph()
+				resetWP()
 				barrier_set(ROBOTS, "IDLE", BVMSTATE, 904)
 				barrier_ready(904)
 				neighbors.broadcast("cmd", 904)
@@ -90,7 +92,8 @@ function nei_cmd_listen() {
 				} else {
 					if(value==20) {
 						AUTO_LAUNCH_STATE = "IDLE"
-						BVMSTATE = "GOHOME"
+						barrier_set(ROBOTS, "GOHOME", BVMSTATE, 20)
+						barrier_ready(20)
 					} else if(value==21 ) {
 						BVMSTATE = "STOP"
 						#neighbors.broadcast("cmd", 777)
@@ -127,6 +130,7 @@ function nei_cmd_listen() {
 }
 
 firsttimeinwp = 1
+WPtab_id = 10
 function check_rc_wp() {
 	if(firsttimeinwp) {
 		v_wp = stigmergy.create(WP_STIG)
@@ -140,6 +144,16 @@ function check_rc_wp() {
 			return
 		} else {
 			var ls = packWP2i(rc_goto.latitude, rc_goto.longitude, 0)
+			if(rc_goto.id>49) {	# Attractor/repulsor table
+				WPtab = v_wp.get(WPtab_id)
+				if(WPtab!=nil)
+					WPtab[rc_goto.id]=ls
+				else {
+					WPtab={}
+					WPtab[rc_goto.id]=ls
+				}
+				v_wp.put(WPtab_id,WPtab)
+			} else  # Individual waypoint
 				v_wp.put(rc_goto.id,ls)
 			reset_rc()
 		}

buzz_scripts/include/act/states.bzz

@@ -19,6 +19,7 @@ WP_STIG = 8
 path_it = 0
 pic_time = 0
 g_it = 0
+homegps = {}
 
 # Core state function when on the ground
 function turnedoff() {
@@ -33,15 +34,16 @@ function idle() {
 # Core state function to launch the robot: takeoff and wait for others, or stop (land)
 function launch() {
 	BVMSTATE = "LAUNCH"
+	neighbors.broadcast("cmd", 22)
 	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}
+		homegps = {.lat=pose.position.latitude, .lng=pose.position.longitude}
+		log("Recorded home point: ",homegps.lat, homegps.lng)
 		if(pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
 			barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "STOP", 22)
 			barrier_ready(22)
 		} else {
 			log("Altitude: ", pose.position.altitude)
-			neighbors.broadcast("cmd", 22)
 			uav_takeoff(TARGET_ALTITUDE)
 		}
 	} else {
@@ -74,7 +76,7 @@ gohomeT=100
 function goinghome() {
   BVMSTATE = "GOHOME"
 	if(V_TYPE == 0 or V_TYPE == 1) {	# flying vehicle so TAKE_OFF
-		m_navigation = vec_from_gps(homegps.latitude, homegps.longitude, 0)
+		m_navigation = vec_from_gps(homegps.lat, homegps.lng, 0)
 		#print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation))
 		if(math.vec2.length(m_navigation) < GOTODIST_TOL){ # reached destination
 				BVMSTATE = AUTO_LAUNCH_STATE
@@ -83,14 +85,15 @@ function goinghome() {
 			#m_navigation = LCA(m_navigation)
 			goto_abs(m_navigation.x, m_navigation.y, cur_goal.altitude - pose.position.altitude, 0.0)
 		}
-	}
+	} else
+		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
   neighbors.broadcast("cmd", 21)
+	if(V_TYPE == 0 or V_TYPE == 1) {	# flying vehicle so LAND
 		uav_land()
 		if(pose.position.altitude <= 0.2) {
 			BVMSTATE = "STOP"
@@ -116,8 +119,8 @@ function indiWP() {
 		wp = unpackWP2i(wpi)
 		if(wp.pro == 0) {
 				wpreached = 0
-				storegoal(wp.lat, wp.lon, pose.position.altitude)
+				storegoal(wp.lat, wp.lng, pose.position.altitude)
-				var ls = packWP2i(wp.lat, wp.lon, 1)
+				var ls = packWP2i(wp.lat, wp.lng, 1)
 				v_wp.put(id,ls)
 				return
 			}      
@@ -254,20 +257,22 @@ counter = 0
 function voronoicentroid() {
   BVMSTATE="DEPLOY"
 	check_rc_wp()
-	log("V_wp size:",v_wp.size())
+	wptab = v_wp.get(WPtab_id)
-	if(V_TYPE == 2)		# NOT MOVING!
+	if(wptab==nil)
 		return
-	if(not(v_wp.size() > 0))
+	else if(not(size(wptab) > 0))
+		return
+	log("WP table size:", size(wptab))
+	if(V_TYPE == 2)
 		return
 	it_pts = 0
 	att = {}
-	v_wp.foreach(
+	foreach(wptab, function(key, value){
-    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)
+				att[it_pts]=vec_from_gps(wp.lat, wp.lng, 0)
 			it_pts = it_pts + 1
 	})
 	# Boundaries from Geofence

buzz_scripts/include/taskallocate/waypointlist.csv

@@ -1,4 +1,10 @@
-1,-73.609219,45.510336,1
+1,-13.710661,29.020850,10.0,1
-2,-73.608913,45.510723,0
+2,-13.711061,29.020946,10.0,0
-3,-73.610035,45.510182,1
+3,-13.711605,29.020421,10.0,1
-4,-73.609235,45.510114,0
+4,-13.711925,29.020003,10.0,0
+5,-13.712166,29.020435,10.0,1
+6,-13.710498,29.020341,10.0,0
+7,-13.711506,29.019919,10.0,0
+8,-13.712048,29.020385,10.0,0
+9,-13.711514,29.020261,10.0,1
+10,-13.711621,29.020725,10.0,1

buzz_scripts/include/utils/conversions.bzz

@@ -62,16 +62,17 @@ function gps_from_vec(vec) {
   return Lgoal
 }
 
-GPSoffset = {.lat=45.50, .lon=-73.61}
+GPSoffset = {.lat=45.50, .lng=-73.61}
+GPSoffsetL = {.lat=29.01, .lng=-13.70}
 
 function packWP2i(in_lat, in_long, processed) {
-  var dlat = math.round((in_lat - GPSoffset.lat)*1000000)
+  var dlat = math.round((in_lat - GPSoffsetL.lat)*1000000)
-  var dlon = math.round((in_long - GPSoffset.lon)*1000000)
+  var dlon = math.round((in_long - GPSoffsetL.lng)*1000000)
   return {.dla=dlat, .dlo=dlon*10+processed}
 }
 
 function unpackWP2i(wp_int){
   var dlon = (wp_int.dlo-wp_int.dlo%10)/10.0
   var pro = wp_int.dlo-dlon*10
-  return {.lat=wp_int.dla/1000000.0+GPSoffset.lat, .lon=dlon/1000000.0+GPSoffset.lon, .pro=pro}
+  return {.lat=wp_int.dla/1000000.0+GPSoffsetL.lat, .lng=dlon/1000000.0+GPSoffsetL.lng, .pro=pro}
 }

buzz_scripts/include/utils/table.bzz

@@ -53,7 +53,7 @@ function i2s(value){
 	return "INDIWP"
 	}
   else if(value==7){
-	return "TASK_ALLOCATE"
+	return "GOHOME"
 	}
   else if(value==8){
 	return "LAUNCH"
@@ -87,7 +87,7 @@ function s2i(value){
 	else if(value=="WAYPOINT"){
 	return 6
 	}
-	else if(value=="TASK_ALLOCATE"){
+	else if(value=="GOHOME"){
 	return 7
 	}
 	else if(value=="LAUNCH"){

buzz_scripts/include/utils/takeoff_heights.bzz

@@ -14,6 +14,12 @@ takeoff_heights ={
     .18 = 6.0,
     .19 = 9.0,
     .20 = 3.0,
-    .21 = 6.0,
+    .21 = 3.0,
-    .22 = 9.0
+    .22 = 3.0,
+    .23 = 0.0,
+    .24 = 38.0,
+    .25 = 34.0,
+    .26 = 25.0,
+    .27 = 42.0,
+    .28 = 29.0
 }

src/buzzuav_closures.cpp

@@ -195,12 +195,13 @@ void parse_gpslist()
     double lon = atof(strtok(NULL, DELIMS));
     double lat = atof(strtok(NULL, DELIMS));
     int alt = atoi(strtok(NULL, DELIMS));
-    // int tilt = atoi(strtok(NULL, DELIMS));
+    int tilt = atoi(strtok(NULL, DELIMS));
     //  DEBUG
     // ROS_INFO("%.6f, %.6f, %i %i %i",lat, lon, alt, tilt, tid);
     RB_arr.latitude = lat;
     RB_arr.longitude = lon;
     RB_arr.altitude = alt;
+    RB_arr.r = tilt;
     // Insert elements.
     map<int, buzz_utility::RB_struct>::iterator it = wplist_map.find(tid);
     if (it != wplist_map.end())
@@ -227,8 +228,14 @@ 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] < visibility_radius){
+    if(rb[0] < visibility_radius && (buzz_utility::get_bvmstate()=="WAYPOINT" && it->second.r==0)){
-      ROS_WARN("FOUND A TARGET!!! [%i]", it->first);
+      ROS_WARN("FOUND A TARGET IN WAYPOINT!!! [%i]", it->first);
+      res[0] = it->first;
+      res[1] = it->second.latitude;
+      res[2] = it->second.longitude;
+      res[3] = it->second.altitude;
+    } else if(rb[0] < visibility_radius && (buzz_utility::get_bvmstate()=="DEPLOY" && it->second.r==1)){
+      ROS_WARN("FOUND A TARGET IN WAYPOINT!!! [%i]", it->first);
       res[0] = it->first;
       res[1] = it->second.latitude;
       res[2] = it->second.longitude;
@@ -289,8 +296,8 @@ bool onSegment(Point p, Point q, Point r)
 // 2 --> Counterclockwise 
 int orientation(Point p, Point q, Point r) 
 { 
-    int val = (q.y - p.y) * (r.x - q.x) - 
+    int val =round((q.y - p.y) * (r.x - q.x)*100 - 
-              (q.x - p.x) * (r.y - q.y); 
+              (q.x - p.x) * (r.y - q.y)*100); 
   
     if (val == 0) return 0;  // colinear 
     return (val > 0)? 1: 2; // clock or counterclock wise 
@@ -306,6 +313,8 @@ bool doIntersect(Point p1, Point q1, Point p2, Point q2)
     int o3 = orientation(p2, q2, p1); 
     int o4 = orientation(p2, q2, q1); 
 
+    //ROS_WARN("(%f,%f)->(%f,%f), 1:%d,2:%d,3:%d,4:%d",p1.x,p1.y,q1.x,q1.y,o1,o2,o3,o4);
+  
     // General case   
     if (o1 != o2 && o3 != o4) 
         return true; 
@@ -341,92 +350,6 @@ void sortclose_polygon(vector <Point> *P){
   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;
@@ -487,9 +410,12 @@ void getintersection(Point S, Point D, std::vector <Point> Poly, Point *I) {
       double r = numerator( S, (*itc), (*itc), (*next) ) / d;
       double s = numerator( S, (*itc), S, D ) / d;
 
+      //ROS_INFO("-- (%f,%f)",S.x + r * (D.x - S.x), S.y + r * (D.y - S.y));
       (*I)=Point(S.x + r * (D.x - S.x), S.y + r * (D.y - S.y));
     }
   }
+  if(parallel || collinear)
+    ROS_WARN("Lines are Collinear (%d) or Parallels (%d)",collinear,parallel);
 }
 
 bool isSiteout(Point S, std::vector <Point> Poly) {
@@ -524,6 +450,71 @@ bool isSiteout(Point S, std::vector <Point> Poly) {
   return ((count%2 == 0) && !onedge);
 }
 
+int buzzuav_geofence(buzzvm_t vm)
+{
+    Point P;
+    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);
+    }
+	  std::vector <Point> polygon_bound;
+    for(int32_t i = 0; i < buzzdict_size(t->t.value); ++i) {
+      Point tmp;
+      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);
+      if(i==0){
+        P.x = buzzvm_stack_at(vm, 1)->f.value;
+        //printf("px=%f\n",P.x);
+      }else{
+        tmp.x = buzzvm_stack_at(vm, 1)->f.value;
+        //printf("c%dx=%f\n",i,tmp.x);
+      }
+      buzzvm_pop(vm);
+      buzzvm_dup(vm);
+      buzzvm_pushs(vm, buzzvm_string_register(vm, "y", 1));
+      buzzvm_tget(vm);
+      //ROS_INFO("[%i]---y-->%i",buzz_utility::get_robotid(), tmp);
+      if(i==0){
+        P.y = buzzvm_stack_at(vm, 1)->f.value;
+        //printf("py=%f\n",P.y);
+      }else{
+        tmp.y = buzzvm_stack_at(vm, 1)->f.value;
+        //printf("c%dy=%f\n",i,tmp.y);
+      }
+      buzzvm_pop(vm);
+
+      if(i!=0)
+        polygon_bound.push_back(tmp);
+
+      buzzvm_pop(vm);
+    }
+    sortclose_polygon(&polygon_bound);
+
+    // Check if we are in the zone
+    if(isSiteout(P, polygon_bound)){
+      Point Intersection;
+      getintersection(Point(0.0, 0.0) , P, polygon_bound, &Intersection);
+      double gps[3];
+      double d[2]={Intersection.x,Intersection.y};
+      gps_from_vec(d, gps);
+      set_gpsgoal(gps);
+      ROS_WARN("Geofencing trigered, not going any further (%f,%f)!",d[0],d[1]);
+    }
+
+  return buzzvm_ret0(vm);
+}
+
 int voronoi_center(buzzvm_t vm) {
 
   float dist_max = 300;
@@ -565,22 +556,6 @@ int voronoi_center(buzzvm_t vm) {
     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);
@@ -608,6 +583,22 @@ int voronoi_center(buzzvm_t vm) {
     buzzvm_pop(vm);
   }
   
+  // Check if we are in the zone
+  if(isSiteout(Point(0,0), polygon_bound) || count < 3) {
+    //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);
+  }
+
 	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);
@@ -920,7 +911,7 @@ void set_gpsgoal(double goal[3])
 {
   double rb[3];
   rb_from_gps(goal, rb, cur_pos);
-  if (fabs(rb[0]) < 150.0) {
+  if (fabs(rb[0]) < 250.0) {
     goto_gpsgoal[0] = goal[0];goto_gpsgoal[1] = goal[1];goto_gpsgoal[2] = goal[2];
     ROS_INFO("[%i] Set GPS GOAL TO ---- %f %f %f (%f %f, %f %f)", buzz_utility::get_robotid(), goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]);
   } else