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

beautified and style fixes

This commit is contained in:
dave 2018-12-04 10:34:07 -05:00
parent 5f83aa60f6
commit fbb0f108b1
11 changed files with 2239 additions and 2144 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
include/VoronoiDiagramGenerator.h
View File

@ -36,7 +36,6 @@
#include <stdio.h>
#include <algorithm>
#ifndef NULL
#define NULL 0
#endif
@ -45,8 +44,6 @@
#define le 0
#define re 1
struct Freenode
{
struct Freenode* nextfree;
@ -56,7 +53,6 @@ struct FreeNodeArrayList
{
struct Freenode* memory;
struct FreeNodeArrayList* next;
};
struct Freelist
@ -68,8 +64,12 @@ struct Freelist
struct Point
{
float x, y;
Point(): x( 0.0 ), y( 0.0 ) { }
Point( float x, float y ): x( x ), y( y ) { }
Point() : x(0.0), y(0.0)
{
}
Point(float x, float y) : x(x), y(y)
{
}
};
// structure used both for sites and for vertices
@ -80,8 +80,6 @@ struct Site
int refcnt;
};
struct Edge
{
float a, b, c;
@ -89,7 +87,6 @@ struct Edge
struct Site* reg[2];
int edgenbr;
int sites[2];
};
struct GraphEdge
@ -99,9 +96,6 @@ struct GraphEdge
struct GraphEdge* next;
};
struct Halfedge
{
struct Halfedge *ELleft, *ELright;
@ -113,16 +107,14 @@ struct Halfedge
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);
bool generateVoronoi(float* xValues, float* yValues, int numPoints, float minX, float maxX, float minY, float maxY,
float minDist = 0);
void resetIterator()
{
@ -145,7 +137,6 @@ public:
return true;
}
private:
void cleanup();
void cleanupEdges();
@ -153,13 +144,10 @@ private:
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);
@ -243,12 +231,8 @@ private:
GraphEdge* iteratorEdges;
float minDistanceBetweenSites;
};
int scomp(const void* p1, const void* p2);
#endif

9
include/buzz_utility.h
View File

@ -53,8 +53,12 @@ struct neitime
double relative_rate;
int age;
neitime(uint64_t nht, uint64_t nlt, uint64_t mht, uint64_t mlt, double nr, double mr)
: nei_hardware_time(nht), nei_logical_time(nlt), node_hardware_time(mht), node_logical_time(mlt),
nei_rate(nr), relative_rate(mr) {};
: nei_hardware_time(nht)
, nei_logical_time(nlt)
, node_hardware_time(mht)
, node_logical_time(mlt)
, nei_rate(nr)
, relative_rate(mr){};
neitime()
{
}
@ -101,5 +105,4 @@ int get_inmsg_size();
std::vector<uint8_t*> get_inmsg_vector();
std::string get_bvmstate();
}

4
include/roscontroller.h
View File

@ -107,8 +107,8 @@ private:
uint16_t size;
uint64_t sent_time;
uint64_t received_time;
MsgData(int mi, uint16_t ni, uint16_t s, uint64_t st, uint64_t rt):
msgid(mi), nid(ni), size(s),sent_time(st), received_time(rt){};
MsgData(int mi, uint16_t ni, uint16_t s, uint64_t st, uint64_t rt)
: msgid(mi), nid(ni), size(s), sent_time(st), received_time(rt){};
MsgData(){};
};
typedef struct MsgData msg_data;

95
readme.md
View File

@ -1,62 +1,43 @@
ROS Implemenation of Buzz
ROSBuzz
=========================
What is Buzz?
=============
Buzz is a novel programming language for heterogeneous robots swarms.
Buzz advocates a compositional approach, by offering primitives to define swarm behaviors both in a bottom-up and in a top-down fashion.
Bottom-up primitives include robot-wise commands and manipulation of neighborhood data through mapping/reducing/filtering operations.
Top-down primitives allow for the dynamic management of robot teams, and for sharing information globally across the swarm.
Self-organization results from the fact that the Buzz run-time platform is purely distributed.
The language can be extended to add new primitives (thus supporting heterogeneous robot swarms) and can be laid on top of other frameworks, such as ROS.
More information is available at http://the.swarming.buzz/wiki/doku.php?id=start.
Description:
============
Rosbuzz package is the ROS version of Buzz. The package contains a node called “rosbuzz_node”, which implements buzz virtual machine (BVM) as a node in ROS.
ROSBuzz is a ROS node encompassing Buzz Virtual Machine (BVM) and interfacing with ROS ecosystem for mobile robots. The only node of the package is `rosbuzz_node`. It can be used in simulation-in-the-loop using Gazebo and was tested over many platforms (Clearpath Husky, DJI M100, Intel Aero, 3DR Solos, Pleidis Spiris, etc.). More information about ROSBuzz peripheral nodes is available in [1].
What is Buzz?
=============
Downloading ROS Package
=======================
$ git clone https://github.com/MISTLab/ROSBuzz.git rosbuzz
Buzz is a novel programming language for heterogeneous robots swarms. Buzz advocates a compositional approach, by offering primitives to define swarm behaviors both in a bottom-up and in a top-down fashion. Its official documentation and code are available [Buzz](https://github.com/MISTLab/Buzz).
Requirements
============
* Buzz:
You can download the development sources through git:
Follow the required steps in [Buzz](https://github.com/MISTLab/ROSBuzz).
$ git clone https://github.com/MISTLab/Buzz.git buzz
* ROS **base** binary distribution (Indigo or higher):
* ROS binary distribution (Indigo or higher) with catkin (could be used with older versions of ROS with catkin but not tested)
Follow the required steps in [ROS Kinetic](https://wiki.ros.org/kinetic/Installation/Ubuntu). Note that the guidance and camera node of DJI for the M100 require to use the Indigo distribution.
* ROS additionnal dependencies:
You need the following package:
* mavros_msgs :
You can install using apt-get:
```
$ sudo apt-get install ros-<distro>-mavros ros-<distro>-mavros-extras
```
Compilation
===========
To compile the ros package, execute the following:
$ cd catkin_ws
$ catkin_make
$ source devel/setup.bash
```
mkdir -p ROS_WS/src
cd ROS_WS/src
git clone https://github.com/MISTLab/ROSBuzz rosbuzz
cd ..
catkin_make
```
Run
===
@ -64,42 +45,50 @@ To run the ROSBuzz package using the launch file, execute the following:
$ roslaunch rosbuzz rosbuzz.launch
Note : Before launching the ROSBuzz node, verify all the parameters in the launch file. A launch file using gdb is available also (rosbuzzd.launch).
Have a look at the launch file to understand what parameters are available to suit your usage. All topics and services names are listed in `launch_config/topics.yaml`. Note : Before launching the ROSBuzz node, verify all the parameters in the launch file. A launch file using gdb is available too (rosbuzzd.launch).
* Buzz scripts: Several behavioral scripts are included in the "buzz_Scripts" folder, such as "graphformGPS.bzz" uses in the ICRA publication below and the "testaloneWP.bzz" to control a single drone with a ".csv" list of waypoints. The script "empty.bzz" is a template script.
* Buzz scripts: Several behavioral scripts are included in the "buzz_Scripts" folder, such as "graphformGPS.bzz" uses in [1] and the "testaloneWP.bzz" to control a single drone with a ".csv" list of waypoints. The script "empty.bzz" is a template script.
Publisher
=========
Publishers
-----------
* Messages from Buzz (BVM):
The package publishes mavros_msgs/Mavlink message "outMavlink".
The node publishes `mavros_msgs/Mavlink` message "outMavlink".
* Command to the flight controller:
The package publishes geometry_msgs/PoseStamped message "setpoint_position/local".
The node publishes `geometry_msgs/PoseStamped message` "setpoint_position/local".
* Other information from the swarw:
The node publishes:
- "bvmstate" (`std_msgs/String`)
- "neighbours_pos" (`rosbuzz_msgs/neigh_pos`)
- "fleet_status" (`mavros_msgs/Mavlink`)
Subscribers
-----------
* Current position of the Robot:
The package subscribes to sensor_msgs/NavSatFix message "global_position/global", to a std_msgs/Float64 message "global_position/rel_alt" and to a geometry_msgs/PoseStamped message "local_position/pose".
* Information from the Robot controller (mavros compliant):
The node subscribes to:
- "global_position/global" (`sensor_msgs/NavSatFix message`)
- "global_position/rel_alt" (`std_msgs/Float64`)
- "local_position/pose" (`geometry_msgs/PoseStamped`)
- "battery" (`sensor_msgs/BatteryState`)
- either "extended_state" (`mavros_msgs/ExtendedState`) or "state" (`mavros_msgs/State`)
* Messages to Buzz (BVM):
The package subscribes to mavros_msgs/Mavlink message with a topic name of "inMavlink".
The node subscribes to `mavros_msgs/Mavlink` incoming message with name "inMavlink".
* Status:
The package subscribes to mavros_msgs/BatteryStatus message "battery" and to either a mavros_msgs/ExtendedState message "extended_state" or a mavros_msgs/State message "state".
Service
Services
-------
* Remote Controller:
The package offers a mavros_msgs/CommandLong service "buzzcmd" to control its state. In the "misc" folder a bash script shows how to control the Buzz states from the command line.
The package offers a service "buzzcmd" (`mavros_msgs/CommandLong`) to control it. In the "misc" folder, a bash script shows how to control the swarm state from the command line.
References
------
* ROS and Buzz : consensus-based behaviors for heterogeneous teams. St-Onge, D., Shankar Varadharajan, V., Li, G., Svogor, I. and Beltrame, G. arXiv : https://arxiv.org/abs/1710.08843
* [1] ROS and Buzz : consensus-based behaviors for heterogeneous teams. St-Onge, D., Shankar Varadharajan, V., Li, G., Svogor, I. and Beltrame, G. arXiv : https://arxiv.org/abs/1710.08843
* Over-The-Air Updates for Robotic Swarms. Submitted to IEEE Software (August 2017). 8pgs. Shankar Varadharajan, V., St-Onge, D., Guß, C. and Beltrame, G.
* [2] Over-The-Air Updates for Robotic Swarms. Submitted to IEEE Software (August 2017). 8pgs. Shankar Varadharajan, V., St-Onge, D., Guß, C. and Beltrame, G.
Visual Studio Code
--------------------

229
src/VoronoiDiagramGenerator.cpp
View File

@ -52,9 +52,8 @@ VoronoiDiagramGenerator::~VoronoiDiagramGenerator()
delete allMemoryList;
}
bool VoronoiDiagramGenerator::generateVoronoi(float *xValues, float *yValues, int numPoints, float minX, float maxX, float minY, float maxY, float minDist)
bool VoronoiDiagramGenerator::generateVoronoi(float* xValues, float* yValues, int numPoints, float minX, float maxX,
float minY, float maxY, float minDist)
{
cleanup();
cleanupEdges();
@ -137,7 +136,8 @@ bool VoronoiDiagramGenerator::ELinitialize()
if (ELhash == 0)
return false;
for(i=0; i<ELhashsize; i +=1) ELhash[i] = (struct Halfedge *)NULL;
for (i = 0; i < ELhashsize; i += 1)
ELhash[i] = (struct Halfedge*)NULL;
ELleftend = HEcreate((struct Edge*)NULL, 0);
ELrightend = HEcreate((struct Edge*)NULL, 0);
ELleftend->ELleft = (struct Halfedge*)NULL;
@ -150,7 +150,6 @@ bool VoronoiDiagramGenerator::ELinitialize()
return true;
}
struct Halfedge* VoronoiDiagramGenerator::HEcreate(struct Edge* e, int pm)
{
struct Halfedge* answer;
@ -163,7 +162,6 @@ struct Halfedge* VoronoiDiagramGenerator::HEcreate(struct Edge *e,int pm)
return (answer);
}
void VoronoiDiagramGenerator::ELinsert(struct Halfedge* lb, struct Halfedge* newHe)
{
newHe->ELleft = lb;
@ -196,13 +194,17 @@ struct Halfedge * VoronoiDiagramGenerator::ELleftbnd(struct Point *p)
struct Halfedge* he;
/* Use hash table to get close to desired halfedge */
bucket = (int)((p->x - xmin)/deltax * ELhashsize); //use the hash function to find the place in the hash map that this HalfEdge should be
bucket = (int)((p->x - xmin) / deltax *
ELhashsize); // use the hash function to find the place in the hash map that this HalfEdge should be
if(bucket<0) bucket =0; //make sure that the bucket position in within the range of the hash array
if(bucket>=ELhashsize) bucket = ELhashsize - 1;
if (bucket < 0)
bucket = 0; // make sure that the bucket position in within the range of the hash array
if (bucket >= ELhashsize)
bucket = ELhashsize - 1;
he = ELgethash(bucket);
if(he == (struct Halfedge *) NULL) //if the HE isn't found, search backwards and forwards in the hash map for the first non-null entry
if (he == (struct Halfedge*)NULL) // if the HE isn't found, search backwards and forwards in the hash map for the
// first non-null entry
{
for (i = 1; 1; i += 1)
{
@ -220,7 +222,8 @@ struct Halfedge * VoronoiDiagramGenerator::ELleftbnd(struct Point *p)
do
{
he = he->ELright;
} while (he!=ELrightend && right_of(he,p)); //keep going right on the list until either the end is reached, or you find the 1st edge which the point
} while (he != ELrightend && right_of(he, p)); // keep going right on the list until either the end is reached, or
// you find the 1st edge which the point
he = he->ELleft; // isn't to the right of
}
else // if the point is to the left of the HalfEdge, then search left for the HE just to the left of the point
@ -242,7 +245,6 @@ struct Halfedge * VoronoiDiagramGenerator::ELleftbnd(struct Point *p)
return (he);
}
/* This delete routine can't reclaim node, since pointers from hash
table may be present. */
void VoronoiDiagramGenerator::ELdelete(struct Halfedge* he)
@ -252,7 +254,6 @@ void VoronoiDiagramGenerator::ELdelete(struct Halfedge *he)
he->ELedge = (struct Edge*)DELETED;
}
struct Halfedge* VoronoiDiagramGenerator::ELright(struct Halfedge* he)
{
return (he->ELright);
@ -263,13 +264,11 @@ struct Halfedge * VoronoiDiagramGenerator::ELleft(struct Halfedge *he)
return (he->ELleft);
}
struct Site* VoronoiDiagramGenerator::leftreg(struct Halfedge* he)
{
if (he->ELedge == (struct Edge*)NULL)
return (bottomsite);
return( he -> ELpm == le ?
he -> ELedge -> reg[le] : he -> ELedge -> reg[re]);
return (he->ELpm == le ? he->ELedge->reg[le] : he->ELedge->reg[re]);
}
struct Site* VoronoiDiagramGenerator::rightreg(struct Halfedge* he)
@ -294,7 +293,6 @@ void VoronoiDiagramGenerator::geominit()
deltax = xmax - xmin;
}
struct Edge* VoronoiDiagramGenerator::bisect(struct Site* s1, struct Site* s2)
{
float dx, dy, adx, ady;
@ -317,24 +315,30 @@ struct Edge * VoronoiDiagramGenerator::bisect(struct Site *s1,struct Site *s2)
if (adx > ady)
{
newedge -> a = 1.0; newedge -> b = dy/dx; newedge -> c /= dx;//set formula of line, with x fixed to 1
newedge->a = 1.0;
newedge->b = dy / dx;
newedge->c /= dx; // set formula of line, with x fixed to 1
}
else
{
newedge -> b = 1.0; newedge -> a = dx/dy; newedge -> c /= dy;//set formula of line, with y fixed to 1
newedge->b = 1.0;
newedge->a = dx / dy;
newedge->c /= dy; // set formula of line, with y fixed to 1
};
newedge->edgenbr = nedges;
newedge->sites[0] = s1->sitenbr;
newedge->sites[1] = s2->sitenbr;
//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);
// 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);
}
//create a new site where the HalfEdges el1 and el2 intersect - note that the Point in the argument list is not used, don't know why it's there
// create a new site where the HalfEdges el1 and el2 intersect - note that the Point in the argument list is not used,
// don't know why it's there
struct Site* VoronoiDiagramGenerator::intersect(struct Halfedge* el1, struct Halfedge* el2, struct Point* p)
{
struct Edge *e1, *e2, *e;
@ -360,8 +364,7 @@ struct Site * VoronoiDiagramGenerator::intersect(struct Halfedge *el1, struct Ha
yint = (e2->c * e1->a - e1->c * e2->a) / d;
if ((e1->reg[1]->coord.y < e2->reg[1]->coord.y) ||
(e1->reg[1]->coord.y == e2->reg[1]->coord.y &&
e1->reg[1]->coord.x < e2->reg[1]->coord.x) )
(e1->reg[1]->coord.y == e2->reg[1]->coord.y && e1->reg[1]->coord.x < e2->reg[1]->coord.x))
{
el = el1;
e = e1;
@ -395,31 +398,40 @@ int VoronoiDiagramGenerator::right_of(struct Halfedge *el,struct Point *p)
e = el->ELedge;
topsite = e->reg[1];
right_of_site = p->x > topsite->coord.x;
if(right_of_site && el -> ELpm == le) return(1);
if(!right_of_site && el -> ELpm == re) return (0);
if (right_of_site && el->ELpm == le)
return (1);
if (!right_of_site && el->ELpm == re)
return (0);
if (e->a == 1.0)
{ dyp = p->y - topsite->coord.y;
{
dyp = p->y - topsite->coord.y;
dxp = p->x - topsite->coord.x;
fast = 0;
if ((!right_of_site & (e->b < 0.0)) | (right_of_site & (e->b >= 0.0)))
{ above = dyp>= e->b*dxp;
{
above = dyp >= e->b * dxp;
fast = above;
}
else
{ above = p->x + p->y*e->b > e-> c;
if(e->b<0.0) above = !above;
if (!above) fast = 1;
{
above = p->x + p->y * e->b > e->c;
if (e->b < 0.0)
above = !above;
if (!above)
fast = 1;
};
if (!fast)
{ dxs = topsite->coord.x - (e->reg[0])->coord.x;
above = e->b * (dxp*dxp - dyp*dyp) <
dxs*dyp*(1.0+2.0*dxp/dxs + e->b*e->b);
if(e->b<0.0) above = !above;
{
dxs = topsite->coord.x - (e->reg[0])->coord.x;
above = e->b * (dxp * dxp - dyp * dyp) < dxs * dyp * (1.0 + 2.0 * dxp / dxs + e->b * e->b);
if (e->b < 0.0)
above = !above;
};
}
else /*e->b==1.0 */
{ yl = e->c - e->a*p->x;
{
yl = e->c - e->a * p->x;
t1 = p->y - yl;
t2 = p->x - topsite->coord.x;
t3 = yl - topsite->coord.y;
@ -428,7 +440,6 @@ int VoronoiDiagramGenerator::right_of(struct Halfedge *el,struct Point *p)
return (el->ELpm == le ? above : !above);
}
void VoronoiDiagramGenerator::endpoint(struct Edge* e, int lr, struct Site* s)
{
e->ep[lr] = s;
@ -443,7 +454,6 @@ void VoronoiDiagramGenerator::endpoint(struct Edge *e,int lr,struct Site * s)
makefree((Freenode*)e, &efl);
}
float VoronoiDiagramGenerator::dist(struct Site* s, struct Site* t)
{
float dx, dy;
@ -452,7 +462,6 @@ float VoronoiDiagramGenerator::dist(struct Site *s,struct Site *t)
return (float)(sqrt(dx * dx + dy * dy));
}
void VoronoiDiagramGenerator::makevertex(struct Site* v)
{
v->sitenbr = nvertices;
@ -460,7 +469,6 @@ void VoronoiDiagramGenerator::makevertex(struct Site *v)
out_vertex(v);
}
void VoronoiDiagramGenerator::deref(struct Site* v)
{
v->refcnt -= 1;
@ -483,8 +491,7 @@ void VoronoiDiagramGenerator::PQinsert(struct Halfedge *he,struct Site * v, floa
he->ystar = (float)(v->coord.y + offset);
last = &PQhash[PQbucket(he)];
while ((next = last->PQnext) != (struct Halfedge*)NULL &&
(he -> ystar > next -> ystar ||
(he -> ystar == next -> ystar && v -> coord.x > next->vertex->coord.x)))
(he->ystar > next->ystar || (he->ystar == next->ystar && v->coord.x > next->vertex->coord.x)))
{
last = next;
};
@ -516,25 +523,28 @@ int VoronoiDiagramGenerator::PQbucket(struct Halfedge *he)
int bucket;
bucket = (int)((he->ystar - ymin) / deltay * PQhashsize);
if (bucket<0) bucket = 0;
if (bucket>=PQhashsize) bucket = PQhashsize-1 ;
if (bucket < PQmin) PQmin = bucket;
if (bucket < 0)
bucket = 0;
if (bucket >= PQhashsize)
bucket = PQhashsize - 1;
if (bucket < PQmin)
PQmin = bucket;
return (bucket);
}
int VoronoiDiagramGenerator::PQempty()
{
return (PQcount == 0);
}
struct Point VoronoiDiagramGenerator::PQ_min()
{
struct Point answer;
while(PQhash[PQmin].PQnext == (struct Halfedge *)NULL) {PQmin += 1;};
while (PQhash[PQmin].PQnext == (struct Halfedge*)NULL)
{
PQmin += 1;
};
answer.x = PQhash[PQmin].PQnext->vertex->coord.x;
answer.y = PQhash[PQmin].PQnext->ystar;
return (answer);
@ -550,7 +560,6 @@ struct Halfedge * VoronoiDiagramGenerator::PQextractmin()
return (curr);
}
bool VoronoiDiagramGenerator::PQinitialize()
{
int i;
@ -563,12 +572,12 @@ bool VoronoiDiagramGenerator::PQinitialize()
if (PQhash == 0)
return false;
for(i=0; i<PQhashsize; i+=1) PQhash[i].PQnext = (struct Halfedge *)NULL;
for (i = 0; i < PQhashsize; i += 1)
PQhash[i].PQnext = (struct Halfedge*)NULL;
return true;
}
void VoronoiDiagramGenerator::freeinit(struct Freelist* fl, int size)
{
fl->head = (struct Freenode*)NULL;
@ -600,8 +609,6 @@ char * VoronoiDiagramGenerator::getfree(struct Freelist *fl)
return ((char*)t);
}
void VoronoiDiagramGenerator::makefree(struct Freenode* curr, struct Freelist* fl)
{
curr->nextfree = fl->head;
@ -654,7 +661,6 @@ void VoronoiDiagramGenerator::cleanupEdges()
}
allEdges = 0;
}
void VoronoiDiagramGenerator::pushGraphEdge(float x1, float y1, float x2, float y2, int s[2])
@ -669,7 +675,6 @@ void VoronoiDiagramGenerator::pushGraphEdge(float x1, float y1, float x2, float
std::copy(s, s + 2, newEdge->sites);
}
char* VoronoiDiagramGenerator::myalloc(unsigned n)
{
char* t = 0;
@ -678,54 +683,44 @@ char * VoronoiDiagramGenerator::myalloc(unsigned n)
return (t);
}
/* for those who don't have Cherry's plot */
/* #include <plot.h> */
void VoronoiDiagramGenerator::openpl(){}
void VoronoiDiagramGenerator::openpl()
{
}
void VoronoiDiagramGenerator::line(float x1, float y1, float x2, float y2, int s[2])
{
pushGraphEdge(x1, y1, x2, y2, s);
}
void VoronoiDiagramGenerator::circle(float x, float y, float radius){}
void VoronoiDiagramGenerator::range(float minX, float minY, float maxX, float maxY){}
void VoronoiDiagramGenerator::circle(float x, float y, float radius)
{
}
void VoronoiDiagramGenerator::range(float minX, float minY, float maxX, float maxY)
{
}
void VoronoiDiagramGenerator::out_bisector(struct Edge* e)
{
}
void VoronoiDiagramGenerator::out_ep(struct Edge* e)
{
}
void VoronoiDiagramGenerator::out_vertex(struct Site* v)
{
}
void VoronoiDiagramGenerator::out_site(struct Site* s)
{
if (!triangulate & plot & !debug)
circle(s->coord.x, s->coord.y, cradius);
}
void VoronoiDiagramGenerator::out_triple(struct Site* s1, struct Site* s2, struct Site* s3)
{
}
void VoronoiDiagramGenerator::plotinit()
{
float dx, dy, d;
@ -745,7 +740,8 @@ void VoronoiDiagramGenerator::plotinit()
void VoronoiDiagramGenerator::clip_line(struct Edge* e)
{
struct Site *s1, *s2;
float x1=0,x2=0,y1=0,y2=0, temp = 0;;
float x1 = 0, x2 = 0, y1 = 0, y2 = 0, temp = 0;
;
x1 = e->reg[0]->coord.x;
x2 = e->reg[1]->coord.x;
@ -805,13 +801,25 @@ void VoronoiDiagramGenerator::clip_line(struct Edge *e)
return;
}
if (x1 > pxmax)
{ x1 = pxmax; y1 = (e -> c - x1)/e -> b;};
{
x1 = pxmax;
y1 = (e->c - x1) / e->b;
};
if (x1 < pxmin)
{ x1 = pxmin; y1 = (e -> c - x1)/e -> b;};
{
x1 = pxmin;
y1 = (e->c - x1) / e->b;
};
if (x2 > pxmax)
{ x2 = pxmax; y2 = (e -> c - x2)/e -> b;};
{
x2 = pxmax;
y2 = (e->c - x2) / e->b;
};
if (x2 < pxmin)
{ x2 = pxmin; y2 = (e -> c - x2)/e -> b;};
{
x2 = pxmin;
y2 = (e->c - x2) / e->b;
};
}
else
{
@ -841,20 +849,31 @@ void VoronoiDiagramGenerator::clip_line(struct Edge *e)
return;
}
if (y1 > pymax)
{ y1 = pymax; x1 = (e -> c - y1)/e -> a;};
{
y1 = pymax;
x1 = (e->c - y1) / e->a;
};
if (y1 < pymin)
{ y1 = pymin; x1 = (e -> c - y1)/e -> a;};
{
y1 = pymin;
x1 = (e->c - y1) / e->a;
};
if (y2 > pymax)
{ y2 = pymax; x2 = (e -> c - y2)/e -> a;};
{
y2 = pymax;
x2 = (e->c - y2) / e->a;
};
if (y2 < pymin)
{ y2 = pymin; x2 = (e -> c - y2)/e -> a;};
{
y2 = pymin;
x2 = (e->c - y2) / e->a;
};
};
// printf("Pushing line (%f,%f,%f,%f)\n",x1,y1,x2,y2);
line(x1, y1, x2, y2, e->sites);
}
/* implicit parameters: nsites, sqrt_nsites, xmin, xmax, ymin, ymax,
deltax, deltay (can all be estimates).
Performance suffers if they are wrong; better to make nsites,
@ -880,15 +899,14 @@ bool VoronoiDiagramGenerator::voronoi(int triangulate)
newsite = nextone();
while (1)
{
if (!PQempty())
newintstar = PQ_min();
// if the lowest site has a smaller y value than the lowest vector intersection, process the site
// otherwise process the vector intersection
if (newsite != (struct Site *)NULL && (PQempty() || newsite -> coord.y < newintstar.y
|| (newsite->coord.y == newintstar.y && newsite->coord.x < newintstar.x)))
if (newsite != (struct Site*)NULL && (PQempty() || newsite->coord.y < newintstar.y ||
(newsite->coord.y == newintstar.y && newsite->coord.x < newintstar.x)))
{ /* new site is smallest - this is a site event*/
out_site(newsite); // output the site
lbnd = ELleftbnd(&(newsite->coord)); // get the first HalfEdge to the LEFT of the new site
@ -899,7 +917,9 @@ bool VoronoiDiagramGenerator::voronoi(int triangulate)
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
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
{
PQdelete(lbnd);
PQinsert(lbnd, p, dist(p, newsite));
@ -926,12 +946,15 @@ bool VoronoiDiagramGenerator::voronoi(int triangulate)
out_triple(bot, top, rightreg(lbnd)); // output the triple of sites, stating that a circle goes through them
v = lbnd->vertex; // get the vertex that caused this event
makevertex(v); //set the vertex number - couldn't do this earlier since we didn't know when it would be processed
makevertex(v); // set the vertex number - couldn't do this earlier since we didn't know when it would be
// processed
endpoint(lbnd->ELedge, lbnd->ELpm, v); // set the endpoint of the left HalfEdge to be this vector
endpoint(rbnd->ELedge, rbnd->ELpm, v); // set the endpoint of the right HalfEdge to be this vector
ELdelete(lbnd); //mark the lowest HE for deletion - can't delete yet because there might be pointers to it in Hash Map
ELdelete(lbnd); // mark the lowest HE for deletion - can't delete yet because there might be pointers to it in
// Hash Map
PQdelete(rbnd); // remove all vertex events to do with the right HE
ELdelete(rbnd); //mark the right HE for deletion - can't delete yet because there might be pointers to it in Hash Map
ELdelete(
rbnd); // mark the right HE for deletion - can't delete yet because there might be pointers to it in Hash Map
pm = le; // set the pm variable to zero
if (bot->coord.y > top->coord.y) // if the site to the left of the event is higher than the Site
@ -943,7 +966,8 @@ bool VoronoiDiagramGenerator::voronoi(int triangulate)
}
e = bisect(bot, top); // create an Edge (or line) that is between the two Sites. This creates
// the formula of the line, and assigns a line number to it
bisector = HEcreate(e, pm); //create a HE from the Edge 'e', and make it point to that edge with its ELedge field
bisector = HEcreate(e, pm); // create a HE from the Edge 'e', and make it point to that edge with its ELedge
// field
ELinsert(llbnd, bisector); // insert the new bisector to the right of the left HE
endpoint(e, re - pm, v); // set one endpoint to the new edge to be the vector point 'v'.
// If the site to the left of this bisector is higher than the right
@ -963,12 +987,10 @@ bool VoronoiDiagramGenerator::voronoi(int triangulate)
PQinsert(bisector, p, dist(p, bot));
};
}
else break;
else
break;
};
for (lbnd = ELright(ELleftend); lbnd != ELrightend; lbnd = ELright(lbnd))
{
e = lbnd->ELedge;
@ -979,17 +1001,19 @@ bool VoronoiDiagramGenerator::voronoi(int triangulate)
cleanup();
return true;
}
int scomp(const void* p1, const void* p2)
{
struct Point *s1 = (Point *)p1, *s2 = (Point *)p2;
if(s1 -> y < s2 -> y) return(-1);
if(s1 -> y > s2 -> y) return(1);
if(s1 -> x < s2 -> x) return(-1);
if(s1 -> x > s2 -> x) return(1);
if (s1->y < s2->y)
return (-1);
if (s1->y > s2->y)
return (1);
if (s1->x < s2->x)
return (-1);
if (s1->x > s2->x)
return (1);
return (0);
}
@ -1006,4 +1030,3 @@ struct Site * VoronoiDiagramGenerator::nextone()
else
return ((struct Site*)NULL);
}

51
src/buzz_update.cpp
View File

@ -8,7 +8,8 @@
#include "buzz_update.h"
namespace buzz_update{
namespace buzz_update
{
/*Temp for data collection*/
// static int neigh=-1;
static struct timeval t1, t2;
@ -39,12 +40,14 @@ namespace buzz_update{
buzzvm_pushs(VM, buzzvm_string_register(VM, "updates_active", 1));
buzzvm_gload(VM);
buzzobj_t obj = buzzvm_stack_at(VM, 1);
if(obj->o.type == BUZZTYPE_INT && obj->i.value == 1){
if (obj->o.type == BUZZTYPE_INT && obj->i.value == 1)
{
Robot_id = robot_id;
dbgf_name = dbgfname;
bcfname = bo_filename;
ROS_WARN("UPDATES TURNED ON (modifying .bzz script file will update all robots)");
if(debug) ROS_INFO("Initializing file monitor...");
if (debug)
ROS_INFO("Initializing file monitor...");
fd = inotify_init1(IN_NONBLOCK);
if (fd < 0)
{
@ -124,7 +127,8 @@ namespace buzz_update{
fclose(fp);
return 1;
}
else{
else
{
ROS_WARN("UPDATES TURNED OFF... (Hint: Include update.bzz to turn on updates)");
return 0;
}
@ -180,7 +184,8 @@ namespace buzz_update{
std::stringstream patch_exsisting;
patch_exsisting << "bspatch " << path << name1 << ".bo " << path << name1 << "-patched.bo " << path << name1
<< "tmp_patch.bo";
if(debug) ROS_WARN("Launching bspatch command: %s", patch_exsisting.str().c_str());
if (debug)
ROS_WARN("Launching bspatch command: %s", patch_exsisting.str().c_str());
if (system(patch_exsisting.str().c_str()))
return 0;
else
@ -195,7 +200,8 @@ namespace buzz_update{
/*Read the exsisting file to simulate the patching*/
std::stringstream read_patched;
read_patched << path << name1 << ".bo";
if(debug){
if (debug)
{
ROS_WARN("Simulation patching ...");
ROS_WARN("Read file for patching %s", read_patched.str().c_str());
}
@ -228,7 +234,8 @@ namespace buzz_update{
/*Read the new patched file*/
std::stringstream read_patched;
read_patched << path << name1 << "-patched.bo";
if(debug) {
if (debug)
{
ROS_WARN("Robot patching ...");
ROS_WARN("Read file for patching %s", read_patched.str().c_str());
}
@ -304,7 +311,8 @@ namespace buzz_update{
size += sizeof(uint16_t);
*(uint16_t*)(updater->outmsg_queue->size) = size + CODE_REQUEST_PADDING;
updater_msg_ready = 1;
if(debug) ROS_WARN("[Debug] Requesting update no. %u for rebroadcast, try: %u", update_no, update_try_counter);
if (debug)
ROS_WARN("[Debug] Requesting update no. %u for rebroadcast, try: %u", update_no, update_try_counter);
}
void code_message_inqueue_append(uint8_t* msg, uint16_t size)
@ -325,7 +333,8 @@ namespace buzz_update{
{
int size = 0;
updater_code_t out_code = NULL;
if(debug) {
if (debug)
{
ROS_WARN("[Debug] Updater processing in msg with mode %d", *(int*)(updater->mode));
ROS_WARN("[Debug] Current update no: %u, Received update no: %u", (*(uint16_t*)(updater->update_no)),
(*(uint16_t*)(updater->inmsg_queue->queue + sizeof(uint8_t))));
@ -363,7 +372,8 @@ namespace buzz_update{
if (test_set_code((out_code->bcode), (char*)dbgf_name, (size_t)(*(uint16_t*)out_code->bcode_size)))
{
if(debug) ROS_WARN("Received patch PASSED tests!");
if (debug)
ROS_WARN("Received patch PASSED tests!");
*(uint16_t*)updater->update_no = update_no;
/*Clear exisiting patch if any*/
delete_p(updater->patch);
@ -391,7 +401,8 @@ namespace buzz_update{
size = 0;
if (*(uint8_t*)(updater->inmsg_queue->queue) == RESEND_CODE)
{
if(debug) ROS_WARN("Patch rebroadcast request received.");
if (debug)
ROS_WARN("Patch rebroadcast request received.");
size += sizeof(uint8_t);
if (*(uint16_t*)(updater->inmsg_queue->queue + size) == *(uint16_t*)(updater->update_no))
{
@ -399,7 +410,8 @@ namespace buzz_update{
if (*(uint16_t*)(updater->inmsg_queue->queue + size) > update_try_counter)
{
update_try_counter = *(uint16_t*)(updater->inmsg_queue->queue + size);
if(debug) ROS_WARN("Rebroadcasting patch, try : %u", update_try_counter);
if (debug)
ROS_WARN("Rebroadcasting patch, try : %u", update_try_counter);
code_message_outqueue_append();
}
if (update_try_counter > MAX_UPDATE_TRY)
@ -427,7 +439,8 @@ namespace buzz_update{
/*Launch bsdiff and create a patch*/
genpatch << "bsdiff " << path << name1 << ".bo " << path << name2 << ".bo " << path << "diff.bo";
if(debug) ROS_WARN("Launching bsdiff: %s", genpatch.str().c_str());
if (debug)
ROS_WARN("Launching bsdiff: %s", genpatch.str().c_str());
system(genpatch.str().c_str());
/*Delete old files & rename new files*/
@ -514,12 +527,14 @@ namespace buzz_update{
create_update_patch();
VM = buzz_utility::get_vm();
if(debug) ROS_INFO("Current Update no %d", *(uint16_t*)(updater->update_no));
if (debug)
ROS_INFO("Current Update no %d", *(uint16_t*)(updater->update_no));
buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1));
buzzvm_pushi(VM, *(uint16_t*)(updater->update_no));
buzzvm_gstore(VM);
neigh = -1;
if(debug) ROS_INFO("Broadcasting patch ...");
if (debug)
ROS_INFO("Broadcasting patch ...");
code_message_outqueue_append();
}
delete_p(BO_BUF);
@ -573,13 +588,15 @@ namespace buzz_update{
{
if (buzz_utility::buzz_update_init_test(BO_BUF, dbgfname, bcode_size))
{
if(debug) ROS_WARN("Initializtion test passed");
if (debug)
ROS_WARN("Initializtion test passed");
if (buzz_utility::update_step_test())
{
/*data logging*/
old_byte_code_size = *(size_t*)updater->bcode_size;
/*data logging*/
if(debug) ROS_WARN("Step test passed");
if (debug)
ROS_WARN("Step test passed");
*(int*)(updater->mode) = CODE_STANDBY;
// fprintf(stdout,"updater value = %i\n",updater->mode);
delete_p(updater->bcode);

10
src/buzz_utility.cpp
View File

@ -572,7 +572,8 @@ int get_inmsg_size()
return IN_MSG.size();
}
std::vector<uint8_t*> get_inmsg_vector(){
std::vector<uint8_t*> get_inmsg_vector()
{
return IN_MSG;
}
@ -590,7 +591,8 @@ string get_bvmstate()
---------------------------------------*/
{
std::string uav_state = "Unknown";
if(VM && VM->strings){
if (VM && VM->strings)
{
buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1));
buzzvm_gload(VM);
buzzobj_t obj = buzzvm_stack_at(VM, 1);
@ -603,8 +605,8 @@ string get_bvmstate()
return uav_state;
}
int get_swarmsize() {
int get_swarmsize()
{
return (int)buzzdict_size(VM->swarmmembers) + 1;
}
}

245
src/buzzuav_closures.cpp
View File

@ -45,8 +45,12 @@ struct Point
{
float x;
float y;
Point(): x( 0.0 ), y( 0.0 ) { }
Point( float x, float y ): x( x ), y( y ) { }
Point() : x(0.0), y(0.0)
{
}
Point(float x, float y) : x(x), y(y)
{
}
};
string WPlistname = "";
@ -123,7 +127,8 @@ 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);
voronoicsv.open(path + "/log/voronoi_" + std::to_string(buzz_utility::get_robotid()) + ".csv",
std::ios_base::trunc | std::ios_base::out);
logVoronoi = true;
}
@ -152,13 +157,16 @@ void rb_from_gps(double nei[], double out[], double cur[])
out[2] = 0.0;
}
void gps_from_vec(double vec[], double gps[]) {
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));
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];
@ -228,13 +236,16 @@ void check_targets_sim(double lat, double lon, double *res)
double ref[2] = { lat, lon };
double tar[2] = { it->second.latitude, it->second.longitude };
rb_from_gps(tar, rb, ref);
if(rb[0] < visibility_radius && (buzz_utility::get_bvmstate()=="WAYPOINT" && it->second.r==0)){
if (rb[0] < visibility_radius && (buzz_utility::get_bvmstate() == "WAYPOINT" && it->second.r == 0))
{
ROS_WARN("FOUND A TARGET IN WAYPOINT!!! [%i]", it->first);
res[0] = it->first;
res[1] = it->second.latitude;
res[2] = it->second.longitude;
res[3] = it->second.altitude;
} else if(rb[0] < visibility_radius && (buzz_utility::get_bvmstate()=="DEPLOY" && it->second.r==1)){
}
else if (rb[0] < visibility_radius && (buzz_utility::get_bvmstate() == "DEPLOY" && it->second.r == 1))
{
ROS_WARN("FOUND A TARGET IN WAYPOINT!!! [%i]", it->first);
res[0] = it->first;
res[1] = it->second.latitude;
@ -255,12 +266,14 @@ int buzz_exportmap(buzzvm_t vm)
buzzvm_lload(vm, 1);
buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE); // dictionary
buzzobj_t t = buzzvm_stack_at(vm, 1);
for(int32_t i = 1; i <= buzzdict_size(t->t.value); ++i) {
for (int32_t i = 1; i <= buzzdict_size(t->t.value); ++i)
{
buzzvm_dup(vm);
buzzvm_pushi(vm, i);
buzzvm_tget(vm);
std::map<int, int> row;
for(int32_t j = 1; j <= buzzdict_size(buzzvm_stack_at(vm, 1)->t.value); ++j) {
for (int32_t j = 1; j <= buzzdict_size(buzzvm_stack_at(vm, 1)->t.value); ++j)
{
buzzvm_dup(vm);
buzzvm_pushi(vm, j);
buzzvm_tget(vm);
@ -284,8 +297,7 @@ int buzz_exportmap(buzzvm_t vm)
// 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))
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;
}
@ -296,10 +308,10 @@ bool onSegment(Point p, Point q, Point r)
// 2 --> Counterclockwise
int orientation(Point p, Point q, Point r)
{
int val =round((q.y - p.y) * (r.x - q.x)*100 -
(q.x - p.x) * (r.y - q.y)*100);
int val = round((q.y - p.y) * (r.x - q.x) * 100 - (q.x - p.x) * (r.y - q.y) * 100);
if (val == 0) return 0; // colinear
if (val == 0)
return 0; // colinear
return (val > 0) ? 1 : 2; // clock or counterclock wise
}
// The function that returns true if line segment 'p1q1'
@ -321,16 +333,20 @@ bool doIntersect(Point p1, Point q1, Point p2, Point q2)
// Special Cases
// p1, q1 and p2 are colinear and p2 lies on segment p1q1
if (o1 == 0 && onSegment(p1, p2, q1)) return true;
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;
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;
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;
if (o4 == 0 && onSegment(p2, q1, q2))
return true;
return false; // Doesn't fall in any of the above cases
}
@ -345,18 +361,20 @@ bool clockwise_compare_points( const Point& a, const Point& b )
return clockwise_angle_of(a) < clockwise_angle_of(b);
}
void sortclose_polygon(vector <Point> *P){
void sortclose_polygon(vector<Point>* P)
{
std::sort(P->begin(), P->end(), clockwise_compare_points);
P->push_back((*P)[0]);
}
float pol_area(vector <Point> vert) {
float pol_area(vector<Point> vert)
{
float a = 0.0;
// ROS_INFO("Polygone %d edges area.",vert.size());
vector<Point>::iterator it;
vector<Point>::iterator next;
for (it = vert.begin(); it != vert.end()-1; ++it){
for (it = vert.begin(); it != vert.end() - 1; ++it)
{
next = it + 1;
a += it->x * next->y - next->x * it->y;
}
@ -365,12 +383,14 @@ float pol_area(vector <Point> vert) {
return a;
}
double* polygone_center(vector <Point> vert, double *c) {
double* polygone_center(vector<Point> vert, double* c)
{
float A = pol_area(vert);
int i1 = 1;
vector<Point>::iterator it;
vector<Point>::iterator next;
for (it = vert.begin(); it != vert.end()-1; ++it){
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;
@ -381,16 +401,24 @@ double* polygone_center(vector <Point> vert, double *c) {
return c;
}
double numerator ( Point A, Point C, Point E, Point F ) { return (A.y - C.y) * (F.x - E.x) - (A.x - C.x) * (F.y - E.y); }
double denominator( Point A, Point B, Point C, Point D ) { return (B.x - A.x) * (D.y - C.y) - (B.y - A.y) * (D.x - C.x); }
double numerator(Point A, Point C, Point E, Point F)
{
return (A.y - C.y) * (F.x - E.x) - (A.x - C.x) * (F.y - E.y);
}
double denominator(Point A, Point B, Point C, Point D)
{
return (B.x - A.x) * (D.y - C.y) - (B.y - A.y) * (D.x - C.x);
}
void getintersection(Point S, Point D, std::vector <Point> Poly, Point *I) {
void getintersection(Point S, Point D, std::vector<Point> Poly, Point* I)
{
// printf("Points for intersection 1(%f,%f->%f,%f) and 2(%f,%f->%f,%f)\n",q1.x,q1.y,p1.x,p1.y,q2.x,q2.y,p2.x,p2.y);
bool parallel = false;
bool collinear = false;
std::vector<Point>::iterator itc;
std::vector<Point>::iterator next;
for (itc = Poly.begin(); itc != Poly.end()-1; ++itc) {
for (itc = Poly.begin(); itc != Poly.end() - 1; ++itc)
{
next = itc + 1;
if (doIntersect((*itc), (*next), S, D))
{
@ -418,7 +446,8 @@ void getintersection(Point S, Point D, std::vector <Point> Poly, Point *I) {
ROS_WARN("Lines are Collinear (%d) or Parallels (%d)", collinear, parallel);
}
bool isSiteout(Point S, std::vector <Point> Poly) {
bool isSiteout(Point S, std::vector<Point> Poly)
{
bool onedge = false;
// Create a point for line segment from p to infinite
@ -428,7 +457,8 @@ bool isSiteout(Point S, std::vector <Point> Poly) {
int count = 0;
std::vector<Point>::iterator itc;
std::vector<Point>::iterator next;
for (itc = Poly.begin(); itc != Poly.end()-1; ++itc) {
for (itc = Poly.begin(); itc != Poly.end() - 1; ++itc)
{
next = itc + 1;
// Check if the line segment from 'p' to 'extreme' intersects
@ -438,7 +468,8 @@ bool isSiteout(Point S, std::vector <Point> Poly) {
// 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((*itc), S, (*next)) == 0) {
if (orientation((*itc), S, (*next)) == 0)
{
onedge = onSegment((*itc), S, (*next));
if (onedge)
break;
@ -459,12 +490,14 @@ int buzzuav_geofence(buzzvm_t vm)
buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE); // dictionary
buzzobj_t t = buzzvm_stack_at(vm, 1);
if(buzzdict_size(t->t.value) < 5) {
if (buzzdict_size(t->t.value) < 5)
{
ROS_ERROR("Wrong Geofence input size (%i).", buzzdict_size(t->t.value));
return buzzvm_ret0(vm);
}
std::vector<Point> polygon_bound;
for(int32_t i = 0; i < buzzdict_size(t->t.value); ++i) {
for (int32_t i = 0; i < buzzdict_size(t->t.value); ++i)
{
Point tmp;
buzzvm_dup(vm);
buzzvm_pushi(vm, i);
@ -473,10 +506,13 @@ int buzzuav_geofence(buzzvm_t vm)
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "x", 1));
buzzvm_tget(vm);
if(i==0){
if (i == 0)
{
P.x = buzzvm_stack_at(vm, 1)->f.value;
// printf("px=%f\n",P.x);
}else{
}
else
{
tmp.x = buzzvm_stack_at(vm, 1)->f.value;
// printf("c%dx=%f\n",i,tmp.x);
}
@ -485,10 +521,13 @@ int buzzuav_geofence(buzzvm_t vm)
buzzvm_pushs(vm, buzzvm_string_register(vm, "y", 1));
buzzvm_tget(vm);
// ROS_INFO("[%i]---y-->%i",buzz_utility::get_robotid(), tmp);
if(i==0){
if (i == 0)
{
P.y = buzzvm_stack_at(vm, 1)->f.value;
// printf("py=%f\n",P.y);
}else{
}
else
{
tmp.y = buzzvm_stack_at(vm, 1)->f.value;
// printf("c%dy=%f\n",i,tmp.y);
}
@ -502,7 +541,8 @@ int buzzuav_geofence(buzzvm_t vm)
sortclose_polygon(&polygon_bound);
// Check if we are in the zone
if(isSiteout(P, polygon_bound)){
if (isSiteout(P, polygon_bound))
{
Point Intersection;
getintersection(Point(0.0, 0.0), P, polygon_bound, &Intersection);
double gps[3];
@ -515,8 +555,8 @@ int buzzuav_geofence(buzzvm_t vm)
return buzzvm_ret0(vm);
}
int voronoi_center(buzzvm_t vm) {
int voronoi_center(buzzvm_t vm)
{
float dist_max = 300;
buzzvm_lnum_assert(vm, 1);
@ -532,7 +572,8 @@ int voronoi_center(buzzvm_t vm) {
buzzvm_pop(vm);
std::vector<Point> polygon_bound;
for(int32_t i = 0; i < Poly_vert; ++i) {
for (int32_t i = 0; i < Poly_vert; ++i)
{
buzzvm_dup(vm);
buzzvm_pushi(vm, i);
buzzvm_tget(vm);
@ -558,10 +599,31 @@ int voronoi_center(buzzvm_t vm) {
sortclose_polygon(&polygon_bound);
int count = buzzdict_size(t->t.value) - (Poly_vert + 1);
// Check if we are in the zone
if (isSiteout(Point(0, 0), polygon_bound) || count < 3)
{
// ROS_WARN("Not in the Zone!!!");
double goal_tmp[2];
do
{
goal_tmp[0] = polygon_bound[0].x + (rand() % 100) / 100.0 * (polygon_bound[2].x - polygon_bound[0].x);
goal_tmp[1] = polygon_bound[0].y + (rand() % 100) / 100.0 * (polygon_bound[2].y - polygon_bound[0].y);
// ROS_WARN(" in the Zone (%f,%f)!",goal_tmp[0],goal_tmp[1]);
} while (isSiteout(Point(goal_tmp[0], goal_tmp[1]), polygon_bound));
ROS_WARN("Sending at a random location in the Zone (%f,%f)!", goal_tmp[0], goal_tmp[1]);
double gps[3];
gps_from_vec(goal_tmp, gps);
set_gpsgoal(gps);
return buzzvm_ret0(vm);
}
ROS_WARN("NP: %d, Sites: %d", Poly_vert, count);
float* xValues = new float[count];
float* yValues = new float[count];
for(int32_t i = 0; i < count; ++i) {
for (int32_t i = 0; i < count; ++i)
{
int index = i + Poly_vert;
buzzvm_dup(vm);
buzzvm_pushi(vm, index);
@ -583,33 +645,20 @@ int voronoi_center(buzzvm_t vm) {
buzzvm_pop(vm);
}
// Check if we are in the zone
if(isSiteout(Point(0,0), polygon_bound) || count < 3) {
//ROS_WARN("Not in the Zone!!!");
double goal_tmp[2];
do{
goal_tmp[0] = polygon_bound[0].x + (rand()%100)/100.0*(polygon_bound[2].x- polygon_bound[0].x);
goal_tmp[1] = polygon_bound[0].y + (rand()%100)/100.0*(polygon_bound[2].y- polygon_bound[0].y);
//ROS_WARN(" in the Zone (%f,%f)!",goal_tmp[0],goal_tmp[1]);
} while(isSiteout(Point(goal_tmp[0],goal_tmp[1]), polygon_bound));
ROS_WARN("Sending at a random location in the Zone (%f,%f)!",goal_tmp[0],goal_tmp[1]);
double gps[3];
gps_from_vec(goal_tmp, gps);
set_gpsgoal(gps);
return buzzvm_ret0(vm);
}
VoronoiDiagramGenerator vdg;
ROS_WARN("[%i] Voronoi Bounded tessellation starting with %i sites...", buzz_utility::get_robotid(), count);
vdg.generateVoronoi(xValues, yValues, count, -dist_max, dist_max, -dist_max, dist_max, 3.0);
if(logVoronoi) voronoicsv << ros::Time::now().toNSec() << ",";
if (logVoronoi)
voronoicsv << ros::Time::now().toNSec() << ",";
vdg.resetIterator();
// ROS_WARN("[%i] Voronoi Bounded tessellation done!", buzz_utility::get_robotid());
std::vector<Point>::iterator itc, next;
for (itc = polygon_bound.begin(); itc != polygon_bound.end()-1; ++itc) {
for (itc = polygon_bound.begin(); itc != polygon_bound.end() - 1; ++itc)
{
next = itc + 1;
if(logVoronoi) voronoicsv << itc->x << "," << itc->y << "," << next->x << "," << next->y << "," << 0 << "," << 0 << ",";
if (logVoronoi)
voronoicsv << itc->x << "," << itc->y << "," << next->x << "," << next->y << "," << 0 << "," << 0 << ",";
}
float x1, y1, x2, y2;
@ -624,32 +673,44 @@ int voronoi_center(buzzvm_t vm) {
continue;
bool isout1 = isSiteout(Point(x1, y1), polygon_bound);
bool isout2 = isSiteout(Point(x2, y2), polygon_bound);
if(isout1 && isout2){
if (isout1 && isout2)
{
// ROS_INFO("Line out of area!");
continue;
}else if(isout1) {
}
else if (isout1)
{
getintersection(Point(x2, y2), Point(x1, y1), polygon_bound, &Intersection);
x1 = Intersection.x;
y1 = Intersection.y;
// ROS_INFO("Site out 1 -> (%f,%f)", x1, y1);
}else if(isout2) {
}
else if (isout2)
{
getintersection(Point(x1, y1), Point(x2, y2), polygon_bound, &Intersection);
x2 = Intersection.x;
y2 = Intersection.y;
// ROS_INFO("Site out 2 -> (%f,%f)", x2, y2);
}
if(logVoronoi) voronoicsv << 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)) {
if(cell_vert.empty()){
if ((s[0] == 0 || s[1] == 0))
{
if (cell_vert.empty())
{
cell_vert.push_back(Point(x1, y1));
cell_vert.push_back(Point(x2, y2));
} else {
}
else
{
bool alreadyin = false;
vector<Point>::iterator itc;
for (itc = cell_vert.begin(); itc != cell_vert.end(); ++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) {
if (dist < 0.1)
{
alreadyin = true;
break;
}
@ -657,9 +718,11 @@ int voronoi_center(buzzvm_t vm) {
if (!alreadyin)
cell_vert.push_back(Point(x1, y1));
alreadyin = false;
for (itc = cell_vert.begin(); itc != cell_vert.end(); ++itc) {
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) {
if (dist < 0.1)
{
alreadyin = true;
break;
}
@ -669,8 +732,11 @@ int voronoi_center(buzzvm_t vm) {
}
}
}
if(cell_vert.size()<3){
if (cell_vert.size() < 3)
{
ROS_WARN("[%i] Voronoi Bounded tessellation failed (%d)!", buzz_utility::get_robotid(), cell_vert.size());
delete xValues;
delete yValues;
return buzzvm_ret0(vm);
}
std::sort(cell_vert.begin(), cell_vert.end(), clockwise_compare_points);
@ -678,14 +744,18 @@ int voronoi_center(buzzvm_t vm) {
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;
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);
delete xValues;
delete yValues;
return buzzvm_ret0(vm);
}
@ -769,7 +839,8 @@ int buzzuav_addNeiStatus(buzzvm_t vm)
buzzvm_lload(vm, 1); // state table
buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE);
buzzobj_t t = buzzvm_stack_at(vm, 1);
if(buzzdict_size(t->t.value) != 5) {
if (buzzdict_size(t->t.value) != 5)
{
ROS_ERROR("Wrong neighbor status size.");
return buzzvm_ret0(vm);
}
@ -911,12 +982,17 @@ void set_gpsgoal(double goal[3])
{
double rb[3];
rb_from_gps(goal, rb, cur_pos);
if (fabs(rb[0]) < 250.0) {
goto_gpsgoal[0] = goal[0];goto_gpsgoal[1] = goal[1];goto_gpsgoal[2] = goal[2];
ROS_INFO("[%i] Set GPS GOAL TO ---- %f %f %f (%f %f, %f %f)", buzz_utility::get_robotid(), goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]);
} else
ROS_WARN("[%i] GPS GOAL TOO FAR !!-- %f %f %f (%f %f, %f %f)", buzz_utility::get_robotid(), goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]);
if (fabs(rb[0]) < 250.0)
{
goto_gpsgoal[0] = goal[0];
goto_gpsgoal[1] = goal[1];
goto_gpsgoal[2] = goal[2];
ROS_INFO("[%i] Set GPS GOAL TO ---- %f %f %f (%f %f, %f %f)", buzz_utility::get_robotid(), goal[0], goal[1],
goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]);
}
else
ROS_WARN("[%i] GPS GOAL TOO FAR !!-- %f %f %f (%f %f, %f %f)", buzz_utility::get_robotid(), goal[0], goal[1],
goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]);
}
int buzzuav_arm(buzzvm_t vm)
@ -1199,7 +1275,8 @@ void update_neighbors(buzzvm_t vm)
}
// Clear neighbours pos
void clear_neighbours_pos(){
void clear_neighbours_pos()
{
neighbors_map.clear();
}

62
src/roscontroller.cpp
View File

@ -12,8 +12,8 @@ namespace rosbuzz_node
{
const string roscontroller::CAPTURE_SRV_DEFAULT_NAME = "/image_sender/capture_image";
roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv):
logical_clock(ros::Time()), previous_step_time(ros::Time())
roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv)
: logical_clock(ros::Time()), previous_step_time(ros::Time())
/*
/ roscontroller class Constructor
---------------*/
@ -138,7 +138,8 @@ void roscontroller::RosControllerRun()
grid_publisher();
send_MPpayload();
// Check updater state and step code
if(update) buzz_update::update_routine();
if (update)
buzz_update::update_routine();
if (time_step == BUZZRATE)
{
time_step = 0;
@ -163,10 +164,12 @@ void roscontroller::RosControllerRun()
// Call Step from buzz script
buzz_utility::buzz_script_step();
// Force a refresh on neighbors array once in a while
if (timer_step >= 20*BUZZRATE){
if (timer_step >= 20 * BUZZRATE)
{
clear_pos();
timer_step = 0;
} else
}
else
timer_step++;
// Prepare messages and publish them
prepare_msg_and_publish();
@ -178,7 +181,8 @@ void roscontroller::RosControllerRun()
// Set ROBOTS variable (swarm size)
get_number_of_robots();
buzz_utility::set_robot_var(no_of_robots);
if(update) buzz_update::updates_set_robots(no_of_robots);
if (update)
buzz_update::updates_set_robots(no_of_robots);
// get_xbee_status(); // commented out because it may slow down the node too much, to be tested
ros::spinOnce();
@ -203,12 +207,12 @@ void roscontroller::initcsvlog()
/ Create the CSV log file
/-------------------------------------------------------*/
{
std::string path =
bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
path = path.substr(0, bzzfile_name.find_last_of("\\/")) + "/log/";
std::string folder_check = "mkdir -p " + path;
system(folder_check.c_str());
for(int i=5;i>0;i--){
for (int i = 5; i > 0; i--)
{
rename((path + "logger_" + std::to_string((uint8_t)robot_id) + "_" + std::to_string(i - 1) + ".log").c_str(),
(path + "logger_" + std::to_string((uint8_t)robot_id) + "_" + std::to_string(i) + ".log").c_str());
}
@ -226,8 +230,7 @@ void roscontroller::logtocsv()
// hardware time now
log << ros::Time::now().toNSec() << ",";
log << cur_pos.latitude << "," << cur_pos.longitude << ","
<< cur_pos.altitude << ",";
log << cur_pos.latitude << "," << cur_pos.longitude << "," << cur_pos.altitude << ",";
log << (int)no_of_robots << ",";
log << neighbours_pos_map.size() << ",";
log << (int)inmsgdata.size() << "," << message_number << ",";
@ -235,18 +238,16 @@ void roscontroller::logtocsv()
log << out_msg_size << ",";
log << buzz_utility::get_bvmstate();
map<int, buzz_utility::Pos_struct>::iterator it =
neighbours_pos_map.begin();
map<int, buzz_utility::Pos_struct>::iterator it = neighbours_pos_map.begin();
for (; it != neighbours_pos_map.end(); ++it)
{
log << "," << it->first << ",";
log << (double)it->second.x << "," << (double)it->second.y
<< "," << (double)it->second.z;
log << (double)it->second.x << "," << (double)it->second.y << "," << (double)it->second.z;
}
for (std::vector<msg_data>::iterator it = inmsgdata.begin(); it != inmsgdata.end(); ++it)
{
log << "," << (int)it->nid << "," << (int)it->msgid << "," << (int)it->size << "," <<
it->sent_time << ","<< it->received_time;
log << "," << (int)it->nid << "," << (int)it->msgid << "," << (int)it->size << "," << it->sent_time << ","
<< it->received_time;
}
inmsgdata.clear();
log << std::endl;
@ -485,7 +486,6 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c, ros::NodeHandle& n_
capture_srv = n_c.serviceClient<mavros_msgs::CommandBool>(capture_srv_name);
multi_msg = true;
}
@ -587,7 +587,8 @@ void roscontroller::neighbours_pos_publisher()
// Check if any simulated target are in range
double tf[4] = { -1, 0, 0, 0 };
buzzuav_closures::check_targets_sim((it->second).x, (it->second).y, tf);
if(tf[0]!=-1){
if (tf[0] != -1)
{
buzz_utility::Pos_struct pos_tmp;
pos_tmp.x = tf[1];
pos_tmp.y = tf[2];
@ -646,7 +647,8 @@ void roscontroller::grid_publisher()
int g_w = itr->second.size();
int g_h = grid.size();
if(g_w!=0 && g_h!=0) {
if (g_w != 0 && g_h != 0)
{
// DEBUG
// ROS_INFO("------> Publishing a grid of %i x %i", g_h, g_w);
auto current_time = ros::Time::now();
@ -666,19 +668,21 @@ void roscontroller::grid_publisher()
grid_msg.info.origin.orientation.w = 1.0;
grid_msg.data.resize(g_w * g_h);
for (itr=grid.begin(); itr!=grid.end(); ++itr) {
for (itr = grid.begin(); itr != grid.end(); ++itr)
{
std::map<int, int>::iterator itc = itr->second.begin();
for (itc=itr->second.begin(); itc!=itr->second.end(); ++itc) {
for (itc = itr->second.begin(); itc != itr->second.end(); ++itc)
{
grid_msg.data[(itr->first - 1) * g_w + itc->first] = itc->second;
// DEBUG
//ROS_INFO("--------------> index: %i (%i,%i): %i", (itr->first-1)*g_w+itc->first, itr->first, itc->first, grid_msg.data[(itr->first-1)*g_w+itc->first]);
// ROS_INFO("--------------> index: %i (%i,%i): %i", (itr->first-1)*g_w+itc->first, itr->first, itc->first,
// grid_msg.data[(itr->first-1)*g_w+itc->first]);
}
}
grid_pub.publish(grid_msg);
}
}
void roscontroller::Arm()
/*
/ Functions handling the local MAV ROS flight controller
@ -825,7 +829,8 @@ script
cmd_srv.request.command = buzzuav_closures::getcmd();
if (!armstate)
{
if(setmode){
if (setmode)
{
SetMode("LOITER", 0);
ros::Duration(0.5).sleep();
}
@ -950,7 +955,6 @@ void roscontroller::clear_pos()
/Refresh neighbours Position for every ten step
/---------------------------------------------*/
{
neighbours_pos_map.clear();
raw_neighbours_pos_map.clear();
buzzuav_closures::clear_neighbours_pos();
@ -1098,11 +1102,7 @@ void roscontroller::local_pos_callback(const geometry_msgs::PoseStamped::ConstPt
buzzuav_closures::set_currentNEDpos(msg->pose.position.y, msg->pose.position.x);
// cur_pos.z = pose->pose.position.z; // Using relative altitude topic instead
tf::Quaternion q(
msg->pose.orientation.x,
msg->pose.orientation.y,
msg->pose.orientation.z,
msg->pose.orientation.w);
tf::Quaternion q(msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z, msg->pose.orientation.w);
tf::Matrix3x3 m(q);
double roll, pitch, yaw;
m.getRPY(roll, pitch, yaw);