From 8f7b8fa4e6d48b4b2228092f505f8d08a9b22deb Mon Sep 17 00:00:00 2001 From: dave Date: Sat, 10 Nov 2018 03:46:27 -0500 Subject: [PATCH] first working implementation of Voronoi deployment with geofence --- buzz_scripts/include/act/naviguation.bzz | 1 - buzz_scripts/include/act/states.bzz | 45 ++---- include/VoronoiDiagramGenerator.h | 16 +- include/buzzuav_closures.h | 1 + src/VoronoiDiagramGenerator.cpp | 70 ++++++++- src/buzzuav_closures.cpp | 177 +++++++++++++++-------- src/roscontroller.cpp | 3 +- 7 files changed, 210 insertions(+), 103 deletions(-) diff --git a/buzz_scripts/include/act/naviguation.bzz b/buzz_scripts/include/act/naviguation.bzz index b60af08..1425145 100644 --- a/buzz_scripts/include/act/naviguation.bzz +++ b/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) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 9df236e..d7722e6 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/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) - it_pts = it_pts + 1}) + if(rid!=0){ #remove GS (?) + 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.ymaxY){ + if(value.y>maxY) maxY = value.y - maxYid = key - } + if(value.x>maxX) + maxX = value.x + if(value.x 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} + return {.miny=minY, .minx=minX, .maxy=maxY, .maxx=maxX, .oa=0.0} } # Custom state function for the developer to play with diff --git a/include/VoronoiDiagramGenerator.h b/include/VoronoiDiagramGenerator.h index a812c78..c0ef396 100644 --- a/include/VoronoiDiagramGenerator.h +++ b/include/VoronoiDiagramGenerator.h @@ -33,6 +33,8 @@ #include #include #include +#include +#include #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; diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index 557034f..95140dc 100644 --- a/include/buzzuav_closures.h +++ b/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); /* diff --git a/src/VoronoiDiagramGenerator.cpp b/src/VoronoiDiagramGenerator.cpp index 49c1b5b..e87cb02 100644 --- a/src/VoronoiDiagramGenerator.cpp +++ b/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 */ 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 { diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 43c05d0..2eed680 100644 --- a/src/buzzuav_closures.cpp +++ b/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 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 vert) { + float a = 0.0; + vector ::iterator it; + vector ::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 vert, double *c) { + float A = pol_area(vert); + int i1 = 1; + vector ::iterator it; + vector ::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> edges; - while(vdg.getNext(x1,y1,x2,y2)) + int s[2]; + vector 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); - edges.insert(make_pair(sqrt((x2+x1)*(x2+x1)/4+(y2+y1)*(y2+y1)/4), std::array{x1,y1,x2,y2})); - } - double center_dist[2] = {0,0}; - float closest_points[8]; - int nit = 1; - float prev; - map>::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>::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("GOT Line (%f,%f)->(%f,%f) between sites %d,%d\n",x1,y1,x2,y2,s[0],s[1]); + if(logVoronoi) voronoicsv << x1 << "," << y1 << "," << x2 << "," << y2 << "," << s[0] << "," << s[1] << ","; + i++; + if((s[0]==0 || s[1]==0) && sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1))>0.1) { + if(cell_vert.empty()){ + cell_vert.push_back(Pointf(x1,y1)); + cell_vert.push_back(Pointf(x2,y2)); + } else { + bool alreadyin = false; + vector ::iterator itc; + for (itc = cell_vert.begin(); itc != cell_vert.end(); ++itc) { + double dist = sqrt((itc->x - x1) * (itc->x - x1) + (itc->y - y1) * (itc->y - y1)); + if(dist < 0.1) { + alreadyin = true; + break; + } } - 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++; - } - 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); + } + } + 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); @@ -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]; diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 91b17ce..b198752 100644 --- a/src/roscontroller.cpp +++ b/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;