From 78638ce8b8d8427245ac319f26ce61e20ea12828 Mon Sep 17 00:00:00 2001 From: dave Date: Fri, 9 Nov 2018 14:25:58 -0500 Subject: [PATCH] enhanced geofencing --- buzz_scripts/include/act/naviguation.bzz | 49 +++----- buzz_scripts/main.bzz | 4 +- include/buzzuav_closures.h | 1 + src/buzz_utility.cpp | 3 + src/buzzuav_closures.cpp | 150 ++++++++++++++++++++++- 5 files changed, 172 insertions(+), 35 deletions(-) diff --git a/buzz_scripts/include/act/naviguation.bzz b/buzz_scripts/include/act/naviguation.bzz index 614e2ea..b60af08 100644 --- a/buzz_scripts/include/act/naviguation.bzz +++ b/buzz_scripts/include/act/naviguation.bzz @@ -2,44 +2,31 @@ GOTO_MAXVEL = 1.5 # m/steps GOTO_MAXDIST = 150 # m. GOTODIST_TOL = 0.4 # m. GOTOANG_TOL = 0.1 # rad. -GPSlimit = {.1={.lat=45.510386, .lng=-73.610400}, - .2={.lat=45.509839, .lng=-73.610047}, - .3={.lat=45.510859, .lng=-73.608714}, - .4={.lat=45.510327, .lng=-73.608393}} +GPSlimit = {.1={.lat=45.510400, .lng=-73.610421}, + .2={.lat=45.510896, .lng=-73.608731}, + .3={.lat=45.510355, .lng=-73.608404}, + .4={.lat=45.509840, .lng=-73.610072}} # Core naviguation function to travel to a GPS target location. function goto_gps(transf) { - if(Geofence()) { - m_navigation = vec_from_gps(cur_goal.latitude, cur_goal.longitude, 0) - #print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation)) - if(math.vec2.length(m_navigation)>GOTO_MAXDIST) - log("Sorry this is too far (", math.vec2.length(m_navigation), " / ", GOTO_MAXDIST, " )") - else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL){ # reached destination - transf() - } else { - m_navigation = LimitSpeed(m_navigation, 1.0) - #m_navigation = LCA(m_navigation) - goto_abs(m_navigation.x, m_navigation.y, cur_goal.altitude - pose.position.altitude, 0.0) - } - } else - log("Geofencing prevents from going to that location!") + m_navigation = vec_from_gps(cur_goal.latitude, cur_goal.longitude, 0) + #print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation)) + if(math.vec2.length(m_navigation)>GOTO_MAXDIST) + log("Sorry this is too far (", math.vec2.length(m_navigation), " / ", GOTO_MAXDIST, " )") + else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL){ # reached destination + transf() + } else { + m_navigation = LimitSpeed(m_navigation, 1.0) + table_print(m_navigation) + gf = {.0=m_navigation, .1=vec_from_gps(GPSlimit[1].lat, GPSlimit[1].lng, 0), .2=vec_from_gps(GPSlimit[2].lat, GPSlimit[2].lng, 0), .3=vec_from_gps(GPSlimit[3].lat, GPSlimit[3].lng, 0), .4=vec_from_gps(GPSlimit[4].lat, GPSlimit[4].lng, 0)} + geofence(gf) + #m_navigation = LCA(m_navigation) + goto_abs(m_navigation.x, m_navigation.y, cur_goal.altitude - pose.position.altitude, 0.0) + } } function LimitSpeed(vel_vec, factor){ if(math.vec2.length(vel_vec)>GOTO_MAXVEL*factor) vel_vec = math.vec2.scale(vel_vec, GOTO_MAXVEL*factor/math.vec2.length(vel_vec)) return vel_vec -} - -function Geofence(){ #TODO: rotate the fence box to really fit the coordinates - if(cur_goal.latitude > GPSlimit[1].lat and cur_goal.latitude > GPSlimit[2].lat and cur_goal.latitude > GPSlimit[3].lat and cur_goal.latitude > GPSlimit[4].lat) - return 0; - if(cur_goal.latitude < GPSlimit[1].lat and cur_goal.latitude < GPSlimit[2].lat and cur_goal.latitude < GPSlimit[3].lat and cur_goal.latitude < GPSlimit[4].lat) - return 0; - if(cur_goal.longitude > GPSlimit[1].lng and cur_goal.longitude > GPSlimit[2].lng and cur_goal.longitude > GPSlimit[3].lng and cur_goal.longitude > GPSlimit[4].lng) - return 0; - if(cur_goal.longitude < GPSlimit[1].lng and cur_goal.longitude < GPSlimit[2].lng and cur_goal.longitude < GPSlimit[3].lng and cur_goal.longitude < GPSlimit[4].lng) - return 0; - - return 1 } \ No newline at end of file diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index d9703f4..24826f6 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -12,7 +12,7 @@ include "utils/takeoff_heights.bzz" #State launched after takeoff -AUTO_LAUNCH_STATE = "IDLE" +AUTO_LAUNCH_STATE = "DEPLOY" TARGET = 9.0 EPSILON = 30.0 ROOT_ID = 3 @@ -44,7 +44,7 @@ function init() { nei_cmd_listen() # Starting state: TURNEDOFF to wait for user input. - BVMSTATE = "TURNEDOFF" + BVMSTATE = "LAUNCH" } # Executed at each time step. diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index 11efd3a..557034f 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -154,6 +154,7 @@ int buzzuav_land(buzzvm_t vm); * Command the UAV to go to home location */ int buzzuav_gohome(buzzvm_t vm); +int buzzuav_geofence(buzzvm_t vm); /* * Updates battery information in Buzz diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index daedd8f..4686ffd 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -243,6 +243,9 @@ static int buzz_register_hooks() buzzvm_pushs(VM, buzzvm_string_register(VM, "voronoi", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::voronoi_center)); buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "geofence", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_geofence)); + buzzvm_gstore(VM); return VM->state; } diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index ad91e83..43c05d0 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -376,6 +376,152 @@ int voronoi_center(buzzvm_t vm) { return buzzvm_ret0(vm); } +/* + * Geofence(): test for a point in a polygon + * TAKEN from https://www.geeksforgeeks.org/how-to-check-if-a-given-point-lies-inside-a-polygon/ + */ +struct Point +{ + int x; + int y; +}; +// Given three colinear points p, q, r, the function checks if +// point q lies on line segment 'pr' +bool onSegment(Point p, Point q, Point r) +{ + if (q.x <= max(p.x, r.x) && q.x >= min(p.x, r.x) && + q.y <= max(p.y, r.y) && q.y >= min(p.y, r.y)) + return true; + return false; +} +// To find orientation of ordered triplet (p, q, r). +// The function returns following values +// 0 --> p, q and r are colinear +// 1 --> Clockwise +// 2 --> Counterclockwise +int orientation(Point p, Point q, Point r) +{ + int val = (q.y - p.y) * (r.x - q.x) - + (q.x - p.x) * (r.y - q.y); + + if (val == 0) return 0; // colinear + return (val > 0)? 1: 2; // clock or counterclock wise +} +// The function that returns true if line segment 'p1q1' +// and 'p2q2' intersect. +bool doIntersect(Point p1, Point q1, Point p2, Point q2) +{ + // Find the four orientations needed for general and + // special cases + int o1 = orientation(p1, q1, p2); + int o2 = orientation(p1, q1, q2); + int o3 = orientation(p2, q2, p1); + int o4 = orientation(p2, q2, q1); + + // General case + if (o1 != o2 && o3 != o4) + return true; + + // Special Cases + // p1, q1 and p2 are colinear and p2 lies on segment p1q1 + if (o1 == 0 && onSegment(p1, p2, q1)) return true; + + // p1, q1 and p2 are colinear and q2 lies on segment p1q1 + if (o2 == 0 && onSegment(p1, q2, q1)) return true; + + // p2, q2 and p1 are colinear and p1 lies on segment p2q2 + if (o3 == 0 && onSegment(p2, p1, q2)) return true; + + // p2, q2 and q1 are colinear and q1 lies on segment p2q2 + if (o4 == 0 && onSegment(p2, q1, q2)) return true; + + return false; // Doesn't fall in any of the above cases +} +int buzzuav_geofence(buzzvm_t vm) +{ + bool onedge = false; + Point P; + Point V[4]; + int tmp; + buzzvm_lnum_assert(vm, 1); + // Get the parameter + buzzvm_lload(vm, 1); + buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE); // dictionary + buzzobj_t t = buzzvm_stack_at(vm, 1); + + if(buzzdict_size(t->t.value) != 5) { + ROS_ERROR("Wrong Geofence input size (%i).", buzzdict_size(t->t.value)); + return buzzvm_ret0(vm); + } + for(int32_t i = 0; i < buzzdict_size(t->t.value); ++i) { + buzzvm_dup(vm); + buzzvm_pushi(vm, i); + buzzvm_tget(vm); + + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "x", 1)); + buzzvm_tget(vm); + tmp = round(buzzvm_stack_at(vm, 1)->f.value*10); + ROS_INFO("[%i]---x-->%i",buzz_utility::get_robotid(), tmp); + if(i==0) + P.x = tmp; + else + V[i-1].x = tmp; + buzzvm_pop(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "y", 1)); + buzzvm_tget(vm); + tmp = round(buzzvm_stack_at(vm, 1)->f.value*10); + ROS_INFO("[%i]---x-->%i",buzz_utility::get_robotid(), tmp); + if(i==0) + P.y = tmp; + else + V[i-1].y = tmp; + buzzvm_pop(vm); + + buzzvm_pop(vm); + } + + // simple polygon: rectangle, 4 points + int n = 4; + // Create a point for line segment from p to infinite + Point extreme = {10000, P.y}; + + // Count intersections of the above line with sides of polygon + int count = 0, i = 0; + do + { + int next = (i+1)%n; + + // Check if the line segment from 'p' to 'extreme' intersects + // with the line segment from 'polygon[i]' to 'polygon[next]' + if (doIntersect(V[i], V[next], P, extreme)) + { + // If the point 'p' is colinear with line segment 'i-next', + // then check if it lies on segment. If it lies, return true, + // otherwise false + if (orientation(V[i], P, V[next]) == 0) { + onedge = onSegment(V[i], P, V[next]); + if(onedge) + break; + } + + count++; + } + i = next; + } while (i != 0); + + ROS_INFO("[%i] Geofence: %i, %i",buzz_utility::get_robotid(),count, onedge); + + if((count%2 == 0) || onedge) { + goto_gpsgoal[0] = cur_pos[0]; + goto_gpsgoal[1] = cur_pos[1]; + ROS_WARN("Geofencing trigered, not going any further!"); + } + + return buzzvm_ret0(vm); +} + int buzzuav_moveto(buzzvm_t vm) /* / Buzz closure to move following a 3D vector + Yaw @@ -457,8 +603,8 @@ int buzzuav_addNeiStatus(buzzvm_t vm) buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE); buzzobj_t t = buzzvm_stack_at(vm, 1); if(buzzdict_size(t->t.value) != 5) { - ROS_WARN("Wrong neighbor status size."); - return vm->state; + ROS_ERROR("Wrong neighbor status size."); + return buzzvm_ret0(vm); } buzz_utility::neighbors_status newRS;