diff --git a/include/buzz_update.h b/include/buzz_update.h index baf1b4a..3e99d86 100644 --- a/include/buzz_update.h +++ b/include/buzz_update.h @@ -16,8 +16,6 @@ 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; @@ -47,33 +45,34 @@ struct updater_msgqueue_s { struct buzz_updater_elem_s { /* robot id */ uint16_t robotid; - /*current Bytecode content */ + /*current Bytecode content */ uint8_t* bcode; /*current bcode size*/ - size_t bcode_size; + size_t* bcode_size; + /*current Bytecode content */ + uint8_t* standby_bcode; + /*current bcode size*/ + size_t* standby_bcode_size; /*updater out msg queue */ updater_msgqueue_t outmsg_queue; /*updater in msg queue*/ updater_msgqueue_t inmsg_queue; /*Current state of the updater one in code_states_e ENUM*/ - int mode; - /*Table with state of other neighbours*/ - buzzdict_t state_dict; - /*Update number to ensure consistency*/ - uint8_t update_no; + 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*/ /**************************************************************************/ -int update_routine(const char* bcfname, - const char* dbgfname, int destroy); +void update_routine(const char* bcfname, + const char* dbgfname); /************************************************/ /*Initalizes the updater */ /************************************************/ -void init_update_monitor(const char* bo_filename,int barrier); +void init_update_monitor(const char* bo_filename,const char* stand_by_script,int barrier); /*********************************************************/ @@ -113,6 +112,8 @@ int get_update_mode(); /***************************************************/ void set_bzz_file(const char* in_bzz_file); +int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_size); + /****************************************************/ /*Destroys the updater*/ /***************************************************/ diff --git a/include/buzz_utility.h b/include/buzz_utility.h index 54417ba..2675e8b 100644 --- a/include/buzz_utility.h +++ b/include/buzz_utility.h @@ -51,4 +51,5 @@ int update_step_test(); uint16_t get_robotid(); +buzzvm_t get_vm(); } diff --git a/include/roscontroller.h b/include/roscontroller.h index 90fdc42..105f56c 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -46,7 +46,7 @@ private: //int oldcmdID=0; int rc_cmd; int barrier; - std::string bzzfile_name, fcclient_name, rcservice_name,bcfname,dbgfname,out_payload,in_payload; //, rcclient; + std::string bzzfile_name, fcclient_name, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by; //, rcclient; bool rcclient; ros::ServiceClient mav_client; ros::Publisher payload_pub; diff --git a/launch/rosbuzz_2_parallel.launch b/launch/rosbuzz_2_parallel.launch index 2c792c4..44841ea 100644 --- a/launch/rosbuzz_2_parallel.launch +++ b/launch/rosbuzz_2_parallel.launch @@ -11,6 +11,7 @@ + diff --git a/launch/rosbuzzm100.launch b/launch/rosbuzzm100.launch index 0c5c75c..3dd8394 100644 --- a/launch/rosbuzzm100.launch +++ b/launch/rosbuzzm100.launch @@ -10,6 +10,7 @@ + diff --git a/src/buzz_update.cpp b/src/buzz_update.cpp index 7131d4f..a877945 100644 --- a/src/buzz_update.cpp +++ b/src/buzz_update.cpp @@ -8,7 +8,7 @@ #include #include #include -#define MAX_BUCKETS 10 + /*Temp for data collection*/ //static int neigh=-1; @@ -23,35 +23,9 @@ static buzz_updater_elem_t updater; static int no_of_robot; static char* dbgf_name; static const char* bzz_file; +static int neigh=0; -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){ +void init_update_monitor(const char* bo_filename, const char* stand_by_script,int barrier){ fprintf(stdout,"intiialized file monitor.\n"); fd=inotify_init1(IN_NONBLOCK); if ( fd < 0 ) { @@ -80,23 +54,43 @@ void init_update_monitor(const char* bo_filename,int barrier){ //return 0; } fclose(fp); + uint8_t* STD_BO_BUF = 0; + fp = fopen(stand_by_script, "rb"); + + if(!fp) { + perror(stand_by_script); + + } + fseek(fp, 0, SEEK_END); + + size_t stdby_bcode_size = ftell(fp); + rewind(fp); + + STD_BO_BUF = (uint8_t*)malloc(stdby_bcode_size); + if(fread(STD_BO_BUF, 1, stdby_bcode_size, fp) < stdby_bcode_size) { + perror(stand_by_script); + + fclose(fp); + //return 0; + } + fclose(fp); 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; + updater->bcode_size = (size_t*) malloc(sizeof(size_t)); + updater->update_no = (uint8_t*) malloc(sizeof(uint16_t)); + *(uint16_t*)updater->update_no =0; + *(size_t*)(updater->bcode_size)=bcode_size; + updater->standby_bcode = STD_BO_BUF; + updater->standby_bcode_size = (size_t*)malloc(sizeof(size_t)); + *(size_t*)(updater->standby_bcode_size)=stdby_bcode_size; + updater->mode=(int*)malloc(sizeof(int)); + *(int*)updater->mode=CODE_RUNNING; no_of_robot=barrier; + //neigh = 0; //updater->outmsg_queue= // update_table->barrier=nvs; @@ -143,71 +137,25 @@ int check_update(){ } -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 : ]updater rid = %i \n",(int)updater->robotid); - fprintf(stdout,"[Debug : ]updater mode = %i \n",(int)updater->mode); - fprintf(stdout,"[Debug : ]update number = %i \n",(int)updater->update_no); - - *(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(4*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; - fprintf(stdout,"[Debug : ]updater rid = %i \n",(int)updater->robotid); - fprintf(stdout,"[Debug : ]updater mode = %i \n",(int)updater->mode); - fprintf(stdout,"[Debug : ]update number = %i \n",(int)updater->update_no); - - /*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_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*/ + *(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; + size+=sizeof(uint16_t); + memcpy(updater->outmsg_queue->queue+size, updater->bcode, *(size_t*)updater->bcode_size); + size+=*(size_t*)updater->bcode_size; + /*FILE *fp; + fp=fopen("update.bo", "wb"); + fwrite((updater->bcode), updater->bcode_size, 1, fp); + fclose(fp);*/ + *(uint16_t*)updater->outmsg_queue->size=size; + + //fprintf(stdout,"out mes append transfer code %d\n", transfer_code); } void code_message_inqueue_append(uint8_t* msg,uint16_t size){ @@ -221,75 +169,34 @@ memcpy(updater->inmsg_queue->queue, msg, 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_RUNNING){ - 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)){ - if(updater->mode==CODE_RUNNING){ - uint8_t update_no_obt=*(uint8_t*)(updater->inmsg_queue->queue+size); - size+=sizeof(uint8_t); - 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->update_no=update_no_obt; - 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(STATE_MSG); - updater->mode=CODE_STANDBY; - - //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; - } - } +if(*(int*)updater->mode==CODE_RUNNING){ + if(*(uint16_t*)(updater->inmsg_queue->queue+size) > *(uint16_t*) updater->update_no){ + uint16_t update_no=*(uint16_t*)(updater->inmsg_queue->queue+size); + size +=sizeof(uint16_t); + 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(test_set_code((updater->inmsg_queue->queue+size), + (char*) dbgf_name,(size_t)update_bcode_size)) { + *(uint16_t*)updater->update_no=update_no; + neigh=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; +void update_routine(const char* bcfname, + const char* dbgfname){ +//dbgf_name=(char*)dbgfname; //fprintf(stdout,"[Debug : ]updater value = %i \n",updater->mode); - if(updater->mode==CODE_RUNNING){ + if(*(int*)updater->mode==CODE_RUNNING){ if(check_update()){ std::string bzzfile_name(bzz_file); stringstream bzzfile_in_compile; @@ -347,86 +254,38 @@ dbgf_name=(char*)dbgfname; } 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; - } + if(test_set_code(BO_BUF, dbgfname,bcode_size)){ + uint16_t update_no =*(uint16_t*)(updater->update_no); + *(uint16_t*)(updater->update_no) =update_no +1; + code_message_outqueue_append(); + fprintf(stdout,"Update no %d\n", update_no); + } + neigh=0; + delete_p(BO_BUF); } } - code_message_outqueue_append(STATE_MSG); + } - else if (updater->mode==CODE_UPDATE){ - - code_message_outqueue_append(SEND_CODE); - updater->mode=CODE_STANDBY; + else{ - } - 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(SEND_CODE); - 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); + if(neigh==0) code_message_outqueue_append(); + buzzvm_t VM = buzz_utility::get_vm(); + buzzvm_pushs(VM, buzzvm_string_register(VM, "barrier_val",1)); + buzzvm_gload(VM); + buzzobj_t tObj = buzzvm_stack_at(VM, 1); + buzzvm_pop(VM); + fprintf(stdout,"Barrier ..................... %i \n",tObj->i.value); + if(tObj->i.value==no_of_robot) { + *(int*)updater->mode=CODE_RUNNING; //collect_data(); - buzz_utility::buzz_update_set((updater)->bcode, dbgf_name, (updater)->bcode_size); + buzz_utility::buzz_update_init_test((updater)->bcode, (char*)dbgfname, *(size_t*)(updater->bcode_size)); + //buzzvm_function_call(m_tBuzzVM, "updated", 0); } - - delete_p(tmp); - //fprintf(stdout,"freed tmp\n"); - + } - if(destroy){ - - destroy_updater(); - fprintf(stdout,"updater destoryed.\n"); - } -return updater->mode; } @@ -439,7 +298,44 @@ 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; +return (uint8_t*)updater->outmsg_queue->size; +} + +int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_size){ + if(buzz_utility::buzz_update_init_test(BO_BUF, dbgfname,bcode_size)){ + fprintf(stdout,"Initializtion of script test passed\n"); + if(buzz_utility::update_step_test()){ + /*data logging*/ + //start =1; + /*data logging*/ + fprintf(stdout,"Step test passed\n"); + *(int*)updater->mode = CODE_STANDBY; + //fprintf(stdout,"updater value = %i\n",updater->mode); + delete_p(updater->bcode); + updater->bcode = (uint8_t*)malloc(bcode_size); + memcpy(updater->bcode, BO_BUF, bcode_size); + *(size_t*)updater->bcode_size = bcode_size; + buzz_utility::buzz_update_init_test((updater)->standby_bcode, + (char*)dbgfname, *(size_t*)(updater->standby_bcode_size)); + buzzvm_t VM = buzz_utility::get_vm(); + buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); + buzzvm_pushi(VM, no_of_robot); + buzzvm_gstore(VM); + return 1; + } + /*Unable to step something wrong*/ + else{ + fprintf(stdout,"step test failed, stick to old script\n"); + buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, *(size_t*)(updater->bcode_size)); + return 0; + + } + } + else { + fprintf(stdout,"Initialization test failed\n"); + buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname,*(size_t*) (updater->bcode_size)); + return 0; + } } void destroy_out_msg_queue(){ @@ -450,19 +346,16 @@ delete_p(updater->outmsg_queue); } int get_update_mode(){ -return updater->mode; +return *(int*)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"); - +delete_p(updater->bcode_size); +delete_p(updater->standby_bcode); +delete_p(updater->standby_bcode_size); +delete_p(updater->mode); +delete_p(updater->update_no); if(updater->outmsg_queue){ delete_p(updater->outmsg_queue->queue); delete_p(updater->outmsg_queue->size); diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index ffaf378..9932676 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -84,39 +84,35 @@ namespace buzz_utility{ }*/ /* Go through the messages until there's nothing else to read */ uint16_t unMsgSize; + uint8_t is_msg_pres=*(uint8_t*)(pl + tot); + tot+=sizeof(uint8_t); + if(is_msg_pres){ unMsgSize = *(uint16_t*)(pl + tot); - - fprintf(stdout,"received Msg size : %i\n",(int) *(uint16_t*)(pl + tot)); - /*Xbee seems to send a lots of unknown message check later, added to avoid it, safe anyways*/ - if(*(uint16_t*)(pl + tot) >= 4){ - tot += sizeof(uint16_t); - code_message_inqueue_append(pl+tot,unMsgSize); - fprintf(stdout,"Msg 1 : %i , and Msg 2: %i\n",(int) *(uint16_t*)(pl+tot),(int) *(uint8_t*)(pl+tot+sizeof(uint16_t))); - 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); - } + tot+=sizeof(uint16_t); + // fprintf(stdout,"%u : read msg size : %u \n",m_unRobotId,unMsgSize); + if(unMsgSize>0){ + code_message_inqueue_append((uint8_t*)(pl + tot),unMsgSize); + tot+=unMsgSize; + code_message_inqueue_process(); } - /* Process messages */ - buzzvm_process_inmsgs(VM); } + unMsgSize=0; + + /*Obtain Buzz messages only when they are present*/ + 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); } /***************************************************/ /*Obtains messages from buzz out message Queue*/ @@ -130,20 +126,28 @@ namespace buzz_utility{ /* Send robot id */ *(uint16_t*)(buff_send+tot) = (uint16_t) VM->robot; tot += sizeof(uint16_t); - /*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*)(buff_send + 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*)(buff_send + tot),(int) *(uint8_t*)(buff_send + 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 */ + uint8_t updater_msg_pre = 0; + uint16_t updater_msgSize= 0; + if((int)get_update_mode()!=CODE_RUNNING){ + //fprintf(stdout,"transfer code %d\n", transfer_code); + updater_msg_pre =1; + //transfer_code=0; + *(uint8_t*)(buff_send + tot) = (uint8_t)updater_msg_pre; + tot += sizeof(uint8_t); + /*Append updater msg size*/ + *(uint16_t*)(buff_send + tot) = *(uint16_t*) (getupdate_out_msg_size()); + tot += sizeof(uint16_t); + /*Append updater msgs*/ + memcpy(buff_send + tot, (uint8_t*)(getupdater_out_msg()), updater_msgSize); + tot += updater_msgSize; + /*destroy the updater out msg queue*/ + destroy_out_msg_queue(); + } + else{ + *(uint8_t*)(buff_send + tot) = (uint8_t)updater_msg_pre; + tot += sizeof(uint8_t); + } + /* Send messages from FIFO */ do { /* Are there more messages? */ if(buzzoutmsg_queue_isempty(VM)) break; @@ -169,11 +173,7 @@ namespace buzz_utility{ buzzoutmsg_queue_next(VM); buzzmsg_payload_destroy(&m); } 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; @@ -489,7 +489,10 @@ namespace buzz_utility{ buzz_error_info()); buzzvm_dump(VM); } - /* Print swarm */ + /*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/ + /* look into this currently we don't have swarm feature at all */ + /*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/ + /*Print swarm*/ //buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot); /* Check swarm state */ /* int status = 1; @@ -560,6 +563,9 @@ uint16_t get_robotid(){ return (uint16_t)id; } +buzzvm_t get_vm(){ +return VM; +} } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index d5decde..9f34cf7 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -33,7 +33,7 @@ namespace rosbzz_node{ /* Set the Buzz bytecode */ if(buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(),robot_id)) { fprintf(stdout, "Bytecode file found and set\n"); - init_update_monitor(bcfname.c_str(),barrier); + init_update_monitor(bcfname.c_str(),stand_by.c_str(),barrier); while (ros::ok() && !buzz_utility::buzz_script_done()) { /*Update neighbors position inside Buzz*/ @@ -56,12 +56,14 @@ namespace rosbzz_node{ //std::cout<<"long obt"< ..." include "/home/ubuntu/buzz/src/include/vec2.bzz" +#################################################################################################### +# Updater related +# This should be here for the updater to work, changing position of code will crash the updater +#################################################################################################### +updated="update_ack" + +function updated_neigh(){ +neighbors.broadcast(updated, update_no) +} TARGET_ALTITUDE = 10.1 @@ -46,7 +55,7 @@ function hexagon() { # Constants # BARRIER_VSTIG = 1 -ROBOTS = 3 # number of robots in the swarm +#ROBOTS = 3 # number of robots in the swarm # # Sets a barrier