From fbb0f108b1ae0f2e33709d6553f49c25202c896b Mon Sep 17 00:00:00 2001 From: dave Date: Tue, 4 Dec 2018 10:34:07 -0500 Subject: [PATCH] beautified and style fixes --- include/VoronoiDiagramGenerator.h | 290 +++--- include/buzz_update.h | 232 ++--- include/buzz_utility.h | 9 +- include/buzzuav_closures.h | 4 +- include/roscontroller.h | 14 +- readme.md | 99 +- src/VoronoiDiagramGenerator.cpp | 1551 +++++++++++++++-------------- src/buzz_update.cpp | 1281 ++++++++++++------------ src/buzz_utility.cpp | 14 +- src/buzzuav_closures.cpp | 691 +++++++------ src/roscontroller.cpp | 198 ++-- 11 files changed, 2239 insertions(+), 2144 deletions(-) diff --git a/include/VoronoiDiagramGenerator.h b/include/VoronoiDiagramGenerator.h index 4491a42..9876acf 100644 --- a/include/VoronoiDiagramGenerator.h +++ b/include/VoronoiDiagramGenerator.h @@ -12,9 +12,9 @@ * OF THIS SOFTWARE OR ITS FITNESS FOR ANY PARTICULAR PURPOSE. */ -/* -* This code was originally written by Stephan Fortune in C code. I, Shane O'Sullivan, -* have since modified it, encapsulating it in a C++ class and, fixing memory leaks and +/* +* This code was originally written by Stephan Fortune in C code. I, Shane O'Sullivan, +* have since modified it, encapsulating it in a C++ class and, fixing memory leaks and * adding accessors to the Voronoi Edges. * Permission to use, copy, modify, and distribute this software for any * purpose without fee is hereby granted, provided that this entire notice @@ -36,7 +36,6 @@ #include #include - #ifndef NULL #define NULL 0 #endif @@ -45,210 +44,195 @@ #define le 0 #define re 1 - - -struct Freenode +struct Freenode { - struct Freenode *nextfree; + struct Freenode* nextfree; }; struct FreeNodeArrayList { - struct Freenode* memory; - struct FreeNodeArrayList* next; - + struct Freenode* memory; + struct FreeNodeArrayList* next; }; -struct Freelist +struct Freelist { - struct Freenode *head; - int nodesize; + struct Freenode* head; + int nodesize; }; -struct Point +struct Point { - float x,y; - Point(): x( 0.0 ), y( 0.0 ) { } - Point( float x, float y ): x( x ), y( y ) { } + float x, 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 -struct Site +// structure used both for sites and for vertices +struct Site { - struct Point coord; - int sitenbr; - int refcnt; + struct Point coord; + int sitenbr; + int refcnt; }; - - -struct Edge +struct Edge { - float a,b,c; - struct Site *ep[2]; - struct Site *reg[2]; - int edgenbr; - int sites[2]; - + float a, b, c; + struct Site* ep[2]; + struct Site* reg[2]; + int edgenbr; + int sites[2]; }; struct GraphEdge { - float x1,y1,x2,y2; - int sites[2]; - struct GraphEdge* next; + float x1, y1, x2, y2; + int sites[2]; + struct GraphEdge* next; }; - - - -struct Halfedge +struct Halfedge { - struct Halfedge *ELleft, *ELright; - struct Edge *ELedge; - int ELrefcnt; - char ELpm; - struct Site *vertex; - float ystar; - struct Halfedge *PQnext; + struct Halfedge *ELleft, *ELright; + struct Edge* ELedge; + int ELrefcnt; + char ELpm; + struct Site* vertex; + float ystar; + struct Halfedge* PQnext; }; - - - class VoronoiDiagramGenerator { 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() - { - iteratorEdges = allEdges; - } + void resetIterator() + { + iteratorEdges = allEdges; + } - bool getNext(float& x1, float& y1, float& x2, float& y2, int *s) - { - if(iteratorEdges == 0) - return false; - - x1 = iteratorEdges->x1; - x2 = iteratorEdges->x2; - y1 = iteratorEdges->y1; - y2 = iteratorEdges->y2; - std::copy(iteratorEdges->sites, iteratorEdges->sites+2, s); + bool getNext(float& x1, float& y1, float& x2, float& y2, int* s) + { + if (iteratorEdges == 0) + return false; - 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: - void cleanup(); - void cleanupEdges(); - char *getfree(struct Freelist *fl); - struct Halfedge *PQfind(); - int PQempty(); + void cleanup(); + void cleanupEdges(); + char* getfree(struct Freelist* fl); + struct Halfedge* PQfind(); + int PQempty(); + struct Halfedge** ELhash; + struct Halfedge *HEcreate(), *ELleft(), *ELright(), *ELleftbnd(); + struct Halfedge* HEcreate(struct Edge* e, int pm); - - struct Halfedge **ELhash; - struct Halfedge *HEcreate(), *ELleft(), *ELright(), *ELleftbnd(); - struct Halfedge *HEcreate(struct Edge *e,int pm); + struct Point PQ_min(); + struct Halfedge* PQextractmin(); + void freeinit(struct Freelist* fl, int size); + void makefree(struct Freenode* curr, struct Freelist* fl); + void geominit(); + void plotinit(); + bool voronoi(int triangulate); + void ref(struct Site* v); + void deref(struct Site* v); + void endpoint(struct Edge* e, int lr, struct Site* s); + void ELdelete(struct Halfedge* he); + struct Halfedge* ELleftbnd(struct Point* p); + struct Halfedge* ELright(struct Halfedge* he); + void makevertex(struct Site* v); + void out_triple(struct Site* s1, struct Site* s2, struct Site* s3); - struct Point PQ_min(); - struct Halfedge *PQextractmin(); - void freeinit(struct Freelist *fl,int size); - void makefree(struct Freenode *curr,struct Freelist *fl); - void geominit(); - void plotinit(); - bool voronoi(int triangulate); - void ref(struct Site *v); - void deref(struct Site *v); - void endpoint(struct Edge *e,int lr,struct Site * s); + void PQinsert(struct Halfedge* he, struct Site* v, float offset); + void PQdelete(struct Halfedge* he); + bool ELinitialize(); + void ELinsert(struct Halfedge* lb, struct Halfedge* newHe); + struct Halfedge* ELgethash(int b); + struct Halfedge* ELleft(struct Halfedge* he); + struct Site* leftreg(struct Halfedge* he); + void out_site(struct Site* s); + bool PQinitialize(); + int PQbucket(struct Halfedge* he); + void clip_line(struct Edge* e); + char* myalloc(unsigned n); + int right_of(struct Halfedge* el, struct Point* p); - 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 Site* rightreg(struct Halfedge* he); + struct Edge* bisect(struct Site* s1, struct Site* s2); + float dist(struct Site* s, struct Site* t); + struct Site* intersect(struct Halfedge* el1, struct Halfedge* el2, struct Point* p = 0); - void PQinsert(struct Halfedge *he,struct Site * v, float offset); - void PQdelete(struct Halfedge *he); - bool ELinitialize(); - void ELinsert(struct Halfedge *lb, struct Halfedge *newHe); - struct Halfedge * ELgethash(int b); - struct Halfedge *ELleft(struct Halfedge *he); - struct Site *leftreg(struct Halfedge *he); - void out_site(struct Site *s); - bool PQinitialize(); - int PQbucket(struct Halfedge *he); - void clip_line(struct Edge *e); - char *myalloc(unsigned n); - int right_of(struct Halfedge *el,struct Point *p); + void out_bisector(struct Edge* e); + void out_ep(struct Edge* e); + void out_vertex(struct Site* v); + struct Site* nextone(); - struct Site *rightreg(struct Halfedge *he); - struct Edge *bisect(struct Site *s1,struct Site *s2); - float dist(struct Site *s,struct Site *t); - struct Site *intersect(struct Halfedge *el1, struct Halfedge *el2, struct Point *p=0); + void pushGraphEdge(float x1, float y1, float x2, float y2, int s[2]); - void out_bisector(struct Edge *e); - void out_ep(struct Edge *e); - void out_vertex(struct Site *v); - struct Site *nextone(); + void openpl(); + void line(float x1, float y1, float x2, float y2, int s[2]); + void circle(float x, float y, float radius); + void range(float minX, float minY, float maxX, float maxY); - void pushGraphEdge(float x1, float y1, float x2, float y2, int s[2]); + struct Freelist hfl; + struct Halfedge *ELleftend, *ELrightend; + int ELhashsize; - void openpl(); - void line(float x1, float y1, float x2, float y2, int s[2]); - void circle(float x, float y, float radius); - void range(float minX, float minY, float maxX, float maxY); + int triangulate, sorted, plot, debug; + float xmin, xmax, ymin, ymax, deltax, deltay; - struct Freelist hfl; - struct Halfedge *ELleftend, *ELrightend; - int ELhashsize; + struct Site* sites; + int nsites; + int siteidx; + int sqrt_nsites; + int nvertices; + struct Freelist sfl; + struct Site* bottomsite; - int triangulate, sorted, plot, debug; - float xmin, xmax, ymin, ymax, deltax, deltay; + int nedges; + struct Freelist efl; + int PQhashsize; + struct Halfedge* PQhash; + int PQcount; + int PQmin; - struct Site *sites; - int nsites; - int siteidx; - int sqrt_nsites; - int nvertices; - struct Freelist sfl; - struct Site *bottomsite; + int ntry, totalsearch; + float pxmin, pxmax, pymin, pymax, cradius; + int total_alloc; - int nedges; - struct Freelist efl; - int PQhashsize; - struct Halfedge *PQhash; - int PQcount; - int PQmin; + float borderMinX, borderMaxX, borderMinY, borderMaxY; - int ntry, totalsearch; - float pxmin, pxmax, pymin, pymax, cradius; - int total_alloc; + FreeNodeArrayList* allMemoryList; + FreeNodeArrayList* currentMemoryBlock; - float borderMinX, borderMaxX, borderMinY, borderMaxY; + GraphEdge* allEdges; + GraphEdge* iteratorEdges; - FreeNodeArrayList* allMemoryList; - FreeNodeArrayList* currentMemoryBlock; - - GraphEdge* allEdges; - GraphEdge* iteratorEdges; - - float minDistanceBetweenSites; - + float minDistanceBetweenSites; }; -int scomp(const void *p1,const void *p2); - +int scomp(const void* p1, const void* p2); #endif - - diff --git a/include/buzz_update.h b/include/buzz_update.h index 4b43e86..af9d41f 100644 --- a/include/buzz_update.h +++ b/include/buzz_update.h @@ -23,149 +23,149 @@ } while (0) namespace buzz_update { - static const uint16_t CODE_REQUEST_PADDING = 250; - static const uint16_t MIN_UPDATE_PACKET = 251; - static const uint16_t UPDATE_CODE_HEADER_SIZE = 5; - static const uint16_t TIMEOUT_FOR_ROLLBACK = 50; - /*********************/ - /* Updater states */ - /********************/ +static const uint16_t CODE_REQUEST_PADDING = 250; +static const uint16_t MIN_UPDATE_PACKET = 251; +static const uint16_t UPDATE_CODE_HEADER_SIZE = 5; +static const uint16_t TIMEOUT_FOR_ROLLBACK = 50; +/*********************/ +/* Updater states */ +/********************/ - typedef enum { - CODE_RUNNING = 0, // Code executing - CODE_STANDBY, // Standing by for others to update - } code_states_e; +typedef enum { + CODE_RUNNING = 0, // Code executing + CODE_STANDBY, // Standing by for others to update +} code_states_e; - /*********************/ - /*Message types */ - /********************/ +/*********************/ +/*Message types */ +/********************/ - typedef enum { - SENT_CODE = 0, // Broadcast code - RESEND_CODE, // ReBroadcast request - } code_message_e; +typedef enum { + SENT_CODE = 0, // Broadcast code + RESEND_CODE, // ReBroadcast request +} code_message_e; - /*************************/ - /*Updater message queue */ - /*************************/ +/*************************/ +/*Updater message queue */ +/*************************/ - struct updater_msgqueue_s - { - uint8_t* queue; - uint8_t* size; - }; - typedef struct updater_msgqueue_s* updater_msgqueue_t; +struct updater_msgqueue_s +{ + uint8_t* queue; + uint8_t* size; +}; +typedef struct updater_msgqueue_s* updater_msgqueue_t; - struct updater_code_s - { - uint8_t* bcode; - uint8_t* bcode_size; - }; - typedef struct updater_code_s* updater_code_t; +struct updater_code_s +{ + uint8_t* bcode; + uint8_t* bcode_size; +}; +typedef struct updater_code_s* updater_code_t; - /**************************/ - /*Updater data*/ - /**************************/ +/**************************/ +/*Updater data*/ +/**************************/ - struct buzz_updater_elem_s - { - /* robot id */ - // uint16_t robotid; - /*current Bytecode content */ - uint8_t* bcode; - /*old Bytecode name */ - const char* old_bcode; - /*current bcode size*/ - size_t* bcode_size; - /*Update patch*/ - uint8_t* patch; - /* Update patch size*/ - size_t* patch_size; - /*current Bytecode content */ - uint8_t* standby_bcode; - /*current bcode size*/ - size_t* standby_bcode_size; - /*updater out msg queue */ - updater_msgqueue_t outmsg_queue; - /*updater in msg queue*/ - updater_msgqueue_t inmsg_queue; - /*Current state of the updater one in code_states_e ENUM*/ - int* mode; - uint8_t* update_no; - }; - typedef struct buzz_updater_elem_s* buzz_updater_elem_t; +struct buzz_updater_elem_s +{ + /* robot id */ + // uint16_t robotid; + /*current Bytecode content */ + uint8_t* bcode; + /*old Bytecode name */ + const char* old_bcode; + /*current bcode size*/ + size_t* bcode_size; + /*Update patch*/ + uint8_t* patch; + /* Update patch size*/ + size_t* patch_size; + /*current Bytecode content */ + uint8_t* standby_bcode; + /*current bcode size*/ + size_t* standby_bcode_size; + /*updater out msg queue */ + updater_msgqueue_t outmsg_queue; + /*updater in msg queue*/ + updater_msgqueue_t inmsg_queue; + /*Current state of the updater one in code_states_e ENUM*/ + int* mode; + uint8_t* update_no; +}; +typedef struct buzz_updater_elem_s* buzz_updater_elem_t; - /**************************************************************************/ - /*Updater routine from msg processing to file checks to be called from main*/ - /**************************************************************************/ - void update_routine(); +/**************************************************************************/ +/*Updater routine from msg processing to file checks to be called from main*/ +/**************************************************************************/ +void update_routine(); - /************************************************/ - /*Initalizes the updater */ - /************************************************/ - int init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id); +/************************************************/ +/*Initalizes the updater */ +/************************************************/ +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*/ - /*******************************************************/ - uint8_t* getupdater_out_msg(); +/*****************************************************/ +/*Obtains messages from out msgs queue of the updater*/ +/*******************************************************/ +uint8_t* getupdater_out_msg(); - /******************************************************/ - /*Obtains out msg queue size*/ - /*****************************************************/ - uint8_t* getupdate_out_msg_size(); +/******************************************************/ +/*Obtains out msg queue size*/ +/*****************************************************/ +uint8_t* getupdate_out_msg_size(); - /**************************************************/ - /*Destroys the out msg queue*/ - /*************************************************/ - void destroy_out_msg_queue(); +/**************************************************/ +/*Destroys the out msg queue*/ +/*************************************************/ +void destroy_out_msg_queue(); - // buzz_updater_elem_t get_updater(); - /***************************************************/ - /*Sets bzz file name*/ - /***************************************************/ - void set_bzz_file(const char* in_bzz_file, bool dbg); +// buzz_updater_elem_t get_updater(); +/***************************************************/ +/*Sets bzz file name*/ +/***************************************************/ +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 diff --git a/include/buzz_utility.h b/include/buzz_utility.h index 31197aa..c5aa69e 100644 --- a/include/buzz_utility.h +++ b/include/buzz_utility.h @@ -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 get_inmsg_vector(); std::string get_bvmstate(); - } diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index 95140dc..71a1d6a 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -22,7 +22,7 @@ namespace buzzuav_closures int buzzros_print(buzzvm_t vm); void setWPlist(std::string file); void setVorlog(std::string path); -void check_targets_sim(double lat, double lon, double *res); +void check_targets_sim(double lat, double lon, double* res); /* * closure to move following a vector @@ -98,7 +98,7 @@ double* getgoto(); /* * returns the current grid */ -std::map> getgrid(); +std::map> getgrid(); int voronoi_center(buzzvm_t vm); /* diff --git a/include/roscontroller.h b/include/roscontroller.h index 6edb678..23ef2a1 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -42,14 +42,14 @@ * ROSBuzz message types */ typedef enum { - ROS_BUZZ_MSG_NIL = 0, // dummy msg - UPDATER_MESSAGE, // Update msg - BUZZ_MESSAGE, // Broadcast message - BUZZ_MESSAGE_TIME, // Broadcast message with time info + ROS_BUZZ_MSG_NIL = 0, // dummy msg + UPDATER_MESSAGE, // Update msg + BUZZ_MESSAGE, // Broadcast message + BUZZ_MESSAGE_TIME, // Broadcast message with time info } rosbuzz_msgtype; // 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 MOVING_AVERAGE_ALPHA 0.1 #define MAX_NUMBER_OF_ROBOTS 10 @@ -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; diff --git a/readme.md b/readme.md index d61cdb4..6ab29db 100644 --- a/readme.md +++ b/readme.md @@ -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 : +* 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--mavros ros--mavros-extras +``` +$ sudo apt-get install ros--mavros ros--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 -------------------- diff --git a/src/VoronoiDiagramGenerator.cpp b/src/VoronoiDiagramGenerator.cpp index 9423752..cafafc9 100644 --- a/src/VoronoiDiagramGenerator.cpp +++ b/src/VoronoiDiagramGenerator.cpp @@ -12,9 +12,9 @@ * OF THIS SOFTWARE OR ITS FITNESS FOR ANY PARTICULAR PURPOSE. */ -/* -* This code was originally written by Stephan Fortune in C code. I, Shane O'Sullivan, -* have since modified it, encapsulating it in a C++ class and, fixing memory leaks and +/* +* This code was originally written by Stephan Fortune in C code. I, Shane O'Sullivan, +* have since modified it, encapsulating it in a C++ class and, fixing memory leaks and * adding accessors to the Voronoi Edges. * Permission to use, copy, modify, and distribute this software for any * purpose without fee is hereby granted, provided that this entire notice @@ -31,830 +31,849 @@ VoronoiDiagramGenerator::VoronoiDiagramGenerator() { - siteidx = 0; - sites = 0; + siteidx = 0; + sites = 0; - allMemoryList = new FreeNodeArrayList; - allMemoryList->memory = 0; - allMemoryList->next = 0; - currentMemoryBlock = allMemoryList; - allEdges = 0; - iteratorEdges = 0; - minDistanceBetweenSites = 0; + allMemoryList = new FreeNodeArrayList; + allMemoryList->memory = 0; + allMemoryList->next = 0; + currentMemoryBlock = allMemoryList; + allEdges = 0; + iteratorEdges = 0; + minDistanceBetweenSites = 0; } VoronoiDiagramGenerator::~VoronoiDiagramGenerator() { - cleanup(); - cleanupEdges(); + cleanup(); + cleanupEdges(); - if(allMemoryList != 0) - delete allMemoryList; + if (allMemoryList != 0) + 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(); - int i; + cleanup(); + cleanupEdges(); + int i; - minDistanceBetweenSites = minDist; + minDistanceBetweenSites = minDist; - nsites=numPoints; - plot = 0; - triangulate = 0; - debug = 1; - sorted = 0; - freeinit(&sfl, sizeof (Site)); - - sites = (struct Site *) myalloc(nsites*sizeof( *sites)); + nsites = numPoints; + plot = 0; + triangulate = 0; + debug = 1; + sorted = 0; + freeinit(&sfl, sizeof(Site)); - if(sites == 0) - return false; + sites = (struct Site*)myalloc(nsites * sizeof(*sites)); - xmin = xValues[0]; - ymin = yValues[0]; - xmax = xValues[0]; - ymax = yValues[0]; + if (sites == 0) + return false; - for(i = 0; i< nsites; i++) - { - sites[i].coord.x = xValues[i]; - sites[i].coord.y = yValues[i]; - sites[i].sitenbr = i; - sites[i].refcnt = 0; + xmin = xValues[0]; + ymin = yValues[0]; + xmax = xValues[0]; + ymax = yValues[0]; - if(xValues[i] < xmin) - xmin = xValues[i]; - else if(xValues[i] > xmax) - xmax = xValues[i]; + for (i = 0; i < nsites; i++) + { + sites[i].coord.x = xValues[i]; + sites[i].coord.y = yValues[i]; + sites[i].sitenbr = i; + sites[i].refcnt = 0; - if(yValues[i] < ymin) - ymin = yValues[i]; - else if(yValues[i] > ymax) - ymax = yValues[i]; + if (xValues[i] < xmin) + xmin = xValues[i]; + else if (xValues[i] > xmax) + xmax = xValues[i]; - //printf("\n%f %f\n",xValues[i],yValues[i]); - } - - qsort(sites, nsites, sizeof (*sites), scomp); - - siteidx = 0; - geominit(); - float temp = 0; - if(minX > maxX) - { - temp = minX; - minX = maxX; - maxX = temp; - } - if(minY > maxY) - { - temp = minY; - minY = maxY; - maxY = temp; - } - borderMinX = minX; - borderMinY = minY; - borderMaxX = maxX; - borderMaxY = maxY; - - siteidx = 0; - voronoi(triangulate); + if (yValues[i] < ymin) + ymin = yValues[i]; + else if (yValues[i] > ymax) + ymax = yValues[i]; - return true; + // printf("\n%f %f\n",xValues[i],yValues[i]); + } + + qsort(sites, nsites, sizeof(*sites), scomp); + + siteidx = 0; + geominit(); + float temp = 0; + if (minX > maxX) + { + temp = minX; + minX = maxX; + maxX = temp; + } + if (minY > maxY) + { + temp = minY; + minY = maxY; + maxY = temp; + } + borderMinX = minX; + borderMinY = minY; + borderMaxX = maxX; + borderMaxY = maxY; + + siteidx = 0; + voronoi(triangulate); + + return true; } bool VoronoiDiagramGenerator::ELinitialize() { - int i; - freeinit(&hfl, sizeof **ELhash); - ELhashsize = 2 * sqrt_nsites; - ELhash = (struct Halfedge **) myalloc ( sizeof *ELhash * ELhashsize); + int i; + freeinit(&hfl, sizeof **ELhash); + ELhashsize = 2 * sqrt_nsites; + ELhash = (struct Halfedge**)myalloc(sizeof *ELhash * ELhashsize); - if(ELhash == 0) - return false; + if (ELhash == 0) + return false; - for(i=0; i ELleft = (struct Halfedge *)NULL; - ELleftend -> ELright = ELrightend; - ELrightend -> ELleft = ELleftend; - ELrightend -> ELright = (struct Halfedge *)NULL; - ELhash[0] = ELleftend; - ELhash[ELhashsize-1] = ELrightend; + 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; + ELleftend->ELright = ELrightend; + ELrightend->ELleft = ELleftend; + ELrightend->ELright = (struct Halfedge*)NULL; + ELhash[0] = ELleftend; + ELhash[ELhashsize - 1] = ELrightend; - return true; + return true; } - -struct Halfedge* VoronoiDiagramGenerator::HEcreate(struct Edge *e,int pm) +struct Halfedge* VoronoiDiagramGenerator::HEcreate(struct Edge* e, int pm) { - struct Halfedge *answer; - answer = (struct Halfedge *) getfree(&hfl); - answer -> ELedge = e; - answer -> ELpm = pm; - answer -> PQnext = (struct Halfedge *) NULL; - answer -> vertex = (struct Site *) NULL; - answer -> ELrefcnt = 0; - return(answer); + struct Halfedge* answer; + answer = (struct Halfedge*)getfree(&hfl); + answer->ELedge = e; + answer->ELpm = pm; + answer->PQnext = (struct Halfedge*)NULL; + answer->vertex = (struct Site*)NULL; + answer->ELrefcnt = 0; + return (answer); } - -void VoronoiDiagramGenerator::ELinsert(struct Halfedge *lb, struct Halfedge *newHe) +void VoronoiDiagramGenerator::ELinsert(struct Halfedge* lb, struct Halfedge* newHe) { - newHe -> ELleft = lb; - newHe -> ELright = lb -> ELright; - (lb -> ELright) -> ELleft = newHe; - lb -> ELright = newHe; + newHe->ELleft = lb; + newHe->ELright = lb->ELright; + (lb->ELright)->ELleft = newHe; + lb->ELright = newHe; } /* Get entry from hash table, pruning any deleted nodes */ -struct Halfedge * VoronoiDiagramGenerator::ELgethash(int b) +struct Halfedge* VoronoiDiagramGenerator::ELgethash(int b) { - struct Halfedge *he; - - if(b<0 || b>=ELhashsize) - return((struct Halfedge *) NULL); - he = ELhash[b]; - if (he == (struct Halfedge *) NULL || he->ELedge != (struct Edge *) DELETED ) - return (he); - - /* Hash table points to deleted half edge. Patch as necessary. */ - ELhash[b] = (struct Halfedge *) NULL; - if ((he -> ELrefcnt -= 1) == 0) - makefree((Freenode*)he, &hfl); - return ((struct Halfedge *) NULL); -} + struct Halfedge* he; -struct Halfedge * VoronoiDiagramGenerator::ELleftbnd(struct Point *p) -{ - int i, bucket; - 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 + if (b < 0 || b >= ELhashsize) + return ((struct Halfedge*)NULL); + he = ELhash[b]; + if (he == (struct Halfedge*)NULL || he->ELedge != (struct Edge*)DELETED) + return (he); - 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 - { - for(i=1; 1 ; i += 1) - { - if ((he=ELgethash(bucket-i)) != (struct Halfedge *) NULL) - break; - if ((he=ELgethash(bucket+i)) != (struct Halfedge *) NULL) - break; - }; - totalsearch += i; - }; - ntry += 1; - /* Now search linear list of halfedges for the correct one */ - if (he==ELleftend || (he != ELrightend && right_of(he,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 - 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 - do - { - he = he -> ELleft; - } while (he!=ELleftend && !right_of(he,p)); - - /* Update hash table and reference counts */ - if(bucket > 0 && bucket ELrefcnt -= 1; - } - ELhash[bucket] = he; - ELhash[bucket] -> ELrefcnt += 1; - }; - return (he); + /* Hash table points to deleted half edge. Patch as necessary. */ + ELhash[b] = (struct Halfedge*)NULL; + if ((he->ELrefcnt -= 1) == 0) + makefree((Freenode*)he, &hfl); + return ((struct Halfedge*)NULL); } +struct Halfedge* VoronoiDiagramGenerator::ELleftbnd(struct Point* p) +{ + int i, bucket; + 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 + + 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 + { + for (i = 1; 1; i += 1) + { + if ((he = ELgethash(bucket - i)) != (struct Halfedge*)NULL) + break; + if ((he = ELgethash(bucket + i)) != (struct Halfedge*)NULL) + break; + }; + totalsearch += i; + }; + ntry += 1; + /* Now search linear list of halfedges for the correct one */ + if (he == ELleftend || (he != ELrightend && right_of(he, 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 + 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 + do + { + he = he->ELleft; + } while (he != ELleftend && !right_of(he, p)); + + /* Update hash table and reference counts */ + if (bucket > 0 && bucket < ELhashsize - 1) + { + if (ELhash[bucket] != (struct Halfedge*)NULL) + { + ELhash[bucket]->ELrefcnt -= 1; + } + ELhash[bucket] = he; + ELhash[bucket]->ELrefcnt += 1; + }; + return (he); +} /* This delete routine can't reclaim node, since pointers from hash table may be present. */ -void VoronoiDiagramGenerator::ELdelete(struct Halfedge *he) +void VoronoiDiagramGenerator::ELdelete(struct Halfedge* he) { - (he -> ELleft) -> ELright = he -> ELright; - (he -> ELright) -> ELleft = he -> ELleft; - he -> ELedge = (struct Edge *)DELETED; + (he->ELleft)->ELright = he->ELright; + (he->ELright)->ELleft = he->ELleft; + he->ELedge = (struct Edge*)DELETED; } - -struct Halfedge * VoronoiDiagramGenerator::ELright(struct Halfedge *he) +struct Halfedge* VoronoiDiagramGenerator::ELright(struct Halfedge* he) { - return (he -> ELright); + return (he->ELright); } -struct Halfedge * VoronoiDiagramGenerator::ELleft(struct Halfedge *he) +struct Halfedge* VoronoiDiagramGenerator::ELleft(struct Halfedge* he) { - return (he -> ELleft); + return (he->ELleft); } - -struct Site * VoronoiDiagramGenerator::leftreg(struct Halfedge *he) +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]); + if (he->ELedge == (struct Edge*)NULL) + return (bottomsite); + return (he->ELpm == le ? he->ELedge->reg[le] : he->ELedge->reg[re]); } -struct Site * VoronoiDiagramGenerator::rightreg(struct Halfedge *he) +struct Site* VoronoiDiagramGenerator::rightreg(struct Halfedge* he) { - if(he -> ELedge == (struct Edge *)NULL) //if this halfedge has no edge, return the bottom site (whatever that is) - return(bottomsite); + if (he->ELedge == (struct Edge*)NULL) // if this halfedge has no edge, return the bottom site (whatever that is) + return (bottomsite); - //if the ELpm field is zero, return the site 0 that this edge bisects, otherwise return site number 1 - return( he -> ELpm == le ? he -> ELedge -> reg[re] : he -> ELedge -> reg[le]); + // if the ELpm field is zero, return the site 0 that this edge bisects, otherwise return site number 1 + return (he->ELpm == le ? he->ELedge->reg[re] : he->ELedge->reg[le]); } void VoronoiDiagramGenerator::geominit() -{ - float sn; +{ + float sn; - freeinit(&efl, sizeof(Edge)); - nvertices = 0; - nedges = 0; - sn = (float)nsites+4; - sqrt_nsites = (int)sqrt(sn); - deltay = ymax - ymin; - deltax = xmax - xmin; + freeinit(&efl, sizeof(Edge)); + nvertices = 0; + nedges = 0; + sn = (float)nsites + 4; + sqrt_nsites = (int)sqrt(sn); + deltay = ymax - ymin; + deltax = xmax - xmin; } - -struct Edge * VoronoiDiagramGenerator::bisect(struct Site *s1,struct Site *s2) +struct Edge* VoronoiDiagramGenerator::bisect(struct Site* s1, struct Site* s2) { - float dx,dy,adx,ady; - struct Edge *newedge; + float dx, dy, adx, ady; + struct Edge* newedge; - newedge = (struct Edge *) getfree(&efl); - - newedge -> reg[0] = s1; //store the sites that this edge is bisecting - newedge -> reg[1] = s2; - ref(s1); - ref(s2); - newedge -> ep[0] = (struct Site *) NULL; //to begin with, there are no endpoints on the bisector - it goes to infinity - newedge -> ep[1] = (struct Site *) NULL; - - dx = s2->coord.x - s1->coord.x; //get the difference in x dist between the sites - dy = s2->coord.y - s1->coord.y; - adx = dx>0 ? dx : -dx; //make sure that the difference in positive - ady = dy>0 ? dy : -dy; - newedge -> c = (float)(s1->coord.x * dx + s1->coord.y * dy + (dx*dx + dy*dy)*0.5);//get the slope of the line + newedge = (struct Edge*)getfree(&efl); - if (adx>ady) - { - 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 -> edgenbr = nedges; - newedge -> sites[0] = s1->sitenbr; - newedge -> sites[1] = s2->sitenbr; + newedge->reg[0] = s1; // store the sites that this edge is bisecting + newedge->reg[1] = s2; + ref(s1); + ref(s2); + newedge->ep[0] = (struct Site*)NULL; // to begin with, there are no endpoints on the bisector - it goes to infinity + newedge->ep[1] = (struct Site*)NULL; - //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); + dx = s2->coord.x - s1->coord.x; // get the difference in x dist between the sites + dy = s2->coord.y - s1->coord.y; + adx = dx > 0 ? dx : -dx; // make sure that the difference in positive + ady = dy > 0 ? dy : -dy; + newedge->c = (float)(s1->coord.x * dx + s1->coord.y * dy + (dx * dx + dy * dy) * 0.5); // get the slope of the line + + if (adx > ady) + { + 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->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); + + 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 -struct Site * VoronoiDiagramGenerator::intersect(struct Halfedge *el1, struct Halfedge *el2, struct Point *p) +// 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; - struct Halfedge *el; - float d, xint, yint; - int right_of_site; - struct Site *v; - - e1 = el1 -> ELedge; - e2 = el2 -> ELedge; - if(e1 == (struct Edge*)NULL || e2 == (struct Edge*)NULL) - return ((struct Site *) NULL); + struct Edge *e1, *e2, *e; + struct Halfedge* el; + float d, xint, yint; + int right_of_site; + struct Site* v; - //if the two edges bisect the same parent, return null - if (e1->reg[1] == e2->reg[1]) - return ((struct Site *) NULL); - - d = e1->a * e2->b - e1->b * e2->a; - if (-1.0e-10c*e2->b - e2->c*e1->b)/d; - 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) ) - { - el = el1; - e = e1; - } - else - { - el = el2; - e = e2; - }; - - right_of_site = xint >= e -> reg[1] -> coord.x; - if ((right_of_site && el -> ELpm == le) || (!right_of_site && el -> ELpm == re)) - return ((struct Site *) NULL); - - //create a new site at the point of intersection - this is a new vector event waiting to happen - v = (struct Site *) getfree(&sfl); - v -> refcnt = 0; - v -> coord.x = xint; - v -> coord.y = yint; - return(v); + e1 = el1->ELedge; + e2 = el2->ELedge; + if (e1 == (struct Edge*)NULL || e2 == (struct Edge*)NULL) + return ((struct Site*)NULL); + + // if the two edges bisect the same parent, return null + if (e1->reg[1] == e2->reg[1]) + return ((struct Site*)NULL); + + d = e1->a * e2->b - e1->b * e2->a; + if (-1.0e-10 < d && d < 1.0e-10) + return ((struct Site*)NULL); + + xint = (e1->c * e2->b - e2->c * e1->b) / d; + 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)) + { + el = el1; + e = e1; + } + else + { + el = el2; + e = e2; + }; + + right_of_site = xint >= e->reg[1]->coord.x; + if ((right_of_site && el->ELpm == le) || (!right_of_site && el->ELpm == re)) + return ((struct Site*)NULL); + + // create a new site at the point of intersection - this is a new vector event waiting to happen + v = (struct Site*)getfree(&sfl); + v->refcnt = 0; + v->coord.x = xint; + v->coord.y = yint; + return (v); } /* returns 1 if p is to right of halfedge e */ -int VoronoiDiagramGenerator::right_of(struct Halfedge *el,struct Point *p) +int VoronoiDiagramGenerator::right_of(struct Halfedge* el, struct Point* p) { - struct Edge *e; - struct Site *topsite; - int right_of_site, above, fast; - float dxp, dyp, dxs, t1, t2, t3, yl; - - 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 (e->a == 1.0) - { 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; - fast = above; - } - else - { 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; - }; - } - else /*e->b==1.0 */ - { yl = e->c - e->a*p->x; - t1 = p->y - yl; - t2 = p->x - topsite->coord.x; - t3 = yl - topsite->coord.y; - above = t1*t1 > t2*t2 + t3*t3; - }; - return (el->ELpm==le ? above : !above); + struct Edge* e; + struct Site* topsite; + int right_of_site, above, fast; + float dxp, dyp, dxs, t1, t2, t3, yl; + + 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 (e->a == 1.0) + { + 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; + fast = above; + } + else + { + 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; + }; + } + else /*e->b==1.0 */ + { + yl = e->c - e->a * p->x; + t1 = p->y - yl; + t2 = p->x - topsite->coord.x; + t3 = yl - topsite->coord.y; + above = t1 * t1 > t2 * t2 + t3 * t3; + }; + return (el->ELpm == le ? above : !above); } - -void VoronoiDiagramGenerator::endpoint(struct Edge *e,int lr,struct Site * s) +void VoronoiDiagramGenerator::endpoint(struct Edge* e, int lr, struct Site* s) { - e -> ep[lr] = s; - ref(s); - if(e -> ep[re-lr]== (struct Site *) NULL) - return; + e->ep[lr] = s; + ref(s); + if (e->ep[re - lr] == (struct Site*)NULL) + return; - clip_line(e); + clip_line(e); - deref(e->reg[le]); - deref(e->reg[re]); - makefree((Freenode*)e, &efl); + deref(e->reg[le]); + deref(e->reg[re]); + makefree((Freenode*)e, &efl); } - -float VoronoiDiagramGenerator::dist(struct Site *s,struct Site *t) +float VoronoiDiagramGenerator::dist(struct Site* s, struct Site* t) { - float dx,dy; - dx = s->coord.x - t->coord.x; - dy = s->coord.y - t->coord.y; - return (float)(sqrt(dx*dx + dy*dy)); + float dx, dy; + dx = s->coord.x - t->coord.x; + dy = s->coord.y - t->coord.y; + return (float)(sqrt(dx * dx + dy * dy)); } - -void VoronoiDiagramGenerator::makevertex(struct Site *v) +void VoronoiDiagramGenerator::makevertex(struct Site* v) { - v -> sitenbr = nvertices; - nvertices += 1; - out_vertex(v); + v->sitenbr = nvertices; + nvertices += 1; + out_vertex(v); } - -void VoronoiDiagramGenerator::deref(struct Site *v) +void VoronoiDiagramGenerator::deref(struct Site* v) { - v -> refcnt -= 1; - if (v -> refcnt == 0 ) - makefree((Freenode*)v, &sfl); + v->refcnt -= 1; + if (v->refcnt == 0) + makefree((Freenode*)v, &sfl); } -void VoronoiDiagramGenerator::ref(struct Site *v) +void VoronoiDiagramGenerator::ref(struct Site* v) { - v -> refcnt += 1; + v->refcnt += 1; } -//push the HalfEdge into the ordered linked list of vertices -void VoronoiDiagramGenerator::PQinsert(struct Halfedge *he,struct Site * v, float offset) +// push the HalfEdge into the ordered linked list of vertices +void VoronoiDiagramGenerator::PQinsert(struct Halfedge* he, struct Site* v, float offset) { - struct Halfedge *last, *next; - - he -> vertex = v; - ref(v); - 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))) - { - last = next; - }; - he -> PQnext = last -> PQnext; - last -> PQnext = he; - PQcount += 1; + struct Halfedge *last, *next; + + he->vertex = v; + ref(v); + 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))) + { + last = next; + }; + he->PQnext = last->PQnext; + last->PQnext = he; + PQcount += 1; } -//remove the HalfEdge from the list of vertices -void VoronoiDiagramGenerator::PQdelete(struct Halfedge *he) +// remove the HalfEdge from the list of vertices +void VoronoiDiagramGenerator::PQdelete(struct Halfedge* he) { - struct Halfedge *last; - - if(he -> vertex != (struct Site *) NULL) - { - last = &PQhash[PQbucket(he)]; - while (last -> PQnext != he) - last = last -> PQnext; + struct Halfedge* last; - last -> PQnext = he -> PQnext; - PQcount -= 1; - deref(he -> vertex); - he -> vertex = (struct Site *) NULL; - }; + if (he->vertex != (struct Site*)NULL) + { + last = &PQhash[PQbucket(he)]; + while (last->PQnext != he) + last = last->PQnext; + + last->PQnext = he->PQnext; + PQcount -= 1; + deref(he->vertex); + he->vertex = (struct Site*)NULL; + }; } -int VoronoiDiagramGenerator::PQbucket(struct Halfedge *he) +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; - return(bucket); + 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; + return (bucket); } - - int VoronoiDiagramGenerator::PQempty() { - return(PQcount==0); + return (PQcount == 0); } - struct Point VoronoiDiagramGenerator::PQ_min() { - struct Point answer; - - 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); + struct Point answer; + + 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); } -struct Halfedge * VoronoiDiagramGenerator::PQextractmin() +struct Halfedge* VoronoiDiagramGenerator::PQextractmin() { - struct Halfedge *curr; - - curr = PQhash[PQmin].PQnext; - PQhash[PQmin].PQnext = curr -> PQnext; - PQcount -= 1; - return(curr); -} + struct Halfedge* curr; + curr = PQhash[PQmin].PQnext; + PQhash[PQmin].PQnext = curr->PQnext; + PQcount -= 1; + return (curr); +} bool VoronoiDiagramGenerator::PQinitialize() { - int i; - - PQcount = 0; - PQmin = 0; - PQhashsize = 4 * sqrt_nsites; - PQhash = (struct Halfedge *) myalloc(PQhashsize * sizeof *PQhash); + int i; - if(PQhash == 0) - return false; + PQcount = 0; + PQmin = 0; + PQhashsize = 4 * sqrt_nsites; + PQhash = (struct Halfedge*)myalloc(PQhashsize * sizeof *PQhash); - for(i=0; i head = (struct Freenode *) NULL; - fl -> nodesize = size; + fl->head = (struct Freenode*)NULL; + fl->nodesize = size; } -char * VoronoiDiagramGenerator::getfree(struct Freelist *fl) +char* VoronoiDiagramGenerator::getfree(struct Freelist* fl) { - int i; - struct Freenode *t; + int i; + struct Freenode* t; - if(fl->head == (struct Freenode *) NULL) - { - t = (struct Freenode *) myalloc(sqrt_nsites * fl->nodesize); + if (fl->head == (struct Freenode*)NULL) + { + t = (struct Freenode*)myalloc(sqrt_nsites * fl->nodesize); - if(t == 0) - return 0; - - currentMemoryBlock->next = new FreeNodeArrayList; - currentMemoryBlock = currentMemoryBlock->next; - currentMemoryBlock->memory = t; - currentMemoryBlock->next = 0; + if (t == 0) + return 0; - for(i=0; inodesize), fl); - }; - t = fl -> head; - fl -> head = (fl -> head) -> nextfree; - return((char *)t); + currentMemoryBlock->next = new FreeNodeArrayList; + currentMemoryBlock = currentMemoryBlock->next; + currentMemoryBlock->memory = t; + currentMemoryBlock->next = 0; + + for (i = 0; i < sqrt_nsites; i += 1) + makefree((struct Freenode*)((char*)t + i * fl->nodesize), fl); + }; + t = fl->head; + fl->head = (fl->head)->nextfree; + return ((char*)t); } - - -void VoronoiDiagramGenerator::makefree(struct Freenode *curr,struct Freelist *fl) +void VoronoiDiagramGenerator::makefree(struct Freenode* curr, struct Freelist* fl) { - curr -> nextfree = fl -> head; - fl -> head = curr; + curr->nextfree = fl->head; + fl->head = curr; } void VoronoiDiagramGenerator::cleanup() { - if(sites != 0) - { - free(sites); - sites = 0; - } + if (sites != 0) + { + free(sites); + sites = 0; + } - FreeNodeArrayList* current=0, *prev = 0; + FreeNodeArrayList *current = 0, *prev = 0; - current = prev = allMemoryList; + current = prev = allMemoryList; - while(current->next != 0) - { - prev = current; - current = current->next; - free(prev->memory); - delete prev; - prev = 0; - } + while (current->next != 0) + { + prev = current; + current = current->next; + free(prev->memory); + delete prev; + prev = 0; + } - if(current != 0 && current->memory != 0) - { - free(current->memory); - delete current; - } + if (current != 0 && current->memory != 0) + { + free(current->memory); + delete current; + } - allMemoryList = new FreeNodeArrayList; - allMemoryList->next = 0; - allMemoryList->memory = 0; - currentMemoryBlock = allMemoryList; + allMemoryList = new FreeNodeArrayList; + allMemoryList->next = 0; + allMemoryList->memory = 0; + currentMemoryBlock = allMemoryList; } void VoronoiDiagramGenerator::cleanupEdges() { - GraphEdge* geCurrent = 0, *gePrev = 0; - geCurrent = gePrev = allEdges; + GraphEdge *geCurrent = 0, *gePrev = 0; + geCurrent = gePrev = allEdges; - while(geCurrent != 0 && geCurrent->next != 0) - { - gePrev = geCurrent; - geCurrent = geCurrent->next; - delete gePrev; - } - - allEdges = 0; + while (geCurrent != 0 && geCurrent->next != 0) + { + gePrev = geCurrent; + geCurrent = geCurrent->next; + delete gePrev; + } + allEdges = 0; } void VoronoiDiagramGenerator::pushGraphEdge(float x1, float y1, float x2, float y2, int s[2]) { - GraphEdge* newEdge = new GraphEdge; - newEdge->next = allEdges; - allEdges = newEdge; - newEdge->x1 = x1; - newEdge->y1 = y1; - newEdge->x2 = x2; - newEdge->y2 = y2; - std::copy(s, s+2, newEdge->sites); + GraphEdge* newEdge = new GraphEdge; + newEdge->next = allEdges; + allEdges = newEdge; + newEdge->x1 = x1; + newEdge->y1 = y1; + newEdge->x2 = x2; + newEdge->y2 = y2; + std::copy(s, s + 2, newEdge->sites); } - -char * VoronoiDiagramGenerator::myalloc(unsigned n) +char* VoronoiDiagramGenerator::myalloc(unsigned n) { - char *t=0; - t=(char*)malloc(n); - total_alloc += n; - return(t); + char* t = 0; + t = (char*)malloc(n); + total_alloc += n; + return (t); } - /* for those who don't have Cherry's plot */ /* #include */ -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::out_bisector(struct Edge *e) { - - + pushGraphEdge(x1, y1, x2, y2, s); } - - -void VoronoiDiagramGenerator::out_ep(struct Edge *e) +void VoronoiDiagramGenerator::circle(float x, float y, float radius) { - - } - -void VoronoiDiagramGenerator::out_vertex(struct Site *v) +void VoronoiDiagramGenerator::range(float minX, float minY, float maxX, float maxY) { - } - -void VoronoiDiagramGenerator::out_site(struct Site *s) +void VoronoiDiagramGenerator::out_bisector(struct Edge* e) { - 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::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; - - dy = ymax - ymin; - dx = xmax - xmin; - d = (float)(( dx > dy ? dx : dy) * 1.1); - pxmin = (float)(xmin - (d-dx)/2.0); - pxmax = (float)(xmax + (d-dx)/2.0); - pymin = (float)(ymin - (d-dy)/2.0); - pymax = (float)(ymax + (d-dy)/2.0); - cradius = (float)((pxmax - pxmin)/350.0); - openpl(); - range(pxmin, pymin, pxmax, pymax); + float dx, dy, d; + + dy = ymax - ymin; + dx = xmax - xmin; + d = (float)((dx > dy ? dx : dy) * 1.1); + pxmin = (float)(xmin - (d - dx) / 2.0); + pxmax = (float)(xmax + (d - dx) / 2.0); + pymin = (float)(ymin - (d - dy) / 2.0); + pymax = (float)(ymax + (d - dy) / 2.0); + cradius = (float)((pxmax - pxmin) / 350.0); + openpl(); + range(pxmin, pymin, pxmax, pymax); } -void VoronoiDiagramGenerator::clip_line(struct Edge *e) +void VoronoiDiagramGenerator::clip_line(struct Edge* e) { - struct Site *s1, *s2; - float x1=0,x2=0,y1=0,y2=0, temp = 0;; + struct Site *s1, *s2; + float x1 = 0, x2 = 0, y1 = 0, y2 = 0, temp = 0; + ; - x1 = e->reg[0]->coord.x; - x2 = e->reg[1]->coord.x; - y1 = e->reg[0]->coord.y; - y2 = e->reg[1]->coord.y; + x1 = e->reg[0]->coord.x; + x2 = e->reg[1]->coord.x; + y1 = e->reg[0]->coord.y; + y2 = e->reg[1]->coord.y; - //if the distance between the two points this line was created from is less than - //the square root of 2, then ignore it - if(sqrt(((x2 - x1) * (x2 - x1)) + ((y2 - y1) * (y2 - y1))) < minDistanceBetweenSites) - { - return; - } - pxmin = borderMinX; - pxmax = borderMaxX; - pymin = borderMinY; - pymax = borderMaxY; + // if the distance between the two points this line was created from is less than + // the square root of 2, then ignore it + if (sqrt(((x2 - x1) * (x2 - x1)) + ((y2 - y1) * (y2 - y1))) < minDistanceBetweenSites) + { + return; + } + pxmin = borderMinX; + pxmax = borderMaxX; + pymin = borderMinY; + pymax = borderMaxY; - if(e -> a == 1.0 && e ->b >= 0.0) - { - s1 = e -> ep[1]; - s2 = e -> ep[0]; - } - else - { - s1 = e -> ep[0]; - s2 = e -> ep[1]; - }; + if (e->a == 1.0 && e->b >= 0.0) + { + s1 = e->ep[1]; + s2 = e->ep[0]; + } + else + { + s1 = e->ep[0]; + s2 = e->ep[1]; + }; - if(e -> a == 1.0) - { - y1 = pymin; - if (s1!=(struct Site *)NULL && s1->coord.y > pymin) - { - y1 = s1->coord.y; - } - if(y1>pymax) - { - // printf("\nClipped (1) y1 = %f to %f",y1,pymax); - y1 = pymax; - //return; - } - x1 = e -> c - e -> b * y1; - y2 = pymax; - if (s2!=(struct Site *)NULL && s2->coord.y < pymax) - y2 = s2->coord.y; + if (e->a == 1.0) + { + y1 = pymin; + if (s1 != (struct Site*)NULL && s1->coord.y > pymin) + { + y1 = s1->coord.y; + } + if (y1 > pymax) + { + // printf("\nClipped (1) y1 = %f to %f",y1,pymax); + y1 = pymax; + // return; + } + x1 = e->c - e->b * y1; + y2 = pymax; + if (s2 != (struct Site*)NULL && s2->coord.y < pymax) + y2 = s2->coord.y; - if(y2c) - (e->b) * y2; - if (((x1> pxmax) & (x2>pxmax)) | ((x1 pxmax) - { x1 = pxmax; y1 = (e -> c - x1)/e -> b;}; - if(x1 c - x1)/e -> b;}; - if(x2>pxmax) - { x2 = pxmax; y2 = (e -> c - x2)/e -> b;}; - if(x2 c - x2)/e -> b;}; - } - else - { - x1 = pxmin; - if (s1!=(struct Site *)NULL && s1->coord.x > pxmin) - x1 = s1->coord.x; - if(x1>pxmax) - { - //printf("\nClipped (3) x1 = %f to %f",x1,pxmin); - //return; - x1 = pxmax; - } - y1 = e -> c - e -> a * x1; - x2 = pxmax; - if (s2!=(struct Site *)NULL && s2->coord.x < pxmax) - x2 = s2->coord.x; - if(x2 c - e -> a * x2; - if (((y1> pymax) & (y2>pymax)) | ((y1 pymax) - { y1 = pymax; x1 = (e -> c - y1)/e -> a;}; - if(y1 c - y1)/e -> a;}; - if(y2>pymax) - { y2 = pymax; x2 = (e -> c - y2)/e -> a;}; - if(y2 c - y2)/e -> a;}; - }; - - //printf("Pushing line (%f,%f,%f,%f)\n",x1,y1,x2,y2); - line(x1,y1,x2,y2,e->sites); + if (y2 < pymin) + { + // printf("\nClipped (2) y2 = %f to %f",y2,pymin); + y2 = pymin; + // return; + } + x2 = (e->c) - (e->b) * y2; + if (((x1 > pxmax) & (x2 > pxmax)) | ((x1 < pxmin) & (x2 < pxmin))) + { + // printf("\nClipLine jumping out(3), x1 = %f, pxmin = %f, pxmax = %f",x1,pxmin,pxmax); + return; + } + if (x1 > pxmax) + { + x1 = pxmax; + y1 = (e->c - x1) / e->b; + }; + if (x1 < pxmin) + { + x1 = pxmin; + y1 = (e->c - x1) / e->b; + }; + if (x2 > pxmax) + { + x2 = pxmax; + y2 = (e->c - x2) / e->b; + }; + if (x2 < pxmin) + { + x2 = pxmin; + y2 = (e->c - x2) / e->b; + }; + } + else + { + x1 = pxmin; + if (s1 != (struct Site*)NULL && s1->coord.x > pxmin) + x1 = s1->coord.x; + if (x1 > pxmax) + { + // printf("\nClipped (3) x1 = %f to %f",x1,pxmin); + // return; + x1 = pxmax; + } + y1 = e->c - e->a * x1; + x2 = pxmax; + if (s2 != (struct Site*)NULL && s2->coord.x < pxmax) + x2 = s2->coord.x; + if (x2 < pxmin) + { + // printf("\nClipped (4) x2 = %f to %f",x2,pxmin); + // return; + x2 = pxmin; + } + y2 = e->c - e->a * x2; + if (((y1 > pymax) & (y2 > pymax)) | ((y1 < pymin) & (y2 < pymin))) + { + // printf("\nClipLine jumping out(6), y1 = %f, pymin = %f, pymax = %f",y2,pymin,pymax); + return; + } + if (y1 > pymax) + { + y1 = pymax; + x1 = (e->c - y1) / e->a; + }; + if (y1 < pymin) + { + y1 = pymin; + x1 = (e->c - y1) / e->a; + }; + if (y2 > pymax) + { + y2 = pymax; + x2 = (e->c - y2) / e->a; + }; + if (y2 < pymin) + { + 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, @@ -862,148 +881,152 @@ deltax, and deltay too big than too small. (?) */ bool VoronoiDiagramGenerator::voronoi(int triangulate) { - struct Site *newsite, *bot, *top, *temp, *p; - struct Site *v; - struct Point newintstar; - int pm; - struct Halfedge *lbnd, *rbnd, *llbnd, *rrbnd, *bisector; - struct Edge *e; - - PQinitialize(); - bottomsite = nextone(); - out_site(bottomsite); - bool retval = ELinitialize(); + struct Site *newsite, *bot, *top, *temp, *p; + struct Site* v; + struct Point newintstar; + int pm; + struct Halfedge *lbnd, *rbnd, *llbnd, *rrbnd, *bisector; + struct Edge* e; - if(!retval) - return false; - - newsite = nextone(); - while(1) - { + PQinitialize(); + bottomsite = nextone(); + out_site(bottomsite); + bool retval = ELinitialize(); - 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 (!retval) + return false; - 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 - rbnd = ELright(lbnd); //get the first HalfEdge to the RIGHT of the new site - bot = rightreg(lbnd); //if this halfedge has no edge, , bot = bottom site (whatever that is) - e = bisect(bot, newsite); //create a new edge that bisects - bisector = HEcreate(e, le); //create a new HalfEdge, setting its ELpm field to 0 - ELinsert(lbnd, bisector); //insert this new bisector edge between the left and right vectors in a linked list - //printf("Newsite %d: %f,%f\n",newsite->sitenbr,newsite->coord.x,newsite->coord.y); + newsite = nextone(); + while (1) + { + if (!PQempty()) + newintstar = PQ_min(); - 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)); - }; - lbnd = bisector; - bisector = HEcreate(e, re); //create a new HalfEdge, setting its ELpm field to 1 - ELinsert(lbnd, bisector); //insert the new HE to the right of the original bisector earlier in the IF stmt + // if the lowest site has a smaller y value than the lowest vector intersection, process the site + // otherwise process the vector intersection - if ((p = intersect(bisector, rbnd)) != (struct Site *) NULL) //if this new bisector intersects with the - { - PQinsert(bisector, p, dist(p,newsite)); //push the HE into the ordered linked list of vertices - }; - newsite = nextone(); - } - else if (!PQempty()) /* intersection is smallest - this is a vector event */ - { - lbnd = PQextractmin(); //pop the HalfEdge with the lowest vector off the ordered list of vectors - llbnd = ELleft(lbnd); //get the HalfEdge to the left of the above HE - rbnd = ELright(lbnd); //get the HalfEdge to the right of the above HE - rrbnd = ELright(rbnd); //get the HalfEdge to the right of the HE to the right of the lowest HE - bot = leftreg(lbnd); //get the Site to the left of the left HE which it bisects - top = rightreg(rbnd); //get the Site to the right of the right HE which it bisects + 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 + rbnd = ELright(lbnd); // get the first HalfEdge to the RIGHT of the new site + bot = rightreg(lbnd); // if this halfedge has no edge, , bot = bottom site (whatever that is) + e = bisect(bot, newsite); // create a new edge that bisects + bisector = HEcreate(e, le); // create a new HalfEdge, setting its ELpm field to 0 + ELinsert(lbnd, bisector); // insert this new bisector edge between the left and right vectors in a linked list + // printf("Newsite %d: %f,%f\n",newsite->sitenbr,newsite->coord.x,newsite->coord.y); - out_triple(bot, top, rightreg(lbnd)); //output the triple of sites, stating that a circle goes through them + 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)); + }; + lbnd = bisector; + bisector = HEcreate(e, re); // create a new HalfEdge, setting its ELpm field to 1 + ELinsert(lbnd, bisector); // insert the new HE to the right of the original bisector earlier in the IF stmt - 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 - 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 - 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 - 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 - { //to the right of it, then swap them and set the 'pm' variable to 1 - temp = bot; - bot = top; - top = temp; - pm = re; - } - 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 - 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 - //Site, then this endpoint is put in position 0; otherwise in pos 1 - deref(v); //delete the vector 'v' + if ((p = intersect(bisector, rbnd)) != (struct Site*)NULL) // if this new bisector intersects with the + { + PQinsert(bisector, p, dist(p, newsite)); // push the HE into the ordered linked list of vertices + }; + newsite = nextone(); + } + else if (!PQempty()) /* intersection is smallest - this is a vector event */ + { + lbnd = PQextractmin(); // pop the HalfEdge with the lowest vector off the ordered list of vectors + llbnd = ELleft(lbnd); // get the HalfEdge to the left of the above HE + rbnd = ELright(lbnd); // get the HalfEdge to the right of the above HE + rrbnd = ELright(rbnd); // get the HalfEdge to the right of the HE to the right of the lowest HE + bot = leftreg(lbnd); // get the Site to the left of the left HE which it bisects + top = rightreg(rbnd); // get the Site to the right of the right HE which it bisects - //if left HE and the new bisector don't intersect, then delete the left HE, and reinsert it - if((p = intersect(llbnd, bisector)) != (struct Site *) NULL) - { - PQdelete(llbnd); - PQinsert(llbnd, p, dist(p,bot)); - }; + out_triple(bot, top, rightreg(lbnd)); // output the triple of sites, stating that a circle goes through them - //if right HE and the new bisector don't intersect, then reinsert it - if ((p = intersect(bisector, rrbnd)) != (struct Site *) NULL) - { - PQinsert(bisector, p, dist(p,bot)); - }; - } - else break; - }; + 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 + 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 + 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 + 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 + { // to the right of it, then swap them and set the 'pm' variable to 1 + temp = bot; + bot = top; + top = temp; + pm = re; + } + 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 + 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 + // Site, then this endpoint is put in position 0; otherwise in pos 1 + deref(v); // delete the vector 'v' + // if left HE and the new bisector don't intersect, then delete the left HE, and reinsert it + if ((p = intersect(llbnd, bisector)) != (struct Site*)NULL) + { + PQdelete(llbnd); + PQinsert(llbnd, p, dist(p, bot)); + }; - for(lbnd=ELright(ELleftend); lbnd != ELrightend; lbnd=ELright(lbnd)) - { - e = lbnd -> ELedge; + // if right HE and the new bisector don't intersect, then reinsert it + if ((p = intersect(bisector, rrbnd)) != (struct Site*)NULL) + { + PQinsert(bisector, p, dist(p, bot)); + }; + } + else + break; + }; - clip_line(e); - }; + for (lbnd = ELright(ELleftend); lbnd != ELrightend; lbnd = ELright(lbnd)) + { + e = lbnd->ELedge; - cleanup(); + clip_line(e); + }; - return true; - + cleanup(); + + return true; } - -int scomp(const void *p1,const void *p2) +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); - return(0); + 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); + return (0); } /* return a single in-storage site */ -struct Site * VoronoiDiagramGenerator::nextone() +struct Site* VoronoiDiagramGenerator::nextone() { - struct Site *s; - if(siteidx < nsites) - { - s = &sites[siteidx]; - siteidx += 1; - return(s); - } - else - return( (struct Site *)NULL); + struct Site* s; + if (siteidx < nsites) + { + s = &sites[siteidx]; + siteidx += 1; + return (s); + } + else + return ((struct Site*)NULL); } - diff --git a/src/buzz_update.cpp b/src/buzz_update.cpp index 8624c3a..72d5787 100644 --- a/src/buzz_update.cpp +++ b/src/buzz_update.cpp @@ -8,626 +8,622 @@ #include "buzz_update.h" -namespace buzz_update{ - /*Temp for data collection*/ - // static int neigh=-1; - static struct timeval t1, t2; - static int timer_steps = 0; - // static clock_t end; +namespace buzz_update +{ +/*Temp for data collection*/ +// static int neigh=-1; +static struct timeval t1, t2; +static int timer_steps = 0; +// static clock_t end; - /*Temp end */ - static int fd, wd = 0; - static int old_update = 0; - static buzz_updater_elem_t updater; - static int no_of_robot; - static const char* dbgf_name; - static const char* bcfname; - static const char* old_bcfname = "old_bcode.bo"; - static const char* bzz_file; - static int Robot_id = 0; - static int neigh = -1; - static int updater_msg_ready; - static uint16_t update_try_counter = 0; - static const uint16_t MAX_UPDATE_TRY = 5; - static size_t old_byte_code_size = 0; - static bool debug = false; +/*Temp end */ +static int fd, wd = 0; +static int old_update = 0; +static buzz_updater_elem_t updater; +static int no_of_robot; +static const char* dbgf_name; +static const char* bcfname; +static const char* old_bcfname = "old_bcode.bo"; +static const char* bzz_file; +static int Robot_id = 0; +static int neigh = -1; +static int updater_msg_ready; +static uint16_t update_try_counter = 0; +static const uint16_t MAX_UPDATE_TRY = 5; +static size_t old_byte_code_size = 0; +static bool debug = false; - /*Initialize updater*/ - int init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id) +/*Initialize updater*/ +int init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id) +{ + buzzvm_t VM = buzz_utility::get_vm(); + 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) { - buzzvm_t VM = buzz_utility::get_vm(); - 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){ - 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..."); - fd = inotify_init1(IN_NONBLOCK); - if (fd < 0) - { - perror("inotify_init error"); - } - /*If simulation set the file monitor only for robot 0*/ - if (SIMULATION == 1 && robot_id == 0) - { - /* watch /.bzz file for any activity and report it back to update */ - wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS); - } - else if (SIMULATION == 0) - { - /* watch /.bzz file for any activity and report it back to update */ - wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS); - } - /*load the .bo under execution into the updater*/ - uint8_t* BO_BUF = 0; - FILE* fp = fopen(bo_filename, "rb"); - if (!fp) - { - perror(bo_filename); - } - fseek(fp, 0, SEEK_END); - size_t bcode_size = ftell(fp); - rewind(fp); - BO_BUF = (uint8_t*)malloc(bcode_size); - if (fread(BO_BUF, 1, bcode_size, fp) < bcode_size) - { - perror(bo_filename); - // fclose(fp); - // return 0; - } - fclose(fp); - /*Load stand_by .bo file into the updater*/ - uint8_t* STD_BO_BUF = 0; - fp = fopen(stand_by_script, "rb"); - if (!fp) - { - perror(stand_by_script); - } - fseek(fp, 0, SEEK_END); - size_t stdby_bcode_size = ftell(fp); - rewind(fp); - STD_BO_BUF = (uint8_t*)malloc(stdby_bcode_size); - if (fread(STD_BO_BUF, 1, stdby_bcode_size, fp) < stdby_bcode_size) - { - perror(stand_by_script); - // fclose(fp); - // return 0; - } - fclose(fp); - /*Create the updater*/ - updater = (buzz_updater_elem_t)malloc(sizeof(struct buzz_updater_elem_s)); - /*Intialize the updater with the required data*/ - updater->bcode = BO_BUF; - /*Store a copy of the Bcode for rollback*/ - updater->outmsg_queue = NULL; - updater->inmsg_queue = NULL; - updater->patch = NULL; - updater->patch_size = (size_t*)malloc(sizeof(size_t)); - updater->bcode_size = (size_t*)malloc(sizeof(size_t)); - updater->update_no = (uint8_t*)malloc(sizeof(uint16_t)); - *(uint16_t*)(updater->update_no) = 0; - *(size_t*)(updater->bcode_size) = bcode_size; - updater->standby_bcode = STD_BO_BUF; - updater->standby_bcode_size = (size_t*)malloc(sizeof(size_t)); - *(size_t*)(updater->standby_bcode_size) = stdby_bcode_size; - updater->mode = (int*)malloc(sizeof(int)); - *(int*)(updater->mode) = CODE_RUNNING; - // no_of_robot=barrier; - updater_msg_ready = 0; - - /*Write the bcode to a file for rollback*/ - fp = fopen("old_bcode.bo", "wb"); - fwrite((updater->bcode), *(size_t*)updater->bcode_size, 1, fp); - fclose(fp); - return 1; - } - else{ - ROS_WARN("UPDATES TURNED OFF... (Hint: Include update.bzz to turn on updates)"); - return 0; - } - } - /*Check for .bzz file chages*/ - int check_update() - { - char buf[1024]; - int check = 0; - int i = 0; - int len = read(fd, buf, 1024); - while (i < len) + 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..."); + fd = inotify_init1(IN_NONBLOCK); + if (fd < 0) { - struct inotify_event* event = (struct inotify_event*)&buf[i]; - /*Report file changes and self deletes*/ - if (event->mask & (IN_MODIFY | IN_DELETE_SELF)) - { - /*Respawn watch if the file is self deleted */ - inotify_rm_watch(fd, wd); - close(fd); - fd = inotify_init1(IN_NONBLOCK); - wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS); - /*To mask multiple writes from editors*/ - if (!old_update) - { - check = 1; - old_update = 1; - } - } - /*Update index to start of next event*/ - i += sizeof(struct inotify_event) + event->len; + perror("inotify_init error"); } - if (!check) - old_update = 0; - return check; - } - - int test_patch(std::string& path, std::string& name1, size_t update_patch_size, uint8_t* patch) - { - if (SIMULATION == 1) + /*If simulation set the file monitor only for robot 0*/ + if (SIMULATION == 1 && robot_id == 0) { - return 1; + /* watch /.bzz file for any activity and report it back to update */ + wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS); } - else + else if (SIMULATION == 0) { - /*Patch the old bo with new patch*/ - std::stringstream patch_writefile; - patch_writefile << path << name1 << "tmp_patch.bo"; - /*Write the patch to a file and call bsdiff to patch*/ - FILE* fp = fopen(patch_writefile.str().c_str(), "wb"); - fwrite(patch, update_patch_size, 1, fp); - fclose(fp); - 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 (system(patch_exsisting.str().c_str())) - return 0; - else - return 1; + /* watch /.bzz file for any activity and report it back to update */ + wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS); } - } - - updater_code_t obtain_patched_bo(std::string& path, std::string& name1) - { - if (SIMULATION == 1) - { - /*Read the exsisting file to simulate the patching*/ - std::stringstream read_patched; - read_patched << path << name1 << ".bo"; - if(debug){ - ROS_WARN("Simulation patching ..."); - ROS_WARN("Read file for patching %s", read_patched.str().c_str()); - } - uint8_t* patched_bo_buf = 0; - FILE* fp = fopen(read_patched.str().c_str(), "rb"); - if (!fp) - { - perror(read_patched.str().c_str()); - } - fseek(fp, 0, SEEK_END); - size_t patched_size = ftell(fp); - rewind(fp); - patched_bo_buf = (uint8_t*)malloc(patched_size); - if (fread(patched_bo_buf, 1, patched_size, fp) < patched_size) - { - perror(read_patched.str().c_str()); - } - fclose(fp); - /*Write the patched to a code struct and return*/ - updater_code_t update_code = (updater_code_t)malloc(sizeof(struct updater_code_s)); - update_code->bcode = patched_bo_buf; - update_code->bcode_size = (uint8_t*)malloc(sizeof(uint16_t)); - *(uint16_t*)(update_code->bcode_size) = patched_size; - - return update_code; - } - - else - { - /*Read the new patched file*/ - std::stringstream read_patched; - read_patched << path << name1 << "-patched.bo"; - if(debug) { - ROS_WARN("Robot patching ..."); - ROS_WARN("Read file for patching %s", read_patched.str().c_str()); - } - uint8_t* patched_bo_buf = 0; - FILE* fp = fopen(read_patched.str().c_str(), "rb"); - if (!fp) - { - perror(read_patched.str().c_str()); - } - fseek(fp, 0, SEEK_END); - size_t patched_size = ftell(fp); - rewind(fp); - patched_bo_buf = (uint8_t*)malloc(patched_size); - if (fread(patched_bo_buf, 1, patched_size, fp) < patched_size) - { - perror(read_patched.str().c_str()); - } - fclose(fp); - - /* delete old bo file & rename new bo file */ - remove((path + name1 + ".bo").c_str()); - rename((path + name1 + "-patched.bo").c_str(), (path + name1 + ".bo").c_str()); - - /*Write the patched to a code struct and return*/ - updater_code_t update_code = (updater_code_t)malloc(sizeof(struct updater_code_s)); - update_code->bcode = patched_bo_buf; - update_code->bcode_size = (uint8_t*)malloc(sizeof(uint16_t)); - *(uint16_t*)(update_code->bcode_size) = patched_size; - - return update_code; - } - } - - void code_message_outqueue_append() - { - updater->outmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); - /* if size less than 250 Pad with zeors to assure transmission*/ - uint16_t size = UPDATE_CODE_HEADER_SIZE + *(size_t*)(updater->patch_size); - uint16_t padding_size = (size > MIN_UPDATE_PACKET) ? 0 : MIN_UPDATE_PACKET - size; - size += padding_size; - updater->outmsg_queue->queue = (uint8_t*)malloc(size); - memset(updater->outmsg_queue->queue, 0, size); - updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); - *(uint16_t*)(updater->outmsg_queue->size) = size; - size = 0; - /*Append message type*/ - *(uint8_t*)(updater->outmsg_queue->queue + size) = SENT_CODE; - size += sizeof(uint8_t); - /*Append the update no, code size and code to out msg*/ - *(uint16_t*)(updater->outmsg_queue->queue + size) = *(uint16_t*)(updater->update_no); - size += sizeof(uint16_t); - *(uint16_t*)(updater->outmsg_queue->queue + size) = (uint16_t) * (size_t*)(updater->patch_size); - size += sizeof(uint16_t); - memcpy(updater->outmsg_queue->queue + size, updater->patch, *(size_t*)(updater->patch_size)); - // size += (uint16_t) * (size_t*)(updater->patch_size); - updater_msg_ready = 1; - } - - void outqueue_append_code_request(uint16_t update_no) - { - updater->outmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); - uint16_t size = 0; - updater->outmsg_queue->queue = (uint8_t*)malloc(2 * sizeof(uint16_t) + sizeof(uint8_t) + CODE_REQUEST_PADDING); - updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); - memset(updater->outmsg_queue->queue, 0, sizeof(uint16_t) + sizeof(uint8_t) + CODE_REQUEST_PADDING); - /*Append message type*/ - *(uint8_t*)(updater->outmsg_queue->queue + size) = RESEND_CODE; - size += sizeof(uint8_t); - /*Append the update no, code size and code to out msg*/ - *(uint16_t*)(updater->outmsg_queue->queue + size) = update_no; - size += sizeof(uint16_t); - *(uint16_t*)(updater->outmsg_queue->queue + size) = update_try_counter; - 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); - } - - void code_message_inqueue_append(uint8_t* msg, uint16_t size) - { - updater->inmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); - // ROS_INFO("[DEBUG] Updater append code of size %d", (int) size); - updater->inmsg_queue->queue = (uint8_t*)malloc(size); - updater->inmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); - memcpy(updater->inmsg_queue->queue, msg, size); - *(uint16_t*)(updater->inmsg_queue->size) = size; - } - /*Used for data logging*/ - /*void set_packet_id(int packet_id) - { - packet_id_ = packet_id; - }*/ - void code_message_inqueue_process() - { - int size = 0; - updater_code_t out_code = NULL; - 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)))); - ROS_WARN("[Debug] Updater received patch of size %u", - (*(uint16_t*)(updater->inmsg_queue->queue + sizeof(uint16_t) + sizeof(uint8_t)))); - } - if (*(int*)(updater->mode) == CODE_RUNNING) - { - // fprintf(stdout,"[debug]Inside inmsg code running"); - if (*(uint8_t*)(updater->inmsg_queue->queue) == SENT_CODE) - { - size += sizeof(uint8_t); - if (*(uint16_t*)(updater->inmsg_queue->queue + size) > *(uint16_t*)(updater->update_no)) - { - // fprintf(stdout,"[debug]Inside update number comparision"); - uint16_t update_no = *(uint16_t*)(updater->inmsg_queue->queue + size); - size += sizeof(uint16_t); - uint16_t update_patch_size = *(uint16_t*)(updater->inmsg_queue->queue + size); - size += sizeof(uint16_t); - /*Generate patch*/ - std::string bzzfile_name(bzz_file); - std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; - std::string name1 = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); - name1 = name1.substr(0, name1.find_last_of(".")); - if (test_patch(path, name1, update_patch_size, (updater->inmsg_queue->queue + size))) - { - out_code = obtain_patched_bo(path, name1); - - // fprintf(stdout,"in queue process Update no %d\n", (int) update_no); - // fprintf(stdout,"in queue process bcode size %d\n", (int) update_bcode_size); - // FILE *fp; - // fp=fopen("update.bo", "wb"); - // fwrite((updater->inmsg_queue->queue+size), update_bcode_size, 1, fp); - // fclose(fp); - - 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!"); - *(uint16_t*)updater->update_no = update_no; - /*Clear exisiting patch if any*/ - delete_p(updater->patch); - /*copy the patch into the updater*/ - updater->patch = (uint8_t*)malloc(update_patch_size); - memcpy(updater->patch, (updater->inmsg_queue->queue + size), update_patch_size); - *(size_t*)(updater->patch_size) = update_patch_size; - // code_message_outqueue_append(); - neigh = 1; - } - /*clear the temp code buff*/ - delete_p(out_code->bcode); - delete_p(out_code->bcode_size); - delete_p(out_code); - } - else - { - ROS_ERROR("Patching the .bo file failed"); - update_try_counter++; - outqueue_append_code_request(update_no); - } - } - } - } - size = 0; - if (*(uint8_t*)(updater->inmsg_queue->queue) == RESEND_CODE) - { - 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)) - { - size += sizeof(uint16_t); - 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); - code_message_outqueue_append(); - } - if (update_try_counter > MAX_UPDATE_TRY) - ROS_ERROR("TODO: Max rebroadcast retry reached, ROLL BACK !!"); - } - } - // fprintf(stdout,"in queue freed\n"); - delete_p(updater->inmsg_queue->queue); - delete_p(updater->inmsg_queue->size); - delete_p(updater->inmsg_queue); - } - - void create_update_patch() - { - std::stringstream genpatch; - - std::string bzzfile_name(bzz_file); - - std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; - - std::string name1 = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); - name1 = name1.substr(0, name1.find_last_of(".")); - - std::string name2 = name1 + "-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()); - system(genpatch.str().c_str()); - - /*Delete old files & rename new files*/ - - remove((path + name1 + ".bo").c_str()); - remove((path + name1 + ".bdb").c_str()); - - rename((path + name2 + ".bo").c_str(), (path + name1 + ".bo").c_str()); - rename((path + name2 + ".bdb").c_str(), (path + name1 + ".bdb").c_str()); - - /*Read the diff file */ - std::stringstream patchfileName; - patchfileName << path << "diff.bo"; - - uint8_t* patch_buff = 0; - FILE* fp = fopen(patchfileName.str().c_str(), "rb"); + /*load the .bo under execution into the updater*/ + uint8_t* BO_BUF = 0; + FILE* fp = fopen(bo_filename, "rb"); if (!fp) { - perror(patchfileName.str().c_str()); + perror(bo_filename); } fseek(fp, 0, SEEK_END); - size_t patch_size = ftell(fp); + size_t bcode_size = ftell(fp); rewind(fp); - patch_buff = (uint8_t*)malloc(patch_size); - if (fread(patch_buff, 1, patch_size, fp) < patch_size) + BO_BUF = (uint8_t*)malloc(bcode_size); + if (fread(BO_BUF, 1, bcode_size, fp) < bcode_size) { - perror(patchfileName.str().c_str()); + perror(bo_filename); + // fclose(fp); + // return 0; } fclose(fp); - delete_p(updater->patch); - updater->patch = patch_buff; - *(size_t*)(updater->patch_size) = patch_size; + /*Load stand_by .bo file into the updater*/ + uint8_t* STD_BO_BUF = 0; + fp = fopen(stand_by_script, "rb"); + if (!fp) + { + perror(stand_by_script); + } + fseek(fp, 0, SEEK_END); + size_t stdby_bcode_size = ftell(fp); + rewind(fp); + STD_BO_BUF = (uint8_t*)malloc(stdby_bcode_size); + if (fread(STD_BO_BUF, 1, stdby_bcode_size, fp) < stdby_bcode_size) + { + perror(stand_by_script); + // fclose(fp); + // return 0; + } + fclose(fp); + /*Create the updater*/ + updater = (buzz_updater_elem_t)malloc(sizeof(struct buzz_updater_elem_s)); + /*Intialize the updater with the required data*/ + updater->bcode = BO_BUF; + /*Store a copy of the Bcode for rollback*/ + updater->outmsg_queue = NULL; + updater->inmsg_queue = NULL; + updater->patch = NULL; + updater->patch_size = (size_t*)malloc(sizeof(size_t)); + updater->bcode_size = (size_t*)malloc(sizeof(size_t)); + updater->update_no = (uint8_t*)malloc(sizeof(uint16_t)); + *(uint16_t*)(updater->update_no) = 0; + *(size_t*)(updater->bcode_size) = bcode_size; + updater->standby_bcode = STD_BO_BUF; + updater->standby_bcode_size = (size_t*)malloc(sizeof(size_t)); + *(size_t*)(updater->standby_bcode_size) = stdby_bcode_size; + updater->mode = (int*)malloc(sizeof(int)); + *(int*)(updater->mode) = CODE_RUNNING; + // no_of_robot=barrier; + updater_msg_ready = 0; - /* Delete the diff file */ - remove(patchfileName.str().c_str()); + /*Write the bcode to a file for rollback*/ + fp = fopen("old_bcode.bo", "wb"); + fwrite((updater->bcode), *(size_t*)updater->bcode_size, 1, fp); + fclose(fp); + return 1; + } + else + { + ROS_WARN("UPDATES TURNED OFF... (Hint: Include update.bzz to turn on updates)"); + return 0; + } +} +/*Check for .bzz file chages*/ +int check_update() +{ + char buf[1024]; + int check = 0; + int i = 0; + int len = read(fd, buf, 1024); + while (i < len) + { + struct inotify_event* event = (struct inotify_event*)&buf[i]; + /*Report file changes and self deletes*/ + if (event->mask & (IN_MODIFY | IN_DELETE_SELF)) + { + /*Respawn watch if the file is self deleted */ + inotify_rm_watch(fd, wd); + close(fd); + fd = inotify_init1(IN_NONBLOCK); + wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS); + /*To mask multiple writes from editors*/ + if (!old_update) + { + check = 1; + old_update = 1; + } + } + /*Update index to start of next event*/ + i += sizeof(struct inotify_event) + event->len; + } + if (!check) + old_update = 0; + return check; +} + +int test_patch(std::string& path, std::string& name1, size_t update_patch_size, uint8_t* patch) +{ + if (SIMULATION == 1) + { + return 1; + } + else + { + /*Patch the old bo with new patch*/ + std::stringstream patch_writefile; + patch_writefile << path << name1 << "tmp_patch.bo"; + /*Write the patch to a file and call bsdiff to patch*/ + FILE* fp = fopen(patch_writefile.str().c_str(), "wb"); + fwrite(patch, update_patch_size, 1, fp); + fclose(fp); + 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 (system(patch_exsisting.str().c_str())) + return 0; + else + return 1; + } +} + +updater_code_t obtain_patched_bo(std::string& path, std::string& name1) +{ + if (SIMULATION == 1) + { + /*Read the exsisting file to simulate the patching*/ + std::stringstream read_patched; + read_patched << path << name1 << ".bo"; + if (debug) + { + ROS_WARN("Simulation patching ..."); + ROS_WARN("Read file for patching %s", read_patched.str().c_str()); + } + uint8_t* patched_bo_buf = 0; + FILE* fp = fopen(read_patched.str().c_str(), "rb"); + if (!fp) + { + perror(read_patched.str().c_str()); + } + fseek(fp, 0, SEEK_END); + size_t patched_size = ftell(fp); + rewind(fp); + patched_bo_buf = (uint8_t*)malloc(patched_size); + if (fread(patched_bo_buf, 1, patched_size, fp) < patched_size) + { + perror(read_patched.str().c_str()); + } + fclose(fp); + /*Write the patched to a code struct and return*/ + updater_code_t update_code = (updater_code_t)malloc(sizeof(struct updater_code_s)); + update_code->bcode = patched_bo_buf; + update_code->bcode_size = (uint8_t*)malloc(sizeof(uint16_t)); + *(uint16_t*)(update_code->bcode_size) = patched_size; + + return update_code; } - void update_routine() + else { - buzzvm_t VM = buzz_utility::get_vm(); - buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); - buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); - buzzvm_gstore(VM); - // fprintf(stdout,"[Debug : ]updater value = %i \n",updater->mode); - if (*(int*)updater->mode == CODE_RUNNING) + /*Read the new patched file*/ + std::stringstream read_patched; + read_patched << path << name1 << "-patched.bo"; + if (debug) { - buzzvm_function_call(VM, "updated_no_bct", 0); - // Check for update - if (check_update()) - { - ROS_INFO("Update found \tUpdating script ..."); + ROS_WARN("Robot patching ..."); + ROS_WARN("Read file for patching %s", read_patched.str().c_str()); + } + uint8_t* patched_bo_buf = 0; + FILE* fp = fopen(read_patched.str().c_str(), "rb"); + if (!fp) + { + perror(read_patched.str().c_str()); + } + fseek(fp, 0, SEEK_END); + size_t patched_size = ftell(fp); + rewind(fp); + patched_bo_buf = (uint8_t*)malloc(patched_size); + if (fread(patched_bo_buf, 1, patched_size, fp) < patched_size) + { + perror(read_patched.str().c_str()); + } + fclose(fp); - if (compile_bzz(bzz_file)) + /* delete old bo file & rename new bo file */ + remove((path + name1 + ".bo").c_str()); + rename((path + name1 + "-patched.bo").c_str(), (path + name1 + ".bo").c_str()); + + /*Write the patched to a code struct and return*/ + updater_code_t update_code = (updater_code_t)malloc(sizeof(struct updater_code_s)); + update_code->bcode = patched_bo_buf; + update_code->bcode_size = (uint8_t*)malloc(sizeof(uint16_t)); + *(uint16_t*)(update_code->bcode_size) = patched_size; + + return update_code; + } +} + +void code_message_outqueue_append() +{ + updater->outmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); + /* if size less than 250 Pad with zeors to assure transmission*/ + uint16_t size = UPDATE_CODE_HEADER_SIZE + *(size_t*)(updater->patch_size); + uint16_t padding_size = (size > MIN_UPDATE_PACKET) ? 0 : MIN_UPDATE_PACKET - size; + size += padding_size; + updater->outmsg_queue->queue = (uint8_t*)malloc(size); + memset(updater->outmsg_queue->queue, 0, size); + updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); + *(uint16_t*)(updater->outmsg_queue->size) = size; + size = 0; + /*Append message type*/ + *(uint8_t*)(updater->outmsg_queue->queue + size) = SENT_CODE; + size += sizeof(uint8_t); + /*Append the update no, code size and code to out msg*/ + *(uint16_t*)(updater->outmsg_queue->queue + size) = *(uint16_t*)(updater->update_no); + size += sizeof(uint16_t); + *(uint16_t*)(updater->outmsg_queue->queue + size) = (uint16_t) * (size_t*)(updater->patch_size); + size += sizeof(uint16_t); + memcpy(updater->outmsg_queue->queue + size, updater->patch, *(size_t*)(updater->patch_size)); + // size += (uint16_t) * (size_t*)(updater->patch_size); + updater_msg_ready = 1; +} + +void outqueue_append_code_request(uint16_t update_no) +{ + updater->outmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); + uint16_t size = 0; + updater->outmsg_queue->queue = (uint8_t*)malloc(2 * sizeof(uint16_t) + sizeof(uint8_t) + CODE_REQUEST_PADDING); + updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); + memset(updater->outmsg_queue->queue, 0, sizeof(uint16_t) + sizeof(uint8_t) + CODE_REQUEST_PADDING); + /*Append message type*/ + *(uint8_t*)(updater->outmsg_queue->queue + size) = RESEND_CODE; + size += sizeof(uint8_t); + /*Append the update no, code size and code to out msg*/ + *(uint16_t*)(updater->outmsg_queue->queue + size) = update_no; + size += sizeof(uint16_t); + *(uint16_t*)(updater->outmsg_queue->queue + size) = update_try_counter; + 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); +} + +void code_message_inqueue_append(uint8_t* msg, uint16_t size) +{ + updater->inmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); + // ROS_INFO("[DEBUG] Updater append code of size %d", (int) size); + updater->inmsg_queue->queue = (uint8_t*)malloc(size); + updater->inmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); + memcpy(updater->inmsg_queue->queue, msg, size); + *(uint16_t*)(updater->inmsg_queue->size) = size; +} +/*Used for data logging*/ +/*void set_packet_id(int packet_id) +{ + packet_id_ = packet_id; +}*/ +void code_message_inqueue_process() +{ + int size = 0; + updater_code_t out_code = NULL; + 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)))); + ROS_WARN("[Debug] Updater received patch of size %u", + (*(uint16_t*)(updater->inmsg_queue->queue + sizeof(uint16_t) + sizeof(uint8_t)))); + } + if (*(int*)(updater->mode) == CODE_RUNNING) + { + // fprintf(stdout,"[debug]Inside inmsg code running"); + if (*(uint8_t*)(updater->inmsg_queue->queue) == SENT_CODE) + { + size += sizeof(uint8_t); + if (*(uint16_t*)(updater->inmsg_queue->queue + size) > *(uint16_t*)(updater->update_no)) + { + // fprintf(stdout,"[debug]Inside update number comparision"); + uint16_t update_no = *(uint16_t*)(updater->inmsg_queue->queue + size); + size += sizeof(uint16_t); + uint16_t update_patch_size = *(uint16_t*)(updater->inmsg_queue->queue + size); + size += sizeof(uint16_t); + /*Generate patch*/ + std::string bzzfile_name(bzz_file); + std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; + std::string name1 = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); + name1 = name1.substr(0, name1.find_last_of(".")); + if (test_patch(path, name1, update_patch_size, (updater->inmsg_queue->queue + size))) { - ROS_ERROR("Error in compiling script, resuming old script."); + out_code = obtain_patched_bo(path, name1); + + // fprintf(stdout,"in queue process Update no %d\n", (int) update_no); + // fprintf(stdout,"in queue process bcode size %d\n", (int) update_bcode_size); + // FILE *fp; + // fp=fopen("update.bo", "wb"); + // fwrite((updater->inmsg_queue->queue+size), update_bcode_size, 1, fp); + // fclose(fp); + + 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!"); + *(uint16_t*)updater->update_no = update_no; + /*Clear exisiting patch if any*/ + delete_p(updater->patch); + /*copy the patch into the updater*/ + updater->patch = (uint8_t*)malloc(update_patch_size); + memcpy(updater->patch, (updater->inmsg_queue->queue + size), update_patch_size); + *(size_t*)(updater->patch_size) = update_patch_size; + // code_message_outqueue_append(); + neigh = 1; + } + /*clear the temp code buff*/ + delete_p(out_code->bcode); + delete_p(out_code->bcode_size); + delete_p(out_code); } else { - std::string bzzfile_name(bzz_file); - stringstream bzzfile_in_compile; - std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; - std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); - name = name.substr(0, name.find_last_of(".")); - bzzfile_in_compile << path << name << "-update.bo"; - uint8_t* BO_BUF = 0; - FILE* fp = fopen(bzzfile_in_compile.str().c_str(), "rb"); - if (!fp) - { - perror(bzzfile_in_compile.str().c_str()); - } - fseek(fp, 0, SEEK_END); - size_t bcode_size = ftell(fp); - rewind(fp); - BO_BUF = (uint8_t*)malloc(bcode_size); - if (fread(BO_BUF, 1, bcode_size, fp) < bcode_size) - { - perror(bcfname); - } - fclose(fp); - if (test_set_code(BO_BUF, dbgf_name, bcode_size)) - { - uint16_t update_no = *(uint16_t*)(updater->update_no); - *(uint16_t*)(updater->update_no) = update_no + 1; - - create_update_patch(); - VM = buzz_utility::get_vm(); - 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 ..."); - code_message_outqueue_append(); - } - delete_p(BO_BUF); + ROS_ERROR("Patching the .bo file failed"); + update_try_counter++; + outqueue_append_code_request(update_no); } } } - - else + } + size = 0; + if (*(uint8_t*)(updater->inmsg_queue->queue) == RESEND_CODE) + { + 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)) { - timer_steps++; - buzzvm_pushs(VM, buzzvm_string_register(VM, "barrier_val", 1)); - buzzvm_gload(VM); - buzzobj_t tObj = buzzvm_stack_at(VM, 1); - buzzvm_pop(VM); - ROS_INFO("Barrier ............. No. of robots deployed: %i", tObj->i.value); - if (tObj->i.value == no_of_robot) + size += sizeof(uint16_t); + if (*(uint16_t*)(updater->inmsg_queue->queue + size) > update_try_counter) { - ROS_WARN("Patch deployment successful, rolling forward"); - *(int*)(updater->mode) = CODE_RUNNING; - gettimeofday(&t2, NULL); - // collect_data(); - buzz_utility::buzz_update_set((updater)->bcode, (char*)dbgf_name, *(size_t*)(updater->bcode_size)); - // buzzvm_function_call(m_tBuzzVM, "updated", 0); - update_try_counter = 0; - timer_steps = 0; - } - else if (timer_steps > TIMEOUT_FOR_ROLLBACK) - { - ROS_ERROR("TIME OUT Reached, rolling back"); - /*Time out hit deceided to roll back*/ - *(int*)(updater->mode) = CODE_RUNNING; - buzz_utility::buzz_script_set(old_bcfname, dbgf_name, (int)VM->robot); - update_try_counter = 0; - timer_steps = 0; + update_try_counter = *(uint16_t*)(updater->inmsg_queue->queue + size); + if (debug) + ROS_WARN("Rebroadcasting patch, try : %u", update_try_counter); + code_message_outqueue_append(); } + if (update_try_counter > MAX_UPDATE_TRY) + ROS_ERROR("TODO: Max rebroadcast retry reached, ROLL BACK !!"); } } + // fprintf(stdout,"in queue freed\n"); + delete_p(updater->inmsg_queue->queue); + delete_p(updater->inmsg_queue->size); + delete_p(updater->inmsg_queue); +} - uint8_t* getupdater_out_msg() +void create_update_patch() +{ + std::stringstream genpatch; + + std::string bzzfile_name(bzz_file); + + std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; + + std::string name1 = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); + name1 = name1.substr(0, name1.find_last_of(".")); + + std::string name2 = name1 + "-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()); + system(genpatch.str().c_str()); + + /*Delete old files & rename new files*/ + + remove((path + name1 + ".bo").c_str()); + remove((path + name1 + ".bdb").c_str()); + + rename((path + name2 + ".bo").c_str(), (path + name1 + ".bo").c_str()); + rename((path + name2 + ".bdb").c_str(), (path + name1 + ".bdb").c_str()); + + /*Read the diff file */ + std::stringstream patchfileName; + patchfileName << path << "diff.bo"; + + uint8_t* patch_buff = 0; + FILE* fp = fopen(patchfileName.str().c_str(), "rb"); + if (!fp) { - return (uint8_t*)updater->outmsg_queue->queue; + perror(patchfileName.str().c_str()); } - - uint8_t* getupdate_out_msg_size() + fseek(fp, 0, SEEK_END); + size_t patch_size = ftell(fp); + rewind(fp); + patch_buff = (uint8_t*)malloc(patch_size); + if (fread(patch_buff, 1, patch_size, fp) < patch_size) { - // fprintf(stdout,"[Debug from get out size in util: ]size = %i \n",*(uint16_t*)updater->outmsg_queue->size); - return (uint8_t*)updater->outmsg_queue->size; + perror(patchfileName.str().c_str()); } + fclose(fp); + delete_p(updater->patch); + updater->patch = patch_buff; + *(size_t*)(updater->patch_size) = patch_size; - int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size) + /* Delete the diff file */ + remove(patchfileName.str().c_str()); +} + +void update_routine() +{ + buzzvm_t VM = buzz_utility::get_vm(); + buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); + buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); + buzzvm_gstore(VM); + // fprintf(stdout,"[Debug : ]updater value = %i \n",updater->mode); + if (*(int*)updater->mode == CODE_RUNNING) { - if (buzz_utility::buzz_update_init_test(BO_BUF, dbgfname, bcode_size)) + buzzvm_function_call(VM, "updated_no_bct", 0); + // Check for update + if (check_update()) { - if(debug) ROS_WARN("Initializtion test passed"); - if (buzz_utility::update_step_test()) + ROS_INFO("Update found \tUpdating script ..."); + + if (compile_bzz(bzz_file)) { - /*data logging*/ - old_byte_code_size = *(size_t*)updater->bcode_size; - /*data logging*/ - if(debug) ROS_WARN("Step test passed"); - *(int*)(updater->mode) = CODE_STANDBY; - // fprintf(stdout,"updater value = %i\n",updater->mode); - delete_p(updater->bcode); - updater->bcode = (uint8_t*)malloc(bcode_size); - memcpy(updater->bcode, BO_BUF, bcode_size); - *(size_t*)updater->bcode_size = bcode_size; - buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname, - (size_t) * (size_t*)(updater->standby_bcode_size)); - buzzvm_t VM = buzz_utility::get_vm(); - gettimeofday(&t1, NULL); - buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); - buzzvm_pushi(VM, no_of_robot); - buzzvm_gstore(VM); - return 1; + ROS_ERROR("Error in compiling script, resuming old script."); } - /*Unable to step something wrong*/ else { - if (*(int*)(updater->mode) == CODE_RUNNING) + std::string bzzfile_name(bzz_file); + stringstream bzzfile_in_compile; + std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; + std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); + name = name.substr(0, name.find_last_of(".")); + bzzfile_in_compile << path << name << "-update.bo"; + uint8_t* BO_BUF = 0; + FILE* fp = fopen(bzzfile_in_compile.str().c_str(), "rb"); + if (!fp) { - ROS_ERROR("Step test failed, resuming old script"); - buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (size_t) * (size_t*)(updater->bcode_size)); + perror(bzzfile_in_compile.str().c_str()); } - else + fseek(fp, 0, SEEK_END); + size_t bcode_size = ftell(fp); + rewind(fp); + BO_BUF = (uint8_t*)malloc(bcode_size); + if (fread(BO_BUF, 1, bcode_size, fp) < bcode_size) { - /*You will never reach here*/ - ROS_ERROR("Step test failed, returning to standby"); - buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname, - (size_t) * (size_t*)(updater->standby_bcode_size)); - buzzvm_t VM = buzz_utility::get_vm(); - buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); - buzzvm_pushi(VM, no_of_robot); + perror(bcfname); + } + fclose(fp); + if (test_set_code(BO_BUF, dbgf_name, bcode_size)) + { + uint16_t update_no = *(uint16_t*)(updater->update_no); + *(uint16_t*)(updater->update_no) = update_no + 1; + + create_update_patch(); + VM = buzz_utility::get_vm(); + 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 ..."); + code_message_outqueue_append(); } - return 0; + delete_p(BO_BUF); } } + } + + else + { + timer_steps++; + buzzvm_pushs(VM, buzzvm_string_register(VM, "barrier_val", 1)); + buzzvm_gload(VM); + buzzobj_t tObj = buzzvm_stack_at(VM, 1); + buzzvm_pop(VM); + ROS_INFO("Barrier ............. No. of robots deployed: %i", tObj->i.value); + if (tObj->i.value == no_of_robot) + { + ROS_WARN("Patch deployment successful, rolling forward"); + *(int*)(updater->mode) = CODE_RUNNING; + gettimeofday(&t2, NULL); + // collect_data(); + buzz_utility::buzz_update_set((updater)->bcode, (char*)dbgf_name, *(size_t*)(updater->bcode_size)); + // buzzvm_function_call(m_tBuzzVM, "updated", 0); + update_try_counter = 0; + timer_steps = 0; + } + else if (timer_steps > TIMEOUT_FOR_ROLLBACK) + { + ROS_ERROR("TIME OUT Reached, rolling back"); + /*Time out hit deceided to roll back*/ + *(int*)(updater->mode) = CODE_RUNNING; + buzz_utility::buzz_script_set(old_bcfname, dbgf_name, (int)VM->robot); + update_try_counter = 0; + timer_steps = 0; + } + } +} + +uint8_t* getupdater_out_msg() +{ + return (uint8_t*)updater->outmsg_queue->queue; +} + +uint8_t* getupdate_out_msg_size() +{ + // fprintf(stdout,"[Debug from get out size in util: ]size = %i \n",*(uint16_t*)updater->outmsg_queue->size); + return (uint8_t*)updater->outmsg_queue->size; +} + +int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size) +{ + if (buzz_utility::buzz_update_init_test(BO_BUF, dbgfname, bcode_size)) + { + 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"); + *(int*)(updater->mode) = CODE_STANDBY; + // fprintf(stdout,"updater value = %i\n",updater->mode); + delete_p(updater->bcode); + updater->bcode = (uint8_t*)malloc(bcode_size); + memcpy(updater->bcode, BO_BUF, bcode_size); + *(size_t*)updater->bcode_size = bcode_size; + buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname, + (size_t) * (size_t*)(updater->standby_bcode_size)); + buzzvm_t VM = buzz_utility::get_vm(); + gettimeofday(&t1, NULL); + buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); + buzzvm_pushi(VM, no_of_robot); + buzzvm_gstore(VM); + return 1; + } + /*Unable to step something wrong*/ else { if (*(int*)(updater->mode) == CODE_RUNNING) { - ROS_ERROR("Initialization test failed, resuming old script"); - buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (int)*(size_t*)(updater->bcode_size)); + ROS_ERROR("Step test failed, resuming old script"); + buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (size_t) * (size_t*)(updater->bcode_size)); } else { /*You will never reach here*/ - ROS_ERROR("Initialization test failed, returning to standby"); + ROS_ERROR("Step test failed, returning to standby"); buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname, (size_t) * (size_t*)(updater->standby_bcode_size)); buzzvm_t VM = buzz_utility::get_vm(); @@ -638,87 +634,108 @@ namespace buzz_update{ return 0; } } - - void destroy_out_msg_queue() + else + { + if (*(int*)(updater->mode) == CODE_RUNNING) + { + ROS_ERROR("Initialization test failed, resuming old script"); + buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (int)*(size_t*)(updater->bcode_size)); + } + else + { + /*You will never reach here*/ + ROS_ERROR("Initialization test failed, returning to standby"); + buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname, + (size_t) * (size_t*)(updater->standby_bcode_size)); + buzzvm_t VM = buzz_utility::get_vm(); + buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); + buzzvm_pushi(VM, no_of_robot); + buzzvm_gstore(VM); + } + return 0; + } +} + +void destroy_out_msg_queue() +{ + // fprintf(stdout,"out queue freed\n"); + delete_p(updater->outmsg_queue->queue); + delete_p(updater->outmsg_queue->size); + delete_p(updater->outmsg_queue); + updater_msg_ready = 0; +} + +int is_msg_present() +{ + return updater_msg_ready; +} +/*buzz_updater_elem_t get_updater() +{ + return updater; +}*/ +void destroy_updater() +{ + delete_p(updater->bcode); + delete_p(updater->bcode_size); + delete_p(updater->standby_bcode); + delete_p(updater->standby_bcode_size); + delete_p(updater->mode); + delete_p(updater->update_no); + if (updater->outmsg_queue) { - // fprintf(stdout,"out queue freed\n"); delete_p(updater->outmsg_queue->queue); delete_p(updater->outmsg_queue->size); delete_p(updater->outmsg_queue); - updater_msg_ready = 0; } + if (updater->inmsg_queue) + { + delete_p(updater->inmsg_queue->queue); + delete_p(updater->inmsg_queue->size); + delete_p(updater->inmsg_queue); + } + inotify_rm_watch(fd, wd); + close(fd); +} - int is_msg_present() - { - return updater_msg_ready; - } - /*buzz_updater_elem_t get_updater() - { - return updater; - }*/ - void destroy_updater() - { - delete_p(updater->bcode); - delete_p(updater->bcode_size); - delete_p(updater->standby_bcode); - delete_p(updater->standby_bcode_size); - delete_p(updater->mode); - delete_p(updater->update_no); - if (updater->outmsg_queue) - { - delete_p(updater->outmsg_queue->queue); - delete_p(updater->outmsg_queue->size); - delete_p(updater->outmsg_queue); - } - if (updater->inmsg_queue) - { - delete_p(updater->inmsg_queue->queue); - delete_p(updater->inmsg_queue->size); - delete_p(updater->inmsg_queue); - } - inotify_rm_watch(fd, wd); - close(fd); - } +void set_bzz_file(const char* in_bzz_file, bool dbg) +{ + debug = dbg; + bzz_file = in_bzz_file; +} - void set_bzz_file(const char* in_bzz_file, bool dbg) - { - debug=dbg; - bzz_file = in_bzz_file; - } +void updates_set_robots(int robots) +{ + no_of_robot = robots; +} - void updates_set_robots(int robots) - { - no_of_robot = robots; - } +/*-------------------------------------------------------- +/ Create Buzz bytecode from the bzz script input +/-------------------------------------------------------*/ +int compile_bzz(std::string bzz_file) +{ + /*Compile the buzz code .bzz to .bo*/ + std::string bzzfile_name(bzz_file); + stringstream bzzfile_in_compile; + std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; + std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); + name = name.substr(0, name.find_last_of(".")); + bzzfile_in_compile << "bzzc -I " << path << "include/"; //<<" "<bcode_size << "," << *(size_t*)updater->patch_size << "," - << (int)*(uint8_t*)updater->update_no << "," << (int)packet_id_; - }*/ +/*void collect_data(std::ofstream& logger) +{ + double time_spent = (t2.tv_sec - t1.tv_sec) * 1000.0; //(double)(end - begin) / CLOCKS_PER_SEC; + time_spent += (t2.tv_usec - t1.tv_usec) / 1000.0; + // RID,update trigger,time steps taken,old byte code size, new bytecode size, patch size,update number, + // +Patch_packets_received_counter,Patch_request_packets_received,Patch_packets_sent_counter,Patch_request_packets_sent_counter + logger << (int)no_of_robot << "," << neigh << "," << (double)time_spent << "," << (int)timer_steps << "," + << old_byte_code_size << "," << *(size_t*)updater->bcode_size << "," << *(size_t*)updater->patch_size << "," + << (int)*(uint8_t*)updater->update_no << "," << (int)packet_id_; +}*/ } \ No newline at end of file diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 458ddfd..af2606a 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -17,7 +17,7 @@ static buzzvm_t VM = 0; static char* BO_FNAME = 0; static uint8_t* BO_BUF = 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 std::vector IN_MSG; std::map users_map; @@ -572,7 +572,8 @@ int get_inmsg_size() return IN_MSG.size(); } -std::vector get_inmsg_vector(){ +std::vector get_inmsg_vector() +{ return IN_MSG; } @@ -590,11 +591,12 @@ 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); - if(obj->o.type == BUZZTYPE_STRING) + if (obj->o.type == BUZZTYPE_STRING) uav_state = string(obj->s.value.str); else uav_state = "Not Available"; @@ -603,8 +605,8 @@ string get_bvmstate() return uav_state; } -int get_swarmsize() { +int get_swarmsize() +{ return (int)buzzdict_size(VM->swarmmembers) + 1; } - } diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index baf6f4f..fd04090 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -42,11 +42,15 @@ static bool logVoronoi = false; std::ofstream voronoicsv; struct Point -{ - float x; - float y; - Point(): x( 0.0 ), y( 0.0 ) { } - Point( float x, float y ): x( x ), y( y ) { } +{ + float x; + float y; + Point() : x(0.0), y(0.0) + { + } + Point(float x, float y) : x(x), y(y) + { + } }; string WPlistname = ""; @@ -54,7 +58,7 @@ std::map targets_map; std::map wplist_map; std::map neighbors_map; std::map neighbors_status_map; -std::map> grid; +std::map> grid; /****************************************/ /****************************************/ @@ -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,15 +157,18 @@ 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)); - gps[0] = target_lat*180.0/M_PI; - gps[1] = target_lon*180.0/M_PI; + double latR = cur_pos[0] * M_PI / 180.0; + double lonR = cur_pos[1] * M_PI / 180.0; + double target_lat = + asin(sin(latR) * cos(Vrange / EARTH_RADIUS) + cos(latR) * sin(Vrange / EARTH_RADIUS) * cos(Vbearing)); + double target_lon = lonR + atan2(sin(Vbearing) * sin(Vrange / EARTH_RADIUS) * cos(latR), + cos(Vrange / EARTH_RADIUS) - sin(latR) * sin(target_lat)); + gps[0] = target_lat * 180.0 / M_PI; + gps[1] = target_lon * 180.0 / M_PI; gps[2] = cur_pos[2]; } @@ -215,7 +223,7 @@ void parse_gpslist() 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 ----------------------------------------------------------- */ @@ -225,16 +233,19 @@ void check_targets_sim(double lat, double lon, double *res) for (it = wplist_map.begin(); it != wplist_map.end(); ++it) { double rb[3]; - double ref[2]={lat, lon}; - double tar[2]={it->second.latitude, it->second.longitude}; + 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; @@ -253,21 +264,23 @@ int buzz_exportmap(buzzvm_t vm) buzzvm_lnum_assert(vm, 1); // Get the parameter 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); - 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 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); - row.insert(std::pair(j, 100.0 - round(buzzvm_stack_at(vm, 1)->f.value*100.0))); + row.insert(std::pair(j, 100.0 - round(buzzvm_stack_at(vm, 1)->f.value * 100.0))); buzzvm_pop(vm); } - grid.insert(std::pair>(i,row)); + grid.insert(std::pair>(i, row)); buzzvm_pop(vm); } // DEBUG @@ -277,252 +290,279 @@ int buzz_exportmap(buzzvm_t vm) /* * Geofence(): test for a point in a polygon - * TAKEN from https://www.geeksforgeeks.org/how-to-check-if-a-given-point-lies-inside-a-polygon/ + * 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 -// 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)) - 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 ) +// Given three colinear points p, q, r, the function checks if +// point q lies on line segment 'pr' +bool onSegment(Point p, Point q, Point r) { - 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; } - -bool clockwise_compare_points( const Point& a, const Point& b ) +// 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) { - 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 *P){ - std::sort( P->begin(), P->end(), clockwise_compare_points ); +float clockwise_angle_of(const Point& p) +{ + 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* P) +{ + std::sort(P->begin(), P->end(), clockwise_compare_points); P->push_back((*P)[0]); } - -float pol_area(vector vert) { +float pol_area(vector vert) +{ float a = 0.0; - //ROS_INFO("Polygone %d edges area.",vert.size()); - vector ::iterator it; - vector ::iterator next; - for (it = vert.begin(); it != vert.end()-1; ++it){ - next = it+1; + // ROS_INFO("Polygone %d edges area.",vert.size()); + vector::iterator it; + vector::iterator next; + for (it = vert.begin(); it != vert.end() - 1; ++it) + { + next = it + 1; a += it->x * next->y - next->x * it->y; } a *= 0.5; - //ROS_INFO("Polygon area: %f",a); + // ROS_INFO("Polygon area: %f",a); return a; } -double* polygone_center(vector vert, double *c) { +double* polygone_center(vector vert, double* c) +{ float A = pol_area(vert); int i1 = 1; - vector ::iterator it; - vector ::iterator next; - for (it = vert.begin(); it != vert.end()-1; ++it){ - next = it+1; - float t = it->x*next->y - next->x*it->y; - c[0] += (it->x+next->x) * t; - c[1] += (it->y+next->y) * t; + vector::iterator it; + vector::iterator next; + for (it = vert.begin(); it != vert.end() - 1; ++it) + { + next = it + 1; + float t = it->x * next->y - next->x * it->y; + c[0] += (it->x + next->x) * t; + c[1] += (it->y + next->y) * t; } c[0] = c[0] / (6.0 * A); c[1] = c[1] / (6.0 * A); return c; } -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 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); +void getintersection(Point S, Point D, std::vector 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 ::iterator itc; - std::vector ::iterator next; - for (itc = Poly.begin(); itc != Poly.end()-1; ++itc) { - next = itc+1; - if (doIntersect((*itc), (*next), S, D)) + std::vector::iterator itc; + std::vector::iterator next; + for (itc = Poly.begin(); itc != Poly.end() - 1; ++itc) + { + 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: // 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) - - double d = denominator( S, D, (*itc), (*next) ); - - if (std::abs( d ) < 0.000000001) + + double d = denominator(S, D, (*itc), (*next)); + + if (std::abs(d) < 0.000000001) { - parallel = true; - collinear = abs(numerator( S, D, (*itc), (*next) )) < 0.000000001; + parallel = true; + collinear = abs(numerator(S, D, (*itc), (*next))) < 0.000000001; 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)); - (*I)=Point(S.x + r * (D.x - S.x), S.y + r * (D.y - S.y)); + 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)); + (*I) = Point(S.x + r * (D.x - S.x), S.y + r * (D.y - S.y)); } } - if(parallel || collinear) - ROS_WARN("Lines are Collinear (%d) or Parallels (%d)",collinear,parallel); + if (parallel || collinear) + ROS_WARN("Lines are Collinear (%d) or Parallels (%d)", collinear, parallel); } -bool isSiteout(Point S, std::vector Poly) { +bool isSiteout(Point S, std::vector Poly) +{ bool onedge = false; - // Create a point for line segment from p to infinite - Point extreme = {10000, S.y}; + // Create a point for line segment from p to infinite + Point extreme = { 10000, S.y }; - // Count intersections of the above line with sides of polygon - int count = 0; - std::vector ::iterator itc; - std::vector ::iterator next; - for (itc = Poly.begin(); itc != Poly.end()-1; ++itc) { - next = itc+1; + // Count intersections of the above line with sides of polygon + int count = 0; + std::vector::iterator itc; + std::vector::iterator next; + for (itc = Poly.begin(); itc != Poly.end() - 1; ++itc) + { + next = itc + 1; - // Check if the line segment from 'p' to 'extreme' intersects - // with the line segment from 'polygon[i]' to 'polygon[next]' - if (doIntersect((*itc), (*next), S, extreme)) - { - // 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) { - onedge = onSegment((*itc), S, (*next)); - if(onedge) - break; - } - count++; + // Check if the line segment from 'p' to 'extreme' intersects + // with the line segment from 'polygon[i]' to 'polygon[next]' + if (doIntersect((*itc), (*next), S, extreme)) + { + // 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) + { + onedge = onSegment((*itc), S, (*next)); + if (onedge) + break; + } + count++; } } - return ((count%2 == 0) && !onedge); + return ((count % 2 == 0) && !onedge); } int buzzuav_geofence(buzzvm_t vm) { - Point P; - buzzvm_lnum_assert(vm, 1); - // Get the parameter - buzzvm_lload(vm, 1); - buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE); // dictionary - buzzobj_t t = buzzvm_stack_at(vm, 1); + Point P; + buzzvm_lnum_assert(vm, 1); + // Get the parameter + buzzvm_lload(vm, 1); + buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE); // dictionary + buzzobj_t t = buzzvm_stack_at(vm, 1); - if(buzzdict_size(t->t.value) < 5) { - ROS_ERROR("Wrong Geofence input size (%i).", buzzdict_size(t->t.value)); - return buzzvm_ret0(vm); + 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 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 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); - }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); + else + { + tmp.x = buzzvm_stack_at(vm, 1)->f.value; + // printf("c%dx=%f\n",i,tmp.x); } - 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]); + 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); + + // 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); } -int voronoi_center(buzzvm_t vm) { - +int voronoi_center(buzzvm_t vm) +{ float dist_max = 300; - buzzvm_lnum_assert(vm, 1); + buzzvm_lnum_assert(vm, 1); // Get the parameter 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); buzzvm_dup(vm); @@ -531,161 +571,191 @@ int voronoi_center(buzzvm_t vm) { int Poly_vert = buzzvm_stack_at(vm, 1)->i.value; buzzvm_pop(vm); - std::vector polygon_bound; - for(int32_t i = 0; i < Poly_vert; ++i) { + std::vector polygon_bound; + for (int32_t i = 0; i < Poly_vert; ++i) + { buzzvm_dup(vm); buzzvm_pushi(vm, i); buzzvm_tget(vm); - + buzzvm_dup(vm); buzzvm_pushs(vm, buzzvm_string_register(vm, "x", 1)); buzzvm_tget(vm); - //ROS_INFO("---x-->%f",buzzvm_stack_at(vm, 1)->f.value); + // ROS_INFO("---x-->%f",buzzvm_stack_at(vm, 1)->f.value); float tmpx = 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); + // ROS_INFO("---y-->%f",buzzvm_stack_at(vm, 1)->f.value); float tmpy = buzzvm_stack_at(vm, 1)->f.value; buzzvm_pop(vm); 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); } 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 - if(isSiteout(Point(0,0), polygon_bound) || count < 3) { - //ROS_WARN("Not 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]); + 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() << ","; - vdg.resetIterator(); - //ROS_WARN("[%i] Voronoi Bounded tessellation done!", buzz_utility::get_robotid()); - std::vector ::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 << ","; + 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); } - 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::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]; - vector cell_vert; + vector cell_vert; Point Intersection; - int i=0; - 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]); - if(sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1))<0.1) + int i = 0; + 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]); + if (sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1)) < 0.1) continue; - bool isout1 = isSiteout(Point(x1,y1), polygon_bound); - bool isout2 = isSiteout(Point(x2,y2), polygon_bound); - if(isout1 && isout2){ - //ROS_INFO("Line out of area!"); + bool isout1 = isSiteout(Point(x1, y1), polygon_bound); + bool isout2 = isSiteout(Point(x2, y2), polygon_bound); + if (isout1 && isout2) + { + // ROS_INFO("Line out of area!"); 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; y1 = Intersection.y; - //ROS_INFO("Site out 1 -> (%f,%f)", x1, y1); - }else if(isout2) { - getintersection(Point(x1,y1), Point(x2,y2), polygon_bound, &Intersection); + // ROS_INFO("Site out 1 -> (%f,%f)", x1, y1); + } + 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); + // 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()){ - cell_vert.push_back(Point(x1,y1)); - cell_vert.push_back(Point(x2,y2)); - } else { + 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 + { bool alreadyin = false; - vector ::iterator itc; - for (itc = cell_vert.begin(); itc != cell_vert.end(); ++itc) { + vector::iterator itc; + for (itc = cell_vert.begin(); itc != cell_vert.end(); ++itc) + { double dist = sqrt((itc->x - x1) * (itc->x - x1) + (itc->y - y1) * (itc->y - y1)); - if(dist < 0.1) { + if (dist < 0.1) + { alreadyin = true; break; } } - if(!alreadyin) + 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; } } - if(!alreadyin) + if (!alreadyin) cell_vert.push_back(Point(x2, y2)); } } - } - if(cell_vert.size()<3){ - ROS_WARN("[%i] Voronoi Bounded tessellation failed (%d)!", buzz_utility::get_robotid(),cell_vert.size()); + } + 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 ); + std::sort(cell_vert.begin(), cell_vert.end(), clockwise_compare_points); cell_vert.push_back(cell_vert[0]); - - double center_dist[2] = {0.0, 0.0}; + + double center_dist[2] = { 0.0, 0.0 }; polygone_center(cell_vert, center_dist); - if(logVoronoi) voronoicsv << center_dist[0] << "," << center_dist[1] << std::endl; - center_dist[0]/=2; - center_dist[1]/=2; + 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); } @@ -800,7 +871,7 @@ int buzzuav_addNeiStatus(buzzvm_t vm) buzzvm_tget(vm); newRS.flight_status = buzzvm_stack_at(vm, 1)->i.value; buzzvm_pop(vm); - + map::iterator it = neighbors_status_map.find(id); if (it != neighbors_status_map.end()) neighbors_status_map.erase(it); @@ -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) @@ -987,7 +1063,7 @@ double* getgoto() return goto_pos; } -std::map> getgrid() +std::map> getgrid() /* / return the grid /-------------------------------------------------------------*/ @@ -1199,7 +1275,8 @@ void update_neighbors(buzzvm_t vm) } // Clear neighbours pos -void clear_neighbours_pos(){ +void clear_neighbours_pos() +{ neighbors_map.clear(); } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 50be762..d42e5cd 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -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 ---------------*/ @@ -26,7 +26,7 @@ logical_clock(ros::Time()), previous_step_time(ros::Time()) cur_pos.longitude = 0; cur_pos.latitude = 0; cur_pos.altitude = 0; - + // Obtain parameters from ros parameter server Rosparameters_get(n_c_priv); // 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); bcfname = fname + ".bo"; 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 - if(setmode) + if (setmode) SetMode("LOITER", 0); goto_pos = buzzuav_closures::getgoto(); @@ -67,7 +67,7 @@ logical_clock(ros::Time()), previous_step_time(ros::Time()) logical_clock.fromSec(0); logical_time_rate = 0; time_sync_jumped = false; - out_msg_time=0; + out_msg_time = 0; // Create log dir and open log file initcsvlog(); buzzuav_closures::setWPlist(WPfile); @@ -126,7 +126,7 @@ void roscontroller::RosControllerRun() // set ROS loop rate ros::Rate loop_rate(BUZZRATE); // 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."); // DEBUG // ROS_WARN("[%i] -----------------------STARTING MAIN LOOP!", robot_id); @@ -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; @@ -159,26 +160,29 @@ void roscontroller::RosControllerRun() // log data logtocsv(); - + // 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(); // Call the flight controler service flight_controller_service_call(); // Broadcast local position to FCU - if(BClpose && !setmode) - SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]); + if (BClpose && !setmode) + SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]); // 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,19 +207,19 @@ void roscontroller::initcsvlog() / Create the CSV log file /-------------------------------------------------------*/ { - 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; + 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--){ - 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()); + 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()); } - 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); - // set log data double precision - log <::iterator it = - neighbours_pos_map.begin(); + + map::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 << "," << it->first << ","; + log << (double)it->second.x << "," << (double)it->second.y << "," << (double)it->second.z; } - for (std::vector::iterator it = inmsgdata.begin() ; it != inmsgdata.end(); ++it) + for (std::vector::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; @@ -395,7 +396,7 @@ void roscontroller::PubandServ(ros::NodeHandle& n_c, ros::NodeHandle& node_handl } if (node_handle.getParam("services/modeclient", topic)) { - if(setmode) + if (setmode) mode_client = n_c.serviceClient(topic); } else @@ -480,11 +481,10 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c, ros::NodeHandle& n_ // Publishers and service Clients PubandServ(n_c, n_c_priv); - + ROS_INFO("Ready to receive Mav Commands from RC client"); - + capture_srv = n_c.serviceClient(capture_srv_name); - multi_msg = true; } @@ -583,11 +583,12 @@ void roscontroller::neighbours_pos_publisher() // cout<<"iterator it val: "<< it-> first << " After convertion: " // <<(uint8_t) buzz_utility::get_rid_uint8compac(it->first)<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]; @@ -641,24 +642,25 @@ void roscontroller::grid_publisher() / Publish current Grid from Buzz script /----------------------------------------------------*/ { - std::map> grid = buzzuav_closures::getgrid(); - std::map>::iterator itr = grid.begin(); + std::map> grid = buzzuav_closures::getgrid(); + std::map>::iterator itr = grid.begin(); 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); + // ROS_INFO("------> Publishing a grid of %i x %i", g_h, g_w); auto current_time = ros::Time::now(); nav_msgs::OccupancyGrid grid_msg; grid_msg.header.frame_id = "/world"; 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.resolution = 0.01;//gridMap.getResolution(); + grid_msg.info.resolution = 0.01; // gridMap.getResolution(); grid_msg.info.width = g_w; 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.y = round(g_h/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.z = 0.0; grid_msg.info.origin.orientation.x = 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.data.resize(g_w * g_h); - for (itr=grid.begin(); itr!=grid.end(); ++itr) { - std::map::iterator itc = itr->second.begin(); - for (itc=itr->second.begin(); itc!=itr->second.end(); ++itc) { - grid_msg.data[(itr->first-1)*g_w+itc->first] = itc->second; + for (itr = grid.begin(); itr != grid.end(); ++itr) + { + std::map::iterator itc = itr->second.begin(); + 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 @@ -729,13 +733,13 @@ with size......... | / else message_number++; - //header variables + // header variables uint16_t tmphead[4]; tmphead[1] = (uint16_t)message_number; - //uint32_t stime = (uint32_t)(ros::Time::now().toSec()); //(uint32_t)(logical_clock.toSec() * 100000); - memcpy((void*)(tmphead+2),(void*)&stime,sizeof(uint32_t)); + // uint32_t stime = (uint32_t)(ros::Time::now().toSec()); //(uint32_t)(logical_clock.toSec() * 100000); + memcpy((void*)(tmphead + 2), (void*)&stime, sizeof(uint32_t)); uint64_t rheader[1]; - rheader[0]=0; + rheader[0] = 0; payload_out.sysid = (uint8_t)robot_id; payload_out.msgid = (uint32_t)message_number; @@ -755,7 +759,7 @@ with size......... | / { 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_size = out[0]; // publish prepared messages in respective topic @@ -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(); } @@ -834,13 +839,13 @@ script // Registering HOME POINT. home = cur_pos; // 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); - BClpose = true; + BClpose = true; } if (current_mode != "GUIDED" && setmode) SetMode("GUIDED", 3000); // added for compatibility with 3DR Solo - if(setmode) + if (setmode) { if (mav_client.call(cmd_srv)) { @@ -862,7 +867,7 @@ script armstate = 0; 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; Arm(); @@ -894,17 +899,17 @@ script case COMPONENT_ARM_DISARM: if (!armstate) { - if(setmode) + if (setmode) SetMode("LOITER", 0); armstate = 1; Arm(); } break; - case COMPONENT_ARM_DISARM+1: + case COMPONENT_ARM_DISARM + 1: if (armstate) { - if(setmode) + if (setmode) SetMode("LOITER", 0); armstate = 0; Arm(); @@ -913,7 +918,7 @@ script case NAV_SPLINE_WAYPOINT: goto_pos = buzzuav_closures::getgoto(); - if(setmode) + if (setmode) SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]); break; @@ -950,12 +955,11 @@ 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(); - // raw_neighbours_pos_map.clear(); // TODO: currently not a problem, but - // have to clear ! + neighbours_pos_map.clear(); + raw_neighbours_pos_map.clear(); + buzzuav_closures::clear_neighbours_pos(); + // raw_neighbours_pos_map.clear(); // TODO: currently not a problem, but + // have to clear ! } 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.y = msg->pose.position.y; - if(!BClpose) + if (!BClpose) { - goto_pos[0]=cur_pos.x; - goto_pos[1]=cur_pos.y; - goto_pos[2]=0.0; - BClpose = true; + goto_pos[0] = cur_pos.x; + goto_pos[1] = cur_pos.y; + goto_pos[2] = 0.0; + 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 - 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); @@ -1157,8 +1157,8 @@ void roscontroller::SetLocalPosition(float x, float y, float z, float yaw) moveMsg.pose.orientation.w = q[3]; // To prevent drifting from stable position, uncomment - //if(fabs(x)>0.005 || fabs(y)>0.005) { - localsetpoint_nonraw_pub.publish(moveMsg); + // if(fabs(x)>0.005 || fabs(y)>0.005) { + 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.message_rate = rate; 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::Duration(0.1).sleep(); timeout--; } - if(timeout<1) + if (timeout < 1) ROS_ERROR("Set stream rate timed out!"); else ROS_WARN("Set stream rate call successful"); @@ -1228,12 +1228,12 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) // decode msg header uint64_t rhead = msg->payload64[0]; uint16_t r16head[4]; - memcpy(r16head,&rhead, sizeof(uint64_t)); + memcpy(r16head, &rhead, sizeof(uint64_t)); uint16_t mtype = r16head[0]; uint16_t mid = r16head[1]; - uint32_t temptime=0; - memcpy(&temptime, r16head+2, sizeof(uint32_t)); - //float stime = (float)temptime/(float)100000; + uint32_t temptime = 0; + memcpy(&temptime, r16head + 2, sizeof(uint32_t)); + // float stime = (float)temptime/(float)100000; // 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 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 uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + index)); // 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); if (debug)