From 601b1d91e642d51821f8b5debf2476f30df2520b Mon Sep 17 00:00:00 2001 From: Fang Wu Date: Wed, 7 Nov 2018 18:53:52 -0500 Subject: [PATCH 1/5] Removed some debugging log to reduce the message size: modified: src/rosbuzz/buzz_scripts/include/taskallocate/bidding.bzz --- buzz_scripts/include/taskallocate/bidding.bzz | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/buzz_scripts/include/taskallocate/bidding.bzz b/buzz_scripts/include/taskallocate/bidding.bzz index 01fd468..82101c1 100644 --- a/buzz_scripts/include/taskallocate/bidding.bzz +++ b/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() } From 78638ce8b8d8427245ac319f26ce61e20ea12828 Mon Sep 17 00:00:00 2001 From: dave Date: Fri, 9 Nov 2018 14:25:58 -0500 Subject: [PATCH 2/5] enhanced geofencing --- buzz_scripts/include/act/naviguation.bzz | 49 +++----- buzz_scripts/main.bzz | 4 +- include/buzzuav_closures.h | 1 + src/buzz_utility.cpp | 3 + src/buzzuav_closures.cpp | 150 ++++++++++++++++++++++- 5 files changed, 172 insertions(+), 35 deletions(-) diff --git a/buzz_scripts/include/act/naviguation.bzz b/buzz_scripts/include/act/naviguation.bzz index 614e2ea..b60af08 100644 --- a/buzz_scripts/include/act/naviguation.bzz +++ b/buzz_scripts/include/act/naviguation.bzz @@ -2,44 +2,31 @@ 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}} +GPSlimit = {.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}} # 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) - log("Sorry this is too far (", 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 - transf() - } else { - m_navigation = LimitSpeed(m_navigation, 1.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!") + 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) + log("Sorry this is too far (", 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 + 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) + goto_abs(m_navigation.x, m_navigation.y, cur_goal.altitude - pose.position.altitude, 0.0) + } } function LimitSpeed(vel_vec, factor){ if(math.vec2.length(vel_vec)>GOTO_MAXVEL*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 } \ No newline at end of file diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index d9703f4..24826f6 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -12,7 +12,7 @@ include "utils/takeoff_heights.bzz" #State launched after takeoff -AUTO_LAUNCH_STATE = "IDLE" +AUTO_LAUNCH_STATE = "DEPLOY" TARGET = 9.0 EPSILON = 30.0 ROOT_ID = 3 @@ -44,7 +44,7 @@ function init() { nei_cmd_listen() # Starting state: TURNEDOFF to wait for user input. - BVMSTATE = "TURNEDOFF" + BVMSTATE = "LAUNCH" } # Executed at each time step. diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index 11efd3a..557034f 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -154,6 +154,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 diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index daedd8f..4686ffd 100644 --- a/src/buzz_utility.cpp +++ b/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; } diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index ad91e83..43c05d0 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -376,6 +376,152 @@ int voronoi_center(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/ + */ +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) +{ + 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 +} +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 = round(buzzvm_stack_at(vm, 1)->f.value*10); + 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 = round(buzzvm_stack_at(vm, 1)->f.value*10); + ROS_INFO("[%i]---x-->%i",buzz_utility::get_robotid(), tmp); + if(i==0) + P.y = tmp; + else + V[i-1].y = tmp; + buzzvm_pop(vm); + + buzzvm_pop(vm); + } + + // 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); +} + int buzzuav_moveto(buzzvm_t vm) /* / Buzz closure to move following a 3D vector + Yaw @@ -457,8 +603,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."); - return vm->state; + ROS_ERROR("Wrong neighbor status size."); + return buzzvm_ret0(vm); } buzz_utility::neighbors_status newRS; From 8f7b8fa4e6d48b4b2228092f505f8d08a9b22deb Mon Sep 17 00:00:00 2001 From: dave Date: Sat, 10 Nov 2018 03:46:27 -0500 Subject: [PATCH 3/5] 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; From 4ad8ecf4435da3b70beaf96d45734ae56e3e72e8 Mon Sep 17 00:00:00 2001 From: dave Date: Sat, 10 Nov 2018 13:13:01 -0500 Subject: [PATCH 4/5] added voronoi anim pyscript and integrated Jacopo comments on readme --- buzz_scripts/log/anim_voronoi_cells.py | 28 ++++++++++++++++++++++++++ 1 file changed, 28 insertions(+) create mode 100644 buzz_scripts/log/anim_voronoi_cells.py diff --git a/buzz_scripts/log/anim_voronoi_cells.py b/buzz_scripts/log/anim_voronoi_cells.py new file mode 100644 index 0000000..c637440 --- /dev/null +++ b/buzz_scripts/log/anim_voronoi_cells.py @@ -0,0 +1,28 @@ +#!/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) +axes = plt.gca() +axes.set_xlim([-70,70]) +axes.set_ylim([-70,70]) +datafile = open('src/rosbuzz/buzz_scripts/log/voronoi_4.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) +plt.show() From b3f9b7cf4e39329abcb820ae0892725b25b2f61a Mon Sep 17 00:00:00 2001 From: dave Date: Sun, 11 Nov 2018 15:11:41 -0500 Subject: [PATCH 5/5] added Hull polygon from user inputs and minor enhancement to GIS and state machine --- buzz_scripts/include/act/barrier.bzz | 5 +- buzz_scripts/include/act/naviguation.bzz | 4 +- buzz_scripts/include/act/neighborcomm.bzz | 219 +++++---- buzz_scripts/include/act/states.bzz | 96 ++-- .../include/taskallocate/graphformGPS.bzz | 22 +- buzz_scripts/include/utils/conversions.bzz | 2 +- buzz_scripts/include/utils/quickhull.bzz | 122 +++++ buzz_scripts/include/utils/table.bzz | 26 +- buzz_scripts/include/utils/vec2.bzz | 4 + buzz_scripts/log/anim_voronoi_cells.py | 4 +- buzz_scripts/main.bzz | 15 +- include/VoronoiDiagramGenerator.h | 8 +- src/VoronoiDiagramGenerator.cpp | 59 +-- src/buzzuav_closures.cpp | 463 +++++++++++------- 14 files changed, 630 insertions(+), 419 deletions(-) create mode 100644 buzz_scripts/include/utils/quickhull.bzz diff --git a/buzz_scripts/include/act/barrier.bzz b/buzz_scripts/include/act/barrier.bzz index 3a922c7..48fa853 100644 --- a/buzz_scripts/include/act/barrier.bzz +++ b/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)) { diff --git a/buzz_scripts/include/act/naviguation.bzz b/buzz_scripts/include/act/naviguation.bzz index 1425145..5aec163 100644 --- a/buzz_scripts/include/act/naviguation.bzz +++ b/buzz_scripts/include/act/naviguation.bzz @@ -2,7 +2,7 @@ GOTO_MAXVEL = 1.5 # m/steps GOTO_MAXDIST = 150 # m. GOTODIST_TOL = 0.4 # m. GOTOANG_TOL = 0.1 # rad. -GPSlimit = {.1={.lat=45.510400, .lng=-73.610421}, +GPSlimit = {.1={.lat=45.510523, .lng=-73.611118},#{.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}} @@ -18,7 +18,7 @@ function goto_gps(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) + #geofence(gf) #m_navigation = LCA(m_navigation) goto_abs(m_navigation.x, m_navigation.y, cur_goal.altitude - pose.position.altitude, 0.0) } diff --git a/buzz_scripts/include/act/neighborcomm.bzz b/buzz_scripts/include/act/neighborcomm.bzz index 91b0379..bf9f894 100644 --- a/buzz_scripts/include/act/neighborcomm.bzz +++ b/buzz_scripts/include/act/neighborcomm.bzz @@ -1,71 +1,74 @@ # listens to commands from the remote control (web, commandline, rcclient node, etc) function rc_cmd_listen() { - if(flight.rc_cmd==22) { - log("cmd 22") - flight.rc_cmd=0 - BVMSTATE = "LAUNCH" - neighbors.broadcast("cmd", 22) - } 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" - neighbors.broadcast("cmd", 21) - } else if(flight.rc_cmd==20) { - flight.rc_cmd=0 - AUTO_LAUNCH_STATE = "IDLE" - barrier_set(ROBOTS, "GOHOME", BVMSTATE, 20) - barrier_ready(20) - neighbors.broadcast("cmd", 20) -# } else if(flight.rc_cmd==16) { -# flight.rc_cmd=0 -# 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) - barrier_ready(900) - neighbors.broadcast("cmd", 900) - } else if (flight.rc_cmd==901){ - flight.rc_cmd=0 - destroyGraph() - barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 901) - barrier_ready(901) - neighbors.broadcast("cmd", 901) - } else if (flight.rc_cmd==902){ - flight.rc_cmd=0 - destroyGraph() - barrier_set(ROBOTS, "WAYPOINT", BVMSTATE, 902) - barrier_ready(902) - neighbors.broadcast("cmd", 902) - } else if (flight.rc_cmd==903){ - flight.rc_cmd=0 - destroyGraph() - resetWP() - barrier_set(ROBOTS, "POTENTIAL", BVMSTATE, 903) - barrier_ready(903) - neighbors.broadcast("cmd", 903) - } else if (flight.rc_cmd==904){ - flight.rc_cmd=0 - destroyGraph() - barrier_set(ROBOTS, "IDLE", BVMSTATE, 904) - barrier_ready(904) - neighbors.broadcast("cmd", 904) + if(BVMSTATE=="TURNEDOFF") { + 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==777){ #SYNC + flight.rc_cmd=0 + reinit_time_sync() + neighbors.broadcast("cmd", 777) + } + } else { + if(flight.rc_cmd==21) { + flight.rc_cmd=0 + #barrier_set(ROBOTS, "GOHOME", BVMSTATE, 21) + #barrier_ready(21) + BVMSTATE = "STOP" + neighbors.broadcast("cmd", 21) + } else if(flight.rc_cmd==20) { + flight.rc_cmd=0 + AUTO_LAUNCH_STATE = "IDLE" + barrier_set(ROBOTS, "GOHOME", BVMSTATE, 20) + barrier_ready(20) + neighbors.broadcast("cmd", 20) + } else if(BVMSTATE!="BARRIERWAIT" and BVMSTATE!="LAUNCH") { + if (flight.rc_cmd==666){ + flight.rc_cmd=0 + stattab_send() + } else if (flight.rc_cmd==900){ + flight.rc_cmd=0 + barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900) + barrier_ready(900) + neighbors.broadcast("cmd", 900) + } else if (flight.rc_cmd==901){ + flight.rc_cmd=0 + destroyGraph() + 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) + } else if (flight.rc_cmd==903){ + flight.rc_cmd=0 + destroyGraph() + resetWP() + barrier_set(ROBOTS, "POTENTIAL", BVMSTATE, 903) + barrier_ready(903) + neighbors.broadcast("cmd", 903) + } else if (flight.rc_cmd==904){ + flight.rc_cmd=0 + destroyGraph() + barrier_set(ROBOTS, "IDLE", BVMSTATE, 904) + barrier_ready(904) + neighbors.broadcast("cmd", 904) + } + } } } @@ -74,53 +77,52 @@ function nei_cmd_listen() { neighbors.listen("cmd", function(vid, value, rid) { print("Got (", vid, ",", value, ") #", rid, "(", BVMSTATE, ")") - #if(BVMSTATE!="BARRIERWAIT") { - if(value==22 and BVMSTATE=="TURNEDOFF") { + if(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 - barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900) - #barrier_ready(900) - #neighbors.broadcast("cmd", 900) - } else if(value==901 and BVMSTATE!="BARRIERWAIT"){ # Pursuit - destroyGraph() - barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 901) - #barrier_ready(901) - #neighbors.broadcast("cmd", 901) - } else if(value==902 and BVMSTATE!="BARRIERWAIT" and BVMSTATE!="WAYPOINT"){ # 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 - 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 - destroyGraph() - 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 - # }) + }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!="DEPLOY" ){ # Pursuit + destroyGraph() + barrier_set(ROBOTS, "DEPLOY", BVMSTATE, 901) + #barrier_ready(901) + #neighbors.broadcast("cmd", 901) + } 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!="POTENTIAL" and BVMSTATE!="TURNEDOFF"){ # Formation + destroyGraph() + barrier_set(ROBOTS, "POTENTIAL", BVMSTATE, 903) + #barrier_ready(903) + #neighbors.broadcast("cmd", 903) + } 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) + } } - #} + } }) } @@ -141,7 +143,8 @@ function check_rc_wp() { v_wp.put(rc_goto.id,ls) reset_rc() } - } + } else + v_wp.get(0) } function resetWP() { diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index d7722e6..c766315 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/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,11 +73,13 @@ gohomeT=100 # State function to go back to ROSBuzz recorded home GPS position (at takeoff) function goinghome() { BVMSTATE = "GOHOME" - if(gohomeT > 0) { # TODO: Make a real check if home is reached - gohome() - gohomeT = gohomeT - 1 - } else - BVMSTATE = AUTO_LAUNCH_STATE + 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. @@ -85,14 +89,14 @@ function stop() { neighbors.broadcast("cmd", 21) uav_land() if(pose.position.altitude <= 0.2) { - BVMSTATE = "TURNEDOFF" - #barrier_set(ROBOTS,"TURNEDOFF","STOP", 21) - #barrier_ready(21) + BVMSTATE = "STOP" + barrier_set(ROBOTS,"TURNEDOFF","STOP", 21) + barrier_ready(21) } } else { - BVMSTATE = "TURNEDOFF" - #barrier_set(ROBOTS,"TURNEDOFF","STOP", 21) - #barrier_ready(21) + BVMSTATE = "STOP" + barrier_set(ROBOTS,"TURNEDOFF","STOP", 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,22 +249,54 @@ function lennardjones() { counter = 0 function voronoicentroid() { BVMSTATE="DEPLOY" - if(counter==0 and id!=0) { - pts = getbounds() - pts[0] = {.x=0, .y=0} #add itself - it_pts = 1 - #table_print(pts) + check_rc_wp() + log("V_wp size:",v_wp.size()) + if(V_TYPE == 2) # NOT MOVING! + return + if(not(v_wp.size() > 0)) + return + it_pts = 0 + 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) { if(rid!=0){ #remove GS (?) - pts[it_pts]=math.vec2.newp(data.distance,data.azimuth+pts.oa) + 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 @@ -269,28 +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 - maxY = RBlimit[1].y - minX = RBlimit[1].x - maxX = RBlimit[1].x - foreach(RBlimit, function(key, value) { - if(value.ymaxY) - maxY = value.y - if(value.x>maxX) - maxX = value.x - if(value.xend +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) +} \ No newline at end of file diff --git a/buzz_scripts/include/utils/table.bzz b/buzz_scripts/include/utils/table.bzz index 36c9a8a..68cf21f 100644 --- a/buzz_scripts/include/utils/table.bzz +++ b/buzz_scripts/include/utils/table.bzz @@ -1,7 +1,11 @@ function table_print(t) { - foreach(t, function(key, value) { + if(t==nil) + log("Table do not exist!") + else { + foreach(t, function(key, value) { log(key, " -> ", value) }) + } } function table_copy(t) { @@ -16,8 +20,8 @@ function table_copy(t) { #return the number of value in table # function count(table,value){ - var number=0 - var i=0 + number=0 + i=0 while(i= 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; @@ -805,7 +751,6 @@ 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 @@ -828,7 +773,7 @@ void VoronoiDiagramGenerator::clip_line(struct Edge *e) s1 = e -> ep[0]; s2 = e -> ep[1]; }; - + if(e -> a == 1.0) { y1 = pymin; @@ -905,7 +850,7 @@ 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,e->sites); } diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 2eed680..9fb6c7a 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -41,17 +41,12 @@ static bool logVoronoi = false; std::ofstream voronoicsv; -struct Point -{ - int x; - int y; -}; -struct Pointf +struct Point { float x; float y; - Pointf(): x( 0.0 ), y( 0.0 ) { } - Pointf( float x, float y ): x( x ), y( y ) { } + Point(): x( 0.0 ), y( 0.0 ) { } + Point( float x, float y ): x( x ), y( y ) { } }; string WPlistname = ""; @@ -224,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::iterator it; for (it = wplist_map.begin(); it != wplist_map.end(); ++it) { @@ -231,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; @@ -272,164 +268,6 @@ 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}; - 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); - if(logVoronoi) voronoicsv << ros::Time::now().toNSec() << ","; - vdg.resetIterator(); - - float 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) 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; - } - } - 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)); - } - } - } - 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]); - set_gpsgoal(gps); - - 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/ @@ -488,6 +326,21 @@ bool doIntersect(Point p1, Point q1, Point p2, Point q2) 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 *P){ + std::sort( P->begin(), P->end(), clockwise_compare_points ); + P->push_back((*P)[0]); +} + int buzzuav_geofence(buzzvm_t vm) { bool onedge = false; @@ -512,7 +365,7 @@ int buzzuav_geofence(buzzvm_t vm) buzzvm_dup(vm); buzzvm_pushs(vm, buzzvm_string_register(vm, "x", 1)); buzzvm_tget(vm); - tmp = round(buzzvm_stack_at(vm, 1)->f.value*10); + tmp = buzzvm_stack_at(vm, 1)->f.value; //ROS_INFO("[%i]---x-->%i",buzz_utility::get_robotid(), tmp); if(i==0) P.x = tmp; @@ -522,7 +375,7 @@ int buzzuav_geofence(buzzvm_t vm) buzzvm_dup(vm); buzzvm_pushs(vm, buzzvm_string_register(vm, "y", 1)); buzzvm_tget(vm); - tmp = round(buzzvm_stack_at(vm, 1)->f.value*10); + tmp = buzzvm_stack_at(vm, 1)->f.value; //ROS_INFO("[%i]---y-->%i",buzz_utility::get_robotid(), tmp); if(i==0) P.y = tmp; @@ -532,6 +385,8 @@ int buzzuav_geofence(buzzvm_t vm) buzzvm_pop(vm); } + // TODO: use vector + //sortclose_polygon(&V); // simple polygon: rectangle, 4 points int n = 4; @@ -573,6 +428,276 @@ int buzzuav_geofence(buzzvm_t vm) return buzzvm_ret0(vm); } +float pol_area(vector vert) { + float a = 0.0; + //ROS_INFO("Polygone %d edges area.",vert.size()); + 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; +} + +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 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 ::iterator itc; + std::vector ::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 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 ::iterator itc; + std::vector ::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 dist_max = 300; + + 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, "np", 1)); + buzzvm_tget(vm); + int Poly_vert = buzzvm_stack_at(vm, 1)->i.value; + buzzvm_pop(vm); + + std::vector polygon_bound; + for(int32_t i = 0; i < Poly_vert; ++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); + //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; + 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 ::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; + int s[2]; + vector cell_vert; + Point Intersection; + int i=0; + while(vdg.getNext(x1,y1,x2,y2,s)) + { + //ROS_INFO("GOT Line (%f,%f)->(%f,%f) between sites %d,%d",x1,y1,x2,y2,s[0],s[1]); + 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); + } + if(logVoronoi) voronoicsv << x1 << "," << y1 << "," << x2 << "," << y2 << "," << s[0] << "," << s[1] << ","; + i++; + if((s[0]==0 || s[1]==0)) { + if(cell_vert.empty()){ + cell_vert.push_back(Point(x1,y1)); + cell_vert.push_back(Point(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; + } + } + if(!alreadyin) + cell_vert.push_back(Point(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(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]); + set_gpsgoal(gps); + + return buzzvm_ret0(vm); +} + int buzzuav_moveto(buzzvm_t vm) /* / Buzz closure to move following a 3D vector + Yaw