From 7e99692178e5927b44fea31fb2925dee8c8ad94b Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Thu, 7 Dec 2017 21:49:27 -0500 Subject: [PATCH] update back and working with diff version upto IEEE Software --- include/buzz_update.h | 37 +++- src/buzz_update.cpp | 398 ++++++++++++++++++++++++++++++++++-------- src/buzz_utility.cpp | 66 +------ src/roscontroller.cpp | 108 +++--------- 4 files changed, 379 insertions(+), 230 deletions(-) diff --git a/include/buzz_update.h b/include/buzz_update.h index bee36ef..a0be091 100644 --- a/include/buzz_update.h +++ b/include/buzz_update.h @@ -1,5 +1,8 @@ #ifndef BUZZ_UPDATE_H #define BUZZ_UPDATE_H +/*Simulation or robot check*/ +#define SIMULATION 1 + #include #include #include @@ -9,8 +12,10 @@ #include #define delete_p(p) do { free(p); p = NULL; } while(0) - - +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 */ /********************/ @@ -25,8 +30,8 @@ typedef enum { /********************/ typedef enum { - SEND_CODE = 0, // Broadcast code with state - STATE_MSG, // Broadcast state + SENT_CODE = 0, // Broadcast code + RESEND_CODE, // ReBroadcast request } code_message_e; /*************************/ @@ -39,6 +44,12 @@ struct updater_msgqueue_s { } ; 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; + /**************************/ /*Updater data*/ /**************************/ @@ -48,8 +59,14 @@ struct buzz_updater_elem_s { //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*/ @@ -61,19 +78,19 @@ struct buzz_updater_elem_s { /*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(const char* bcfname, - const char* dbgfname); +void update_routine(); /************************************************/ /*Initalizes the updater */ /************************************************/ -void init_update_monitor(const char* bo_filename,const char* stand_by_script); +void init_update_monitor(const char* bo_filename,const char* stand_by_script, + const char* dbgfname, int robot_id); /*********************************************************/ @@ -129,9 +146,11 @@ int get_update_status(); void set_read_update_status(); -int compile_bzz(); +int compile_bzz(std::string bzz_file); void updates_set_robots(int robots); +void set_packet_id(int packet_id); + void collect_data(std::ofstream &logger); #endif diff --git a/src/buzz_update.cpp b/src/buzz_update.cpp index 98f1b1a..4a1974a 100644 --- a/src/buzz_update.cpp +++ b/src/buzz_update.cpp @@ -21,21 +21,40 @@ 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* 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 int updated=0; +static const uint16_t MAX_UPDATE_TRY=5; +static int packet_id_=0; +static size_t old_byte_code_size=0; + /*Initialize updater*/ -void init_update_monitor(const char* bo_filename, const char* stand_by_script){ +void init_update_monitor(const char* bo_filename, const char* stand_by_script, + const char* dbgfname, int robot_id){ + Robot_id=robot_id; + dbgf_name=dbgfname; + bcfname=bo_filename; ROS_INFO("Initializing file monitor..."); fd=inotify_init1(IN_NONBLOCK); if ( fd < 0 ) { perror( "inotify_init error" ); } - /* watch /.bzz file for any activity and report it back to update */ - wd=inotify_add_watch(fd, bzz_file,IN_ALL_EVENTS ); + /*If simulation set the file monitor only for robot 1*/ + 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"); @@ -73,8 +92,11 @@ void init_update_monitor(const char* bo_filename, const char* stand_by_script){ 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; @@ -86,10 +108,11 @@ void init_update_monitor(const char* bo_filename, const char* stand_by_script){ *(int*)(updater->mode)=CODE_RUNNING; //no_of_robot=barrier; updater_msg_ready=0; - //neigh = 0; - //updater->outmsg_queue= - // update_table->barrier=nvs; - // open logger + + /*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); } /*Check for .bzz file chages*/ @@ -123,27 +146,131 @@ int check_update(){ 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<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<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)); - uint16_t size =0; - updater->outmsg_queue->queue = (uint8_t*)malloc(2*sizeof(uint16_t)+ *(size_t*)(updater->bcode_size)); - updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); - /*append the update no, code size and code to out msg*/ + /* 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) = *(size_t*) (updater->bcode_size); + *(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->bcode, *(size_t*)(updater->bcode_size)); - size+=(uint16_t)*(size_t*)(updater->bcode_size); - /*FILE *fp; - fp=fopen("update.bo", "wb"); - fwrite((updater->bcode), updater->bcode_size, 1, fp); - fclose(fp);*/ + 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; - *(uint16_t*)(updater->outmsg_queue->size)=size; - - //fprintf(stdout,"out msg append transfer code size %d\n", (int)*(size_t*) updater->bcode_size); +} + +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; + ROS_INFO("[Debug] Requested update no. %u with try: %u \n",update_no,update_try_counter); } void code_message_inqueue_append(uint8_t* msg,uint16_t size){ @@ -155,43 +282,158 @@ void code_message_inqueue_append(uint8_t* msg,uint16_t size){ *(uint16_t*)(updater->inmsg_queue->size) = size; } +void set_packet_id(int packet_id){ +/*Used for data logging*/ +packet_id_=packet_id; +} void code_message_inqueue_process(){ int size=0; + updater_code_t out_code=NULL; + ROS_INFO("[Debug] Updater processing in msg with mode %d \n", *(int*)(updater->mode) ); - ROS_INFO("[Debug] %u : Current update number, %u : Received update no \n",( *(uint16_t*) (updater->update_no) ), (*(uint16_t*)(updater->inmsg_queue->queue)) ); - ROS_INFO("[Debug] Updater received code of size %u \n",(*(uint16_t*)(updater->inmsg_queue->queue+sizeof(uint16_t)) ) ); + ROS_INFO("[Debug] %u : Current update number, %u : Received update no \n",( *(uint16_t*) (updater->update_no) ), (*(uint16_t*)(updater->inmsg_queue->queue+sizeof(uint8_t))) ); + ROS_INFO("[Debug] Updater received patch of size %u \n",(*(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( *(uint16_t*)(updater->inmsg_queue->queue) > *(uint16_t*) (updater->update_no) ){ - //fprintf(stdout,"[debug]Inside update number comparision"); - uint16_t update_no=*(uint16_t*)(updater->inmsg_queue->queue); - size +=sizeof(uint16_t); - uint16_t update_bcode_size =*(uint16_t*)(updater->inmsg_queue->queue+size); - size +=sizeof(uint16_t); - //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((uint8_t*)(updater->inmsg_queue->queue+size), - (char*) dbgf_name,(size_t)update_bcode_size) ) { - *(uint16_t*)(updater->update_no)=update_no; - neigh=1; - //gettimeofday(&t1, NULL); + 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) ) ){ + + printf("TEST PASSED!\n"); + *(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, could be corrupt patch\n"); + update_try_counter++; + outqueue_append_code_request(update_no); + } + } } + + } + size=0; + if(*(uint8_t*)(updater->inmsg_queue->queue) == RESEND_CODE){ + 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); + ROS_ERROR("Code appended! update try : %u \n",update_try_counter); + code_message_outqueue_append(); + + } + if(update_try_counter > MAX_UPDATE_TRY) ROS_ERROR("TODO: 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 update_routine(const char* bcfname, - const char* dbgfname){ - dbgf_name=(char*)dbgfname; + + + + + + +void create_update_patch(){ + std::stringstream genpatch; + std::stringstream usepatch; + + 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"; + + // CALL BSDIFF CMD + genpatch<< "bsdiff "<< path << name1 <<".bo "<< path << name2 <<".bo "<< path<<"diff.bo"; + fprintf(stdout,"Launching bsdiff command: %s \n", genpatch.str().c_str()); + system(genpatch.str().c_str()); + + // BETTER: CALL THE FUNCTION IN BSDIFF.CPP + //bsdiff_do(path + name1 + ".bo", path + name2 + ".bo", path + "diff.bo"); + + + + /* 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<patch); + updater->patch = patch_buff; + *(size_t*) (updater->patch_size) = patch_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)); @@ -199,19 +441,26 @@ void update_routine(const char* bcfname, //fprintf(stdout,"[Debug : ]updater value = %i \n",updater->mode); if(*(int*)updater->mode==CODE_RUNNING){ buzzvm_function_call(VM, "updated_neigh", 0); + // Check for update if(check_update()){ ROS_INFO("Update found \nUpdating script ...\n"); - if(compile_bzz()){ + if(compile_bzz(bzz_file)){ ROS_WARN("Errors in comipilg script so staying on old script\n"); } 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<update_no); *(uint16_t*)(updater->update_no) =update_no +1; - code_message_outqueue_append(); + + create_update_patch(); VM = buzz_utility::get_vm(); ROS_INFO("Current Update no %d\n", *(uint16_t*)(updater->update_no)); buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); @@ -244,11 +494,7 @@ void update_routine(const char* bcfname, else{ - if(neigh==0 && (!is_msg_present())){ - ROS_INFO("Sending code... \n"); - code_message_outqueue_append(); - } timer_steps++; buzzvm_pushs(VM, buzzvm_string_register(VM, "barrier_val",1)); buzzvm_gload(VM); @@ -259,10 +505,22 @@ void update_routine(const char* bcfname, *(int*)(updater->mode) = CODE_RUNNING; gettimeofday(&t2, NULL); //collect_data(); - buzz_utility::buzz_update_set((updater)->bcode, (char*)dbgfname, *(size_t*)(updater->bcode_size)); + buzz_utility::buzz_update_set((updater)->bcode, (char*)dbgf_name, *(size_t*)(updater->bcode_size)); //buzzvm_function_call(m_tBuzzVM, "updated", 0); - updated=1; + updated=1; + update_try_counter=0; + timer_steps=0; } + else if (timer_steps>TIMEOUT_FOR_ROLLBACK){ + ROS_ERROR("TIME OUT Reached, decided to roll 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); + updated=1; + update_try_counter=0; + timer_steps=0; + } } @@ -285,7 +543,7 @@ int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_size ){ ROS_WARN("Initializtion of script test passed\n"); if(buzz_utility::update_step_test()){ /*data logging*/ - //start =1; + old_byte_code_size=*(size_t*)updater->bcode_size; /*data logging*/ ROS_WARN("Step test passed\n"); *(int*) (updater->mode) = CODE_STANDBY; @@ -383,7 +641,6 @@ void destroy_updater(){ delete_p(updater->inmsg_queue->size); delete_p(updater->inmsg_queue); } - // inotify_rm_watch(fd,wd); close(fd); } @@ -399,7 +656,7 @@ void updates_set_robots(int robots){ /*-------------------------------------------------------- / Create Buzz bytecode from the bzz script inputed /-------------------------------------------------------*/ -int compile_bzz(){ +int compile_bzz(std::string bzz_file){ /*Compile the buzz code .bzz to .bo*/ std::string bzzfile_name(bzz_file); stringstream bzzfile_in_compile; @@ -407,24 +664,21 @@ int compile_bzz(){ 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<<","<<(int)*(uint8_t*)updater->update_no; - timer_steps=0; - neigh=0; - //fprintf(stdout,"Data logger Info : %i , %i , %f , %ld , %i , %d \n",(int)no_of_robot,neigh,time_spent,*(size_t*)updater->bcode_size,(int)no_of_robot,*(uint8_t*)updater->update_no); - //FILE *Fileptr; - //Fileptr=fopen("/home/ubuntu/ROS_WS/update_logger.csv", "a"); - //fprintf(Fileptr,"%i,%i,%i,%i,%i,%u\n",(int)buzz_utility::get_robotid(),neigh,timer_steps,(int)*(size_t*)updater->bcode_size,(int)no_of_robot, *(uint8_t*)updater->update_no); - //fclose(Fileptr); -} + //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<<","<bcode_size<<"," + <<*(size_t*)updater->patch_size<<","<<(int)*(uint8_t*)updater->update_no<<","<<(int)packet_id_; +} diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 90d5dfa..017c632 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -462,14 +462,8 @@ int create_stig_tables() { int buzz_script_set(const char* bo_filename, const char* bdbg_filename, int robot_id) { ROS_INFO(" Robot ID: %i" , robot_id); - /* Reset the Buzz VM */ - if(VM) buzzvm_destroy(&VM); - Robot_id = robot_id; - VM = buzzvm_new((int)robot_id); - /* Get rid of debug info */ - if(DBG_INFO) buzzdebug_destroy(&DBG_INFO); - DBG_INFO = buzzdebug_new(); - /* Read bytecode and fill in data structure */ + Robot_id=robot_id; + /* Read bytecode from file and fill the bo buffer*/ FILE* fd = fopen(bo_filename, "rb"); if(!fd) { perror(bo_filename); @@ -487,61 +481,11 @@ int create_stig_tables() { 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); - ROS_ERROR("[%i] %s: Error loading Buzz script", Robot_id, bo_filename); - return 0; - } - /* Register hook functions */ - if(buzz_register_hooks() != BUZZVM_STATE_READY) { - buzzvm_destroy(&VM); - buzzdebug_destroy(&DBG_INFO); - ROS_ERROR("[%i] Error registering hooks", Robot_id); - return 0; - } - - // Initialize UAVSTATE variable - buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1)); - buzzvm_pushs(VM, buzzvm_string_register(VM, "TURNEDOFF", 1)); - buzzvm_gstore(VM); - - /* Create vstig tables - if(create_stig_tables() != BUZZVM_STATE_READY) { - buzzvm_destroy(&VM); - buzzdebug_destroy(&DBG_INFO); - ROS_ERROR("[%i] Error creating stigmergy tables", Robot_id); - //cout << "ERROR!!!! ---------- " << VM->errormsg << endl; - //cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl; - return 0; - }*/ - + /* Save bytecode file name */ BO_FNAME = strdup(bo_filename); - // Execute the global part of the script - if(buzzvm_execute_script(VM)!= BUZZVM_STATE_DONE){ - ROS_ERROR("Error executing global part, VM state : %i",VM->state); - return 0; - } - // Call the Init() function - if(buzzvm_function_call(VM, "init", 0) != BUZZVM_STATE_READY){ - ROS_ERROR("Error in calling init, VM state : %i", VM->state); - return 0; - } - /* All OK */ - - ROS_INFO("[%i] INIT DONE!!!", Robot_id); - - return 1;//buzz_update_set(BO_BUF, bdbg_filename, bcode_size); + return buzz_update_set(BO_BUF, bdbg_filename, bcode_size); } /****************************************/ @@ -717,7 +661,7 @@ int create_stig_tables() { void update_sensors(){ /* Update sensors*/ buzzuav_closures::buzzuav_update_battery(VM); - buzzuav_closures::buzzuav_update_xbee_status(VM); + buzzuav_closures::buzzuav_update_xbee_status(VM); buzzuav_closures::buzzuav_update_prox(VM); buzzuav_closures::buzzuav_update_currentpos(VM); buzzuav_closures::update_neighbors(VM); diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index b03ac07..9674e03 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -48,17 +48,6 @@ roscontroller::roscontroller(ros::NodeHandle &n_c, ros::NodeHandle &n_c_priv) } else { robot_id = strtol(robot_name.c_str() + 5, NULL, 10); } - 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()); - } - path += "logger_"+std::to_string(robot_id)+"_0.log"; - log.open(path.c_str(), std::ios_base::trunc | std::ios_base::out); } /*--------------------- @@ -196,11 +185,13 @@ void roscontroller::RosControllerRun() /* Set the Buzz bytecode */ if (buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(), robot_id)) { + ROS_INFO("[%i] INIT DONE!!!", robot_id); int packet_loss_tmp,time_step=0; double cur_packet_loss=0; ROS_INFO("[%i] Bytecode file found and set", robot_id); std::string standby_bo = Compile_bzz(stand_by) + ".bo"; - //init_update_monitor(bcfname.c_str(), standby_bo.c_str()); + /*Intialize the update monitor*/ + init_update_monitor(bcfname.c_str(), standby_bo.c_str(),dbgfname.c_str(),robot_id); /*loop rate of ros*/ ros::Rate loop_rate(BUZZRATE); /////////////////////////////////////////////////////// @@ -208,31 +199,11 @@ void roscontroller::RosControllerRun() ////////////////////////////////////////////////////// //ROS_WARN("[%i] -----------------------STARTING MAIN LOOP!", robot_id); while (ros::ok() && !buzz_utility::buzz_script_done()) { - /*Update neighbors position inside Buzz*/ - // buzz_closure::neighbour_pos_callback(neighbours_pos_map); - - /*log ROS Time stamp, pos (lat,long,alt), Swarm size, no. of neigh, - neigh pos, RSSI val, Packet loss, filtered packet loss*/ - log<::iterator it = - neighbours_pos_map.begin(); - log << neighbours_pos_map.size()<< ","; - for (; it != neighbours_pos_map.end(); ++it) - { - log << (double)it->second.x << "," << (double)it->second.y - << "," << (double)it->second.z << ","; - } - const uint8_t shrt_id= 0xFF; - float result; - /*Neighbours of the robot published with id in respective topic*/ neighbours_pos_publisher(); send_MPpayload(); /*Check updater state and step code*/ - // update_routine(bcfname.c_str(), dbgfname.c_str()); + update_routine(); if(time_step==BUZZRATE){ time_step=0; cur_packet_loss= 1 -( (double)packet_loss_tmp/(BUZZRATE*( (int)no_of_robots-1 )) ); @@ -245,8 +216,6 @@ void roscontroller::RosControllerRun() time_step++; } if(debug) ROS_WARN("CURRENT PACKET DROP : %f ",cur_packet_loss); - /*Log In Msg queue size*/ - log<<(int)buzz_utility::get_inmsg_size()<<","; /*Step buzz script */ buzz_utility::buzz_script_step(); /*Prepare messages and publish them in respective topic*/ @@ -254,37 +223,14 @@ void roscontroller::RosControllerRun() /*call flight controler service to set command long*/ flight_controller_service_call(); /*Set multi message available after update*/ - //if (get_update_status()) - //{ - /* set_read_update_status(); - multi_msg = true; - log << cur_pos.latitude << "," << cur_pos.longitude << "," - << cur_pos.altitude << ","; - collect_data(log); - map::iterator it = - neighbours_pos_map.begin(); - log << "," << neighbours_pos_map.size(); - for (; it != neighbours_pos_map.end(); ++it) - { - log << "," << (double)it->second.x << "," << (double)it->second.y - << "," << (double)it->second.z; - } - log << std::endl;*/ - //} + if (get_update_status()) + { + set_read_update_status(); + } /*Set ROBOTS variable for barrier in .bzz from neighbours count*/ - // no_of_robots=get_number_of_robots(); get_number_of_robots(); buzz_utility::set_robot_var(no_of_robots); - /*Retrive the state of the graph and uav and log*/ - std::stringstream state_buff; - state_buff << buzzuav_closures::getuavstate(); - log<0) no_of_robots - // =neighbours_pos_map.size()+1; - // buzz_utility::set_robot_var(no_of_robots); - /*Set no of robots for updates TODO only when not updating*/ - // if(multi_msg) - //updates_set_robots(no_of_robots); + updates_set_robots(no_of_robots); //get_xbee_status(); // commented out because it may slow down the node too much, to be tested /*run once*/ ros::spinOnce(); @@ -299,11 +245,8 @@ void roscontroller::RosControllerRun() /*sleep for the mentioned loop rate*/ timer_step += 1; maintain_pos(timer_step); - // std::cout<< "HOME: " << home.latitude << ", " << home.longitude; } - /* Destroy updater and Cleanup */ - // update_routine(bcfname.c_str(), dbgfname.c_str(),1); } } @@ -605,6 +548,7 @@ void roscontroller::prepare_msg_and_publish() tmp[2] = cur_pos.altitude; memcpy(position, tmp, 3 * sizeof(uint64_t)); mavros_msgs::Mavlink payload_out; + payload_out.payload64.push_back(XBEE_MESSAGE_CONSTANT); payload_out.payload64.push_back(position[0]); payload_out.payload64.push_back(position[1]); payload_out.payload64.push_back(position[2]); @@ -621,30 +565,25 @@ void roscontroller::prepare_msg_and_publish() payload_out.sysid = (uint8_t)robot_id; payload_out.msgid = (uint32_t)message_number; - /*Log out message id and message size*/ - log<<(int)message_number<<","; - log<<(int)out[0]<<","; /*publish prepared messages in respective topic*/ payload_pub.publish(payload_out); delete[] out; delete[] payload_out_ptr; /*Check for updater message if present send*/ - /* if ((int)get_update_mode() != CODE_RUNNING && is_msg_present() == 1 && - multi_msg) + if (is_msg_present()) { uint8_t *buff_send = 0; uint16_t updater_msgSize = *(uint16_t *)(getupdate_out_msg_size()); ; int tot = 0; mavros_msgs::Mavlink update_packets; - fprintf(stdout, "Transfering code \n"); + fprintf(stdout, "Appending code into message ...\n"); fprintf(stdout, "Sent Update packet Size: %u \n", updater_msgSize); // allocate mem and clear it buff_send = (uint8_t *)malloc(sizeof(uint16_t) + updater_msgSize); memset(buff_send, 0, sizeof(uint16_t) + updater_msgSize); // Append updater msg size *(uint16_t *)(buff_send + tot) = updater_msgSize; - // fprintf(stdout,"Updater sent msg size : %i \n", (int)updater_msgSize); tot += sizeof(uint16_t); // Append updater msgs memcpy(buff_send + tot, (uint8_t *)(getupdater_out_msg()), updater_msgSize); @@ -669,9 +608,9 @@ void roscontroller::prepare_msg_and_publish() update_packets.sysid = (uint8_t)robot_id; update_packets.msgid = (uint32_t)message_number; payload_pub.publish(update_packets); - multi_msg = false; + //multi_msg = false; delete[] payload_64; - }*/ + } } /*--------------------------------------------------------------------------------- /Flight controller service call every step if there is a command set from bzz @@ -1041,8 +980,7 @@ void roscontroller::SetStreamRate(int id, int rate, int on_off) { /*******************************************************************************************************/ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr &msg) { /*Check for Updater message, if updater message push it into updater FIFO*/ - if ((uint64_t)msg->payload64[0] == (uint64_t)UPDATER_MESSAGE_CONSTANT && - msg->payload64.size() > 5) { + if ((uint64_t)msg->payload64[0] == (uint64_t)UPDATER_MESSAGE_CONSTANT) { uint16_t obt_msg_size = sizeof(uint64_t) * (msg->payload64.size()); uint64_t message_obt[obt_msg_size]; /* Go throught the obtained payload*/ @@ -1055,26 +993,20 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr &msg) { /* Copy packet into temporary buffer neglecting update constant */ memcpy((void *)pl, (void *)(message_obt + 1), obt_msg_size); uint16_t unMsgSize = *(uint16_t *)(pl); - // uint16_t tot; - // tot+=sizeof(uint16_t); - fprintf(stdout, "Update packet, read msg size : %u \n", unMsgSize); + fprintf(stdout, "Update packet received, read msg size : %u \n", unMsgSize); if (unMsgSize > 0) { code_message_inqueue_append((uint8_t *)(pl + sizeof(uint16_t)), unMsgSize); - // fprintf(stdout,"before in queue process : utils\n"); code_message_inqueue_process(); - // fprintf(stdout,"after in queue process : utils\n"); } free(pl); } /*BVM FIFO message*/ - else if (msg->payload64.size() > 3) { - uint64_t message_obt[msg->payload64.size()]; + else if (msg->payload64[0]==(uint64_t)XBEE_MESSAGE_CONSTANT) { + uint64_t message_obt[msg->payload64.size()-1]; /* Go throught the obtained payload*/ - for (int i = 0; i < (int)msg->payload64.size(); i++) { - message_obt[i] = (uint64_t)msg->payload64[i]; - // cout<<"[Debug:] obtaind message "<payload64.size(); i++) { + message_obt[i-1] = (uint64_t)msg->payload64[i]; } /* Extract neighbours position from payload*/ double neighbours_pos_payload[3];