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

Integration of update, before test on the drones

This commit is contained in:
vivek-shankar 2017-01-08 13:17:13 -05:00
parent b9fe7a6e2c
commit c0b930927c
11 changed files with 950 additions and 116 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
CMakeLists.txt
View File

@ -38,7 +38,8 @@ add_executable(rosbuzz_node src/rosbuzz.cpp
src/roscontroller.cpp
src/buzz_utility.cpp
src/buzzuav_closures.cpp
src/uav_utility.cpp)
src/uav_utility.cpp
src/buzz_update.cpp)
target_link_libraries(rosbuzz_node ${catkin_LIBRARIES} /usr/local/lib/libbuzz.so /usr/local/lib/libbuzzdbg.so -lpthread)
# Executables and libraries for installation to do

122
include/buzz_update.h Normal file
View File

@ -0,0 +1,122 @@
#ifndef BUZZ_UPDATE_H
#define BUZZ_UPDATE_H
#include <stdlib.h>
#include <stdio.h>
#include <buzz/buzztype.h>
#include <buzz/buzzdict.h>
#include <buzz/buzzdarray.h>
#include <buzz/buzzvstig.h>
#define delete_p(p) do { free(p); p = NULL; } while(0)
/*********************/
/* Updater states */
/********************/
typedef enum {
CODE_RUNNING = 0, // Code executing
CODE_UPDATE, // Updating code
CODE_NEIGHBOUR, // Neighbour triggered an update
CODE_STANDBY, // Standing by for others to update
} code_states_e;
/*********************/
/*Message types */
/********************/
typedef enum {
SEND_CODE = 0, // Broadcast code with state
STATE_MSG, // Broadcast state
} code_message_e;
/*************************/
/*Updater message queue */
/*************************/
struct updater_msgqueue_s {
uint8_t* queue;
uint8_t* size;
} ;
typedef struct updater_msgqueue_s* updater_msgqueue_t;
/**************************/
/*Updater data*/
/**************************/
struct buzz_updater_elem_s {
/* robot id */
uint16_t robotid;
/*current Bytecode content */
uint8_t* bcode;
/*current bcode size*/
size_t 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;
/*Table with state of other neighbours*/
buzzdict_t state_dict;
/*Update number to ensure consistency*/
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*/
/**************************************************************************/
int update_routine(const char* bcfname,
const char* dbgfname, int destroy);
/************************************************/
/*Initalizes the updater */
/************************************************/
void init_update_monitor(const char* bo_filename,int barrier);
/*********************************************************/
/*Appends buffer of given size to in msg queue of updater*/
/*********************************************************/
void code_message_inqueue_append(uint8_t* msg,uint16_t size);
/*********************************************************/
/*Processes messages inside the queue of the updater*/
/*********************************************************/
void code_message_inqueue_process();
/*****************************************************/
/* 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();
/**************************************************/
/*destroys the out msg queue*/
/*************************************************/
void destroy_out_msg_queue();
/***************************************************/
/*obatins updater state*/
/***************************************************/
int get_update_mode();
/***************************************************/
/*sets bzz file name*/
/***************************************************/
void set_bzz_file(const char* in_bzz_file);
/****************************************************/
/*Destroys the updater*/
/***************************************************/
void destroy_updater();
#endif

11
include/buzz_utility.h
View File

@ -2,6 +2,7 @@
#include <stdio.h>
#include "buzz_utility.h"
#include "buzzuav_closures.h"
#include "buzz_update.h"
#include <buzz/buzzdebug.h>
#include <stdlib.h>
#include <string.h>
@ -35,10 +36,20 @@ uint64_t* out_msg_process();
int buzz_script_set(const char* bo_filename,
const char* bdbg_filename, int robot_id);
int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size);
int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size);
void buzz_script_step();
void buzz_script_destroy();
int buzz_script_done();
int update_step_test();
uint16_t get_robotid();
}

1
include/buzzuav_closures.h
View File

@ -74,5 +74,6 @@ int buzzuav_update_prox(buzzvm_t vm);
int bzz_cmd();
int dummy_closure(buzzvm_t vm);
//#endif
}

1
include/roscontroller.h
View File

@ -42,6 +42,7 @@ private:
int robot_id=0;
//int oldcmdID=0;
int rc_cmd;
int barrier;
std::string bzzfile_name, fcclient_name, rcservice_name,bcfname,dbgfname,out_payload,in_payload; //, rcclient;
bool rcclient;
ros::ServiceClient mav_client;

1
launch/rosbuzz_2_parallel.launch
View File

@ -10,6 +10,7 @@
<!-- param name="in_payload" value="/rosbuzz_node1/outMavlink"-->
<param name="out_payload" value="/outMavlink"/>
<param name="robot_id" value="1"/>
<param name="No_of_Robots" value="1"/>
</node>
<!-- node name="rosbuzz_node1" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" -->
<!-- param name="bzzfile_name" value="/home/vivek/catkin_ws/src/rosbuzz/src/test1.bzz" -->

1
launch/rosbuzzm100.launch
View File

@ -9,6 +9,7 @@
<param name="in_payload" value="/inMavlink"/>
<param name="out_payload" value="/outMavlink"/>
<param name="robot_id" value="3"/>
<param name="No_of_Robots" value="2"/>
</node>

486
src/buzz_update.cpp Normal file
View File

@ -0,0 +1,486 @@
#include <stdio.h>
#include <stdlib.h>
#include <fcntl.h>
#include <unistd.h>
#include <sys/inotify.h>
#include "buzz_update.h"
#include <buzz_utility.h>
#include <string.h>
#include <buzz/buzzdebug.h>
#include <sys/time.h>
#define MAX_BUCKETS 10
/*Temp for data collection*/
//static int neigh=-1;
//static struct timeval t1, t2;
//static clock_t begin;
//static clock_t end;
//void collect_data();
/*Temp end */
static int fd,wd =0;
static int old_update =0;
static buzz_updater_elem_t updater;
static int no_of_robot;
static char* dbgf_name;
static const char* bzz_file;
uint32_t buzz_updater_hashfunp (const void* key){
return (*(uint16_t*)key);
}
int buzz_updater_key_cmp(const void* a, const void* b){
if( *(uint16_t*)a < *(uint16_t*)b ) return -1;
if( *(uint16_t*)a > *(uint16_t*)b ) return 1;
return 0;
}
void updater_entry_destroy(const void* key, void* data, void* params) {
// fprintf(stdout,"freeing element.\n");
free((void*)key);
free((void*)data);
}
void standby_barrier_test(const void* key, const void* data, void* tmp){
//fprintf(stdout,"Checking barrier for robot :%i it has barrier : %i.\n",*(uint16_t*) key,*(uint8_t*) data );
if(*(uint8_t*) data == CODE_STANDBY || *(uint8_t*) data == CODE_RUNNING ){
if(updater->update_no==*(uint8_t*) (data+sizeof(uint8_t)) )
(*(uint8_t*)tmp)+=1;
}
}
void init_update_monitor(const char* bo_filename,int barrier){
fprintf(stdout,"intiialized file monitor.\n");
fd=inotify_init1(IN_NONBLOCK);
if ( fd < 0 ) {
perror( "inotify_init error" );
}
/* watch /.bzz file for any activity and report it back to me */
wd=inotify_add_watch(fd, bzz_file,IN_ALL_EVENTS );
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);
updater = (buzz_updater_elem_t)malloc(sizeof(struct buzz_updater_elem_s));
/* Create a new table for updater*/
updater->bcode = BO_BUF;
updater->bcode_size = bcode_size;
updater->mode=CODE_RUNNING;
updater->robotid= buzz_utility::get_robotid();
updater->state_dict=buzzdict_new(MAX_BUCKETS,
sizeof(uint16_t),
sizeof(uint16_t),
buzz_updater_hashfunp,
buzz_updater_key_cmp,
updater_entry_destroy);
updater->outmsg_queue = NULL;
updater->inmsg_queue = NULL;
updater->update_no=0;
no_of_robot=barrier;
//updater->outmsg_queue=
// update_table->barrier=nvs;
}
int check_update(){
struct inotify_event *event;
char buf[1024];
int check =0;
int i=0;
int len=read(fd,buf,1024);
while(i<len){
event=(struct inotify_event *) &buf[i];
/* file was modified this flag is true in nano and self delet in gedit and other editors */
//fprintf(stdout,"inside file monitor.\n");
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);
//fprintf(stdout,"event.\n");
/* 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;
/*if(update){
buzz_script_set(update_bo, update_bdbg);
update = 0;
}*/
return check;
}
void code_message_outqueue_append(int type){
uint16_t size =0;
//fprintf(stdout,"updater queue append.\n");
//if(updater->outmsg_queue == NULL)
//updater->outmsg_queue=(updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s));
//else {
// destroy_out_msg_queue();
updater->outmsg_queue=(updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s));
// }
if(type==SEND_CODE){
updater->outmsg_queue->queue = (uint8_t*)malloc((6*sizeof(uint8_t))+updater->bcode_size);
updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t));
//*(uint16_t*)updater->outmsg_queue->size =(3*sizeof(uint8_t))+sizeof(size_t)+updater->bcode_size;
*(uint16_t*)(updater->outmsg_queue->queue+size) = (uint16_t) updater->robotid;
size+=sizeof(uint16_t);
*(uint8_t*)(updater->outmsg_queue->queue+size) = (uint8_t) updater->mode;
size+=sizeof(uint8_t);
*(uint8_t*)(updater->outmsg_queue->queue+size) = updater->update_no;
size+=sizeof(uint8_t);
*(uint16_t*)(updater->outmsg_queue->queue+size) = (uint16_t) updater->bcode_size;
size+=sizeof(uint16_t);
memcpy(updater->outmsg_queue->queue+size, updater->bcode, updater->bcode_size);
size+=updater->bcode_size;
FILE *fp;
fp=fopen("update.bo", "wb");
fwrite((updater->bcode), updater->bcode_size, 1, fp);
fclose(fp);
//fprintf(stdout,"[Debug : ]size = %i \n",(int)sizeof(size_t));
*(uint16_t*)updater->outmsg_queue->size=size;
/*Update local dictionary*/
uint8_t* dict_update=(uint8_t*)malloc(sizeof(uint16_t));
*(uint8_t*)dict_update=updater->mode;
*(uint8_t*)(dict_update+sizeof(uint8_t))=updater->update_no;
buzzdict_set(updater->state_dict,(uint8_t*) &(updater->robotid), (uint8_t*)dict_update);
delete_p(dict_update);
}
else if(type==STATE_MSG){
updater->outmsg_queue->queue = (uint8_t*)malloc(3*sizeof(uint8_t));
updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t));
//*(uint16_t*)updater->outmsg_queue->size = 3*sizeof(uint8_t);
*(uint16_t*)(updater->outmsg_queue->queue+size) = (uint16_t) updater->robotid;
size+=sizeof(uint16_t);
*(uint8_t*)(updater->outmsg_queue->queue+size) = (uint8_t) updater->mode;
size+=sizeof(uint8_t);
*(uint8_t*)(updater->outmsg_queue->queue+size) = updater->update_no;
size+=sizeof(uint8_t);
*(uint16_t*)updater->outmsg_queue->size=size;
/*Update local dictionary*/
uint8_t* dict_update=(uint8_t*)malloc(sizeof(uint16_t));
*(uint8_t*)dict_update=updater->mode;
*(uint8_t*)(dict_update+sizeof(uint8_t))=updater->update_no;
buzzdict_set(updater->state_dict,(uint8_t*) &(updater->robotid), (uint8_t*)dict_update);
delete_p(dict_update);
}
}
void code_message_inqueue_append(uint8_t* msg,uint16_t size){
updater->inmsg_queue=(updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s));
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;
}
void code_message_inqueue_process(){
int size=0;
//fprintf(stdout,"received state : %i from robot : %i\n",*(uint8_t*)(updater->inmsg_queue->queue+sizeof(uint16_t)),*(uint16_t*)updater->inmsg_queue->queue);
if(*(uint8_t*)(updater->inmsg_queue->queue+sizeof(uint16_t)) == CODE_UPDATE){
buzzdict_set(updater->state_dict, updater->inmsg_queue->queue,(updater->inmsg_queue->queue+sizeof(uint16_t)));
size +=3*sizeof(uint8_t);
if(*(uint8_t*)(updater->inmsg_queue->queue+size) > (updater->update_no)){
updater->update_no=*(uint8_t*)(updater->inmsg_queue->queue+size);
size+=sizeof(uint8_t);
if(updater->mode==CODE_RUNNING){
uint16_t update_bcode_size =*(uint16_t*)(updater->inmsg_queue->queue+size);
size +=sizeof(uint16_t);
FILE *fp;
fp=fopen("update.bo", "wb");
fwrite((updater->inmsg_queue->queue+size), update_bcode_size, 1, fp);
fclose(fp);
if(buzz_utility::buzz_update_init_test((updater->inmsg_queue->queue+size), dbgf_name,(size_t)update_bcode_size)){
fprintf(stdout,"Initializtion of script test passed\n");
if(buzz_utility::update_step_test()){
/*data logging*/
//neigh=1;
//gettimeofday(&t1, NULL);
//fprintf(stdout,"start and end time in queue process of update Info : %f,%f",(double)begin,(double)end);
/*data logging*/
fprintf(stdout,"Step test passed\n");
updater->mode=CODE_UPDATE;
//fprintf(stdout,"updater value = %i\n",updater->mode);
free(updater->bcode);
updater->bcode = (uint8_t*)malloc(update_bcode_size);
memcpy(updater->bcode, updater->inmsg_queue->queue+size, update_bcode_size);
//updater->bcode = BO_BUF;
updater->bcode_size = (size_t)update_bcode_size;
code_message_outqueue_append(SEND_CODE);
updater->mode=CODE_NEIGHBOUR;
//return updater->mode;
//return 0;
}
/*Unable to step something wrong*/
else{
fprintf(stdout,"Step test failed, stick to old script\n");
buzz_utility::buzz_update_set((updater)->bcode, dbgf_name, (updater)->bcode_size);
//return updater->mode;
//return 1;
}
}
else {
fprintf(stdout,"Initialiation test failed, stick to old script\n");
buzz_utility::buzz_update_set((updater)->bcode, dbgf_name, (updater)->bcode_size);
//return updater->mode;
//return 1;
}
}
}
}
else {
buzzdict_set(updater->state_dict,updater->inmsg_queue->queue, updater->inmsg_queue->queue+sizeof(uint16_t));
}
//fprintf(stdout,"in queue freed\n");
delete_p(updater->inmsg_queue->queue);
delete_p(updater->inmsg_queue->size);
delete_p(updater->inmsg_queue);
}
int update_routine(const char* bcfname,
const char* dbgfname, int destroy){
dbgf_name=(char*)dbgfname;
//fprintf(stdout,"[Debug : ]updater value = %i \n",updater->mode);
if(updater->mode==CODE_RUNNING){
if(check_update()){
std::string bzzfile_name(bzz_file);
stringstream bzzfile_in_compile;
std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/"));
bzzfile_in_compile<<path<<"/";
path = bzzfile_in_compile.str();
bzzfile_in_compile.str("");
std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
name = name.substr(0,name.find_last_of("."));
bzzfile_in_compile << "bzzparse "<<bzzfile_name<<" "<<path<< name<<".basm";
FILE *fp;
int comp=0;
char buf[128];
fprintf(stdout,"Update found \nUpdating script ...\n");
if ((fp = popen(bzzfile_in_compile.str().c_str(), "r")) == NULL) { // to change file edit
fprintf(stdout,"Error opening pipe!\n");
}
while (fgets(buf, 128, fp) != NULL) {
fprintf(stdout,"OUTPUT: %s \n", buf);
comp=1;
}
bzzfile_in_compile.str("");
bzzfile_in_compile <<"bzzasm "<<path<<name<<".basm "<<path<<name<<".bo "<<path<<name<<".bdbg";
if ((fp = popen(bzzfile_in_compile.str().c_str(), "r")) == NULL) { // to change file edit
fprintf(stdout,"Error opening pipe!\n");
}
while (fgets(buf, 128, fp) != NULL) {
fprintf(stdout,"OUTPUT: %s \n", buf);
}
if(pclose(fp) || comp) {
fprintf(stdout,"Errors in comipilg script so staying on old script\n");
// return -1;
}
else {
uint8_t* BO_BUF = 0;
FILE* fp = fopen(bcfname, "rb"); // to change file edit
if(!fp) {
perror(bcfname);
}
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);
}
fclose(fp);
if(buzz_utility::buzz_update_init_test(BO_BUF, dbgfname,bcode_size)){
fprintf(stdout,"Initializtion of script test passed\n");
if(buzz_utility::update_step_test()){
/*data logging*/
//neigh=0;
//gettimeofday(&t1, NULL);
//fprintf(stdout,"start and end time in code running state Info : %f,%f",(double)begin,(double)end);
/*data logging*/
fprintf(stdout,"Step test passed\n");
updater->update_no+=1;
updater->mode=CODE_UPDATE;
//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);
updater->bcode_size = bcode_size;
code_message_outqueue_append(SEND_CODE);
delete_p(BO_BUF);
return updater->mode;
//return 0;
}
/*Unable to step something wrong*/
else{
fprintf(stdout,"step test failed, stick to old script\n");
buzz_utility::buzz_update_set((updater)->bcode, dbgfname, (updater)->bcode_size);
code_message_outqueue_append(STATE_MSG);
delete_p(BO_BUF);
return updater->mode;
//return 1;
}
}
else {
fprintf(stdout,"unable to set new script to switch to old new script\n");
delete_p(BO_BUF);
buzz_utility::buzz_update_set((updater)->bcode, dbgfname, (updater)->bcode_size);
code_message_outqueue_append(STATE_MSG);
return updater->mode;
//return 1;
}
}
}
code_message_outqueue_append(STATE_MSG);
}
else if (updater->mode==CODE_UPDATE){
code_message_outqueue_append(SEND_CODE);
updater->mode=CODE_STANDBY;
}
else if (updater->mode==CODE_NEIGHBOUR){
updater->mode=CODE_STANDBY;
}
else if (updater->mode==CODE_STANDBY){
uint8_t* tmp =(uint8_t*)malloc(sizeof(uint8_t));
*(uint8_t*)tmp=0;
buzzdict_foreach( updater->state_dict,reinterpret_cast<buzzdict_elem_funp>(standby_barrier_test), tmp);
fprintf(stdout,"Standby barrier ................... %i\n",*(uint8_t*)tmp);
code_message_outqueue_append(STATE_MSG);
if(*(uint8_t*)tmp==no_of_robot) {
updater->mode=CODE_RUNNING;
//gettimeofday(&t2, NULL);
//fprintf(stdout,"start and end time in standby state Info : %f,%f",(double)begin,(double)end);
//collect_data();
buzz_utility::buzz_update_set((updater)->bcode, dbgf_name, (updater)->bcode_size);
}
delete_p(tmp);
//fprintf(stdout,"freed tmp\n");
}
if(destroy){
destroy_updater();
fprintf(stdout,"updater destoryed.\n");
}
return updater->mode;
}
uint8_t* getupdater_out_msg(){
return (uint8_t*)updater->outmsg_queue->queue;
}
uint8_t* getupdate_out_msg_size(){
//fprintf(stdout,"[Debug : ]size = %i \n",*(uint16_t*)updater->outmsg_queue->size);
return updater->outmsg_queue->size;
}
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);
}
int get_update_mode(){
return updater->mode;
}
void destroy_updater(){
buzz_utility::buzz_script_destroy();
//dictonary
//fprintf(stdout,"freeing dict.\n");
buzzdict_t* updater_dict_ptr=&(updater->state_dict);
buzzdict_destroy((updater_dict_ptr));
//fprintf(stdout,"freeing complete dict.\n");
delete_p(updater->bcode);
//fprintf(stdout,"freeing complete bcode.\n");
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){
bzz_file=in_bzz_file;
}
void collect_data(){
//fprintf(stdout,"start and end time in data collection Info : %f,%f",(double)begin,(double)end);
//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;
//fprintf(stdout,"Data logger Info : %i,%i,%f,%i,%i,%i\n",(int)updater->robotid,neigh,time_spent,(int)updater->bcode_size,(int)no_of_robot,(int)updater->update_no);
//FILE *Fileptr;
//Fileptr=fopen("logger.csv", "a");
//fprintf(Fileptr,"%i,%i,%f,%i,%i,%i\n",(int)updater->robotid,neigh,time_spent,(int)updater->bcode_size,(int)no_of_robot,(int)updater->update_no);
//fclose(Fileptr);
}

408
src/buzz_utility.cpp
View File

@ -17,7 +17,8 @@ namespace buzz_utility{
static char* BO_FNAME = 0;
static uint8_t* BO_BUF = 0;
static buzzdebug_t DBG_INFO = 0;
static unsigned int MSG_SIZE = 32;
static uint8_t MSG_SIZE = 100; // Only 100 bytes of Buzz messages every step
static int MAX_MSG_SIZE = 10100; // Maximum Msg size for sending update packets
/****************************************/
@ -35,9 +36,9 @@ namespace buzz_utility{
(it->second).z);
}
}
/***************************************/
/*Reinterprets uint64_t into 4 uint16_t*/
/***************************************/
/**************************************************************************/
/*Deserializes uint64_t into 4 uint16_t, freeing out is left to the user */
/**************************************************************************/
uint16_t* u64_cvt_u16(uint64_t u64){
uint16_t* out = new uint16_t[4];
uint32_t int32_1 = u64 & 0xFFFFFFFF;
@ -54,17 +55,26 @@ namespace buzz_utility{
/*Appends obtained messages to buzz in message Queue*/
/***************************************************/
/*******************************************************************************************************************/
/* Message format of payload (Each slot is uint64_t) */
/* _______________________________________________________________________________________________________________ */
/*| | |*/
/*|Size in Uint64_t(but size is Uint16_t)|robot_id|Update msg size|Update msg|Update msgs+Buzz_msgs with size.....|*/
/*|__________________________________________________________________________|____________________________________|*/
/*******************************************************************************************************************/
void in_msg_process(uint64_t* payload){
/* Go through messages and add them to the FIFO */
uint16_t* data= u64_cvt_u16((uint64_t)payload[0]);
/*Size is at first 2 bytes*/
uint16_t size=data[0]*sizeof(uint64_t);
delete[] data;
uint8_t* pl =(uint8_t*)malloc(size);
memset(pl, 0,size);
/* Copy packet into temporary buffer */
memcpy(pl, payload ,size);
/*size and robot id read*/
size_t tot = sizeof(uint32_t);
/*for(int i=0;i<data[0];i++){
uint16_t* out = u64_cvt_u16(payload[i]);
@ -74,18 +84,31 @@ namespace buzz_utility{
}*/
/* Go through the messages until there's nothing else to read */
uint16_t unMsgSize;
do {
/* Get payload size */
unMsgSize = *(uint16_t*)(pl + tot);
tot += sizeof(uint16_t);
/* Append message to the Buzz input message queue */
if(unMsgSize > 0 && unMsgSize <= size - tot ) {
buzzinmsg_queue_append(VM,
buzzmsg_payload_frombuffer(pl +tot, unMsgSize));
tot += unMsgSize;
}
}while(size - tot > sizeof(uint16_t) && unMsgSize > 0);
unMsgSize = *(uint16_t*)(pl + tot);
tot += sizeof(uint16_t);
code_message_inqueue_append(pl+tot,unMsgSize);
tot += unMsgSize;
code_message_inqueue_process();
unMsgSize=0;
/*Check for Buzz messages only when the code is running*/
if(get_update_mode()==CODE_RUNNING){
uint8_t buzz_msg_pre=*(uint8_t*)(pl + tot);
tot+= sizeof(uint8_t);
/*Obtain Buzz messages only when they are present*/
if(buzz_msg_pre){
do {
/* Get payload size */
unMsgSize = *(uint16_t*)(pl + tot);
tot += sizeof(uint16_t);
/* Append message to the Buzz input message queue */
if(unMsgSize > 0 && unMsgSize <= size - tot ) {
buzzinmsg_queue_append(VM,
buzzmsg_payload_frombuffer(pl +tot, unMsgSize));
tot += unMsgSize;
}
}while(size - tot > sizeof(uint16_t) && unMsgSize > 0);
}
}
/* Process messages */
buzzvm_process_inmsgs(VM);
}
@ -95,26 +118,39 @@ namespace buzz_utility{
uint64_t* out_msg_process(){
buzzvm_process_outmsgs(VM);
uint8_t* buff_send =(uint8_t*)malloc(MSG_SIZE);
memset(buff_send, 0, MSG_SIZE);
uint8_t* buff_send =(uint8_t*)malloc(MAX_MSG_SIZE);
memset(buff_send, 0, MAX_MSG_SIZE);
ssize_t tot = sizeof(uint16_t);
/* Send robot id */
*(uint16_t*)(buff_send+tot) = (uint16_t) VM->robot;
tot += sizeof(uint16_t);
/* Send messages from FIFO */
do {
/* Are there more messages? */
if(buzzoutmsg_queue_isempty(VM)) break;
/* Get first message */
buzzmsg_payload_t m = buzzoutmsg_queue_first(VM);
/* Make sure the next message fits the data buffer */
if(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)
>
MSG_SIZE) {
buzzmsg_payload_destroy(&m);
break;
}
/*Append updater msg size*/
memcpy(buff_send + tot, (uint8_t*)getupdate_out_msg_size(), sizeof(uint16_t));
// fprintf(stdout,"Msg size : %i\n",(int) *(uint16_t*)(STREAM_SEND_BUF + tot));
tot+= sizeof(uint16_t);
/*Append updater msgs*/
memcpy(buff_send + tot, (uint8_t*) getupdater_out_msg(),*(uint16_t*)getupdate_out_msg_size());
// fprintf(stdout,"Msg 1 : %i , and Msg 2: %i\n",(int) *(uint16_t*)(STREAM_SEND_BUF + tot),(int) *(uint8_t*)(STREAM_SEND_BUF + tot+sizeof(uint16_t)));
tot+= *(uint16_t*)getupdate_out_msg_size();
/*destroy the updater out msg queue*/
destroy_out_msg_queue();
if(get_update_mode()==CODE_RUNNING){
*(uint8_t*)(buff_send + tot) = 1;
tot+= sizeof(uint8_t);
/* Send messages from FIFO */
do {
/* Are there more messages? */
if(buzzoutmsg_queue_isempty(VM)) break;
/* Get first message */
buzzmsg_payload_t m = buzzoutmsg_queue_first(VM);
/* Make sure the next message makes the data buffer with buzz messages to be less than 100 Bytes */
if(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)
>
MSG_SIZE) {
buzzmsg_payload_destroy(&m);
break;
}
/* Add message length to data buffer */
*(uint16_t*)(buff_send + tot) = (uint16_t)buzzmsg_payload_size(m);
tot += sizeof(uint16_t);
@ -126,11 +162,14 @@ namespace buzz_utility{
/* Get rid of message */
buzzoutmsg_queue_next(VM);
buzzmsg_payload_destroy(&m);
} while(1);
int total_size =(ceil((float)tot/(float)sizeof(uint64_t)));
} while(1);
}
else{
*(uint8_t*)(buff_send + tot) = 0;
tot+= sizeof(uint8_t);
}
uint16_t total_size =(ceil((float)tot/(float)sizeof(uint64_t)));
*(uint16_t*)buff_send = (uint16_t) total_size;
uint64_t* payload_64 = new uint64_t[total_size];
@ -140,7 +179,8 @@ namespace buzz_utility{
}*/
/* Send message */
return payload_64;
}
}
/****************************************/
/*Buzz script not able to load*/
/****************************************/
@ -193,6 +233,33 @@ namespace buzz_utility{
return BUZZVM_STATE_READY;
}
/**************************************************/
/*Register dummy Buzz hooks for test during update*/
/**************************************************/
static int testing_buzz_register_hooks() {
buzzvm_pushs(VM, buzzvm_string_register(VM, "print", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "log", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_goto", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_gohome", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_land", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
return BUZZVM_STATE_READY;
}
/****************************************/
/*Sets the .bzz and .bdbg file*/
/****************************************/
@ -231,29 +298,123 @@ namespace buzz_utility{
return 0;
}
fclose(fd);
/* Read debug information */
//if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) {
// buzzvm_destroy(&VM);
// buzzdebug_destroy(&DBG_INFO);
// perror(bdbg_filename);
// return 0;
//}
/* Set byte code */
//if(buzzvm_set_bcode(VM, BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
// buzzvm_destroy(&VM);
// buzzdebug_destroy(&DBG_INFO);
// fprintf(stdout, "%s: Error loading Buzz script\n\n", bo_filename);
// return 0;
//}
/* Register hook functions */
/*if(buzz_register_hooks() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
fprintf(stdout, "%s: Error registering hooks\n\n", bo_filename);
return 0;
}*/
/* Save bytecode file name */
//BO_FNAME = strdup(bo_filename);
/* Execute the global part of the script */
//buzzvm_execute_script(VM);
/* Call the Init() function */
//buzzvm_function_call(VM, "init", 0);
/* All OK */
return buzz_update_set(BO_BUF, bdbg_filename, bcode_size);
}
/****************************************/
/*Sets a new update */
/****************************************/
int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){
/* Get hostname */
char hstnm[30];
gethostname(hstnm, 30);
/* Make numeric id from hostname */
/* NOTE: here we assume that the hostname is in the format Knn */
int id = strtol(hstnm + 1, NULL, 10);
/* Reset the Buzz VM */
if(VM) buzzvm_destroy(&VM);
VM = buzzvm_new(id);
/* Get rid of debug info */
if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
DBG_INFO = buzzdebug_new();
/* Read debug information */
if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
perror(bdbg_filename);
return 0;
}
}
/* Set byte code */
if(buzzvm_set_bcode(VM, BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
if(buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
fprintf(stdout, "%s: Error loading Buzz script\n\n", bo_filename);
fprintf(stdout, "%s: Error loading Buzz script\n\n", BO_FNAME);
return 0;
}
}
/* Register hook functions */
if(buzz_register_hooks() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
fprintf(stdout, "%s: Error registering hooks\n\n", bo_filename);
return 0;
fprintf(stdout, "%s: Error registering hooks\n\n", BO_FNAME);
return 0;
}
/* Save bytecode file name */
BO_FNAME = strdup(bo_filename);
/* Execute the global part of the script */
buzzvm_execute_script(VM);
/* Call the Init() function */
buzzvm_function_call(VM, "init", 0);
/* All OK */
return 1;
}
/****************************************/
/*Performs a initialization test */
/****************************************/
int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){
/* Get hostname */
char hstnm[30];
gethostname(hstnm, 30);
/* Make numeric id from hostname */
/* NOTE: here we assume that the hostname is in the format Knn */
int id = strtol(hstnm + 1, NULL, 10);
/* Reset the Buzz VM */
if(VM) buzzvm_destroy(&VM);
VM = buzzvm_new(id);
/* Get rid of debug info */
if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
DBG_INFO = buzzdebug_new();
/* Read debug information */
if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
perror(bdbg_filename);
return 0;
}
/* Set byte code */
if(buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
fprintf(stdout, "%s: Error loading Buzz script\n\n", BO_FNAME);
return 0;
}
/* Register hook functions */
if(testing_buzz_register_hooks() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
fprintf(stdout, "%s: Error registering hooks\n\n", BO_FNAME);
return 0;
}
/* Execute the global part of the script */
buzzvm_execute_script(VM);
/* Call the Init() function */
@ -266,72 +427,72 @@ namespace buzz_utility{
/*Swarm struct*/
/****************************************/
struct buzzswarm_elem_s {
buzzdarray_t swarms;
uint16_t age;
};
typedef struct buzzswarm_elem_s* buzzswarm_elem_t;
struct buzzswarm_elem_s {
buzzdarray_t swarms;
uint16_t age;
};
typedef struct buzzswarm_elem_s* buzzswarm_elem_t;
void check_swarm_members(const void* key, void* data, void* params) {
buzzswarm_elem_t e = *(buzzswarm_elem_t*)data;
int* status = (int*)params;
if(*status == 3) return;
if(buzzdarray_size(e->swarms) != 1) {
fprintf(stderr, "Swarm list size is not 1\n");
*status = 3;
}
else {
int sid = 1;
if(*buzzdict_get(VM->swarms, &sid, uint8_t) &&
buzzdarray_get(e->swarms, 0, uint16_t) != sid) {
fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n",
sid,
buzzdarray_get(e->swarms, 0, uint16_t));
*status = 3;
return;
}
sid = 2;
if(*buzzdict_get(VM->swarms, &sid, uint8_t) &&
buzzdarray_get(e->swarms, 0, uint16_t) != sid) {
fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n",
sid,
buzzdarray_get(e->swarms, 0, uint16_t));
*status = 3;
return;
}
}
}
/*Step through the buzz script*/
void check_swarm_members(const void* key, void* data, void* params) {
buzzswarm_elem_t e = *(buzzswarm_elem_t*)data;
int* status = (int*)params;
if(*status == 3) return;
if(buzzdarray_size(e->swarms) != 1) {
fprintf(stderr, "Swarm list size is not 1\n");
*status = 3;
}
else {
int sid = 1;
if(*buzzdict_get(VM->swarms, &sid, uint8_t) &&
buzzdarray_get(e->swarms, 0, uint16_t) != sid) {
fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n",
sid,
buzzdarray_get(e->swarms, 0, uint16_t));
*status = 3;
return;
}
sid = 2;
if(*buzzdict_get(VM->swarms, &sid, uint8_t) &&
buzzdarray_get(e->swarms, 0, uint16_t) != sid) {
fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n",
sid,
buzzdarray_get(e->swarms, 0, uint16_t));
*status = 3;
return;
}
}
}
/*Step through the buzz script*/
void buzz_script_step() {
/*
* Update sensors
*/
buzzuav_closures::buzzuav_update_battery(VM);
buzzuav_closures::buzzuav_update_prox(VM);
buzzuav_closures::buzzuav_update_currentpos(VM);
buzzuav_closures::buzzuav_update_flight_status(VM);
/*
* Call Buzz step() function
*/
if(buzzvm_function_call(VM, "step", 0) != BUZZVM_STATE_READY) {
fprintf(stderr, "%s: execution terminated abnormally: %s\n\n",
BO_FNAME,
buzz_error_info());
buzzvm_dump(VM);
}
/* Print swarm */
//buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
/* Check swarm state */
/* int status = 1;
buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status);
if(status == 1 &&
buzzdict_size(VM->swarmmembers) < 9)
status = 2;
buzzvm_pushs(VM, buzzvm_string_register(VM, "swarm_status", 1));
buzzvm_pushi(VM, status);
buzzvm_gstore(VM);*/
}
void buzz_script_step() {
/*
* Update sensors
*/
buzzuav_closures::buzzuav_update_battery(VM);
buzzuav_closures::buzzuav_update_prox(VM);
buzzuav_closures::buzzuav_update_currentpos(VM);
buzzuav_closures::buzzuav_update_flight_status(VM);
/*
* Call Buzz step() function
*/
if(buzzvm_function_call(VM, "step", 0) != BUZZVM_STATE_READY) {
fprintf(stderr, "%s: execution terminated abnormally: %s\n\n",
BO_FNAME,
buzz_error_info());
buzzvm_dump(VM);
}
/* Print swarm */
//buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
/* Check swarm state */
/* int status = 1;
buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status);
if(status == 1 &&
buzzdict_size(VM->swarmmembers) < 9)
status = 2;
buzzvm_pushs(VM, buzzvm_string_register(VM, "swarm_status", 1));
buzzvm_pushi(VM, status);
buzzvm_gstore(VM);*/
}
/****************************************/
/*Destroy the bvm and other resorces*/
@ -365,6 +526,33 @@ int buzz_script_done() {
return VM->state != BUZZVM_STATE_READY;
}
int update_step_test(){
buzzuav_closures::buzzuav_update_battery(VM);
buzzuav_closures::buzzuav_update_prox(VM);
buzzuav_closures::buzzuav_update_currentpos(VM);
buzzuav_closures::buzzuav_update_flight_status(VM);
int a = buzzvm_function_call(VM, "step", 0);
if(a != BUZZVM_STATE_READY){
fprintf(stdout, "step test VM state %i\n",a);
fprintf(stdout, " execution terminated abnormally: %s\n\n",
buzz_error_info());
}
return a == BUZZVM_STATE_READY;
}
uint16_t get_robotid(){
/* Get hostname */
char hstnm[30];
gethostname(hstnm, 30);
/* Make numeric id from hostname */
/* NOTE: here we assume that the hostname is in the format Knn */
int id = strtol(hstnm + 1, NULL, 10);
return (uint16_t)id;
}
}

2
src/buzzuav_closures.cpp
View File

@ -263,7 +263,7 @@ int buzzuav_update_prox(buzzvm_t vm) {
/****************************************/
/****************************************/
int dummy_closure(buzzvm_t vm){ return buzzvm_ret0(vm);}
}

30
src/roscontroller.cpp
View File

@ -14,6 +14,8 @@ namespace rosbzz_node{
Initialize_pub_sub(n_c);
/*Compile the .bzz file to .basm, .bo and .bdbg*/
Compile_bzz();
set_bzz_file(bzzfile_name.c_str());
init_update_monitor(bcfname.c_str(),barrier);
}
/***Destructor***/
@ -21,7 +23,7 @@ namespace rosbzz_node{
{
/* All done */
/* Cleanup */
buzz_utility::buzz_script_destroy();
//buzz_utility::buzz_script_destroy();
/* Stop the robot */
uav_done();
}
@ -35,6 +37,8 @@ namespace rosbzz_node{
{
/*Update neighbors position inside Buzz*/
buzz_utility::neighbour_pos_callback(neighbours_pos_map);
/*Check updater state and step code*/
if(update_routine(bcfname.c_str(), dbgfname.c_str(),0)==CODE_RUNNING)
/*Step buzz script */
buzz_utility::buzz_script_step();
/*Prepare messages and publish them in respective topic*/
@ -49,6 +53,8 @@ namespace rosbzz_node{
maintain_pos(timer_step);
}
/* Destroy updater and Cleanup */
update_routine(bcfname.c_str(), dbgfname.c_str(),1);
}
}
@ -79,7 +85,8 @@ namespace rosbzz_node{
n_c.getParam("out_payload", out_payload);
/*Obtain in payload name*/
n_c.getParam("in_payload", in_payload);
/*Obtain Number of robots for barrier*/
n_c.getParam("No_of_Robots", barrier);
}
@ -121,7 +128,14 @@ namespace rosbzz_node{
dbgfname = bzzfile_in_compile.str();
}
/*Prepare messages for each step and publish*/
/*******************************************************************************************************/
/* Message format of payload (Each slot is uint64_t) */
/* _________________________________________________________________________________________________ */
/*| | | | | | */
/*|Pos x|Pos y|Pos z|Size in Uint64_t|robot_id|Buzz_msg_size|Buzz_msg|Buzz_msgs with size......... | */
/*|_____|_____|_____|________________________________________________|______________________________| */
/*******************************************************************************************************/
void roscontroller::prepare_msg_and_publish(){
/* flight controller client call if requested from Buzz*/
@ -161,7 +175,6 @@ namespace rosbzz_node{
for(std::vector<long unsigned int>::const_iterator it = payload_out.payload64.begin();
it != payload_out.payload64.end(); ++it){
message_obt[i] =(uint64_t) *it;
i++;
}
for(i=0;i<payload_out.payload64.size();i++){
@ -237,6 +250,15 @@ namespace rosbzz_node{
}
/*payload callback callback*/
/*******************************************************************************************************/
/* Message format of payload (Each slot is uint64_t) */
/* _________________________________________________________________________________________________ */
/*| | | | | | */
/*|Pos x|Pos y|Pos z|Size in Uint64_t|robot_id|Buzz_msg_size|Buzz_msg|Buzz_msgs with size......... | */
/*|_____|_____|_____|________________________________________________|______________________________| */
/*******************************************************************************************************/
void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg){
uint64_t message_obt[msg->payload64.size()];
int i = 0;