Merge commit '12ff37965b577d13a3d3288686ddcbef1fb6cb43' into ros_drones_ws

CMakeLists.txt

@@ -57,6 +57,7 @@ add_executable(rosbuzz_node src/rosbuzz.cpp
   src/roscontroller.cpp
   src/buzz_utility.cpp
   src/buzzuav_closures.cpp
+  src/VoronoiDiagramGenerator.cpp
   src/buzz_update.cpp)
 target_link_libraries(rosbuzz_node ${catkin_LIBRARIES} buzz buzzdbg pthread)
 add_dependencies(rosbuzz_node rosbuzz_generate_messages_cpp)

buzz_scripts/include/act/barrier.bzz

@@ -87,7 +87,7 @@ function barrier_wait(threshold, transf, resumef, bc) {
 function barrier_allgood(barrier, bc) {
   barriergood = 1
   barrier.foreach(function(key, value, robot){
-      log("VS entry : ", key, " ", value, " ", robot)
+      #log("VS entry : ", key, " ", value, " ", robot)
       if(key == "d"){
         if(value == 1)
           return 1

buzz_scripts/include/act/naviguation.bzz

@@ -2,20 +2,27 @@ 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},
+			.2={.lat=45.509839, .lng=-73.610047},
+			.3={.lat=45.510859, .lng=-73.608714},
+			.4={.lat=45.510327, .lng=-73.608393}}
 
 # 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)
+	if(Geofence()) {
-	#print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation))
+		m_navigation = vec_from_gps(cur_goal.latitude, cur_goal.longitude, 0)
- 	if(math.vec2.length(m_navigation)>GOTO_MAXDIST)
+		#print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation))
-		log("Sorry this is too far (", math.vec2.length(m_navigation), " / ", GOTO_MAXDIST, " )")
+		if(math.vec2.length(m_navigation)>GOTO_MAXDIST)
-	else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL){ # reached destination
+			log("Sorry this is too far (", math.vec2.length(m_navigation), " / ", GOTO_MAXDIST, " )")
-			transf()
+		else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL){ # reached destination
-	} else {
+				transf()
-		m_navigation = LimitSpeed(m_navigation, 1.0)
+		} else {
-		#m_navigation = LCA(m_navigation)
+			m_navigation = LimitSpeed(m_navigation, 1.0)
-		goto_abs(m_navigation.x, m_navigation.y, cur_goal.altitude - pose.position.altitude, 0.0)
+			#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){
@@ -23,3 +30,16 @@ 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/states.bzz

@@ -100,7 +100,7 @@ function stop() {
 
 wpreached = 1
 function indiWP() {
-	BVMSTATE = "INDIWP"
+	BVMSTATE = "WAYPOINT"
 	check_rc_wp()
 
 	wpi = v_wp.get(id)
@@ -191,12 +191,12 @@ function pursuit() {
 }
 
 # Lennard-Jones interaction magnitude
-TARGET     = 16.0
+TARGET     = 8.0
 EPSILON    = 30.0 #30
 GAIN_ATT	 = 50.0
 GAIN_REP	 = 30.0
 function lj_magnitude(dist, target, epsilon) {
-	lj = -(epsilon / dist) * ((target / dist)^4)# - (target / dist)^2) #repulse only to avoid each other
+	lj = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) #repulse only to avoid each other
   return lj
 }
 
@@ -239,6 +239,71 @@ function lennardjones() {
   goto_abs(accum_lj.x, accum_lj.y, 0.0, 0.0)
 }
 
+# State function that calculates and actuates Voronoi Centroidal tessellation coverage (attractor/repulsors)
+counter = 0
+function voronoicentroid() {
+  BVMSTATE="DEPLOY"
+	if(counter==0 and id!=0) {
+		pts = getbounds()
+		it_pts = 0
+		#table_print(pts)
+		if(neighbors.count() > 0) {
+			neighbors.foreach(function(rid, data) {
+				pts[it_pts]=math.vec2.newp(data.distance,data.azimuth+pts.oa)
+				it_pts = it_pts + 1})
+			#table_print(pts)
+			voronoi(pts)
+		}
+		counter=10
+	}
+	counter=counter-1
+
+	goto_gps(voronoicentroid_done)
+}
+
+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/main.bzz

@@ -72,7 +72,7 @@ function step() {
 			statef=launch_switch
 	else if(BVMSTATE=="GOHOME") # ends on AUTO_LAUNCH_STATE
 		statef=goinghome
-	else if(BVMSTATE=="INDIWP")
+	else if(BVMSTATE=="WAYPOINT")
 		statef=indiWP
 	else if(BVMSTATE=="IDLE")
 		statef=idle
@@ -86,6 +86,8 @@ function step() {
 		statef=resetGraph
 	else if(BVMSTATE=="BIDDING")	# check the absolute path of the waypointlist csv file in bidding.bzz
 		statef=bidding
+	else if(BVMSTATE=="DEPLOY")	# check the absolute path of the waypointlist csv file in bidding.bzz
+		statef=voronoicentroid
 	else if(BVMSTATE=="GRAPH_FREE")
 		statef=DoFree
 	else if(BVMSTATE=="GRAPH_ASKING")

include/VoronoiDiagramGenerator.h

@@ -0,0 +1,248 @@
+/*
+* The author of this software is Steven Fortune.  Copyright (c) 1994 by AT&T
+* Bell Laboratories.
+* Permission to use, copy, modify, and distribute this software for any
+* purpose without fee is hereby granted, provided that this entire notice
+* is included in all copies of any software which is or includes a copy
+* or modification of this software and in all copies of the supporting
+* documentation for such software.
+* THIS SOFTWARE IS BEING PROVIDED "AS IS", WITHOUT ANY EXPRESS OR IMPLIED
+* WARRANTY.  IN PARTICULAR, NEITHER THE AUTHORS NOR AT&T MAKE ANY
+* REPRESENTATION OR WARRANTY OF ANY KIND CONCERNING THE MERCHANTABILITY
+* OF THIS SOFTWARE OR ITS FITNESS FOR ANY PARTICULAR PURPOSE.
+*/
+
+/* 
+* This code was originally written by Stephan Fortune in C code.  I, Shane O'Sullivan, 
+* have since modified it, encapsulating it in a C++ class and, fixing memory leaks and 
+* adding accessors to the Voronoi Edges.
+* Permission to use, copy, modify, and distribute this software for any
+* purpose without fee is hereby granted, provided that this entire notice
+* is included in all copies of any software which is or includes a copy
+* or modification of this software and in all copies of the supporting
+* documentation for such software.
+* THIS SOFTWARE IS BEING PROVIDED "AS IS", WITHOUT ANY EXPRESS OR IMPLIED
+* WARRANTY.  IN PARTICULAR, NEITHER THE AUTHORS NOR AT&T MAKE ANY
+* REPRESENTATION OR WARRANTY OF ANY KIND CONCERNING THE MERCHANTABILITY
+* OF THIS SOFTWARE OR ITS FITNESS FOR ANY PARTICULAR PURPOSE.
+*/
+
+#ifndef VORONOI_DIAGRAM_GENERATOR
+#define VORONOI_DIAGRAM_GENERATOR
+
+#include <math.h>
+#include <stdlib.h>
+#include <string.h>
+
+
+#ifndef NULL
+#define NULL 0
+#endif
+#define DELETED -2
+
+#define le 0
+#define re 1
+
+
+
+struct	Freenode	
+{
+	struct	Freenode *nextfree;
+};
+
+struct FreeNodeArrayList
+{
+	struct	Freenode* memory;
+	struct	FreeNodeArrayList* next;
+
+};
+
+struct	Freelist	
+{
+	struct	Freenode	*head;
+	int		nodesize;
+};
+
+struct Point	
+{
+	float x,y;
+};
+
+// structure used both for sites and for vertices 
+struct Site	
+{
+	struct	Point	coord;
+	int		sitenbr;
+	int		refcnt;
+};
+
+
+
+struct Edge	
+{
+	float   a,b,c;
+	struct	Site 	*ep[2];
+	struct	Site	*reg[2];
+	int		edgenbr;
+
+};
+
+struct GraphEdge
+{
+	float x1,y1,x2,y2;
+	struct GraphEdge* next;
+};
+
+
+
+
+struct Halfedge 
+{
+	struct	Halfedge	*ELleft, *ELright;
+	struct	Edge	*ELedge;
+	int		ELrefcnt;
+	char	ELpm;
+	struct	Site	*vertex;
+	float	ystar;
+	struct	Halfedge *PQnext;
+};
+
+
+
+
+class VoronoiDiagramGenerator
+{
+public:
+	VoronoiDiagramGenerator();
+	~VoronoiDiagramGenerator();
+
+	bool generateVoronoi(float *xValues, float *yValues, int numPoints, float minX, float maxX, float minY, float maxY, float minDist=0);
+
+	void resetIterator()
+	{
+		iteratorEdges = allEdges;
+	}
+
+	bool getNext(float& x1, float& y1, float& x2, float& y2)
+	{
+		if(iteratorEdges == 0)
+			return false;
+		
+		x1 = iteratorEdges->x1;
+		x2 = iteratorEdges->x2;
+		y1 = iteratorEdges->y1;
+		y2 = iteratorEdges->y2;
+
+		iteratorEdges = iteratorEdges->next;
+
+		return true;
+	}
+
+
+private:
+	void cleanup();
+	void cleanupEdges();
+	char *getfree(struct Freelist *fl);	
+	struct	Halfedge *PQfind();
+	int PQempty();
+
+
+	
+	struct	Halfedge **ELhash;
+	struct	Halfedge *HEcreate(), *ELleft(), *ELright(), *ELleftbnd();
+	struct	Halfedge *HEcreate(struct Edge *e,int pm);
+
+
+	struct Point PQ_min();
+	struct Halfedge *PQextractmin();	
+	void freeinit(struct Freelist *fl,int size);
+	void makefree(struct Freenode *curr,struct Freelist *fl);
+	void geominit();
+	void plotinit();
+	bool voronoi(int triangulate);
+	void ref(struct Site *v);
+	void deref(struct Site *v);
+	void endpoint(struct Edge *e,int lr,struct Site * s);
+
+	void ELdelete(struct Halfedge *he);
+	struct Halfedge *ELleftbnd(struct Point *p);
+	struct Halfedge *ELright(struct Halfedge *he);
+	void makevertex(struct Site *v);
+	void out_triple(struct Site *s1, struct Site *s2,struct Site * s3);
+
+	void PQinsert(struct Halfedge *he,struct Site * v, float offset);
+	void PQdelete(struct Halfedge *he);
+	bool ELinitialize();
+	void ELinsert(struct	Halfedge *lb, struct Halfedge *newHe);
+	struct Halfedge * ELgethash(int b);
+	struct Halfedge *ELleft(struct Halfedge *he);
+	struct Site *leftreg(struct Halfedge *he);
+	void out_site(struct Site *s);
+	bool PQinitialize();
+	int PQbucket(struct Halfedge *he);
+	void clip_line(struct Edge *e);
+	char *myalloc(unsigned n);
+	int right_of(struct Halfedge *el,struct Point *p);
+
+	struct Site *rightreg(struct Halfedge *he);
+	struct Edge *bisect(struct	Site *s1,struct	Site *s2);
+	float dist(struct Site *s,struct Site *t);
+	struct Site *intersect(struct Halfedge *el1, struct Halfedge *el2, struct Point *p=0);
+
+	void out_bisector(struct Edge *e);
+	void out_ep(struct Edge *e);
+	void out_vertex(struct Site *v);
+	struct Site *nextone();
+
+	void pushGraphEdge(float x1, float y1, float x2, float y2);
+
+	void openpl();
+	void line(float x1, float y1, float x2, float y2);
+	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;
+
+	int		triangulate, sorted, plot, debug;
+	float	xmin, xmax, ymin, ymax, deltax, deltay;
+
+	struct	Site	*sites;
+	int		nsites;
+	int		siteidx;
+	int		sqrt_nsites;
+	int		nvertices;
+	struct 	Freelist sfl;
+	struct	Site	*bottomsite;
+
+	int		nedges;
+	struct	Freelist efl;
+	int		PQhashsize;
+	struct	Halfedge *PQhash;
+	int		PQcount;
+	int		PQmin;
+
+	int		ntry, totalsearch;
+	float	pxmin, pxmax, pymin, pymax, cradius;
+	int		total_alloc;
+
+	float borderMinX, borderMaxX, borderMinY, borderMaxY;
+
+	FreeNodeArrayList* allMemoryList;
+	FreeNodeArrayList* currentMemoryBlock;
+
+	GraphEdge* allEdges;
+	GraphEdge* iteratorEdges;
+
+	float minDistanceBetweenSites;
+	
+};
+
+int scomp(const void *p1,const void *p2);
+
+
+#endif
+
+

include/buzzuav_closures.h

@@ -98,7 +98,7 @@ double* getgoto();
  * returns the current grid
  */
 std::map<int, std::map<int,int>> getgrid();
-
+int voronoi_center(buzzvm_t vm);
 
 /*
  * returns the gimbal commands

src/buzz_utility.cpp

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

src/buzzuav_closures.cpp

@@ -8,6 +8,7 @@
 
 #include "buzzuav_closures.h"
 #include "math.h"
+#include "VoronoiDiagramGenerator.h"
 
 namespace buzzuav_closures
 {
@@ -132,6 +133,18 @@ void rb_from_gps(double nei[], double out[], double cur[])
   out[2] = 0.0;
 }
 
+void gps_from_vec(double vec[], double gps[]) {
+  double Vrange = sqrt(vec[0] * vec[0] + vec[1] * vec[1]);
+  double Vbearing = constrainAngle(atan2(vec[1], vec[0]));
+  double latR = cur_pos[0]*M_PI/180.0;
+  double lonR = cur_pos[1]*M_PI/180.0;
+  double target_lat = asin(sin(latR) * cos(Vrange/EARTH_RADIUS) + cos(latR) * sin(Vrange/EARTH_RADIUS) * cos(Vbearing));
+  double target_lon = lonR + atan2(sin(Vbearing) * sin(Vrange/EARTH_RADIUS) * cos(latR), cos(Vrange/EARTH_RADIUS) - sin(latR) * sin(target_lat));
+  gps[0] = target_lat*180.0/M_PI;
+  gps[1] = target_lon*180.0/M_PI;
+  gps[2] = cur_pos[2];
+}
+
 void parse_gpslist()
 /*
 / parse a csv of GPS targets
@@ -235,6 +248,134 @@ int buzz_exportmap(buzzvm_t vm)
   return buzzvm_ret0(vm);
 }
 
+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);
+
+	vdg.resetIterator();
+
+	float x1,y1,x2,y2;
+  map<float, std::array<float, 4>> edges;
+	while(vdg.getNext(x1,y1,x2,y2))
+	{
+		ROS_INFO("GOT Line (%f,%f)->(%f,%f)\n",x1,y1,x2, y2);
+    edges.insert(make_pair(sqrt((x2+x1)*(x2+x1)/4+(y2+y1)*(y2+y1)/4), std::array<float, 4>{x1,y1,x2,y2}));
+	}
+  double center_dist[2] = {0,0};
+  float closest_points[8];
+  int nit = 1;
+  float prev;
+  map<float, std::array<float, 4>>::iterator it;
+  int got3 = 0;
+  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];
+          got3=2;
+        }
+        ROS_INFO("USE Point (%f,%f): %f (%i)\n",it->second[2],it->second[3], it->first, got3);
+      }
+     } else
+      break;
+     nit++;
+  }
+  if(got3==2)
+    center_dist[0]=(closest_points[0]+closest_points[2]+closest_points[4]+closest_points[6])/4;
+  else
+    center_dist[0]=(closest_points[0]+closest_points[2]+closest_points[4])/3;
+  if(got3==2)
+    center_dist[1]=(closest_points[1]+closest_points[3]+closest_points[5]+closest_points[7])/4;
+  else
+    center_dist[1]=(closest_points[1]+closest_points[3]+closest_points[5])/3;
+  center_dist[0]=center_dist[0]*cos(offset_angle)-center_dist[1]*sin(offset_angle);
+  center_dist[1]=center_dist[0]*sin(offset_angle)+center_dist[1]*cos(offset_angle);
+  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