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

first working implementation of Voronoi deployment with geofence

This commit is contained in:
dave 2018-11-10 03:46:27 -05:00
parent 78638ce8b8
commit 8f7b8fa4e6
7 changed files with 210 additions and 103 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -17,7 +17,6 @@ function goto_gps(transf) {
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)

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

@ -245,12 +245,16 @@ function voronoicentroid() {
BVMSTATE="DEPLOY"
if(counter==0 and id!=0) {
pts = getbounds()
it_pts = 0
pts[0] = {.x=0, .y=0} #add itself
it_pts = 1
#table_print(pts)
if(neighbors.count() > 0) {
neighbors.foreach(function(rid, data) {
if(rid!=0){ #remove GS (?)
pts[it_pts]=math.vec2.newp(data.distance,data.azimuth+pts.oa)
it_pts = it_pts + 1})
it_pts = it_pts + 1
}
})
#table_print(pts)
voronoi(pts)
}
@ -271,37 +275,20 @@ function getbounds(){
RBlimit[key] = vec_from_gps(value.lat, value.lng, 0)
})
minY = RBlimit[1].y
minYid = 1
maxY = RBlimit[1].y
maxYid = 1
minX = RBlimit[1].x
maxX = RBlimit[1].x
foreach(RBlimit, function(key, value) {
if(value.y<minY){
if(value.y<minY)
minY = value.y
minYid = key
}
if(value.y>maxY){
if(value.y>maxY)
maxY = value.y
maxYid = key
}
if(value.x>maxX)
maxX = value.x
if(value.x<minX)
minX = value.x
})
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}
return {.miny=minY, .minx=minX, .maxy=maxY, .maxx=maxX, .oa=0.0}
}
# Custom state function for the developer to play with

16
include/VoronoiDiagramGenerator.h
View File

@ -33,6 +33,8 @@
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include <algorithm>
#ifndef NULL
@ -84,12 +86,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 +127,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 +136,7 @@ public:
x2 = iteratorEdges->x2;
y1 = iteratorEdges->y1;
y2 = iteratorEdges->y2;
std::copy(iteratorEdges->sites, iteratorEdges->sites+2, s);
iteratorEdges = iteratorEdges->next;
@ -194,14 +199,19 @@ 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);
bool onSegment(Point p, Point q, Point r);
int orientation(Point p, Point q, Point r);
bool doIntersect(Point p1, Point q1, Point p2, Point q2);
struct Freelist hfl;
struct Halfedge *ELleftend, *ELrightend;
int ELhashsize;

1
include/buzzuav_closures.h
View File

@ -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);
/*

68
src/VoronoiDiagramGenerator.cpp
View File

@ -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 <plot.h> */
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){}
@ -740,6 +743,59 @@ void VoronoiDiagramGenerator::plotinit()
}
// Given three colinear points p, q, r, the function checks if
// point q lies on line segment 'pr'
bool VoronoiDiagramGenerator::onSegment(Point p, Point q, Point r)
{
if (q.x <= std::max(p.x, r.x) && q.x >= std::min(p.x, r.x) &&
q.y <= std::max(p.y, r.y) && q.y >= std::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 VoronoiDiagramGenerator::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 VoronoiDiagramGenerator::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
}
void VoronoiDiagramGenerator::clip_line(struct Edge *e)
{
struct Site *s1, *s2;
@ -749,6 +805,7 @@ void VoronoiDiagramGenerator::clip_line(struct Edge *e)
x2 = e->reg[1]->coord.x;
y1 = e->reg[0]->coord.y;
y2 = e->reg[1]->coord.y;
//printf("Clip line for edges: %d", e->edgenbr);
//if the distance between the two points this line was created from is less than
//the square root of 2, then ignore it
@ -849,7 +906,7 @@ void VoronoiDiagramGenerator::clip_line(struct Edge *e)
};
//printf("\nPushing line (%f,%f,%f,%f)",x1,y1,x2,y2);
line(x1,y1,x2,y2);
line(x1,y1,x2,y2,e->sites);
}
@ -895,6 +952,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
{

175
src/buzzuav_closures.cpp
View File

@ -37,7 +37,22 @@ 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
{
int x;
int y;
};
struct Pointf
{
float x;
float y;
Pointf(): x( 0.0 ), y( 0.0 ) { }
Pointf( float x, float y ): x( x ), y( y ) { }
};
string WPlistname = "";
std::map<int, buzz_utility::RB_struct> targets_map;
@ -55,7 +70,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 +119,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
@ -248,6 +272,45 @@ int buzz_exportmap(buzzvm_t vm)
return buzzvm_ret0(vm);
}
float pol_area(vector <Pointf> vert) {
float a = 0.0;
vector <Pointf>::iterator it;
vector <Pointf>::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 <Pointf> vert, double *c) {
float A = pol_area(vert);
int i1 = 1;
vector <Pointf>::iterator it;
vector <Pointf>::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;
}
float clockwise_angle_of( const Pointf& p )
{
return atan2(p.y,p.x);
}
bool clockwise_compare_points( const Pointf& a, const Pointf& b )
{
return clockwise_angle_of( a ) < clockwise_angle_of( b );
}
int voronoi_center(buzzvm_t vm) {
float *xValues;//[4] = {-22, -17, 4,22};
float *yValues;//[4] = {-9, 31,13,-5};
@ -310,67 +373,58 @@ int voronoi_center(buzzvm_t vm) {
VoronoiDiagramGenerator vdg;
vdg.generateVoronoi(xValues,yValues,count, minx, maxx, miny, maxy,3);
if(logVoronoi) voronoicsv << ros::Time::now().toNSec() << ",";
vdg.resetIterator();
float x1,y1,x2,y2;
map<float, std::array<float, 4>> edges;
while(vdg.getNext(x1,y1,x2,y2))
int s[2];
vector <Pointf> cell_vert;
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<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];
//ROS_INFO("GOT Line (%f,%f)->(%f,%f) between sites %d,%d\n",x1,y1,x2,y2,s[0],s[1]);
if(logVoronoi) voronoicsv << x1 << "," << y1 << "," << x2 << "," << y2 << "," << s[0] << "," << s[1] << ",";
i++;
if((s[0]==0 || s[1]==0) && sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1))>0.1) {
if(cell_vert.empty()){
cell_vert.push_back(Pointf(x1,y1));
cell_vert.push_back(Pointf(x2,y2));
} 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
bool alreadyin = false;
vector <Pointf>::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;
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);
}
if(!alreadyin)
cell_vert.push_back(Pointf(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(Pointf(x2, y2));
}
}
}
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);
@ -380,11 +434,7 @@ int voronoi_center(buzzvm_t 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)
@ -437,6 +487,7 @@ bool doIntersect(Point p1, Point q1, Point p2, Point q2)
return false; // Doesn't fall in any of the above cases
}
int buzzuav_geofence(buzzvm_t vm)
{
bool onedge = false;
@ -462,7 +513,7 @@ int buzzuav_geofence(buzzvm_t 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);
//ROS_INFO("[%i]---x-->%i",buzz_utility::get_robotid(), tmp);
if(i==0)
P.x = tmp;
else
@ -472,7 +523,7 @@ int buzzuav_geofence(buzzvm_t 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);
//ROS_INFO("[%i]---y-->%i",buzz_utility::get_robotid(), tmp);
if(i==0)
P.y = tmp;
else
@ -511,7 +562,7 @@ int buzzuav_geofence(buzzvm_t vm)
i = next;
} while (i != 0);
ROS_INFO("[%i] Geofence: %i, %i",buzz_utility::get_robotid(),count, onedge);
//ROS_INFO("[%i] Geofence: %i, %i",buzz_utility::get_robotid(),count, onedge);
if((count%2 == 0) || onedge) {
goto_gpsgoal[0] = cur_pos[0];

3
src/roscontroller.cpp
View File

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