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

290
include/VoronoiDiagramGenerator.h
View File

@ -12,9 +12,9 @@
* OF THIS SOFTWARE OR ITS FITNESS FOR ANY PARTICULAR PURPOSE. * 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, * 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 * have since modified it, encapsulating it in a C++ class and, fixing memory leaks and
* adding accessors to the Voronoi Edges. * adding accessors to the Voronoi Edges.
* Permission to use, copy, modify, and distribute this software for any * Permission to use, copy, modify, and distribute this software for any
* purpose without fee is hereby granted, provided that this entire notice * purpose without fee is hereby granted, provided that this entire notice
@ -36,7 +36,6 @@
#include <stdio.h> #include <stdio.h>
#include <algorithm> #include <algorithm>
#ifndef NULL #ifndef NULL
#define NULL 0 #define NULL 0
#endif #endif
@ -45,210 +44,195 @@
#define le 0 #define le 0
#define re 1 #define re 1
struct Freenode
struct Freenode
{ {
struct Freenode *nextfree; struct Freenode* nextfree;
}; };
struct FreeNodeArrayList struct FreeNodeArrayList
{ {
struct Freenode* memory; struct Freenode* memory;
struct FreeNodeArrayList* next; struct FreeNodeArrayList* next;
}; };
struct Freelist struct Freelist
{ {
struct Freenode *head; struct Freenode* head;
int nodesize; int nodesize;
}; };
struct Point struct Point
{ {
float x,y; float x, y;
Point(): x( 0.0 ), y( 0.0 ) { } Point() : x(0.0), y(0.0)
Point( float x, float y ): x( x ), y( y ) { } {
}
Point(float x, float y) : x(x), y(y)
{
}
}; };
// structure used both for sites and for vertices // structure used both for sites and for vertices
struct Site struct Site
{ {
struct Point coord; struct Point coord;
int sitenbr; int sitenbr;
int refcnt; int refcnt;
}; };
struct Edge
struct Edge
{ {
float a,b,c; float a, b, c;
struct Site *ep[2]; struct Site* ep[2];
struct Site *reg[2]; struct Site* reg[2];
int edgenbr; int edgenbr;
int sites[2]; int sites[2];
}; };
struct GraphEdge struct GraphEdge
{ {
float x1,y1,x2,y2; float x1, y1, x2, y2;
int sites[2]; int sites[2];
struct GraphEdge* next; struct GraphEdge* next;
}; };
struct Halfedge
struct Halfedge
{ {
struct Halfedge *ELleft, *ELright; struct Halfedge *ELleft, *ELright;
struct Edge *ELedge; struct Edge* ELedge;
int ELrefcnt; int ELrefcnt;
char ELpm; char ELpm;
struct Site *vertex; struct Site* vertex;
float ystar; float ystar;
struct Halfedge *PQnext; struct Halfedge* PQnext;
}; };
class VoronoiDiagramGenerator class VoronoiDiagramGenerator
{ {
public: public:
VoronoiDiagramGenerator(); VoronoiDiagramGenerator();
~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() void resetIterator()
{ {
iteratorEdges = allEdges; iteratorEdges = allEdges;
} }
bool getNext(float& x1, float& y1, float& x2, float& y2, int *s) bool getNext(float& x1, float& y1, float& x2, float& y2, int* s)
{ {
if(iteratorEdges == 0) if (iteratorEdges == 0)
return false; return false;
x1 = iteratorEdges->x1;
x2 = iteratorEdges->x2;
y1 = iteratorEdges->y1;
y2 = iteratorEdges->y2;
std::copy(iteratorEdges->sites, iteratorEdges->sites+2, s);
iteratorEdges = iteratorEdges->next; x1 = iteratorEdges->x1;
x2 = iteratorEdges->x2;
y1 = iteratorEdges->y1;
y2 = iteratorEdges->y2;
std::copy(iteratorEdges->sites, iteratorEdges->sites + 2, s);
return true; iteratorEdges = iteratorEdges->next;
}
return true;
}
private: private:
void cleanup(); void cleanup();
void cleanupEdges(); void cleanupEdges();
char *getfree(struct Freelist *fl); char* getfree(struct Freelist* fl);
struct Halfedge *PQfind(); struct Halfedge* PQfind();
int PQempty(); 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 **ELhash; struct Halfedge* PQextractmin();
struct Halfedge *HEcreate(), *ELleft(), *ELright(), *ELleftbnd(); void freeinit(struct Freelist* fl, int size);
struct Halfedge *HEcreate(struct Edge *e,int pm); 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);
struct Point PQ_min(); void PQinsert(struct Halfedge* he, struct Site* v, float offset);
struct Halfedge *PQextractmin(); void PQdelete(struct Halfedge* he);
void freeinit(struct Freelist *fl,int size); bool ELinitialize();
void makefree(struct Freenode *curr,struct Freelist *fl); void ELinsert(struct Halfedge* lb, struct Halfedge* newHe);
void geominit(); struct Halfedge* ELgethash(int b);
void plotinit(); struct Halfedge* ELleft(struct Halfedge* he);
bool voronoi(int triangulate); struct Site* leftreg(struct Halfedge* he);
void ref(struct Site *v); void out_site(struct Site* s);
void deref(struct Site *v); bool PQinitialize();
void endpoint(struct Edge *e,int lr,struct Site * s); int PQbucket(struct Halfedge* he);
void clip_line(struct Edge* e);
char* myalloc(unsigned n);
int right_of(struct Halfedge* el, struct Point* p);
void ELdelete(struct Halfedge *he); struct Site* rightreg(struct Halfedge* he);
struct Halfedge *ELleftbnd(struct Point *p); struct Edge* bisect(struct Site* s1, struct Site* s2);
struct Halfedge *ELright(struct Halfedge *he); float dist(struct Site* s, struct Site* t);
void makevertex(struct Site *v); struct Site* intersect(struct Halfedge* el1, struct Halfedge* el2, struct Point* p = 0);
void out_triple(struct Site *s1, struct Site *s2,struct Site * s3);
void PQinsert(struct Halfedge *he,struct Site * v, float offset); void out_bisector(struct Edge* e);
void PQdelete(struct Halfedge *he); void out_ep(struct Edge* e);
bool ELinitialize(); void out_vertex(struct Site* v);
void ELinsert(struct Halfedge *lb, struct Halfedge *newHe); struct Site* nextone();
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); void pushGraphEdge(float x1, float y1, float x2, float y2, int s[2]);
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 openpl();
void out_ep(struct Edge *e); void line(float x1, float y1, float x2, float y2, int s[2]);
void out_vertex(struct Site *v); void circle(float x, float y, float radius);
struct Site *nextone(); void range(float minX, float minY, float maxX, float maxY);
void pushGraphEdge(float x1, float y1, float x2, float y2, int s[2]); struct Freelist hfl;
struct Halfedge *ELleftend, *ELrightend;
int ELhashsize;
void openpl(); int triangulate, sorted, plot, debug;
void line(float x1, float y1, float x2, float y2, int s[2]); float xmin, xmax, ymin, ymax, deltax, deltay;
void circle(float x, float y, float radius);
void range(float minX, float minY, float maxX, float maxY);
struct Freelist hfl; struct Site* sites;
struct Halfedge *ELleftend, *ELrightend; int nsites;
int ELhashsize; int siteidx;
int sqrt_nsites;
int nvertices;
struct Freelist sfl;
struct Site* bottomsite;
int triangulate, sorted, plot, debug; int nedges;
float xmin, xmax, ymin, ymax, deltax, deltay; struct Freelist efl;
int PQhashsize;
struct Halfedge* PQhash;
int PQcount;
int PQmin;
struct Site *sites; int ntry, totalsearch;
int nsites; float pxmin, pxmax, pymin, pymax, cradius;
int siteidx; int total_alloc;
int sqrt_nsites;
int nvertices;
struct Freelist sfl;
struct Site *bottomsite;
int nedges; float borderMinX, borderMaxX, borderMinY, borderMaxY;
struct Freelist efl;
int PQhashsize;
struct Halfedge *PQhash;
int PQcount;
int PQmin;
int ntry, totalsearch; FreeNodeArrayList* allMemoryList;
float pxmin, pxmax, pymin, pymax, cradius; FreeNodeArrayList* currentMemoryBlock;
int total_alloc;
float borderMinX, borderMaxX, borderMinY, borderMaxY; GraphEdge* allEdges;
GraphEdge* iteratorEdges;
FreeNodeArrayList* allMemoryList; float minDistanceBetweenSites;
FreeNodeArrayList* currentMemoryBlock;
GraphEdge* allEdges;
GraphEdge* iteratorEdges;
float minDistanceBetweenSites;
}; };
int scomp(const void *p1,const void *p2); int scomp(const void* p1, const void* p2);
#endif #endif

232
include/buzz_update.h
View File

@ -23,149 +23,149 @@
} while (0) } while (0)
namespace buzz_update namespace buzz_update
{ {
static const uint16_t CODE_REQUEST_PADDING = 250; static const uint16_t CODE_REQUEST_PADDING = 250;
static const uint16_t MIN_UPDATE_PACKET = 251; static const uint16_t MIN_UPDATE_PACKET = 251;
static const uint16_t UPDATE_CODE_HEADER_SIZE = 5; static const uint16_t UPDATE_CODE_HEADER_SIZE = 5;
static const uint16_t TIMEOUT_FOR_ROLLBACK = 50; static const uint16_t TIMEOUT_FOR_ROLLBACK = 50;
/*********************/ /*********************/
/* Updater states */ /* Updater states */
/********************/ /********************/
typedef enum { typedef enum {
CODE_RUNNING = 0, // Code executing CODE_RUNNING = 0, // Code executing
CODE_STANDBY, // Standing by for others to update CODE_STANDBY, // Standing by for others to update
} code_states_e; } code_states_e;
/*********************/ /*********************/
/*Message types */ /*Message types */
/********************/ /********************/
typedef enum { typedef enum {
SENT_CODE = 0, // Broadcast code SENT_CODE = 0, // Broadcast code
RESEND_CODE, // ReBroadcast request RESEND_CODE, // ReBroadcast request
} code_message_e; } code_message_e;
/*************************/ /*************************/
/*Updater message queue */ /*Updater message queue */
/*************************/ /*************************/
struct updater_msgqueue_s struct updater_msgqueue_s
{ {
uint8_t* queue; uint8_t* queue;
uint8_t* size; uint8_t* size;
}; };
typedef struct updater_msgqueue_s* updater_msgqueue_t; typedef struct updater_msgqueue_s* updater_msgqueue_t;
struct updater_code_s struct updater_code_s
{ {
uint8_t* bcode; uint8_t* bcode;
uint8_t* bcode_size; uint8_t* bcode_size;
}; };
typedef struct updater_code_s* updater_code_t; typedef struct updater_code_s* updater_code_t;
/**************************/ /**************************/
/*Updater data*/ /*Updater data*/
/**************************/ /**************************/
struct buzz_updater_elem_s struct buzz_updater_elem_s
{ {
/* robot id */ /* robot id */
// uint16_t robotid; // uint16_t robotid;
/*current Bytecode content */ /*current Bytecode content */
uint8_t* bcode; uint8_t* bcode;
/*old Bytecode name */ /*old Bytecode name */
const char* old_bcode; const char* old_bcode;
/*current bcode size*/ /*current bcode size*/
size_t* bcode_size; size_t* bcode_size;
/*Update patch*/ /*Update patch*/
uint8_t* patch; uint8_t* patch;
/* Update patch size*/ /* Update patch size*/
size_t* patch_size; size_t* patch_size;
/*current Bytecode content */ /*current Bytecode content */
uint8_t* standby_bcode; uint8_t* standby_bcode;
/*current bcode size*/ /*current bcode size*/
size_t* standby_bcode_size; size_t* standby_bcode_size;
/*updater out msg queue */ /*updater out msg queue */
updater_msgqueue_t outmsg_queue; updater_msgqueue_t outmsg_queue;
/*updater in msg queue*/ /*updater in msg queue*/
updater_msgqueue_t inmsg_queue; updater_msgqueue_t inmsg_queue;
/*Current state of the updater one in code_states_e ENUM*/ /*Current state of the updater one in code_states_e ENUM*/
int* mode; int* mode;
uint8_t* update_no; uint8_t* update_no;
}; };
typedef struct buzz_updater_elem_s* buzz_updater_elem_t; typedef struct buzz_updater_elem_s* buzz_updater_elem_t;
/**************************************************************************/ /**************************************************************************/
/*Updater routine from msg processing to file checks to be called from main*/ /*Updater routine from msg processing to file checks to be called from main*/
/**************************************************************************/ /**************************************************************************/
void update_routine(); void update_routine();
/************************************************/ /************************************************/
/*Initalizes the updater */ /*Initalizes the updater */
/************************************************/ /************************************************/
int init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id); int init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id);
/*********************************************************/ /*********************************************************/
/*Appends buffer of given size to in msg queue of updater*/ /*Appends buffer of given size to in msg queue of updater*/
/*********************************************************/ /*********************************************************/
void code_message_inqueue_append(uint8_t* msg, uint16_t size); void code_message_inqueue_append(uint8_t* msg, uint16_t size);
/*********************************************************/ /*********************************************************/
/*Processes messages inside the queue of the updater*/ /*Processes messages inside the queue of the updater*/
/*********************************************************/ /*********************************************************/
void code_message_inqueue_process(); void code_message_inqueue_process();
/*****************************************************/ /*****************************************************/
/*Obtains messages from out msgs queue of the updater*/ /*Obtains messages from out msgs queue of the updater*/
/*******************************************************/ /*******************************************************/
uint8_t* getupdater_out_msg(); uint8_t* getupdater_out_msg();
/******************************************************/ /******************************************************/
/*Obtains out msg queue size*/ /*Obtains out msg queue size*/
/*****************************************************/ /*****************************************************/
uint8_t* getupdate_out_msg_size(); uint8_t* getupdate_out_msg_size();
/**************************************************/ /**************************************************/
/*Destroys the out msg queue*/ /*Destroys the out msg queue*/
/*************************************************/ /*************************************************/
void destroy_out_msg_queue(); void destroy_out_msg_queue();
// buzz_updater_elem_t get_updater(); // buzz_updater_elem_t get_updater();
/***************************************************/ /***************************************************/
/*Sets bzz file name*/ /*Sets bzz file name*/
/***************************************************/ /***************************************************/
void set_bzz_file(const char* in_bzz_file, bool dbg); void set_bzz_file(const char* in_bzz_file, bool dbg);
/****************************************************/ /****************************************************/
/*Tests the code from a buffer*/ /*Tests the code from a buffer*/
/***************************************************/ /***************************************************/
int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size); int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size);
/****************************************************/ /****************************************************/
/*Destroys the updater*/ /*Destroys the updater*/
/***************************************************/ /***************************************************/
void destroy_updater(); void destroy_updater();
/****************************************************/ /****************************************************/
/*Checks for updater message*/ /*Checks for updater message*/
/***************************************************/ /***************************************************/
int is_msg_present(); int is_msg_present();
/****************************************************/ /****************************************************/
/*Compiles a bzz script to bo and bdbg*/ /*Compiles a bzz script to bo and bdbg*/
/***************************************************/ /***************************************************/
int compile_bzz(std::string bzz_file); int compile_bzz(std::string bzz_file);
/****************************************************/ /****************************************************/
/*Set number of robots in the updater*/ /*Set number of robots in the updater*/
/***************************************************/ /***************************************************/
void updates_set_robots(int robots); void updates_set_robots(int robots);
} }
#endif #endif

9
include/buzz_utility.h
View File

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

4
include/buzzuav_closures.h
View File

@ -22,7 +22,7 @@ namespace buzzuav_closures
int buzzros_print(buzzvm_t vm); int buzzros_print(buzzvm_t vm);
void setWPlist(std::string file); void setWPlist(std::string file);
void setVorlog(std::string path); void setVorlog(std::string path);
void check_targets_sim(double lat, double lon, double *res); void check_targets_sim(double lat, double lon, double* res);
/* /*
* closure to move following a vector * closure to move following a vector
@ -98,7 +98,7 @@ double* getgoto();
/* /*
* returns the current grid * returns the current grid
*/ */
std::map<int, std::map<int,int>> getgrid(); std::map<int, std::map<int, int>> getgrid();
int voronoi_center(buzzvm_t vm); int voronoi_center(buzzvm_t vm);
/* /*

14
include/roscontroller.h
View File

@ -42,14 +42,14 @@
* ROSBuzz message types * ROSBuzz message types
*/ */
typedef enum { typedef enum {
ROS_BUZZ_MSG_NIL = 0, // dummy msg ROS_BUZZ_MSG_NIL = 0, // dummy msg
UPDATER_MESSAGE, // Update msg UPDATER_MESSAGE, // Update msg
BUZZ_MESSAGE, // Broadcast message BUZZ_MESSAGE, // Broadcast message
BUZZ_MESSAGE_TIME, // Broadcast message with time info BUZZ_MESSAGE_TIME, // Broadcast message with time info
} rosbuzz_msgtype; } rosbuzz_msgtype;
// Time sync algo. constants // Time sync algo. constants
#define COM_DELAY 100000000 // in nano seconds i.e 100 ms #define COM_DELAY 100000000 // in nano seconds i.e 100 ms
#define TIME_SYNC_JUMP_THR 500000000 #define TIME_SYNC_JUMP_THR 500000000
#define MOVING_AVERAGE_ALPHA 0.1 #define MOVING_AVERAGE_ALPHA 0.1
#define MAX_NUMBER_OF_ROBOTS 10 #define MAX_NUMBER_OF_ROBOTS 10
@ -107,8 +107,8 @@ private:
uint16_t size; uint16_t size;
uint64_t sent_time; uint64_t sent_time;
uint64_t received_time; uint64_t received_time;
MsgData(int mi, uint16_t ni, uint16_t s, uint64_t st, uint64_t 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){}; : msgid(mi), nid(ni), size(s), sent_time(st), received_time(rt){};
MsgData(){}; MsgData(){};
}; };
typedef struct MsgData msg_data; typedef struct MsgData msg_data;

99
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: 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 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).
=======================
$ git clone https://github.com/MISTLab/ROSBuzz.git rosbuzz
Requirements Requirements
============ ============
* Buzz : * 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: ```
$ sudo apt-get install ros-<distro>-mavros ros-<distro>-mavros-extras
* mavros_msgs : ```
You can install using apt-get:
$ sudo apt-get install ros-<distro>-mavros ros-<distro>-mavros-extras
Compilation Compilation
=========== ===========
To compile the ros package, execute the following: ```
mkdir -p ROS_WS/src
$ cd catkin_ws cd ROS_WS/src
$ catkin_make git clone https://github.com/MISTLab/ROSBuzz rosbuzz
$ source devel/setup.bash cd ..
catkin_make
```
Run Run
=== ===
@ -64,42 +45,50 @@ To run the ROSBuzz package using the launch file, execute the following:
$ roslaunch rosbuzz rosbuzz.launch $ 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): * 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: * 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 Subscribers
----------- -----------
* Current position of the Robot: * Information from the Robot controller (mavros compliant):
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". 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): * 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: Services
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
------- -------
* Remote Controller: * 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 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 Visual Studio Code
-------------------- --------------------

1551
src/VoronoiDiagramGenerator.cpp
View File

File diff suppressed because it is too large Load Diff

1281
src/buzz_update.cpp
View File

File diff suppressed because it is too large Load Diff

14
src/buzz_utility.cpp
View File

@ -17,7 +17,7 @@ static buzzvm_t VM = 0;
static char* BO_FNAME = 0; static char* BO_FNAME = 0;
static uint8_t* BO_BUF = 0; static uint8_t* BO_BUF = 0;
static buzzdebug_t DBG_INFO = 0; static buzzdebug_t DBG_INFO = 0;
static uint32_t MAX_MSG_SIZE = 190;//250; // Maximum Msg size for sending update packets static uint32_t MAX_MSG_SIZE = 190; // 250; // Maximum Msg size for sending update packets
static uint8_t Robot_id = 0; static uint8_t Robot_id = 0;
static std::vector<uint8_t*> IN_MSG; static std::vector<uint8_t*> IN_MSG;
std::map<int, Pos_struct> users_map; std::map<int, Pos_struct> users_map;
@ -572,7 +572,8 @@ int get_inmsg_size()
return IN_MSG.size(); return IN_MSG.size();
} }
std::vector<uint8_t*> get_inmsg_vector(){ std::vector<uint8_t*> get_inmsg_vector()
{
return IN_MSG; return IN_MSG;
} }
@ -590,11 +591,12 @@ string get_bvmstate()
---------------------------------------*/ ---------------------------------------*/
{ {
std::string uav_state = "Unknown"; std::string uav_state = "Unknown";
if(VM && VM->strings){ if (VM && VM->strings)
{
buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1));
buzzvm_gload(VM); buzzvm_gload(VM);
buzzobj_t obj = buzzvm_stack_at(VM, 1); buzzobj_t obj = buzzvm_stack_at(VM, 1);
if(obj->o.type == BUZZTYPE_STRING) if (obj->o.type == BUZZTYPE_STRING)
uav_state = string(obj->s.value.str); uav_state = string(obj->s.value.str);
else else
uav_state = "Not Available"; uav_state = "Not Available";
@ -603,8 +605,8 @@ string get_bvmstate()
return uav_state; return uav_state;
} }
int get_swarmsize() { int get_swarmsize()
{
return (int)buzzdict_size(VM->swarmmembers) + 1; return (int)buzzdict_size(VM->swarmmembers) + 1;
} }
} }

691
src/buzzuav_closures.cpp
View File

@ -42,11 +42,15 @@ static bool logVoronoi = false;
std::ofstream voronoicsv; std::ofstream voronoicsv;
struct Point struct Point
{ {
float x; float x;
float y; float y;
Point(): x( 0.0 ), y( 0.0 ) { } Point() : x(0.0), y(0.0)
Point( float x, float y ): x( x ), y( y ) { } {
}
Point(float x, float y) : x(x), y(y)
{
}
}; };
string WPlistname = ""; string WPlistname = "";
@ -54,7 +58,7 @@ std::map<int, buzz_utility::RB_struct> targets_map;
std::map<int, buzz_utility::RB_struct> wplist_map; std::map<int, buzz_utility::RB_struct> wplist_map;
std::map<int, buzz_utility::Pos_struct> neighbors_map; std::map<int, buzz_utility::Pos_struct> neighbors_map;
std::map<int, buzz_utility::neighbors_status> neighbors_status_map; std::map<int, buzz_utility::neighbors_status> neighbors_status_map;
std::map<int, std::map<int,int>> grid; std::map<int, std::map<int, int>> grid;
/****************************************/ /****************************************/
/****************************************/ /****************************************/
@ -123,7 +127,8 @@ void setVorlog(string path)
/ set the absolute path for a csv list of waypoints / 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; logVoronoi = true;
} }
@ -152,15 +157,18 @@ void rb_from_gps(double nei[], double out[], double cur[])
out[2] = 0.0; 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 Vrange = sqrt(vec[0] * vec[0] + vec[1] * vec[1]);
double Vbearing = constrainAngle(atan2(vec[1], vec[0])); double Vbearing = constrainAngle(atan2(vec[1], vec[0]));
double latR = cur_pos[0]*M_PI/180.0; double latR = cur_pos[0] * M_PI / 180.0;
double lonR = cur_pos[1]*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_lat =
double target_lon = lonR + atan2(sin(Vbearing) * sin(Vrange/EARTH_RADIUS) * cos(latR), cos(Vrange/EARTH_RADIUS) - sin(latR) * sin(target_lat)); asin(sin(latR) * cos(Vrange / EARTH_RADIUS) + cos(latR) * sin(Vrange / EARTH_RADIUS) * cos(Vbearing));
gps[0] = target_lat*180.0/M_PI; double target_lon = lonR + atan2(sin(Vbearing) * sin(Vrange / EARTH_RADIUS) * cos(latR),
gps[1] = target_lon*180.0/M_PI; 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]; gps[2] = cur_pos[2];
} }
@ -215,7 +223,7 @@ void parse_gpslist()
fin.close(); fin.close();
} }
void check_targets_sim(double lat, double lon, double *res) void check_targets_sim(double lat, double lon, double* res)
/* /*
/ check if a listed target is close / check if a listed target is close
----------------------------------------------------------- */ ----------------------------------------------------------- */
@ -225,16 +233,19 @@ void check_targets_sim(double lat, double lon, double *res)
for (it = wplist_map.begin(); it != wplist_map.end(); ++it) for (it = wplist_map.begin(); it != wplist_map.end(); ++it)
{ {
double rb[3]; double rb[3];
double ref[2]={lat, lon}; double ref[2] = { lat, lon };
double tar[2]={it->second.latitude, it->second.longitude}; double tar[2] = { it->second.latitude, it->second.longitude };
rb_from_gps(tar, rb, ref); 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); ROS_WARN("FOUND A TARGET IN WAYPOINT!!! [%i]", it->first);
res[0] = it->first; res[0] = it->first;
res[1] = it->second.latitude; res[1] = it->second.latitude;
res[2] = it->second.longitude; res[2] = it->second.longitude;
res[3] = it->second.altitude; 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); ROS_WARN("FOUND A TARGET IN WAYPOINT!!! [%i]", it->first);
res[0] = it->first; res[0] = it->first;
res[1] = it->second.latitude; res[1] = it->second.latitude;
@ -253,21 +264,23 @@ int buzz_exportmap(buzzvm_t vm)
buzzvm_lnum_assert(vm, 1); buzzvm_lnum_assert(vm, 1);
// Get the parameter // Get the parameter
buzzvm_lload(vm, 1); buzzvm_lload(vm, 1);
buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE); // dictionary buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE); // dictionary
buzzobj_t t = buzzvm_stack_at(vm, 1); 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_dup(vm);
buzzvm_pushi(vm, i); buzzvm_pushi(vm, i);
buzzvm_tget(vm); buzzvm_tget(vm);
std::map<int, int> row; 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_dup(vm);
buzzvm_pushi(vm, j); buzzvm_pushi(vm, j);
buzzvm_tget(vm); buzzvm_tget(vm);
row.insert(std::pair<int,int>(j, 100.0 - round(buzzvm_stack_at(vm, 1)->f.value*100.0))); row.insert(std::pair<int, int>(j, 100.0 - round(buzzvm_stack_at(vm, 1)->f.value * 100.0)));
buzzvm_pop(vm); buzzvm_pop(vm);
} }
grid.insert(std::pair<int,std::map<int, int>>(i,row)); grid.insert(std::pair<int, std::map<int, int>>(i, row));
buzzvm_pop(vm); buzzvm_pop(vm);
} }
// DEBUG // DEBUG
@ -277,252 +290,279 @@ int buzz_exportmap(buzzvm_t vm)
/* /*
* Geofence(): test for a point in a polygon * 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/ * TAKEN from https://www.geeksforgeeks.org/how-to-check-if-a-given-point-lies-inside-a-polygon/
*/ */
// Given three colinear points p, q, r, the function checks if // Given three colinear points p, q, r, the function checks if
// point q lies on line segment 'pr' // point q lies on line segment 'pr'
bool onSegment(Point p, Point q, Point r) 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))
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 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);
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 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);
//ROS_WARN("(%f,%f)->(%f,%f), 1:%d,2:%d,3:%d,4:%d",p1.x,p1.y,q1.x,q1.y,o1,o2,o3,o4);
// 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
}
float clockwise_angle_of( const Point& p )
{ {
return atan2(p.y,p.x); 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;
} }
// To find orientation of ordered triplet (p, q, r).
bool clockwise_compare_points( const Point& a, const Point& b ) // The function returns following values
// 0 --> p, q and r are colinear
// 1 --> Clockwise
// 2 --> Counterclockwise
int orientation(Point p, Point q, Point r)
{ {
return clockwise_angle_of( a ) < clockwise_angle_of( b ); 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
return (val > 0) ? 1 : 2; // clock or counterclock wise
}
// The function that returns true if line segment 'p1q1'
// and 'p2q2' intersect.
bool 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);
// ROS_WARN("(%f,%f)->(%f,%f), 1:%d,2:%d,3:%d,4:%d",p1.x,p1.y,q1.x,q1.y,o1,o2,o3,o4);
// 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 sortclose_polygon(vector <Point> *P){ float clockwise_angle_of(const Point& p)
std::sort( P->begin(), P->end(), clockwise_compare_points ); {
return atan2(p.y, p.x);
}
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)
{
std::sort(P->begin(), P->end(), clockwise_compare_points);
P->push_back((*P)[0]); P->push_back((*P)[0]);
} }
float pol_area(vector<Point> vert)
float pol_area(vector <Point> vert) { {
float a = 0.0; float a = 0.0;
//ROS_INFO("Polygone %d edges area.",vert.size()); // ROS_INFO("Polygone %d edges area.",vert.size());
vector <Point>::iterator it; vector<Point>::iterator it;
vector <Point>::iterator next; 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; {
next = it + 1;
a += it->x * next->y - next->x * it->y; a += it->x * next->y - next->x * it->y;
} }
a *= 0.5; a *= 0.5;
//ROS_INFO("Polygon area: %f",a); // ROS_INFO("Polygon area: %f",a);
return a; return a;
} }
double* polygone_center(vector <Point> vert, double *c) { double* polygone_center(vector<Point> vert, double* c)
{
float A = pol_area(vert); float A = pol_area(vert);
int i1 = 1; int i1 = 1;
vector <Point>::iterator it; vector<Point>::iterator it;
vector <Point>::iterator next; 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; next = it + 1;
c[0] += (it->x+next->x) * t; float t = it->x * next->y - next->x * it->y;
c[1] += (it->y+next->y) * t; c[0] += (it->x + next->x) * t;
c[1] += (it->y + next->y) * t;
} }
c[0] = c[0] / (6.0 * A); c[0] = c[0] / (6.0 * A);
c[1] = c[1] / (6.0 * A); c[1] = c[1] / (6.0 * A);
return 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 numerator(Point A, Point C, Point E, Point F)
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); } {
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); {
// 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 parallel = false;
bool collinear = false; bool collinear = false;
std::vector <Point>::iterator itc; std::vector<Point>::iterator itc;
std::vector <Point>::iterator next; 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)) next = itc + 1;
if (doIntersect((*itc), (*next), S, D))
{ {
// Uses the determinant of the two lines. For more information, refer to one of the following: // Uses the determinant of the two lines. For more information, refer to one of the following:
// https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection#Given_two_points_on_each_line // https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection#Given_two_points_on_each_line
// http://www.faqs.org/faqs/graphics/algorithms-faq/ (Subject 1.03) // http://www.faqs.org/faqs/graphics/algorithms-faq/ (Subject 1.03)
double d = denominator( S, D, (*itc), (*next) ); double d = denominator(S, D, (*itc), (*next));
if (std::abs( d ) < 0.000000001) if (std::abs(d) < 0.000000001)
{ {
parallel = true; parallel = true;
collinear = abs(numerator( S, D, (*itc), (*next) )) < 0.000000001; collinear = abs(numerator(S, D, (*itc), (*next))) < 0.000000001;
return; return;
} }
double r = numerator( S, (*itc), (*itc), (*next) ) / d;
double s = numerator( S, (*itc), S, D ) / d;
//ROS_INFO("-- (%f,%f)",S.x + r * (D.x - S.x), S.y + r * (D.y - S.y)); double r = numerator(S, (*itc), (*itc), (*next)) / d;
(*I)=Point(S.x + r * (D.x - S.x), S.y + r * (D.y - S.y)); double s = numerator(S, (*itc), S, D) / d;
// ROS_INFO("-- (%f,%f)",S.x + r * (D.x - S.x), S.y + r * (D.y - S.y));
(*I) = Point(S.x + r * (D.x - S.x), S.y + r * (D.y - S.y));
} }
} }
if(parallel || collinear) if (parallel || collinear)
ROS_WARN("Lines are Collinear (%d) or Parallels (%d)",collinear,parallel); 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; bool onedge = false;
// Create a point for line segment from p to infinite // Create a point for line segment from p to infinite
Point extreme = {10000, S.y}; Point extreme = { 10000, S.y };
// Count intersections of the above line with sides of polygon // Count intersections of the above line with sides of polygon
int count = 0; int count = 0;
std::vector <Point>::iterator itc; std::vector<Point>::iterator itc;
std::vector <Point>::iterator next; 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; {
next = itc + 1;
// Check if the line segment from 'p' to 'extreme' intersects // Check if the line segment from 'p' to 'extreme' intersects
// with the line segment from 'polygon[i]' to 'polygon[next]' // with the line segment from 'polygon[i]' to 'polygon[next]'
if (doIntersect((*itc), (*next), S, extreme)) if (doIntersect((*itc), (*next), S, extreme))
{ {
// If the point 'p' is colinear with line segment 'i-next', // If the point 'p' is colinear with line segment 'i-next',
// then check if it lies on segment. If it lies, return true, // then check if it lies on segment. If it lies, return true,
// otherwise false // otherwise false
if (orientation((*itc), S, (*next)) == 0) { if (orientation((*itc), S, (*next)) == 0)
onedge = onSegment((*itc), S, (*next)); {
if(onedge) onedge = onSegment((*itc), S, (*next));
break; if (onedge)
} break;
count++; }
count++;
} }
} }
return ((count%2 == 0) && !onedge); return ((count % 2 == 0) && !onedge);
} }
int buzzuav_geofence(buzzvm_t vm) int buzzuav_geofence(buzzvm_t vm)
{ {
Point P; Point P;
buzzvm_lnum_assert(vm, 1); buzzvm_lnum_assert(vm, 1);
// Get the parameter // Get the parameter
buzzvm_lload(vm, 1); buzzvm_lload(vm, 1);
buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE); // dictionary buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE); // dictionary
buzzobj_t t = buzzvm_stack_at(vm, 1); 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); 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)
{
Point tmp;
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);
if (i == 0)
{
P.x = buzzvm_stack_at(vm, 1)->f.value;
// printf("px=%f\n",P.x);
} }
std::vector <Point> polygon_bound; else
for(int32_t i = 0; i < buzzdict_size(t->t.value); ++i) { {
Point tmp; tmp.x = buzzvm_stack_at(vm, 1)->f.value;
buzzvm_dup(vm); // printf("c%dx=%f\n",i,tmp.x);
buzzvm_pushi(vm, i);
buzzvm_tget(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "x", 1));
buzzvm_tget(vm);
if(i==0){
P.x = buzzvm_stack_at(vm, 1)->f.value;
//printf("px=%f\n",P.x);
}else{
tmp.x = buzzvm_stack_at(vm, 1)->f.value;
//printf("c%dx=%f\n",i,tmp.x);
}
buzzvm_pop(vm);
buzzvm_dup(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){
P.y = buzzvm_stack_at(vm, 1)->f.value;
//printf("py=%f\n",P.y);
}else{
tmp.y = buzzvm_stack_at(vm, 1)->f.value;
//printf("c%dy=%f\n",i,tmp.y);
}
buzzvm_pop(vm);
if(i!=0)
polygon_bound.push_back(tmp);
buzzvm_pop(vm);
} }
sortclose_polygon(&polygon_bound); buzzvm_pop(vm);
buzzvm_dup(vm);
// Check if we are in the zone buzzvm_pushs(vm, buzzvm_string_register(vm, "y", 1));
if(isSiteout(P, polygon_bound)){ buzzvm_tget(vm);
Point Intersection; // ROS_INFO("[%i]---y-->%i",buzz_utility::get_robotid(), tmp);
getintersection(Point(0.0, 0.0) , P, polygon_bound, &Intersection); if (i == 0)
double gps[3]; {
double d[2]={Intersection.x,Intersection.y}; P.y = buzzvm_stack_at(vm, 1)->f.value;
gps_from_vec(d, gps); // printf("py=%f\n",P.y);
set_gpsgoal(gps);
ROS_WARN("Geofencing trigered, not going any further (%f,%f)!",d[0],d[1]);
} }
else
{
tmp.y = buzzvm_stack_at(vm, 1)->f.value;
// printf("c%dy=%f\n",i,tmp.y);
}
buzzvm_pop(vm);
if (i != 0)
polygon_bound.push_back(tmp);
buzzvm_pop(vm);
}
sortclose_polygon(&polygon_bound);
// Check if we are in the zone
if (isSiteout(P, polygon_bound))
{
Point Intersection;
getintersection(Point(0.0, 0.0), P, polygon_bound, &Intersection);
double gps[3];
double d[2] = { Intersection.x, Intersection.y };
gps_from_vec(d, gps);
set_gpsgoal(gps);
ROS_WARN("Geofencing trigered, not going any further (%f,%f)!", d[0], d[1]);
}
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }
int voronoi_center(buzzvm_t vm) { int voronoi_center(buzzvm_t vm)
{
float dist_max = 300; float dist_max = 300;
buzzvm_lnum_assert(vm, 1); buzzvm_lnum_assert(vm, 1);
// Get the parameter // Get the parameter
buzzvm_lload(vm, 1); buzzvm_lload(vm, 1);
buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE); // dictionary buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE); // dictionary
buzzobj_t t = buzzvm_stack_at(vm, 1); buzzobj_t t = buzzvm_stack_at(vm, 1);
buzzvm_dup(vm); buzzvm_dup(vm);
@ -531,161 +571,191 @@ int voronoi_center(buzzvm_t vm) {
int Poly_vert = buzzvm_stack_at(vm, 1)->i.value; int Poly_vert = buzzvm_stack_at(vm, 1)->i.value;
buzzvm_pop(vm); buzzvm_pop(vm);
std::vector <Point> polygon_bound; 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_dup(vm);
buzzvm_pushi(vm, i); buzzvm_pushi(vm, i);
buzzvm_tget(vm); buzzvm_tget(vm);
buzzvm_dup(vm); buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "x", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "x", 1));
buzzvm_tget(vm); buzzvm_tget(vm);
//ROS_INFO("---x-->%f",buzzvm_stack_at(vm, 1)->f.value); // ROS_INFO("---x-->%f",buzzvm_stack_at(vm, 1)->f.value);
float tmpx = buzzvm_stack_at(vm, 1)->f.value; float tmpx = buzzvm_stack_at(vm, 1)->f.value;
buzzvm_pop(vm); buzzvm_pop(vm);
buzzvm_dup(vm); buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "y", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "y", 1));
buzzvm_tget(vm); buzzvm_tget(vm);
//ROS_INFO("---y-->%f",buzzvm_stack_at(vm, 1)->f.value); // ROS_INFO("---y-->%f",buzzvm_stack_at(vm, 1)->f.value);
float tmpy = buzzvm_stack_at(vm, 1)->f.value; float tmpy = buzzvm_stack_at(vm, 1)->f.value;
buzzvm_pop(vm); buzzvm_pop(vm);
polygon_bound.push_back(Point(tmpx, tmpy)); polygon_bound.push_back(Point(tmpx, tmpy));
//ROS_INFO("[%i] Polygon vertex: %f, %f", buzz_utility::get_robotid(),tmpx,tmpy); // ROS_INFO("[%i] Polygon vertex: %f, %f", buzz_utility::get_robotid(),tmpx,tmpy);
buzzvm_pop(vm); buzzvm_pop(vm);
} }
sortclose_polygon(&polygon_bound); sortclose_polygon(&polygon_bound);
int count = buzzdict_size(t->t.value)-(Poly_vert+1);
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) {
int index = i + Poly_vert;
buzzvm_dup(vm);
buzzvm_pushi(vm, index);
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); int count = buzzdict_size(t->t.value) - (Poly_vert + 1);
}
// Check if we are in the zone // Check if we are in the zone
if(isSiteout(Point(0,0), polygon_bound) || count < 3) { if (isSiteout(Point(0, 0), polygon_bound) || count < 3)
//ROS_WARN("Not in the Zone!!!"); {
// ROS_WARN("Not in the Zone!!!");
double goal_tmp[2]; double goal_tmp[2];
do{ 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); goal_tmp[0] = polygon_bound[0].x + (rand() % 100) / 100.0 * (polygon_bound[2].x - polygon_bound[0].x);
//ROS_WARN(" in the Zone (%f,%f)!",goal_tmp[0],goal_tmp[1]); goal_tmp[1] = polygon_bound[0].y + (rand() % 100) / 100.0 * (polygon_bound[2].y - polygon_bound[0].y);
} while(isSiteout(Point(goal_tmp[0],goal_tmp[1]), polygon_bound)); // ROS_WARN(" in the Zone (%f,%f)!",goal_tmp[0],goal_tmp[1]);
ROS_WARN("Sending at a random location 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]; double gps[3];
gps_from_vec(goal_tmp, gps); gps_from_vec(goal_tmp, gps);
set_gpsgoal(gps); set_gpsgoal(gps);
return buzzvm_ret0(vm); 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() << ",";
vdg.resetIterator();
//ROS_WARN("[%i] Voronoi Bounded tessellation done!", buzz_utility::get_robotid());
std::vector <Point>::iterator itc, next; ROS_WARN("NP: %d, Sites: %d", Poly_vert, count);
for (itc = polygon_bound.begin(); itc != polygon_bound.end()-1; ++itc) { float* xValues = new float[count];
next = itc+1; float* yValues = new float[count];
if(logVoronoi) voronoicsv << itc->x << "," << itc->y << "," << next->x << "," << next->y << "," << 0 << "," << 0 << ","; for (int32_t i = 0; i < count; ++i)
{
int index = i + Poly_vert;
buzzvm_dup(vm);
buzzvm_pushi(vm, index);
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);
} }
float x1,y1,x2,y2; 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() << ",";
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)
{
next = itc + 1;
if (logVoronoi)
voronoicsv << itc->x << "," << itc->y << "," << next->x << "," << next->y << "," << 0 << "," << 0 << ",";
}
float x1, y1, x2, y2;
int s[2]; int s[2];
vector <Point> cell_vert; vector<Point> cell_vert;
Point Intersection; Point Intersection;
int i=0; int i = 0;
while(vdg.getNext(x1,y1,x2,y2,s)) while (vdg.getNext(x1, y1, x2, y2, s))
{ {
//ROS_INFO("GOT Line (%f,%f)->(%f,%f) between sites %d,%d",x1,y1,x2,y2,s[0],s[1]); // ROS_INFO("GOT Line (%f,%f)->(%f,%f) between sites %d,%d",x1,y1,x2,y2,s[0],s[1]);
if(sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1))<0.1) if (sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1)) < 0.1)
continue; continue;
bool isout1 = isSiteout(Point(x1,y1), polygon_bound); bool isout1 = isSiteout(Point(x1, y1), polygon_bound);
bool isout2 = isSiteout(Point(x2,y2), polygon_bound); bool isout2 = isSiteout(Point(x2, y2), polygon_bound);
if(isout1 && isout2){ if (isout1 && isout2)
//ROS_INFO("Line out of area!"); {
// ROS_INFO("Line out of area!");
continue; continue;
}else if(isout1) { }
getintersection(Point(x2,y2), Point(x1,y1), polygon_bound, &Intersection); else if (isout1)
{
getintersection(Point(x2, y2), Point(x1, y1), polygon_bound, &Intersection);
x1 = Intersection.x; x1 = Intersection.x;
y1 = Intersection.y; y1 = Intersection.y;
//ROS_INFO("Site out 1 -> (%f,%f)", x1, y1); // ROS_INFO("Site out 1 -> (%f,%f)", x1, y1);
}else if(isout2) { }
getintersection(Point(x1,y1), Point(x2,y2), polygon_bound, &Intersection); else if (isout2)
{
getintersection(Point(x1, y1), Point(x2, y2), polygon_bound, &Intersection);
x2 = Intersection.x; x2 = Intersection.x;
y2 = Intersection.y; y2 = Intersection.y;
//ROS_INFO("Site out 2 -> (%f,%f)", x2, y2); // 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++; i++;
if((s[0]==0 || s[1]==0)) { if ((s[0] == 0 || s[1] == 0))
if(cell_vert.empty()){ {
cell_vert.push_back(Point(x1,y1)); if (cell_vert.empty())
cell_vert.push_back(Point(x2,y2)); {
} else { cell_vert.push_back(Point(x1, y1));
cell_vert.push_back(Point(x2, y2));
}
else
{
bool alreadyin = false; bool alreadyin = false;
vector <Point>::iterator itc; 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)); 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; alreadyin = true;
break; break;
} }
} }
if(!alreadyin) if (!alreadyin)
cell_vert.push_back(Point(x1, y1)); cell_vert.push_back(Point(x1, y1));
alreadyin = false; 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)); 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; alreadyin = true;
break; break;
} }
} }
if(!alreadyin) if (!alreadyin)
cell_vert.push_back(Point(x2, y2)); cell_vert.push_back(Point(x2, y2));
} }
} }
} }
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()); {
ROS_WARN("[%i] Voronoi Bounded tessellation failed (%d)!", buzz_utility::get_robotid(), cell_vert.size());
delete xValues;
delete yValues;
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }
std::sort( cell_vert.begin(), cell_vert.end(), clockwise_compare_points ); std::sort(cell_vert.begin(), cell_vert.end(), clockwise_compare_points);
cell_vert.push_back(cell_vert[0]); cell_vert.push_back(cell_vert[0]);
double center_dist[2] = {0.0, 0.0}; double center_dist[2] = { 0.0, 0.0 };
polygone_center(cell_vert, center_dist); polygone_center(cell_vert, center_dist);
if(logVoronoi) voronoicsv << center_dist[0] << "," << center_dist[1] << std::endl; if (logVoronoi)
center_dist[0]/=2; voronoicsv << center_dist[0] << "," << center_dist[1] << std::endl;
center_dist[1]/=2; center_dist[0] /= 2;
center_dist[1] /= 2;
double gps[3]; double gps[3];
gps_from_vec(center_dist, gps); 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); set_gpsgoal(gps);
delete xValues;
delete yValues;
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }
@ -769,7 +839,8 @@ int buzzuav_addNeiStatus(buzzvm_t vm)
buzzvm_lload(vm, 1); // state table buzzvm_lload(vm, 1); // state table
buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE); buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE);
buzzobj_t t = buzzvm_stack_at(vm, 1); 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."); ROS_ERROR("Wrong neighbor status size.");
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }
@ -800,7 +871,7 @@ int buzzuav_addNeiStatus(buzzvm_t vm)
buzzvm_tget(vm); buzzvm_tget(vm);
newRS.flight_status = buzzvm_stack_at(vm, 1)->i.value; newRS.flight_status = buzzvm_stack_at(vm, 1)->i.value;
buzzvm_pop(vm); buzzvm_pop(vm);
map<int, buzz_utility::neighbors_status>::iterator it = neighbors_status_map.find(id); map<int, buzz_utility::neighbors_status>::iterator it = neighbors_status_map.find(id);
if (it != neighbors_status_map.end()) if (it != neighbors_status_map.end())
neighbors_status_map.erase(it); neighbors_status_map.erase(it);
@ -911,12 +982,17 @@ void set_gpsgoal(double goal[3])
{ {
double rb[3]; double rb[3];
rb_from_gps(goal, rb, cur_pos); rb_from_gps(goal, rb, cur_pos);
if (fabs(rb[0]) < 250.0) { 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]); goto_gpsgoal[0] = goal[0];
} else goto_gpsgoal[1] = goal[1];
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]); 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) int buzzuav_arm(buzzvm_t vm)
@ -987,7 +1063,7 @@ double* getgoto()
return goto_pos; return goto_pos;
} }
std::map<int, std::map<int,int>> getgrid() std::map<int, std::map<int, int>> getgrid()
/* /*
/ return the grid / return the grid
/-------------------------------------------------------------*/ /-------------------------------------------------------------*/
@ -1199,7 +1275,8 @@ void update_neighbors(buzzvm_t vm)
} }
// Clear neighbours pos // Clear neighbours pos
void clear_neighbours_pos(){ void clear_neighbours_pos()
{
neighbors_map.clear(); neighbors_map.clear();
} }

198
src/roscontroller.cpp
View File

@ -12,8 +12,8 @@ namespace rosbuzz_node
{ {
const string roscontroller::CAPTURE_SRV_DEFAULT_NAME = "/image_sender/capture_image"; const string roscontroller::CAPTURE_SRV_DEFAULT_NAME = "/image_sender/capture_image";
roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv): roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv)
logical_clock(ros::Time()), previous_step_time(ros::Time()) : logical_clock(ros::Time()), previous_step_time(ros::Time())
/* /*
/ roscontroller class Constructor / roscontroller class Constructor
---------------*/ ---------------*/
@ -26,7 +26,7 @@ logical_clock(ros::Time()), previous_step_time(ros::Time())
cur_pos.longitude = 0; cur_pos.longitude = 0;
cur_pos.latitude = 0; cur_pos.latitude = 0;
cur_pos.altitude = 0; cur_pos.altitude = 0;
// Obtain parameters from ros parameter server // Obtain parameters from ros parameter server
Rosparameters_get(n_c_priv); Rosparameters_get(n_c_priv);
// Initialize publishers, subscribers and client // Initialize publishers, subscribers and client
@ -35,9 +35,9 @@ logical_clock(ros::Time()), previous_step_time(ros::Time())
std::string fname = Compile_bzz(bzzfile_name); std::string fname = Compile_bzz(bzzfile_name);
bcfname = fname + ".bo"; bcfname = fname + ".bo";
dbgfname = fname + ".bdb"; dbgfname = fname + ".bdb";
buzz_update::set_bzz_file(bzzfile_name.c_str(),debug); buzz_update::set_bzz_file(bzzfile_name.c_str(), debug);
// Initialize variables // Initialize variables
if(setmode) if (setmode)
SetMode("LOITER", 0); SetMode("LOITER", 0);
goto_pos = buzzuav_closures::getgoto(); goto_pos = buzzuav_closures::getgoto();
@ -67,7 +67,7 @@ logical_clock(ros::Time()), previous_step_time(ros::Time())
logical_clock.fromSec(0); logical_clock.fromSec(0);
logical_time_rate = 0; logical_time_rate = 0;
time_sync_jumped = false; time_sync_jumped = false;
out_msg_time=0; out_msg_time = 0;
// Create log dir and open log file // Create log dir and open log file
initcsvlog(); initcsvlog();
buzzuav_closures::setWPlist(WPfile); buzzuav_closures::setWPlist(WPfile);
@ -126,7 +126,7 @@ void roscontroller::RosControllerRun()
// set ROS loop rate // set ROS loop rate
ros::Rate loop_rate(BUZZRATE); ros::Rate loop_rate(BUZZRATE);
// check for BVMSTATE variable // check for BVMSTATE variable
if(buzz_utility::get_bvmstate()=="Not Available") if (buzz_utility::get_bvmstate() == "Not Available")
ROS_ERROR("BVMSTATE undeclared in .bzz file, please set BVMSTATE."); ROS_ERROR("BVMSTATE undeclared in .bzz file, please set BVMSTATE.");
// DEBUG // DEBUG
// ROS_WARN("[%i] -----------------------STARTING MAIN LOOP!", robot_id); // ROS_WARN("[%i] -----------------------STARTING MAIN LOOP!", robot_id);
@ -138,7 +138,8 @@ void roscontroller::RosControllerRun()
grid_publisher(); grid_publisher();
send_MPpayload(); send_MPpayload();
// Check updater state and step code // Check updater state and step code
if(update) buzz_update::update_routine(); if (update)
buzz_update::update_routine();
if (time_step == BUZZRATE) if (time_step == BUZZRATE)
{ {
time_step = 0; time_step = 0;
@ -159,26 +160,29 @@ void roscontroller::RosControllerRun()
// log data // log data
logtocsv(); logtocsv();
// Call Step from buzz script // Call Step from buzz script
buzz_utility::buzz_script_step(); buzz_utility::buzz_script_step();
// Force a refresh on neighbors array once in a while // Force a refresh on neighbors array once in a while
if (timer_step >= 20*BUZZRATE){ if (timer_step >= 20 * BUZZRATE)
{
clear_pos(); clear_pos();
timer_step = 0; timer_step = 0;
} else }
else
timer_step++; timer_step++;
// Prepare messages and publish them // Prepare messages and publish them
prepare_msg_and_publish(); prepare_msg_and_publish();
// Call the flight controler service // Call the flight controler service
flight_controller_service_call(); flight_controller_service_call();
// Broadcast local position to FCU // Broadcast local position to FCU
if(BClpose && !setmode) if (BClpose && !setmode)
SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]); SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]);
// Set ROBOTS variable (swarm size) // Set ROBOTS variable (swarm size)
get_number_of_robots(); get_number_of_robots();
buzz_utility::set_robot_var(no_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 // get_xbee_status(); // commented out because it may slow down the node too much, to be tested
ros::spinOnce(); ros::spinOnce();
@ -203,19 +207,19 @@ void roscontroller::initcsvlog()
/ Create the CSV log file / Create the CSV log file
/-------------------------------------------------------*/ /-------------------------------------------------------*/
{ {
std::string path = std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; path = path.substr(0, bzzfile_name.find_last_of("\\/")) + "/log/";
path = path.substr(0, bzzfile_name.find_last_of("\\/"))+"/log/"; std::string folder_check = "mkdir -p " + path;
std::string folder_check="mkdir -p "+path;
system(folder_check.c_str()); 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()); 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());
} }
path += "logger_"+std::to_string(robot_id)+"_0.log"; path += "logger_" + std::to_string(robot_id) + "_0.log";
log.open(path.c_str(), std::ios_base::trunc | std::ios_base::out); log.open(path.c_str(), std::ios_base::trunc | std::ios_base::out);
// set log data double precision // set log data double precision
log <<std::setprecision(15) << std::fixed; log << std::setprecision(15) << std::fixed;
} }
void roscontroller::logtocsv() void roscontroller::logtocsv()
@ -225,28 +229,25 @@ void roscontroller::logtocsv()
{ {
// hardware time now // hardware time now
log << ros::Time::now().toNSec() << ","; log << ros::Time::now().toNSec() << ",";
log << cur_pos.latitude << "," << cur_pos.longitude << "," log << cur_pos.latitude << "," << cur_pos.longitude << "," << cur_pos.altitude << ",";
<< cur_pos.altitude << ",";
log << (int)no_of_robots << ","; log << (int)no_of_robots << ",";
log << neighbours_pos_map.size() << ","; log << neighbours_pos_map.size() << ",";
log << (int)inmsgdata.size() << "," << message_number << ","; log << (int)inmsgdata.size() << "," << message_number << ",";
log << out_msg_time <<","; log << out_msg_time << ",";
log << out_msg_size<<","; log << out_msg_size << ",";
log << buzz_utility::get_bvmstate(); log << buzz_utility::get_bvmstate();
map<int, buzz_utility::Pos_struct>::iterator it = map<int, buzz_utility::Pos_struct>::iterator it = neighbours_pos_map.begin();
neighbours_pos_map.begin();
for (; it != neighbours_pos_map.end(); ++it) for (; it != neighbours_pos_map.end(); ++it)
{ {
log << "," << it->first << ","; log << "," << it->first << ",";
log << (double)it->second.x << "," << (double)it->second.y log << (double)it->second.x << "," << (double)it->second.y << "," << (double)it->second.z;
<< "," << (double)it->second.z;
} }
for (std::vector<msg_data>::iterator it = inmsgdata.begin() ; it != inmsgdata.end(); ++it) for (std::vector<msg_data>::iterator it = inmsgdata.begin(); it != inmsgdata.end(); ++it)
{ {
log << "," << (int)it->nid << "," << (int)it->msgid << "," << (int)it->size << "," << log << "," << (int)it->nid << "," << (int)it->msgid << "," << (int)it->size << "," << it->sent_time << ","
it->sent_time << ","<< it->received_time; << it->received_time;
} }
inmsgdata.clear(); inmsgdata.clear();
log << std::endl; log << std::endl;
@ -395,7 +396,7 @@ void roscontroller::PubandServ(ros::NodeHandle& n_c, ros::NodeHandle& node_handl
} }
if (node_handle.getParam("services/modeclient", topic)) if (node_handle.getParam("services/modeclient", topic))
{ {
if(setmode) if (setmode)
mode_client = n_c.serviceClient<mavros_msgs::SetMode>(topic); mode_client = n_c.serviceClient<mavros_msgs::SetMode>(topic);
} }
else else
@ -480,11 +481,10 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c, ros::NodeHandle& n_
// Publishers and service Clients // Publishers and service Clients
PubandServ(n_c, n_c_priv); PubandServ(n_c, n_c_priv);
ROS_INFO("Ready to receive Mav Commands from RC client"); ROS_INFO("Ready to receive Mav Commands from RC client");
capture_srv = n_c.serviceClient<mavros_msgs::CommandBool>(capture_srv_name); capture_srv = n_c.serviceClient<mavros_msgs::CommandBool>(capture_srv_name);
multi_msg = true; multi_msg = true;
} }
@ -583,11 +583,12 @@ void roscontroller::neighbours_pos_publisher()
// cout<<"iterator it val: "<< it-> first << " After convertion: " // cout<<"iterator it val: "<< it-> first << " After convertion: "
// <<(uint8_t) buzz_utility::get_rid_uint8compac(it->first)<<endl; // <<(uint8_t) buzz_utility::get_rid_uint8compac(it->first)<<endl;
// std::cout<<"long obt"<<neigh_tmp.longitude<<endl; // std::cout<<"long obt"<<neigh_tmp.longitude<<endl;
// Check if any simulated target are in range // Check if any simulated target are in range
double tf[4] = {-1, 0, 0, 0}; double tf[4] = { -1, 0, 0, 0 };
buzzuav_closures::check_targets_sim((it->second).x, (it->second).y, tf); 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; buzz_utility::Pos_struct pos_tmp;
pos_tmp.x = tf[1]; pos_tmp.x = tf[1];
pos_tmp.y = tf[2]; pos_tmp.y = tf[2];
@ -641,24 +642,25 @@ void roscontroller::grid_publisher()
/ Publish current Grid from Buzz script / Publish current Grid from Buzz script
/----------------------------------------------------*/ /----------------------------------------------------*/
{ {
std::map<int, std::map<int,int>> grid = buzzuav_closures::getgrid(); std::map<int, std::map<int, int>> grid = buzzuav_closures::getgrid();
std::map<int, std::map<int,int>>::iterator itr = grid.begin(); std::map<int, std::map<int, int>>::iterator itr = grid.begin();
int g_w = itr->second.size(); int g_w = itr->second.size();
int g_h = grid.size(); int g_h = grid.size();
if(g_w!=0 && g_h!=0) { if (g_w != 0 && g_h != 0)
{
// DEBUG // DEBUG
//ROS_INFO("------> Publishing a grid of %i x %i", g_h, g_w); // ROS_INFO("------> Publishing a grid of %i x %i", g_h, g_w);
auto current_time = ros::Time::now(); auto current_time = ros::Time::now();
nav_msgs::OccupancyGrid grid_msg; nav_msgs::OccupancyGrid grid_msg;
grid_msg.header.frame_id = "/world"; grid_msg.header.frame_id = "/world";
grid_msg.header.stamp = current_time; grid_msg.header.stamp = current_time;
grid_msg.info.map_load_time = current_time; // Same as header stamp as we do not load the map. grid_msg.info.map_load_time = current_time; // Same as header stamp as we do not load the map.
grid_msg.info.resolution = 0.01;//gridMap.getResolution(); grid_msg.info.resolution = 0.01; // gridMap.getResolution();
grid_msg.info.width = g_w; grid_msg.info.width = g_w;
grid_msg.info.height = g_h; grid_msg.info.height = g_h;
grid_msg.info.origin.position.x = round(g_w/2.0) * grid_msg.info.resolution; grid_msg.info.origin.position.x = round(g_w / 2.0) * grid_msg.info.resolution;
grid_msg.info.origin.position.y = round(g_h/2.0) * grid_msg.info.resolution; grid_msg.info.origin.position.y = round(g_h / 2.0) * grid_msg.info.resolution;
grid_msg.info.origin.position.z = 0.0; grid_msg.info.origin.position.z = 0.0;
grid_msg.info.origin.orientation.x = 0.0; grid_msg.info.origin.orientation.x = 0.0;
grid_msg.info.origin.orientation.y = 0.0; grid_msg.info.origin.orientation.y = 0.0;
@ -666,19 +668,21 @@ void roscontroller::grid_publisher()
grid_msg.info.origin.orientation.w = 1.0; grid_msg.info.origin.orientation.w = 1.0;
grid_msg.data.resize(g_w * g_h); 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) { std::map<int, int>::iterator itc = itr->second.begin();
grid_msg.data[(itr->first-1)*g_w+itc->first] = itc->second; for (itc = itr->second.begin(); itc != itr->second.end(); ++itc)
{
grid_msg.data[(itr->first - 1) * g_w + itc->first] = itc->second;
// DEBUG // 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); grid_pub.publish(grid_msg);
} }
} }
void roscontroller::Arm() void roscontroller::Arm()
/* /*
/ Functions handling the local MAV ROS flight controller / Functions handling the local MAV ROS flight controller
@ -729,13 +733,13 @@ with size......... | /
else else
message_number++; message_number++;
//header variables // header variables
uint16_t tmphead[4]; uint16_t tmphead[4];
tmphead[1] = (uint16_t)message_number; tmphead[1] = (uint16_t)message_number;
//uint32_t stime = (uint32_t)(ros::Time::now().toSec()); //(uint32_t)(logical_clock.toSec() * 100000); // uint32_t stime = (uint32_t)(ros::Time::now().toSec()); //(uint32_t)(logical_clock.toSec() * 100000);
memcpy((void*)(tmphead+2),(void*)&stime,sizeof(uint32_t)); memcpy((void*)(tmphead + 2), (void*)&stime, sizeof(uint32_t));
uint64_t rheader[1]; uint64_t rheader[1];
rheader[0]=0; rheader[0] = 0;
payload_out.sysid = (uint8_t)robot_id; payload_out.sysid = (uint8_t)robot_id;
payload_out.msgid = (uint32_t)message_number; payload_out.msgid = (uint32_t)message_number;
@ -755,7 +759,7 @@ with size......... | /
{ {
payload_out.payload64.push_back(payload_out_ptr[i]); payload_out.payload64.push_back(payload_out_ptr[i]);
} }
//Store out msg time // Store out msg time
out_msg_time = ros::Time::now().toNSec(); out_msg_time = ros::Time::now().toNSec();
out_msg_size = out[0]; out_msg_size = out[0];
// publish prepared messages in respective topic // publish prepared messages in respective topic
@ -825,7 +829,8 @@ script
cmd_srv.request.command = buzzuav_closures::getcmd(); cmd_srv.request.command = buzzuav_closures::getcmd();
if (!armstate) if (!armstate)
{ {
if(setmode){ if (setmode)
{
SetMode("LOITER", 0); SetMode("LOITER", 0);
ros::Duration(0.5).sleep(); ros::Duration(0.5).sleep();
} }
@ -834,13 +839,13 @@ script
// Registering HOME POINT. // Registering HOME POINT.
home = cur_pos; home = cur_pos;
// Initialize GPS goal for safety. // Initialize GPS goal for safety.
double gpspgoal[3] = {cur_pos.latitude,cur_pos.longitude,cur_pos.altitude}; double gpspgoal[3] = { cur_pos.latitude, cur_pos.longitude, cur_pos.altitude };
buzzuav_closures::set_gpsgoal(gpspgoal); buzzuav_closures::set_gpsgoal(gpspgoal);
BClpose = true; BClpose = true;
} }
if (current_mode != "GUIDED" && setmode) if (current_mode != "GUIDED" && setmode)
SetMode("GUIDED", 3000); // added for compatibility with 3DR Solo SetMode("GUIDED", 3000); // added for compatibility with 3DR Solo
if(setmode) if (setmode)
{ {
if (mav_client.call(cmd_srv)) if (mav_client.call(cmd_srv))
{ {
@ -862,7 +867,7 @@ script
armstate = 0; armstate = 0;
Arm(); Arm();
} }
else if(cur_pos.altitude < 0.4) //disarm only when close to ground else if (cur_pos.altitude < 0.4) // disarm only when close to ground
{ {
armstate = 0; armstate = 0;
Arm(); Arm();
@ -894,17 +899,17 @@ script
case COMPONENT_ARM_DISARM: case COMPONENT_ARM_DISARM:
if (!armstate) if (!armstate)
{ {
if(setmode) if (setmode)
SetMode("LOITER", 0); SetMode("LOITER", 0);
armstate = 1; armstate = 1;
Arm(); Arm();
} }
break; break;
case COMPONENT_ARM_DISARM+1: case COMPONENT_ARM_DISARM + 1:
if (armstate) if (armstate)
{ {
if(setmode) if (setmode)
SetMode("LOITER", 0); SetMode("LOITER", 0);
armstate = 0; armstate = 0;
Arm(); Arm();
@ -913,7 +918,7 @@ script
case NAV_SPLINE_WAYPOINT: case NAV_SPLINE_WAYPOINT:
goto_pos = buzzuav_closures::getgoto(); goto_pos = buzzuav_closures::getgoto();
if(setmode) if (setmode)
SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]); SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]);
break; break;
@ -950,12 +955,11 @@ void roscontroller::clear_pos()
/Refresh neighbours Position for every ten step /Refresh neighbours Position for every ten step
/---------------------------------------------*/ /---------------------------------------------*/
{ {
neighbours_pos_map.clear();
neighbours_pos_map.clear(); raw_neighbours_pos_map.clear();
raw_neighbours_pos_map.clear(); buzzuav_closures::clear_neighbours_pos();
buzzuav_closures::clear_neighbours_pos(); // raw_neighbours_pos_map.clear(); // TODO: currently not a problem, but
// raw_neighbours_pos_map.clear(); // TODO: currently not a problem, but // have to clear !
// have to clear !
} }
void roscontroller::neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr) void roscontroller::neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr)
@ -1088,21 +1092,17 @@ void roscontroller::local_pos_callback(const geometry_msgs::PoseStamped::ConstPt
cur_pos.x = msg->pose.position.x; cur_pos.x = msg->pose.position.x;
cur_pos.y = msg->pose.position.y; cur_pos.y = msg->pose.position.y;
if(!BClpose) if (!BClpose)
{ {
goto_pos[0]=cur_pos.x; goto_pos[0] = cur_pos.x;
goto_pos[1]=cur_pos.y; goto_pos[1] = cur_pos.y;
goto_pos[2]=0.0; goto_pos[2] = 0.0;
BClpose = true; BClpose = true;
} }
buzzuav_closures::set_currentNEDpos(msg->pose.position.y,msg->pose.position.x); buzzuav_closures::set_currentNEDpos(msg->pose.position.y, msg->pose.position.x);
// cur_pos.z = pose->pose.position.z; // Using relative altitude topic instead // cur_pos.z = pose->pose.position.z; // Using relative altitude topic instead
tf::Quaternion q( tf::Quaternion q(msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z, msg->pose.orientation.w);
msg->pose.orientation.x,
msg->pose.orientation.y,
msg->pose.orientation.z,
msg->pose.orientation.w);
tf::Matrix3x3 m(q); tf::Matrix3x3 m(q);
double roll, pitch, yaw; double roll, pitch, yaw;
m.getRPY(roll, pitch, yaw); m.getRPY(roll, pitch, yaw);
@ -1157,8 +1157,8 @@ void roscontroller::SetLocalPosition(float x, float y, float z, float yaw)
moveMsg.pose.orientation.w = q[3]; moveMsg.pose.orientation.w = q[3];
// To prevent drifting from stable position, uncomment // To prevent drifting from stable position, uncomment
//if(fabs(x)>0.005 || fabs(y)>0.005) { // if(fabs(x)>0.005 || fabs(y)>0.005) {
localsetpoint_nonraw_pub.publish(moveMsg); localsetpoint_nonraw_pub.publish(moveMsg);
//} //}
} }
@ -1197,15 +1197,15 @@ void roscontroller::SetStreamRate(int id, int rate, int on_off)
message.request.stream_id = id; message.request.stream_id = id;
message.request.message_rate = rate; message.request.message_rate = rate;
message.request.on_off = on_off; message.request.on_off = on_off;
int timeout = 20; // 2sec at 10Hz int timeout = 20; // 2sec at 10Hz
while (!stream_client.call(message) && timeout>0) while (!stream_client.call(message) && timeout > 0)
{ {
ROS_INFO("Set stream rate call failed!, trying again..."); ROS_INFO("Set stream rate call failed!, trying again...");
ros::Duration(0.1).sleep(); ros::Duration(0.1).sleep();
timeout--; timeout--;
} }
if(timeout<1) if (timeout < 1)
ROS_ERROR("Set stream rate timed out!"); ROS_ERROR("Set stream rate timed out!");
else else
ROS_WARN("Set stream rate call successful"); ROS_WARN("Set stream rate call successful");
@ -1228,12 +1228,12 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg)
// decode msg header // decode msg header
uint64_t rhead = msg->payload64[0]; uint64_t rhead = msg->payload64[0];
uint16_t r16head[4]; uint16_t r16head[4];
memcpy(r16head,&rhead, sizeof(uint64_t)); memcpy(r16head, &rhead, sizeof(uint64_t));
uint16_t mtype = r16head[0]; uint16_t mtype = r16head[0];
uint16_t mid = r16head[1]; uint16_t mid = r16head[1];
uint32_t temptime=0; uint32_t temptime = 0;
memcpy(&temptime, r16head+2, sizeof(uint32_t)); memcpy(&temptime, r16head + 2, sizeof(uint32_t));
//float stime = (float)temptime/(float)100000; // float stime = (float)temptime/(float)100000;
// if(debug) ROS_INFO("Received Msg: sent time %f for id %u",stime, mid); // if(debug) ROS_INFO("Received Msg: sent time %f for id %u",stime, mid);
// Check for Updater message, if updater message push it into updater FIFO // Check for Updater message, if updater message push it into updater FIFO
if ((uint64_t)msg->payload64[0] == (uint64_t)UPDATER_MESSAGE) if ((uint64_t)msg->payload64[0] == (uint64_t)UPDATER_MESSAGE)
@ -1284,7 +1284,7 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg)
// Extract robot id of the neighbour // Extract robot id of the neighbour
uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + index)); uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + index));
// store in msg data for data log // store in msg data for data log
msg_data inm(mid,out[1],out[0]*sizeof(uint64_t),stime,ros::Time::now().toNSec()); msg_data inm(mid, out[1], out[0] * sizeof(uint64_t), stime, ros::Time::now().toNSec());
inmsgdata.push_back(inm); inmsgdata.push_back(inm);
if (debug) if (debug)