Merge commit '12ff37965b577d13a3d3288686ddcbef1fb6cb43' into ros_drones_ws
This commit is contained in:
dave
commit
5800cb8b8c
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -2,20 +2,27 @@ 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){
|
||||
|
|
@ -23,3 +30,16 @@ function LimitSpeed(vel_vec, 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
|
||||
}
|
||||
|
|
@ -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"
|
||||
|
|
|
|||
|
|
@ -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")
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
||||
|
|
@ -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
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in New Issue