first working implementation of Voronoi deployment with geofence

buzz_scripts/include/act/naviguation.bzz

@@ -17,7 +17,6 @@ function goto_gps(transf) {
 			transf()
 	} else {
 		m_navigation = LimitSpeed(m_navigation, 1.0)
-		table_print(m_navigation)
 		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)

buzz_scripts/include/act/states.bzz

@@ -245,12 +245,16 @@ function voronoicentroid() {
   BVMSTATE="DEPLOY"
 	if(counter==0 and id!=0) {
 		pts = getbounds()
-		it_pts = 0
+		pts[0] = {.x=0, .y=0}	#add itself
+		it_pts = 1
 		#table_print(pts)
 		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+pts.oa)
+					it_pts = it_pts + 1
+				}
+			})
 			#table_print(pts)
 			voronoi(pts)
 		}
@@ -271,37 +275,20 @@ function getbounds(){
 		RBlimit[key] = vec_from_gps(value.lat, value.lng, 0)
 	})
 	minY = RBlimit[1].y
-	minYid = 1
 	maxY = RBlimit[1].y
-	maxYid = 1
+	minX = RBlimit[1].x
+	maxX = RBlimit[1].x
 	foreach(RBlimit, function(key, value) {
-		if(value.y<minY){
+		if(value.y<minY)
 			minY = value.y
-			minYid = key
+		if(value.y>maxY)
-		}
-		if(value.y>maxY){
 			maxY = value.y
-			maxYid = key
+		if(value.x>maxX)
-		}
+			maxX = value.x
+		if(value.x<minX)
+			minX = value.x
 	})
-	minY2 = maxY
+	return {.miny=minY, .minx=minX, .maxy=maxY, .maxx=maxX, .oa=0.0}
-	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

include/VoronoiDiagramGenerator.h

@@ -33,6 +33,8 @@
 #include <math.h>
 #include <stdlib.h>
 #include <string.h>
+#include <stdio.h>
+#include <algorithm>
 
 
 #ifndef NULL
@@ -84,12 +86,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 +127,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 +136,7 @@ public:
 		x2 = iteratorEdges->x2;
 		y1 = iteratorEdges->y1;
 		y2 = iteratorEdges->y2;
+		std::copy(iteratorEdges->sites, iteratorEdges->sites+2, s);
 
 		iteratorEdges = iteratorEdges->next;
 
@@ -194,14 +199,19 @@ 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);
 
 
+	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;

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);
 
 /*

src/VoronoiDiagramGenerator.cpp

@@ -295,7 +295,7 @@ void VoronoiDiagramGenerator::geominit()
 }
 
 
-struct Edge * VoronoiDiagramGenerator::bisect(struct Site *s1,struct	Site *s2)
+struct Edge * VoronoiDiagramGenerator::bisect(struct Site *s1,struct Site *s2)
 {
 	float dx,dy,adx,ady;
 	struct Edge *newedge;	
@@ -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){}
@@ -740,6 +743,59 @@ void VoronoiDiagramGenerator::plotinit()
 }
 
 
+// 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;
@@ -749,6 +805,7 @@ 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
@@ -849,7 +906,7 @@ void VoronoiDiagramGenerator::clip_line(struct Edge *e)
 	};
 	
 	//printf("\nPushing line (%f,%f,%f,%f)",x1,y1,x2,y2);
-	line(x1,y1,x2,y2);
+	line(x1,y1,x2,y2,e->sites);
 }
 
 
@@ -895,6 +952,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/buzzuav_closures.cpp

@@ -37,7 +37,22 @@ 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 
+{ 
+    int x; 
+    int y; 
+};
+struct Pointf 
+{ 
+    float x; 
+    float y;
+    Pointf(): x( 0.0 ), y( 0.0 ) { }
+    Pointf( float x, float y ): x( x ), y( y ) { }
+};
 string WPlistname = "";
 
 std::map<int, buzz_utility::RB_struct> targets_map;
@@ -55,7 +70,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 +119,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
@@ -248,6 +272,45 @@ 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};
@@ -310,67 +373,58 @@ int voronoi_center(buzzvm_t 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;
-  map<float, std::array<float, 4>> edges;
+  int s[2];
-	while(vdg.getNext(x1,y1,x2,y2))
+  vector <Pointf> cell_vert;
+  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\n",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(logVoronoi)  voronoicsv << x1 << "," << y1 << "," << x2 << "," << y2 << "," << s[0] << "," << s[1] << ",";
-	}
+    i++;
-  double center_dist[2] = {0,0};
+    if((s[0]==0 || s[1]==0) && sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1))>0.1) {
-  float closest_points[8];
+      if(cell_vert.empty()){
-  int nit = 1;
+        cell_vert.push_back(Pointf(x1,y1));
-  float prev;
+        cell_vert.push_back(Pointf(x2,y2));
-  map<float, std::array<float, 4>>::iterator it;
+      } else {
-  int got3 = 0;
+        bool alreadyin = false;
-  for (it = edges.begin(); it != edges.end(); ++it)
+        vector <Pointf>::iterator itc;
-  {
+        for (itc = cell_vert.begin(); itc != cell_vert.end(); ++itc) {
-     if(nit == 1) {
+          double dist = sqrt((itc->x - x1) * (itc->x - x1) + (itc->y - y1) * (itc->y - y1));
-      //center_dist[0] += it->second[0] + it->second[2];
+          if(dist < 0.1) {
-      //center_dist[1] += it->second[1] + it->second[3];
+            alreadyin = true;
-      closest_points[0] = it->second[0]; closest_points[1] = it->second[1]; closest_points[2] = it->second[2]; closest_points[3] = it->second[3];
+            break;
-      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);
+        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));
       }
-     } else
+    }
-      break;
+	}
-     nit++;
+  std::sort( cell_vert.begin(), cell_vert.end(), clockwise_compare_points );
-  }
+  cell_vert.push_back(cell_vert[0]);
-  if(got3==2)
+  
-    center_dist[0]=(closest_points[0]+closest_points[2]+closest_points[4]+closest_points[6])/4;
+  double center_dist[2] = {0.0, 0.0};
-  else
+  polygone_center(cell_vert, center_dist);
-    center_dist[0]=(closest_points[0]+closest_points[2]+closest_points[4])/3;
+  if(logVoronoi)  voronoicsv << center_dist[0] << "," << center_dist[1] << std::endl;
-  if(got3==2)
+  center_dist[0]/=2;
-    center_dist[1]=(closest_points[1]+closest_points[3]+closest_points[5]+closest_points[7])/4;
+  center_dist[1]/=2;
-  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]);
+  //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);
@@ -380,11 +434,7 @@ int voronoi_center(buzzvm_t 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/  
  */
-struct Point 
+
-{ 
-    int x; 
-    int y; 
-};
 // 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) 
@@ -437,6 +487,7 @@ bool doIntersect(Point p1, Point q1, Point p2, Point q2)
   
     return false; // Doesn't fall in any of the above cases 
 }
+
 int buzzuav_geofence(buzzvm_t vm)
 {
     bool onedge = false;
@@ -462,7 +513,7 @@ int buzzuav_geofence(buzzvm_t vm)
       buzzvm_pushs(vm, buzzvm_string_register(vm, "x", 1));
       buzzvm_tget(vm);
       tmp = round(buzzvm_stack_at(vm, 1)->f.value*10);
-      ROS_INFO("[%i]---x-->%i",buzz_utility::get_robotid(), tmp);
+      //ROS_INFO("[%i]---x-->%i",buzz_utility::get_robotid(), tmp);
       if(i==0)
         P.x = tmp;
       else
@@ -472,7 +523,7 @@ int buzzuav_geofence(buzzvm_t vm)
       buzzvm_pushs(vm, buzzvm_string_register(vm, "y", 1));
       buzzvm_tget(vm);
       tmp = round(buzzvm_stack_at(vm, 1)->f.value*10);
-      ROS_INFO("[%i]---x-->%i",buzz_utility::get_robotid(), tmp);
+      //ROS_INFO("[%i]---y-->%i",buzz_utility::get_robotid(), tmp);
       if(i==0)
         P.y = tmp;
       else
@@ -511,7 +562,7 @@ int buzzuav_geofence(buzzvm_t vm)
       i = next; 
     } while (i != 0); 
   
-  ROS_INFO("[%i] Geofence: %i, %i",buzz_utility::get_robotid(),count, onedge);
+  //ROS_INFO("[%i] Geofence: %i, %i",buzz_utility::get_robotid(),count, onedge);
 
   if((count%2 == 0) || onedge) {
     goto_gpsgoal[0] = cur_pos[0];

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;