From 4fbdb07ce43f637dfe0b61aa0ef82937664d0a7d Mon Sep 17 00:00:00 2001 From: dave Date: Fri, 16 Nov 2018 18:00:36 +0100 Subject: [PATCH] added popup on waypoint, batt level yellow color, fix gps location for pangeae and enhanced robustness to packet lost --- buzz_scripts/include/act/barrier.bzz | 48 +++-- buzz_scripts/include/act/naviguation.bzz | 11 +- buzz_scripts/include/act/neighborcomm.bzz | 11 +- buzz_scripts/include/act/states.bzz | 17 +- buzz_scripts/include/utils/conversions.bzz | 9 +- src/buzzuav_closures.cpp | 196 ++++++++++----------- 6 files changed, 151 insertions(+), 141 deletions(-) diff --git a/buzz_scripts/include/act/barrier.bzz b/buzz_scripts/include/act/barrier.bzz index 48fa853..b21db79 100644 --- a/buzz_scripts/include/act/barrier.bzz +++ b/buzz_scripts/include/act/barrier.bzz @@ -22,18 +22,19 @@ function barrier_create() { timeW = 1 # create barrier vstig #log("---> Prev. br. ", barrier, " ", BARRIER_VSTIG) - if(barrier!=nil) { - barrier=nil - if(hvs) { - BARRIER_VSTIG = BARRIER_VSTIG -1 - hvs = 0 - }else{ - BARRIER_VSTIG = BARRIER_VSTIG +1 - hvs = 1 - } - } + #if(barrier!=nil) { + # barrier=nil + #if(hvs) { + # BARRIER_VSTIG = BARRIER_VSTIG -1 + # hvs = 0 + #}else{ + # BARRIER_VSTIG = BARRIER_VSTIG +1 + # hvs = 1 + #} + #} #log("---> New. br. ", barrier, " ", BARRIER_VSTIG) - barrier = stigmergy.create(BARRIER_VSTIG) + if(barrier==nil) + barrier = stigmergy.create(BARRIER_VSTIG) } function barrier_set(threshold, transf, resumef, bc) { @@ -51,6 +52,7 @@ function barrier_ready(bc) { #log("BARRIER READY -------") barrier.put(id, bc) barrier.put("d", 0) + barrier.put("n", 1) } # @@ -61,22 +63,33 @@ function barrier_wait(threshold, transf, resumef, bc) { barrier_create() barrier.put(id, bc) - log("--->BS: ", barrier.size(), " / ", threshold, " (", BARRIER_VSTIG, " - ", barrier.get("d"), ") t= ", timeW) - #if(barrier.size()-1 >= threshold or barrier.get("d") == 1) { + if(barrier.get("n")1) { + neighbors.foreach(function (nei,data){ + barrier.get(nei) + }) + } + + # Not to miss any RC inputs, for waypoints, potential and deploy states + check_rc_wp() + + #log("--->BS: ", barrier.size(), " / ", threshold, " (", BARRIER_VSTIG, " - ", barrier.get("d"), ") t= ", timeW) + if(barrier.size()-2 >= barrier.get("n")) { if(barrier_allgood(barrier,bc)) { barrier.put("d", 1) timeW = 0 BVMSTATE = transf } else barrier.put("d", 0) - #} + } if(timeW >= BARRIER_TIMEOUT) { log("------> Barrier Timeout !!!!") - barrier = nil + #barrier = nil timeW = 0 BVMSTATE = resumef - } else if(timeW % 100 == 0 and bc > 0) + } else if(timeW % 10 == 0 and bc > 0) neighbors.broadcast("cmd", bc) timeW = timeW+1 @@ -86,11 +99,12 @@ function barrier_wait(threshold, transf, resumef, bc) { function barrier_allgood(barrier, bc) { barriergood = 1 barrier.foreach(function(key, value, robot){ + barrier.get(key) #log("VS entry : ", key, " ", value, " ", robot) if(key == "d"){ if(value == 1) return 1 - } else if(value != bc) + } else if(key!="n" and value != bc) barriergood = 0 }) return barriergood diff --git a/buzz_scripts/include/act/naviguation.bzz b/buzz_scripts/include/act/naviguation.bzz index 1425145..4c60583 100644 --- a/buzz_scripts/include/act/naviguation.bzz +++ b/buzz_scripts/include/act/naviguation.bzz @@ -1,12 +1,17 @@ GOTO_MAXVEL = 1.5 # m/steps -GOTO_MAXDIST = 150 # m. +GOTO_MAXDIST = 250 # m. GOTODIST_TOL = 0.4 # m. GOTOANG_TOL = 0.1 # rad. -GPSlimit = {.1={.lat=45.510400, .lng=-73.610421}, +GPSlimit_CEPSUM = {.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}} +GPSlimit_PANGEAE = {.1={.lat=29.020871, .lng=-13.712477}, + .2={.lat=29.019850, .lng=-13.712378}, + .3={.lat=29.019875, .lng=-13.710096}, + .4={.lat=29.021245, .lng=-13.710184}} + # Core naviguation function to travel to a GPS target location. function goto_gps(transf) { m_navigation = vec_from_gps(cur_goal.latitude, cur_goal.longitude, 0) @@ -17,7 +22,7 @@ function goto_gps(transf) { 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)} + gf = {.0=m_navigation, .1=vec_from_gps(GPSlimit_PANGEAE[1].lat, GPSlimit_PANGEAE[1].lng, 0), .2=vec_from_gps(GPSlimit_PANGEAE[2].lat, GPSlimit_PANGEAE[2].lng, 0), .3=vec_from_gps(GPSlimit_PANGEAE[3].lat, GPSlimit_PANGEAE[3].lng, 0), .4=vec_from_gps(GPSlimit_PANGEAE[4].lat, GPSlimit_PANGEAE[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) diff --git a/buzz_scripts/include/act/neighborcomm.bzz b/buzz_scripts/include/act/neighborcomm.bzz index 845d6ee..13ca0d9 100644 --- a/buzz_scripts/include/act/neighborcomm.bzz +++ b/buzz_scripts/include/act/neighborcomm.bzz @@ -43,14 +43,14 @@ function rc_cmd_listen() { } else if (flight.rc_cmd==901){ flight.rc_cmd=0 destroyGraph() - resetWP() + check_rc_wp() 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() + check_rc_wp() barrier_set(ROBOTS, "WAYPOINT", BVMSTATE, 902) barrier_ready(902) neighbors.broadcast("cmd", 902) @@ -58,12 +58,14 @@ function rc_cmd_listen() { flight.rc_cmd=0 destroyGraph() resetWP() + check_rc_wp() barrier_set(ROBOTS, "POTENTIAL", BVMSTATE, 903) barrier_ready(903) neighbors.broadcast("cmd", 903) } else if (flight.rc_cmd==904){ flight.rc_cmd=0 destroyGraph() + resetWP() barrier_set(ROBOTS, "IDLE", BVMSTATE, 904) barrier_ready(904) neighbors.broadcast("cmd", 904) @@ -90,7 +92,8 @@ function nei_cmd_listen() { } else { if(value==20) { AUTO_LAUNCH_STATE = "IDLE" - BVMSTATE = "GOHOME" + barrier_set(ROBOTS, "GOHOME", BVMSTATE, 20) + barrier_ready(20) } else if(value==21 ) { BVMSTATE = "STOP" #neighbors.broadcast("cmd", 777) @@ -150,7 +153,7 @@ function check_rc_wp() { WPtab[rc_goto.id]=ls } v_wp.put(WPtab_id,WPtab) - } else + } else # Individual waypoint v_wp.put(rc_goto.id,ls) reset_rc() } diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 1082fe1..8d86360 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -34,6 +34,7 @@ function idle() { # Core state function to launch the robot: takeoff and wait for others, or stop (land) function launch() { BVMSTATE = "LAUNCH" + neighbors.broadcast("cmd", 22) 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, .lng=pose.position.longitude} @@ -43,7 +44,6 @@ function launch() { barrier_ready(22) } else { log("Altitude: ", pose.position.altitude) - neighbors.broadcast("cmd", 22) uav_takeoff(TARGET_ALTITUDE) } } else { @@ -85,14 +85,15 @@ function goinghome() { #m_navigation = LCA(m_navigation) goto_abs(m_navigation.x, m_navigation.y, cur_goal.altitude - pose.position.altitude, 0.0) } - } + } else + BVMSTATE = AUTO_LAUNCH_STATE } # Core state function to stop and land. function stop() { BVMSTATE = "STOP" + neighbors.broadcast("cmd", 21) if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so LAND - neighbors.broadcast("cmd", 21) uav_land() if(pose.position.altitude <= 0.2) { BVMSTATE = "STOP" @@ -118,8 +119,8 @@ function indiWP() { wp = unpackWP2i(wpi) if(wp.pro == 0) { wpreached = 0 - storegoal(wp.lat, wp.lon, pose.position.altitude) - var ls = packWP2i(wp.lat, wp.lon, 1) + storegoal(wp.lat, wp.lng, pose.position.altitude) + var ls = packWP2i(wp.lat, wp.lng, 1) v_wp.put(id,ls) return } @@ -257,11 +258,13 @@ function voronoicentroid() { BVMSTATE="DEPLOY" check_rc_wp() wptab = v_wp.get(WPtab_id) - if(wptab==nil or V_TYPE == 2) + if(wptab==nil) return else if(not(size(wptab) > 0)) return log("WP table size:", size(wptab)) + if(V_TYPE == 2) + return it_pts = 0 att = {} foreach(wptab, function(key, value){ @@ -269,7 +272,7 @@ function voronoicentroid() { 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) + att[it_pts]=vec_from_gps(wp.lat, wp.lng, 0) it_pts = it_pts + 1 }) # Boundaries from Geofence diff --git a/buzz_scripts/include/utils/conversions.bzz b/buzz_scripts/include/utils/conversions.bzz index e060e57..0efb2ad 100644 --- a/buzz_scripts/include/utils/conversions.bzz +++ b/buzz_scripts/include/utils/conversions.bzz @@ -62,16 +62,17 @@ function gps_from_vec(vec) { return Lgoal } -GPSoffset = {.lat=45.50, .lon=-73.61} +GPSoffset = {.lat=45.50, .lng=-73.61} +GPSoffsetL = {.lat=29.01, .lng=-13.70} function packWP2i(in_lat, in_long, processed) { - var dlat = math.round((in_lat - GPSoffset.lat)*1000000) - var dlon = math.round((in_long - GPSoffset.lon)*1000000) + var dlat = math.round((in_lat - GPSoffsetL.lat)*1000000) + var dlon = math.round((in_long - GPSoffsetL.lng)*1000000) return {.dla=dlat, .dlo=dlon*10+processed} } function unpackWP2i(wp_int){ var dlon = (wp_int.dlo-wp_int.dlo%10)/10.0 var pro = wp_int.dlo-dlon*10 - return {.lat=wp_int.dla/1000000.0+GPSoffset.lat, .lon=dlon/1000000.0+GPSoffset.lon, .pro=pro} + return {.lat=wp_int.dla/1000000.0+GPSoffsetL.lat, .lng=dlon/1000000.0+GPSoffsetL.lng, .pro=pro} } \ No newline at end of file diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 9fb6c7a..144af11 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -289,8 +289,8 @@ bool onSegment(Point p, Point q, Point r) // 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); + int val =round((q.y - p.y) * (r.x - q.x)*100 - + (q.x - p.x) * (r.y - q.y)*100); if (val == 0) return 0; // colinear return (val > 0)? 1: 2; // clock or counterclock wise @@ -305,8 +305,10 @@ bool doIntersect(Point p1, Point q1, Point p2, Point q2) int o2 = orientation(p1, q1, q2); int o3 = orientation(p2, q2, p1); int o4 = orientation(p2, q2, q1); + + //ROS_WARN("(%f,%f)->(%f,%f), 1:%d,2:%d,3:%d,4:%d",p1.x,p1.y,q1.x,q1.y,o1,o2,o3,o4); - // General case + // General case if (o1 != o2 && o3 != o4) return true; @@ -341,92 +343,6 @@ void sortclose_polygon(vector *P){ 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; @@ -487,9 +403,12 @@ void getintersection(Point S, Point D, std::vector Poly, Point *I) { double r = numerator( S, (*itc), (*itc), (*next) ) / d; double s = numerator( S, (*itc), S, D ) / d; + //ROS_INFO("-- (%f,%f)",S.x + r * (D.x - S.x), S.y + r * (D.y - S.y)); (*I)=Point(S.x + r * (D.x - S.x), S.y + r * (D.y - S.y)); } } + if(parallel || collinear) + ROS_WARN("Lines are Collinear (%d) or Parallels (%d)",collinear,parallel); } bool isSiteout(Point S, std::vector Poly) { @@ -524,6 +443,71 @@ bool isSiteout(Point S, std::vector Poly) { return ((count%2 == 0) && !onedge); } +int buzzuav_geofence(buzzvm_t vm) +{ + Point P; + 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); + } + std::vector polygon_bound; + for(int32_t i = 0; i < buzzdict_size(t->t.value); ++i) { + Point tmp; + 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); + if(i==0){ + P.x = buzzvm_stack_at(vm, 1)->f.value; + //printf("px=%f\n",P.x); + }else{ + tmp.x = buzzvm_stack_at(vm, 1)->f.value; + //printf("c%dx=%f\n",i,tmp.x); + } + buzzvm_pop(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "y", 1)); + buzzvm_tget(vm); + //ROS_INFO("[%i]---y-->%i",buzz_utility::get_robotid(), tmp); + if(i==0){ + P.y = buzzvm_stack_at(vm, 1)->f.value; + //printf("py=%f\n",P.y); + }else{ + tmp.y = buzzvm_stack_at(vm, 1)->f.value; + //printf("c%dy=%f\n",i,tmp.y); + } + buzzvm_pop(vm); + + if(i!=0) + polygon_bound.push_back(tmp); + + buzzvm_pop(vm); + } + sortclose_polygon(&polygon_bound); + + // Check if we are in the zone + if(isSiteout(P, polygon_bound)){ + Point Intersection; + getintersection(Point(0.0, 0.0) , P, polygon_bound, &Intersection); + double gps[3]; + double d[2]={Intersection.x,Intersection.y}; + gps_from_vec(d, gps); + set_gpsgoal(gps); + ROS_WARN("Geofencing trigered, not going any further (%f,%f)!",d[0],d[1]); + } + + return buzzvm_ret0(vm); +} + int voronoi_center(buzzvm_t vm) { float dist_max = 300; @@ -565,23 +549,7 @@ int voronoi_center(buzzvm_t vm) { 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]; @@ -607,6 +575,22 @@ int voronoi_center(buzzvm_t vm) { buzzvm_pop(vm); } + + // Check if we are in the zone + if(isSiteout(Point(0,0), polygon_bound) || count < 3) { + //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); + } VoronoiDiagramGenerator vdg; ROS_WARN("[%i] Voronoi Bounded tessellation starting with %i sites...", buzz_utility::get_robotid(),count); @@ -920,7 +904,7 @@ void set_gpsgoal(double goal[3]) { double rb[3]; rb_from_gps(goal, rb, cur_pos); - if (fabs(rb[0]) < 150.0) { + if (fabs(rb[0]) < 250.0) { goto_gpsgoal[0] = goal[0];goto_gpsgoal[1] = goal[1];goto_gpsgoal[2] = goal[2]; ROS_INFO("[%i] Set GPS GOAL TO ---- %f %f %f (%f %f, %f %f)", buzz_utility::get_robotid(), goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]); } else