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