From 817b2951d6821c58258e82db630809a227b617fb Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Wed, 3 May 2017 15:07:36 -0400 Subject: [PATCH 1/3] fix test bzz --- src/roscontroller.cpp | 3 ++- src/testflockfev.bzz | 16 ++++++++-------- 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index d4f0830..26f5508 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -150,7 +150,8 @@ namespace rosbzz_node{ if(!xbeeplugged){ if(n_c.getParam("name", robot_name)); else {ROS_ERROR("Provide the xbee plugged boolean in Launch file"); system("rosnode kill rosbuzz_node");} - } + }else + n_c.getParam("xbee_status_srv", xbeesrv_name); GetSubscriptionParameters(n_c); // initialize topics to null? diff --git a/src/testflockfev.bzz b/src/testflockfev.bzz index 30e5b80..1845eb9 100644 --- a/src/testflockfev.bzz +++ b/src/testflockfev.bzz @@ -43,15 +43,15 @@ function hexagon() { math.vec2.scale(accum, 1.0 / neighbors.count()) # Move according to vector #print("Robot ", id, "must push ",accum.length, "; ", accum.angle) -# uav_moveto(accum.x,accum.y) + uav_moveto(accum.x,accum.y) - if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS - timeW =0 - statef=land - } else { - timeW = timeW+1 - uav_moveto(0.0,0.0) - } +# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS +# timeW =0 +# statef=land +# } else { +# timeW = timeW+1 +# uav_moveto(0.0,0.0) +# } } ######################################## From 50327cb6b680c230ab6c5dae2c6f1e150286bb15 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Sat, 6 May 2017 18:42:27 -0400 Subject: [PATCH 2/3] Swarm table fix --- launch/rosbuzzm100.launch | 10 ++++++---- src/buzz_utility.cpp | 12 +++++++----- src/roscontroller.cpp | 12 ++++++------ 3 files changed, 19 insertions(+), 15 deletions(-) diff --git a/launch/rosbuzzm100.launch b/launch/rosbuzzm100.launch index c44b99e..0c0b0e7 100644 --- a/launch/rosbuzzm100.launch +++ b/launch/rosbuzzm100.launch @@ -2,15 +2,17 @@ - + + - + - - + + + diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index c9cbd69..cf39566 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -450,8 +450,8 @@ buzzvm_dup(VM); buzzswarm_elem_t e = *(buzzswarm_elem_t*)data; int* status = (int*)params; if(*status == 3) return; - fprintf(stderr, "CHECKING SWARM MEMBERS:%i\n",buzzdarray_get(e->swarms, 0, uint16_t)); - if(buzzdarray_size(e->swarms) != 1) { + fprintf(stderr, "CHECKING SWARM :%i, memeber: %i, age: %i \n",buzzdarray_get(e->swarms, 0, uint16_t), *(uint16_t*)key, e->age); + if(buzzdarray_size(e->swarms) != 1) { fprintf(stderr, "Swarm list size is not 1\n"); *status = 3; } @@ -504,16 +504,18 @@ buzzvm_dup(VM); buzz_error_info()); buzzvm_dump(VM); } + /* Update swarm membership */ + buzzswarm_members_update(VM->swarmmembers); //buzzvm_process_outmsgs(VM); //--> done in out_msg_process() function called each step //usleep(10000); /*Print swarm*/ - //buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot); + buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot); //int SwarmSize = buzzdict_size(VM->swarmmembers)+1; //fprintf(stderr, "Real Swarm Size: %i\n",SwarmSize); /* Check swarm state -- Not crashing thanks to test added in check_swarm_members */ -// int status = 1; -// buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status); + int status = 1; + buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status); } /****************************************/ diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 26f5508..faf8c29 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -26,7 +26,7 @@ namespace rosbzz_node{ if(xbeeplugged) GetRobotId(); else - robot_id=strtol(robot_name.c_str() + 5, NULL, 10);; + robot_id= strtol(robot_name.c_str() + 5, NULL, 10);; setpoint_counter = 0; fcu_timeout = TIMEOUT; home[0]=0.0;home[1]=0.0;home[2]=0.0; @@ -268,16 +268,16 @@ namespace rosbzz_node{ bzzfile_in_compile.str(""); std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); name = name.substr(0,name.find_last_of(".")); - bzzfile_in_compile << "bzzparse "< Date: Sat, 6 May 2017 23:07:52 -0400 Subject: [PATCH 3/3] updates addition and swarm table correction --- include/buzz_utility.h | 6 +- launch/rosbuzzm100.launch | 2 +- src/buzz_update.cpp | 361 ++++++++++++++++++-------------------- src/buzz_utility.cpp | 320 ++++++++++++++++----------------- src/roscontroller.cpp | 20 +-- 5 files changed, 333 insertions(+), 376 deletions(-) diff --git a/include/buzz_utility.h b/include/buzz_utility.h index 3667363..60df181 100644 --- a/include/buzz_utility.h +++ b/include/buzz_utility.h @@ -32,9 +32,11 @@ void neighbour_pos_callback(std::map< int, Pos_struct> neighbours_pos_map); int buzz_update_users_stigmergy(buzzobj_t tbl); -void in_msg_process(uint64_t* payload); +void in_msg_append(uint64_t* payload); -uint64_t* out_msg_process(); +uint64_t* obt_out_msg(); + +void update_sensors(); int buzz_script_set(const char* bo_filename, const char* bdbg_filename, int robot_id); diff --git a/launch/rosbuzzm100.launch b/launch/rosbuzzm100.launch index 0c0b0e7..01d9402 100644 --- a/launch/rosbuzzm100.launch +++ b/launch/rosbuzzm100.launch @@ -13,7 +13,7 @@ - + diff --git a/src/buzz_update.cpp b/src/buzz_update.cpp index d622228..3a77dba 100644 --- a/src/buzz_update.cpp +++ b/src/buzz_update.cpp @@ -27,80 +27,72 @@ static int neigh=-1; static int updater_msg_ready ; static int updated=0; +/*Initialize updater*/ void init_update_monitor(const char* bo_filename, const char* stand_by_script){ 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 */ + 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 ); - + /*load the .bo under execution into the updater*/ uint8_t* BO_BUF = 0; FILE* fp = fopen(bo_filename, "rb"); - - if(!fp) { - perror(bo_filename); - - } - fseek(fp, 0, SEEK_END); - - size_t bcode_size = ftell(fp); - rewind(fp); - - BO_BUF = (uint8_t*)malloc(bcode_size); - if(fread(BO_BUF, 1, bcode_size, fp) < bcode_size) { - perror(bo_filename); - - fclose(fp); - //return 0; - } - fclose(fp); + if(!fp) { + perror(bo_filename); + } + fseek(fp, 0, SEEK_END); + size_t bcode_size = ftell(fp); + rewind(fp); + BO_BUF = (uint8_t*)malloc(bcode_size); + if(fread(BO_BUF, 1, bcode_size, fp) < bcode_size) { + perror(bo_filename); + fclose(fp); + //return 0; + } + fclose(fp); + /*Load stand_by .bo file into the updater*/ uint8_t* STD_BO_BUF = 0; fp = fopen(stand_by_script, "rb"); + if(!fp) { + perror(stand_by_script); - 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->outmsg_queue = NULL; - updater->inmsg_queue = NULL; - updater->bcode_size = (size_t*) malloc(sizeof(size_t)); - updater->update_no = (uint8_t*) malloc(sizeof(uint16_t)); - *(uint16_t*)(updater->update_no) =0; - *(size_t*)(updater->bcode_size)=bcode_size; - updater->standby_bcode = STD_BO_BUF; - updater->standby_bcode_size = (size_t*)malloc(sizeof(size_t)); - *(size_t*)(updater->standby_bcode_size)=stdby_bcode_size; - updater->mode=(int*)malloc(sizeof(int)); - *(int*)(updater->mode)=CODE_RUNNING; - //no_of_robot=barrier; - updater_msg_ready=0; - //neigh = 0; - //updater->outmsg_queue= - // update_table->barrier=nvs; + } + fseek(fp, 0, SEEK_END); + size_t stdby_bcode_size = ftell(fp); + rewind(fp); + STD_BO_BUF = (uint8_t*)malloc(stdby_bcode_size); + if(fread(STD_BO_BUF, 1, stdby_bcode_size, fp) < stdby_bcode_size) { + perror(stand_by_script); + fclose(fp); + //return 0; + } + fclose(fp); + /*Create the updater*/ + updater = (buzz_updater_elem_t)malloc(sizeof(struct buzz_updater_elem_s)); + /*Intialize the updater with the required data*/ + updater->bcode = BO_BUF; + updater->outmsg_queue = NULL; + updater->inmsg_queue = NULL; + updater->bcode_size = (size_t*) malloc(sizeof(size_t)); + updater->update_no = (uint8_t*) malloc(sizeof(uint16_t)); + *(uint16_t*)(updater->update_no) =0; + *(size_t*)(updater->bcode_size)=bcode_size; + updater->standby_bcode = STD_BO_BUF; + updater->standby_bcode_size = (size_t*)malloc(sizeof(size_t)); + *(size_t*)(updater->standby_bcode_size)=stdby_bcode_size; + updater->mode=(int*)malloc(sizeof(int)); + *(int*)(updater->mode)=CODE_RUNNING; + //no_of_robot=barrier; + updater_msg_ready=0; + //neigh = 0; + //updater->outmsg_queue= + // update_table->barrier=nvs; } - +/*Check for .bzz file chages*/ int check_update(){ - struct inotify_event *event; char buf[1024]; int check =0; @@ -108,7 +100,6 @@ int check_update(){ int len=read(fd,buf,1024); while(imask & (IN_MODIFY| IN_DELETE_SELF)){ @@ -123,20 +114,12 @@ int check_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; +return check; } @@ -163,57 +146,56 @@ void code_message_outqueue_append(){ } void code_message_inqueue_append(uint8_t* msg,uint16_t size){ -updater->inmsg_queue=(updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); -fprintf(stdout,"in ms append code size %d\n", (int) size); -updater->inmsg_queue->queue = (uint8_t*)malloc(size); -updater->inmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); -memcpy(updater->inmsg_queue->queue, msg, size); -*(uint16_t*)(updater->inmsg_queue->size) = size; - + updater->inmsg_queue=(updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); + fprintf(stdout,"in ms append code size %d\n", (int) size); + updater->inmsg_queue->queue = (uint8_t*)malloc(size); + updater->inmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); + memcpy(updater->inmsg_queue->queue, msg, size); + *(uint16_t*)(updater->inmsg_queue->size) = size; } void code_message_inqueue_process(){ -int size=0; -fprintf(stdout,"[debug]Updater mode %d \n", *(int*)(updater->mode) ); -fprintf(stdout,"[debug] %u : current update number, %u : received update no \n",( *(uint16_t*) (updater->update_no) ), (*(uint16_t*)(updater->inmsg_queue->queue)) ); -fprintf(stdout,"[debug]Updater code size %u \n",(*(uint16_t*)(updater->inmsg_queue->queue+sizeof(uint16_t)) ) ); + int size=0; + fprintf(stdout,"[debug]Updater mode %d \n", *(int*)(updater->mode) ); + fprintf(stdout,"[debug] %u : current update number, %u : received update no \n",( *(uint16_t*) (updater->update_no) ), (*(uint16_t*)(updater->inmsg_queue->queue)) ); + fprintf(stdout,"[debug]Updater code size %u \n",(*(uint16_t*)(updater->inmsg_queue->queue+sizeof(uint16_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; + 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; + } } } -} -//fprintf(stdout,"in queue freed\n"); -delete_p(updater->inmsg_queue->queue); -delete_p(updater->inmsg_queue->size); -delete_p(updater->inmsg_queue); + //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; -buzzvm_t VM = buzz_utility::get_vm(); + dbgf_name=(char*)dbgfname; + buzzvm_t VM = buzz_utility::get_vm(); -buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); - buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); - buzzvm_gstore(VM); -//fprintf(stdout,"[Debug : ]updater value = %i \n",updater->mode); + buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); + buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); + buzzvm_gstore(VM); + //fprintf(stdout,"[Debug : ]updater value = %i \n",updater->mode); if(*(int*)updater->mode==CODE_RUNNING){ buzzvm_function_call(VM, "updated_neigh", 0); if(check_update()){ @@ -226,65 +208,55 @@ buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); name = name.substr(0,name.find_last_of(".")); bzzfile_in_compile << "bzzparse "<update_no); - *(uint16_t*)(updater->update_no) =update_no +1; - code_message_outqueue_append(); - VM = buzz_utility::get_vm(); - fprintf(stdout,"Update no %d\n", *(uint16_t*)(updater->update_no)); - buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); - buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); - buzzvm_gstore(VM); - neigh=-1; - fprintf(stdout,"Sending code... \n"); - code_message_outqueue_append(); + uint16_t update_no =*(uint16_t*)(updater->update_no); + *(uint16_t*)(updater->update_no) =update_no +1; + code_message_outqueue_append(); + VM = buzz_utility::get_vm(); + fprintf(stdout,"Update no %d\n", *(uint16_t*)(updater->update_no)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); + buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); + buzzvm_gstore(VM); + neigh=-1; + fprintf(stdout,"Sending code... \n"); + code_message_outqueue_append(); } delete_p(BO_BUF); } @@ -330,7 +302,6 @@ return (uint8_t*)updater->outmsg_queue->queue; uint8_t* getupdate_out_msg_size(){ //fprintf(stdout,"[Debug from get out size in util: ]size = %i \n",*(uint16_t*)updater->outmsg_queue->size); - return (uint8_t*)updater->outmsg_queue->size; } @@ -397,17 +368,17 @@ int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_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); -updater_msg_ready=0; + //fprintf(stdout,"out queue freed\n"); + delete_p(updater->outmsg_queue->queue); + delete_p(updater->outmsg_queue->size); + delete_p(updater->outmsg_queue); + updater_msg_ready=0; } int get_update_status(){ return updated; } void set_read_update_status(){ -updated=0; + updated=0; } int get_update_mode(){ return (int)*(int*)(updater->mode); @@ -420,44 +391,44 @@ buzz_updater_elem_t get_updater(){ return updater; } void destroy_updater(){ -delete_p(updater->bcode); -delete_p(updater->bcode_size); -delete_p(updater->standby_bcode); -delete_p(updater->standby_bcode_size); -delete_p(updater->mode); -delete_p(updater->update_no); -if(updater->outmsg_queue){ -delete_p(updater->outmsg_queue->queue); -delete_p(updater->outmsg_queue->size); -delete_p(updater->outmsg_queue); -} -if(updater->inmsg_queue){ -delete_p(updater->inmsg_queue->queue); -delete_p(updater->inmsg_queue->size); -delete_p(updater->inmsg_queue); -} -// -inotify_rm_watch(fd,wd); -close(fd); -} + delete_p(updater->bcode); + delete_p(updater->bcode_size); + delete_p(updater->standby_bcode); + delete_p(updater->standby_bcode_size); + delete_p(updater->mode); + delete_p(updater->update_no); + if(updater->outmsg_queue){ + delete_p(updater->outmsg_queue->queue); + delete_p(updater->outmsg_queue->size); + delete_p(updater->outmsg_queue); + } + if(updater->inmsg_queue){ + delete_p(updater->inmsg_queue->queue); + delete_p(updater->inmsg_queue->size); + delete_p(updater->inmsg_queue); + } + // + inotify_rm_watch(fd,wd); + close(fd); + } void set_bzz_file(const char* in_bzz_file){ -bzz_file=in_bzz_file; + bzz_file=in_bzz_file; } void updates_set_robots(int robots){ -no_of_robot=robots; + no_of_robot=robots; } 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; -//int bytecodesize=(int); -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); + //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; + //int bytecodesize=(int); + 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); } diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index cf39566..a6297cf 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -45,10 +45,10 @@ namespace buzz_utility{ uint16_t* out = new uint16_t[4]; uint32_t int32_1 = u64 & 0xFFFFFFFF; uint32_t int32_2 = (u64 & 0xFFFFFFFF00000000 ) >> 32; - out[0] = int32_1 & 0xFFFF; - out[1] = (int32_1 & (0xFFFF0000) ) >> 16; - out[2] = int32_2 & 0xFFFF; - out[3] = (int32_2 & (0xFFFF0000) ) >> 16; + out[0] = int32_1 & 0xFFFF; + out[1] = (int32_1 & (0xFFFF0000) ) >> 16; + out[2] = int32_2 & 0xFFFF; + out[3] = (int32_2 & (0xFFFF0000) ) >> 16; //cout << " values " < sizeof(uint16_t) && unMsgSize > 0); free(pl); - /* Process messages */ - buzzvm_process_inmsgs(VM); + } /***************************************************/ /*Obtains messages from buzz out message Queue*/ /***************************************************/ - uint64_t* out_msg_process(){ - buzzvm_process_outmsgs(VM); + uint64_t* obt_out_msg(){ + uint8_t* buff_send =(uint8_t*)malloc(MAX_MSG_SIZE); memset(buff_send, 0, MAX_MSG_SIZE); /*Taking into consideration the sizes included at the end*/ @@ -151,8 +150,8 @@ namespace buzz_utility{ cout<<" payload from out msg "<<*(payload_64+i)<pc, VM->errormsg); } - return msg; + return msg; } /****************************************/ @@ -254,87 +253,86 @@ namespace buzz_utility{ /****************************************/ /*Sets the .bzz and .bdbg file*/ /****************************************/ - int buzz_script_set(const char* bo_filename, const char* bdbg_filename, int robot_id) { - cout << " Robot ID: " <swarms, 0, uint16_t), *(uint16_t*)key, e->age); - if(buzzdarray_size(e->swarms) != 1) { - fprintf(stderr, "Swarm list size is not 1\n"); - *status = 3; - } - else { - int sid = 1; - if(!buzzdict_isempty(VM->swarms)) { - 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; - } - } - if(buzzdict_size(VM->swarms)>1) { - 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; - } - } - } + buzzswarm_elem_t e = *(buzzswarm_elem_t*)data; + int* status = (int*)params; + if(*status == 3) return; + fprintf(stderr, "CHECKING SWARM :%i, memeber: %i, age: %i \n", + buzzdarray_get(e->swarms, 0, uint16_t), *(uint16_t*)key, e->age); + if(buzzdarray_size(e->swarms) != 1) { + fprintf(stderr, "Swarm list size is not 1\n"); + *status = 3; + } + else { + int sid = 1; + if(!buzzdict_isempty(VM->swarms)) { + 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; + } + } + if(buzzdict_size(VM->swarms)>1) { + 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 update_sensors(){ + /* Update sensors*/ + buzzuav_closures::buzzuav_update_battery(VM); + buzzuav_closures::buzzuav_update_prox(VM); + buzzuav_closures::buzzuav_update_currentpos(VM); + buzzobj_t tbl = buzzuav_closures::buzzuav_update_userspos(VM); + buzzuav_closures::buzzuav_update_flight_status(VM); + //buzz_update_users_stigmergy(tbl); + } void buzz_script_step() { - /* - * Update sensors - */ - buzzuav_closures::buzzuav_update_battery(VM); - buzzuav_closures::buzzuav_update_prox(VM); - buzzuav_closures::buzzuav_update_currentpos(VM); - buzzobj_t tbl = buzzuav_closures::buzzuav_update_userspos(VM); - buzzuav_closures::buzzuav_update_flight_status(VM); - - //buzz_update_users_stigmergy(tbl); - - - /* - * Call Buzz step() function - */ - if(buzzvm_function_call(VM, "step", 0) != BUZZVM_STATE_READY) { - fprintf(stderr, "%s: execution terminated abnormally: %s\n\n", + /* Process messages */ + buzzvm_process_inmsgs(VM); + /*Update sensors*/ + update_sensors(); + /* 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); - } - /* Update swarm membership */ - buzzswarm_members_update(VM->swarmmembers); - //buzzvm_process_outmsgs(VM); //--> done in out_msg_process() function called each step - //usleep(10000); - /*Print swarm*/ - buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot); - //int SwarmSize = buzzdict_size(VM->swarmmembers)+1; - //fprintf(stderr, "Real Swarm Size: %i\n",SwarmSize); + buzzvm_dump(VM); + } + /* Process out messages */ + buzzvm_process_outmsgs(VM); + /*Print swarm*/ + buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot); + //int SwarmSize = buzzdict_size(VM->swarmmembers)+1; + //fprintf(stderr, "Real Swarm Size: %i\n",SwarmSize); - /* Check swarm state -- Not crashing thanks to test added in check_swarm_members */ - int status = 1; - buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status); + /* Check swarm state -- Not crashing thanks to test added in check_swarm_members */ + int status = 1; + buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status); } /****************************************/ @@ -565,26 +560,15 @@ buzzvm_dup(VM); 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 + 4, NULL, 10); - //fprintf(stdout, "Robot id from get rid buzz util: %i\n",id); - return (uint16_t)id; - }*/ - buzzvm_t get_vm() { return VM; } -void set_robot_var(int ROBOTS){ - buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); - buzzvm_pushi(VM, ROBOTS); - buzzvm_gstore(VM); -} + void set_robot_var(int ROBOTS){ + buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); + buzzvm_pushi(VM, ROBOTS); + buzzvm_gstore(VM); + } } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index faf8c29..5b7bb9c 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -22,11 +22,11 @@ namespace rosbzz_node{ multi_msg = true; // set stream rate - wait for the FC to be started SetStreamRate(0, 10, 1); - /// Get Robot Id - wait for Xbee to be started + // Get Robot Id - wait for Xbee to be started if(xbeeplugged) GetRobotId(); else - robot_id= strtol(robot_name.c_str() + 5, NULL, 10);; + robot_id= strtol(robot_name.c_str() + 5, NULL, 10); setpoint_counter = 0; fcu_timeout = TIMEOUT; home[0]=0.0;home[1]=0.0;home[2]=0.0; @@ -67,7 +67,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(),stand_by.c_str()); + init_update_monitor(bcfname.c_str(),stand_by.c_str()); /////////////////////////////////////////////////////// // MAIN LOOP ////////////////////////////////////////////////////// @@ -78,7 +78,7 @@ namespace rosbzz_node{ /*Neighbours of the robot published with id in respective topic*/ neighbours_pos_publisher(); /*Check updater state and step code*/ - //update_routine(bcfname.c_str(), dbgfname.c_str()); + update_routine(bcfname.c_str(), dbgfname.c_str()); /*Step buzz script */ buzz_utility::buzz_script_step(); /*Prepare messages and publish them in respective topic*/ @@ -86,10 +86,10 @@ namespace rosbzz_node{ /*call flight controler service to set command long*/ flight_controller_service_call(); /*Set multi message available after update*/ - /*if(get_update_status()){ + if(get_update_status()){ set_read_update_status(); multi_msg=true; - }*/ + } /*Set ROBOTS variable for barrier in .bzz from neighbours count*/ //no_of_robots=get_number_of_robots(); get_number_of_robots(); @@ -334,7 +334,7 @@ namespace rosbzz_node{ /*----------------------------------------------------------------------------------------------------*/ void roscontroller::prepare_msg_and_publish(){ /*obtain Pay load to be sent*/ - uint64_t* payload_out_ptr= buzz_utility::out_msg_process(); + uint64_t* payload_out_ptr= buzz_utility::obt_out_msg(); uint64_t position[3]; /*Appened current position to message*/ memcpy(position, cur_pos, 3*sizeof(uint64_t)); @@ -358,7 +358,7 @@ namespace rosbzz_node{ 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((int)get_update_mode()!=CODE_RUNNING && is_msg_present()==1 && multi_msg){ uint8_t* buff_send = 0; uint16_t updater_msgSize=*(uint16_t*) (getupdate_out_msg_size());; int tot=0; @@ -394,7 +394,7 @@ namespace rosbzz_node{ payload_pub.publish(update_packets); multi_msg=false; delete[] payload_64; - }*/ + } } /*--------------------------------------------------------------------------------- @@ -855,7 +855,7 @@ namespace rosbzz_node{ raw_neighbours_pos_put((int)out[1],raw_neigh_pos); neighbours_pos_put((int)out[1],n_pos); delete[] out; - buzz_utility::in_msg_process((message_obt+3)); + buzz_utility::in_msg_append((message_obt+3)); } }