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 614e2ea..5aec163 100644 --- a/buzz_scripts/include/act/naviguation.bzz +++ b/buzz_scripts/include/act/naviguation.bzz @@ -2,44 +2,30 @@ 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.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}} # 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) + 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/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 9df236e..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,18 +249,54 @@ function lennardjones() { counter = 0 function voronoicentroid() { BVMSTATE="DEPLOY" - if(counter==0 and id!=0) { - pts = getbounds() + 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 - #table_print(pts) + 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) { - 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) + it_pts = it_pts + 1 + } + }) #table_print(pts) voronoi(pts) } - counter=10 + counter=4 } counter=counter-1 @@ -265,45 +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 - minYid = 1 - maxY = RBlimit[1].y - maxYid = 1 - foreach(RBlimit, function(key, value) { - if(value.ymaxY){ - maxY = value.y - maxYid = key - } - }) - minY2 = maxY - minY2id = maxYid - foreach(RBlimit, function(key, value) { - if(value.y math.pi/2.0) - angle_offset = math.pi - angle_offset - #log(angle_offset, minYid, minY2id, maxYid) - dvec = math.vec2.sub(RBlimit[maxYid],RBlimit[minYid]) - new_maxY = math.vec2.add(RBlimit[minYid], math.vec2.newp(math.vec2.length(dvec),math.vec2.angle(dvec)+angle_offset)) - if(new_maxY.x > RBlimit[minYid].x) - return {.miny=minY, .minx=RBlimit[minYid].x, .maxy=new_maxY.y, .maxx=new_maxY.x} - else - return {.miny=minY, .minx=new_maxY.x, .maxy=new_maxY.y, .maxx=RBlimit[minYid].x, .oa=angle_offset} -} - # Custom state function for the developer to play with function cusfun(){ BVMSTATE="CUSFUN" diff --git a/buzz_scripts/include/taskallocate/bidding.bzz b/buzz_scripts/include/taskallocate/bidding.bzz index 96416e9..b54c2c3 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() } diff --git a/buzz_scripts/include/taskallocate/graphformGPS.bzz b/buzz_scripts/include/taskallocate/graphformGPS.bzz index b796a1f..d3036f0 100644 --- a/buzz_scripts/include/taskallocate/graphformGPS.bzz +++ b/buzz_scripts/include/taskallocate/graphformGPS.bzz @@ -110,7 +110,7 @@ function r2i(value){ # function find(table,value){ var ind=nil - var i=0 + i=0 while(iend +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 #include #include +#include +#include #ifndef NULL @@ -66,6 +68,8 @@ struct Freelist struct Point { float x,y; + Point(): x( 0.0 ), y( 0.0 ) { } + Point( float x, float y ): x( x ), y( y ) { } }; // structure used both for sites and for vertices @@ -84,12 +88,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 +129,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 +138,7 @@ public: x2 = iteratorEdges->x2; y1 = iteratorEdges->y1; y2 = iteratorEdges->y2; + std::copy(iteratorEdges->sites, iteratorEdges->sites+2, s); iteratorEdges = iteratorEdges->next; @@ -194,14 +201,13 @@ 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); - struct Freelist hfl; struct Halfedge *ELleftend, *ELrightend; int ELhashsize; diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index 11efd3a..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); /* @@ -154,6 +155,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/VoronoiDiagramGenerator.cpp b/src/VoronoiDiagramGenerator.cpp index 49c1b5b..9423752 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){} @@ -739,7 +742,6 @@ void VoronoiDiagramGenerator::plotinit() range(pxmin, pymin, pxmax, pymax); } - void VoronoiDiagramGenerator::clip_line(struct Edge *e) { struct Site *s1, *s2; @@ -771,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; @@ -848,8 +850,8 @@ 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); - line(x1,y1,x2,y2); + //printf("Pushing line (%f,%f,%f,%f)\n",x1,y1,x2,y2); + line(x1,y1,x2,y2,e->sites); } @@ -895,6 +897,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/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..9fb6c7a 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -37,7 +37,17 @@ 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 +{ + float x; + float y; + Point(): x( 0.0 ), y( 0.0 ) { } + Point( float x, float y ): x( x ), y( y ) { } +}; string WPlistname = ""; std::map targets_map; @@ -55,7 +65,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 +114,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 @@ -200,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) { @@ -207,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; @@ -248,50 +268,330 @@ int buzz_exportmap(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/ + */ + +// 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 +} + +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; + 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 = buzzvm_stack_at(vm, 1)->f.value; + //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 = buzzvm_stack_at(vm, 1)->f.value; + //ROS_INFO("[%i]---y-->%i",buzz_utility::get_robotid(), tmp); + if(i==0) + P.y = tmp; + else + V[i-1].y = tmp; + buzzvm_pop(vm); + + buzzvm_pop(vm); + } + // TODO: use vector + //sortclose_polygon(&V); + + // 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); +} + +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 *xValues;//[4] = {-22, -17, 4,22}; - float *yValues;//[4] = {-9, 31,13,-5}; - float minx, miny, maxx, maxy; + + 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, "maxx", 1)); + buzzvm_pushs(vm, buzzvm_string_register(vm, "np", 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; + int Poly_vert = buzzvm_stack_at(vm, 1)->i.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) { + 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); @@ -309,68 +609,90 @@ int voronoi_center(buzzvm_t vm) { } VoronoiDiagramGenerator vdg; - vdg.generateVoronoi(xValues,yValues,count, minx, maxx, miny, maxy,3); - + 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; - map> edges; - while(vdg.getNext(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)\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",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; + } } - ROS_INFO("USE Point (%f,%f): %f (%i)\n",it->second[2],it->second[3], it->first, got3); + 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)); } - } else - break; - nit++; + } + } + if(cell_vert.size()<3){ + ROS_WARN("[%i] Voronoi Bounded tessellation failed (%d)!", buzz_utility::get_robotid(),cell_vert.size()); + return buzzvm_ret0(vm); } - 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); @@ -457,8 +779,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; diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index bcd4e36..e1e086f 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;