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

Merge commit '12ff37965b577d13a3d3288686ddcbef1fb6cb43' into ros_drones_ws

This commit is contained in:
dave 2018-11-07 12:09:52 -05:00
parent bc4a50e2fe 12ff37965b
commit 5800cb8b8c
10 changed files with 1503 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@ -57,6 +57,7 @@ add_executable(rosbuzz_node src/rosbuzz.cpp
src/roscontroller.cpp
src/buzz_utility.cpp
src/buzzuav_closures.cpp
src/VoronoiDiagramGenerator.cpp
src/buzz_update.cpp)
target_link_libraries(rosbuzz_node ${catkin_LIBRARIES} buzz buzzdbg pthread)
add_dependencies(rosbuzz_node rosbuzz_generate_messages_cpp)

2
buzz_scripts/include/act/barrier.bzz
View File

@ -87,7 +87,7 @@ function barrier_wait(threshold, transf, resumef, bc) {
function barrier_allgood(barrier, bc) {
barriergood = 1
barrier.foreach(function(key, value, robot){
log("VS entry : ", key, " ", value, " ", robot)
#log("VS entry : ", key, " ", value, " ", robot)
if(key == "d"){
if(value == 1)
return 1

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

@ -2,24 +2,44 @@ 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}}
# 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)
#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)
}
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!")
}
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
}

71
buzz_scripts/include/act/states.bzz
View File

@ -100,7 +100,7 @@ function stop() {
wpreached = 1
function indiWP() {
BVMSTATE = "INDIWP"
BVMSTATE = "WAYPOINT"
check_rc_wp()
wpi = v_wp.get(id)
@ -191,12 +191,12 @@ function pursuit() {
}
# Lennard-Jones interaction magnitude
TARGET = 16.0
TARGET = 8.0
EPSILON = 30.0 #30
GAIN_ATT = 50.0
GAIN_REP = 30.0
function lj_magnitude(dist, target, epsilon) {
lj = -(epsilon / dist) * ((target / dist)^4)# - (target / dist)^2) #repulse only to avoid each other
lj = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) #repulse only to avoid each other
return lj
}
@ -239,6 +239,71 @@ function lennardjones() {
goto_abs(accum_lj.x, accum_lj.y, 0.0, 0.0)
}
# State function that calculates and actuates Voronoi Centroidal tessellation coverage (attractor/repulsors)
counter = 0
function voronoicentroid() {
BVMSTATE="DEPLOY"
if(counter==0 and id!=0) {
pts = getbounds()
it_pts = 0
#table_print(pts)
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})
#table_print(pts)
voronoi(pts)
}
counter=10
}
counter=counter-1
goto_gps(voronoicentroid_done)
}
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.y<minY){
minY = value.y
minYid = key
}
if(value.y>maxY){
maxY = value.y
maxYid = key
}
})
minY2 = maxY
minY2id = maxYid
foreach(RBlimit, function(key, value) {
if(value.y<minY2 and key!=minYid){
minY2 = value.y
minY2id = key
}
})
angle_offset = math.atan(RBlimit[minY2id].y-RBlimit[minYid].y,RBlimit[minY2id].x-RBlimit[minYid].x)
if(angle_offset > 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"

4
buzz_scripts/main.bzz
View File

@ -72,7 +72,7 @@ function step() {
statef=launch_switch
else if(BVMSTATE=="GOHOME") # ends on AUTO_LAUNCH_STATE
statef=goinghome
else if(BVMSTATE=="INDIWP")
else if(BVMSTATE=="WAYPOINT")
statef=indiWP
else if(BVMSTATE=="IDLE")
statef=idle
@ -86,6 +86,8 @@ function step() {
statef=resetGraph
else if(BVMSTATE=="BIDDING") # check the absolute path of the waypointlist csv file in bidding.bzz
statef=bidding
else if(BVMSTATE=="DEPLOY") # check the absolute path of the waypointlist csv file in bidding.bzz
statef=voronoicentroid
else if(BVMSTATE=="GRAPH_FREE")
statef=DoFree
else if(BVMSTATE=="GRAPH_ASKING")

248
include/VoronoiDiagramGenerator.h Normal file
View File

@ -0,0 +1,248 @@
/*
* The author of this software is Steven Fortune. Copyright (c) 1994 by AT&T
* Bell Laboratories.
* Permission to use, copy, modify, and distribute this software for any
* purpose without fee is hereby granted, provided that this entire notice
* is included in all copies of any software which is or includes a copy
* or modification of this software and in all copies of the supporting
* documentation for such software.
* THIS SOFTWARE IS BEING PROVIDED "AS IS", WITHOUT ANY EXPRESS OR IMPLIED
* WARRANTY. IN PARTICULAR, NEITHER THE AUTHORS NOR AT&T MAKE ANY
* REPRESENTATION OR WARRANTY OF ANY KIND CONCERNING THE MERCHANTABILITY
* OF THIS SOFTWARE OR ITS FITNESS FOR ANY PARTICULAR PURPOSE.
*/
/*
* This code was originally written by Stephan Fortune in C code. I, Shane O'Sullivan,
* have since modified it, encapsulating it in a C++ class and, fixing memory leaks and
* adding accessors to the Voronoi Edges.
* Permission to use, copy, modify, and distribute this software for any
* purpose without fee is hereby granted, provided that this entire notice
* is included in all copies of any software which is or includes a copy
* or modification of this software and in all copies of the supporting
* documentation for such software.
* THIS SOFTWARE IS BEING PROVIDED "AS IS", WITHOUT ANY EXPRESS OR IMPLIED
* WARRANTY. IN PARTICULAR, NEITHER THE AUTHORS NOR AT&T MAKE ANY
* REPRESENTATION OR WARRANTY OF ANY KIND CONCERNING THE MERCHANTABILITY
* OF THIS SOFTWARE OR ITS FITNESS FOR ANY PARTICULAR PURPOSE.
*/
#ifndef VORONOI_DIAGRAM_GENERATOR
#define VORONOI_DIAGRAM_GENERATOR
#include <math.h>
#include <stdlib.h>
#include <string.h>
#ifndef NULL
#define NULL 0
#endif
#define DELETED -2
#define le 0
#define re 1
struct Freenode
{
struct Freenode *nextfree;
};
struct FreeNodeArrayList
{
struct Freenode* memory;
struct FreeNodeArrayList* next;
};
struct Freelist
{
struct Freenode *head;
int nodesize;
};
struct Point
{
float x,y;
};
// structure used both for sites and for vertices
struct Site
{
struct Point coord;
int sitenbr;
int refcnt;
};
struct Edge
{
float a,b,c;
struct Site *ep[2];
struct Site *reg[2];
int edgenbr;
};
struct GraphEdge
{
float x1,y1,x2,y2;
struct GraphEdge* next;
};
struct Halfedge
{
struct Halfedge *ELleft, *ELright;
struct Edge *ELedge;
int ELrefcnt;
char ELpm;
struct Site *vertex;
float ystar;
struct Halfedge *PQnext;
};
class VoronoiDiagramGenerator
{
public:
VoronoiDiagramGenerator();
~VoronoiDiagramGenerator();
bool generateVoronoi(float *xValues, float *yValues, int numPoints, float minX, float maxX, float minY, float maxY, float minDist=0);
void resetIterator()
{
iteratorEdges = allEdges;
}
bool getNext(float& x1, float& y1, float& x2, float& y2)
{
if(iteratorEdges == 0)
return false;
x1 = iteratorEdges->x1;
x2 = iteratorEdges->x2;
y1 = iteratorEdges->y1;
y2 = iteratorEdges->y2;
iteratorEdges = iteratorEdges->next;
return true;
}
private:
void cleanup();
void cleanupEdges();
char *getfree(struct Freelist *fl);
struct Halfedge *PQfind();
int PQempty();
struct Halfedge **ELhash;
struct Halfedge *HEcreate(), *ELleft(), *ELright(), *ELleftbnd();
struct Halfedge *HEcreate(struct Edge *e,int pm);
struct Point PQ_min();
struct Halfedge *PQextractmin();
void freeinit(struct Freelist *fl,int size);
void makefree(struct Freenode *curr,struct Freelist *fl);
void geominit();
void plotinit();
bool voronoi(int triangulate);
void ref(struct Site *v);
void deref(struct Site *v);
void endpoint(struct Edge *e,int lr,struct Site * s);
void ELdelete(struct Halfedge *he);
struct Halfedge *ELleftbnd(struct Point *p);
struct Halfedge *ELright(struct Halfedge *he);
void makevertex(struct Site *v);
void out_triple(struct Site *s1, struct Site *s2,struct Site * s3);
void PQinsert(struct Halfedge *he,struct Site * v, float offset);
void PQdelete(struct Halfedge *he);
bool ELinitialize();
void ELinsert(struct Halfedge *lb, struct Halfedge *newHe);
struct Halfedge * ELgethash(int b);
struct Halfedge *ELleft(struct Halfedge *he);
struct Site *leftreg(struct Halfedge *he);
void out_site(struct Site *s);
bool PQinitialize();
int PQbucket(struct Halfedge *he);
void clip_line(struct Edge *e);
char *myalloc(unsigned n);
int right_of(struct Halfedge *el,struct Point *p);
struct Site *rightreg(struct Halfedge *he);
struct Edge *bisect(struct Site *s1,struct Site *s2);
float dist(struct Site *s,struct Site *t);
struct Site *intersect(struct Halfedge *el1, struct Halfedge *el2, struct Point *p=0);
void out_bisector(struct Edge *e);
void out_ep(struct Edge *e);
void out_vertex(struct Site *v);
struct Site *nextone();
void pushGraphEdge(float x1, float y1, float x2, float y2);
void openpl();
void line(float x1, float y1, float x2, float y2);
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;
int triangulate, sorted, plot, debug;
float xmin, xmax, ymin, ymax, deltax, deltay;
struct Site *sites;
int nsites;
int siteidx;
int sqrt_nsites;
int nvertices;
struct Freelist sfl;
struct Site *bottomsite;
int nedges;
struct Freelist efl;
int PQhashsize;
struct Halfedge *PQhash;
int PQcount;
int PQmin;
int ntry, totalsearch;
float pxmin, pxmax, pymin, pymax, cradius;
int total_alloc;
float borderMinX, borderMaxX, borderMinY, borderMaxY;
FreeNodeArrayList* allMemoryList;
FreeNodeArrayList* currentMemoryBlock;
GraphEdge* allEdges;
GraphEdge* iteratorEdges;
float minDistanceBetweenSites;
};
int scomp(const void *p1,const void *p2);
#endif

2
include/buzzuav_closures.h
View File

@ -98,7 +98,7 @@ double* getgoto();
* returns the current grid
*/
std::map<int, std::map<int,int>> getgrid();
int voronoi_center(buzzvm_t vm);
/*
* returns the gimbal commands

1006
src/VoronoiDiagramGenerator.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

3
src/buzz_utility.cpp
View File

@ -240,6 +240,9 @@ static int buzz_register_hooks()
buzzvm_pushs(VM, buzzvm_string_register(VM, "reset_rc", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_resetrc));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "voronoi", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::voronoi_center));
buzzvm_gstore(VM);
return VM->state;
}

141
src/buzzuav_closures.cpp
View File

@ -8,6 +8,7 @@
#include "buzzuav_closures.h"
#include "math.h"
#include "VoronoiDiagramGenerator.h"
namespace buzzuav_closures
{
@ -132,6 +133,18 @@ void rb_from_gps(double nei[], double out[], double cur[])
out[2] = 0.0;
}
void gps_from_vec(double vec[], double gps[]) {
double Vrange = sqrt(vec[0] * vec[0] + vec[1] * vec[1]);
double Vbearing = constrainAngle(atan2(vec[1], vec[0]));
double latR = cur_pos[0]*M_PI/180.0;
double lonR = cur_pos[1]*M_PI/180.0;
double target_lat = asin(sin(latR) * cos(Vrange/EARTH_RADIUS) + cos(latR) * sin(Vrange/EARTH_RADIUS) * cos(Vbearing));
double target_lon = lonR + atan2(sin(Vbearing) * sin(Vrange/EARTH_RADIUS) * cos(latR), cos(Vrange/EARTH_RADIUS) - sin(latR) * sin(target_lat));
gps[0] = target_lat*180.0/M_PI;
gps[1] = target_lon*180.0/M_PI;
gps[2] = cur_pos[2];
}
void parse_gpslist()
/*
/ parse a csv of GPS targets
@ -235,6 +248,134 @@ int buzz_exportmap(buzzvm_t vm)
return buzzvm_ret0(vm);
}
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);
vdg.resetIterator();
float x1,y1,x2,y2;
map<float, std::array<float, 4>> edges;
while(vdg.getNext(x1,y1,x2,y2))
{
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<float, 4>{x1,y1,x2,y2}));
}
double center_dist[2] = {0,0};
float closest_points[8];
int nit = 1;
float prev;
map<float, std::array<float, 4>>::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<float, std::array<float, 4>>::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("USE Point (%f,%f): %f (%i)\n",it->second[2],it->second[3], it->first, got3);
}
} else
break;
nit++;
}
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);
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