Spiri
/
ROSBuzz_MISTLab
12
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

enhanced geofencing

This commit is contained in:
dave 2018-11-09 14:25:58 -05:00
parent 601b1d91e6
commit 78638ce8b8
5 changed files with 172 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
buzz_scripts/include/act/naviguation.bzz
View File

@ -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
}

4
buzz_scripts/main.bzz
View File

@ -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.

1
include/buzzuav_closures.h
View File

@ -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

3
src/buzz_utility.cpp
View File

@ -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;
}

150
src/buzzuav_closures.cpp
View File

@ -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;