diff --git a/CMakeLists.txt b/CMakeLists.txt index 1e8ead7..98e62f3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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 diff --git a/include/buzz_update.h b/include/buzz_update.h new file mode 100644 index 0000000..baf1b4a --- /dev/null +++ b/include/buzz_update.h @@ -0,0 +1,122 @@ +#ifndef BUZZ_UPDATE_H +#define BUZZ_UPDATE_H +#include +#include +#include +#include +#include +#include + +#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 diff --git a/include/buzz_utility.h b/include/buzz_utility.h index ee85a0f..9acb3dc 100644 --- a/include/buzz_utility.h +++ b/include/buzz_utility.h @@ -2,6 +2,7 @@ #include #include "buzz_utility.h" #include "buzzuav_closures.h" +#include "buzz_update.h" #include #include #include @@ -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(); + + } diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index 1a9b56f..428c745 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -74,5 +74,6 @@ int buzzuav_update_prox(buzzvm_t vm); int bzz_cmd(); +int dummy_closure(buzzvm_t vm); //#endif } diff --git a/include/roscontroller.h b/include/roscontroller.h index 5ca5220..c03298b 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -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; diff --git a/launch/rosbuzz_2_parallel.launch b/launch/rosbuzz_2_parallel.launch index d3e1c0e..2c792c4 100644 --- a/launch/rosbuzz_2_parallel.launch +++ b/launch/rosbuzz_2_parallel.launch @@ -10,6 +10,7 @@ + diff --git a/launch/rosbuzzm100.launch b/launch/rosbuzzm100.launch index c2f74e8..4af488e 100644 --- a/launch/rosbuzzm100.launch +++ b/launch/rosbuzzm100.launch @@ -9,6 +9,7 @@ + diff --git a/src/buzz_update.cpp b/src/buzz_update.cpp new file mode 100644 index 0000000..c3d2c73 --- /dev/null +++ b/src/buzz_update.cpp @@ -0,0 +1,486 @@ +#include +#include +#include +#include +#include +#include "buzz_update.h" +#include +#include +#include +#include +#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(imask & (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<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(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); +} + diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 0c970e5..2de54e7 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -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 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; +} + + } diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 1965358..a2195c4 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -263,7 +263,7 @@ int buzzuav_update_prox(buzzvm_t vm) { /****************************************/ /****************************************/ - +int dummy_closure(buzzvm_t vm){ return buzzvm_ret0(vm);} } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 82fe479..0f07329 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -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::const_iterator it = payload_out.payload64.begin(); it != payload_out.payload64.end(); ++it){ message_obt[i] =(uint64_t) *it; - i++; } for(i=0;ipayload64.size()]; int i = 0;