first working implementation of Voronoi deployment with geofence
This commit is contained in:
dave
parent
78638ce8b8
commit
8f7b8fa4e6
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
pts[it_pts]=math.vec2.newp(data.distance,data.azimuth+pts.oa)
|
||||
it_pts = it_pts + 1})
|
||||
if(rid!=0){ #remove GS (?)
|
||||
pts[it_pts]=math.vec2.newp(data.distance,data.azimuth+pts.oa)
|
||||
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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
||||
/*
|
||||
|
|
|
|||
|
|
@ -295,7 +295,7 @@ void VoronoiDiagramGenerator::geominit()
|
|||
}
|
||||
|
||||
|
||||
struct Edge * VoronoiDiagramGenerator::bisect(struct Site *s1,struct Site *s2)
|
||||
struct Edge * VoronoiDiagramGenerator::bisect(struct Site *s1,struct Site *s2)
|
||||
{
|
||||
float dx,dy,adx,ady;
|
||||
struct Edge *newedge;
|
||||
|
|
@ -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
|
||||
{
|
||||
|
|
|
|||
|
|
@ -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];
|
||||
} else {
|
||||
closest_points[6] = it->second[2]; closest_points[7] = it->second[3];
|
||||
got3=2;
|
||||
//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 {
|
||||
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;
|
||||
}
|
||||
}
|
||||
ROS_INFO("USE Point (%f,%f): %f (%i)\n",it->second[2],it->second[3], it->first, got3);
|
||||
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));
|
||||
}
|
||||
} 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);
|
||||
}
|
||||
}
|
||||
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];
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
Loading…
Reference in New Issue