diff --git a/buzz_scripts/include/update.bzz b/buzz_scripts/include/update.bzz index 8d64de2..4a5c699 100644 --- a/buzz_scripts/include/update.bzz +++ b/buzz_scripts/include/update.bzz @@ -4,6 +4,7 @@ #################################################################################################### updated="update_ack" update_no=0 +updates_active = 1 function updated_no_bct(){ neighbors.broadcast(updated, update_no) } \ No newline at end of file diff --git a/include/buzz_update.h b/include/buzz_update.h index 1849b56..13bd665 100644 --- a/include/buzz_update.h +++ b/include/buzz_update.h @@ -103,7 +103,7 @@ namespace buzz_update /************************************************/ /*Initalizes the updater */ /************************************************/ - void init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id); + int init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id); /*********************************************************/ /*Appends buffer of given size to in msg queue of updater*/ diff --git a/include/roscontroller.h b/include/roscontroller.h index dedc737..29c97e6 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -96,6 +96,7 @@ private: float fcu_timeout; int armstate; int barrier; + int update; int message_number = 0; uint8_t no_of_robots = 0; bool rcclient; diff --git a/src/buzz_update.cpp b/src/buzz_update.cpp index 210e98f..43ceb7f 100644 --- a/src/buzz_update.cpp +++ b/src/buzz_update.cpp @@ -34,90 +34,101 @@ namespace buzz_update{ static size_t old_byte_code_size = 0; /*Initialize updater*/ - void init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id) + int init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id) { buzzvm_t VM = buzz_utility::get_vm(); - 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"); - } - /*If simulation set the file monitor only for robot 0*/ - 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"); - 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); - } - 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; - /*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; - *(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; + buzzvm_pushs(VM, buzzvm_string_register(VM, "updates_active", 1)); + buzzvm_gload(VM); + buzzobj_t obj = buzzvm_stack_at(VM, 1); + if(obj->o.type == BUZZTYPE_INT && obj->i.value == 1){ + Robot_id = robot_id; + dbgf_name = dbgfname; + bcfname = bo_filename; + ROS_WARN("UPDATES TURNED ON (modifying .bzz script file will update all robots)"); + ROS_INFO("Initializing file monitor..."); + fd = inotify_init1(IN_NONBLOCK); + if (fd < 0) + { + perror("inotify_init error"); + } + /*If simulation set the file monitor only for robot 0*/ + 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"); + 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); + } + 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; + /*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; + *(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; - /*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); + /*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); + return 1; + } + else{ + ROS_WARN("UPDATES TURNED OFF... (Hint: Include update.bzz to turn on updates)"); + return 0; + } } /*Check for .bzz file chages*/ int check_update() @@ -187,7 +198,7 @@ namespace buzz_update{ std::stringstream read_patched; read_patched << path << name1 << ".bo"; fprintf(stdout, "read file name %s \n", read_patched.str().c_str()); - uint8_t* patched_BO_Buf = 0; + uint8_t* patched_bo_buf = 0; FILE* fp = fopen(read_patched.str().c_str(), "rb"); if (!fp) { @@ -196,15 +207,15 @@ namespace buzz_update{ fseek(fp, 0, SEEK_END); size_t patched_size = ftell(fp); rewind(fp); - patched_BO_Buf = (uint8_t*)malloc(patched_size); - if (fread(patched_BO_Buf, 1, patched_size, fp) < patched_size) + patched_bo_buf = (uint8_t*)malloc(patched_size); + if (fread(patched_bo_buf, 1, patched_size, fp) < patched_size) { perror(read_patched.str().c_str()); } fclose(fp); /*Write the patched to a code struct and return*/ updater_code_t update_code = (updater_code_t)malloc(sizeof(struct updater_code_s)); - update_code->bcode = patched_BO_Buf; + update_code->bcode = patched_bo_buf; update_code->bcode_size = (uint8_t*)malloc(sizeof(uint16_t)); *(uint16_t*)(update_code->bcode_size) = patched_size; @@ -217,7 +228,7 @@ namespace buzz_update{ std::stringstream read_patched; read_patched << path << name1 << "-patched.bo"; fprintf(stdout, "read file name %s \n", read_patched.str().c_str()); - uint8_t* patched_BO_Buf = 0; + uint8_t* patched_bo_buf = 0; FILE* fp = fopen(read_patched.str().c_str(), "rb"); if (!fp) { @@ -226,8 +237,8 @@ namespace buzz_update{ fseek(fp, 0, SEEK_END); size_t patched_size = ftell(fp); rewind(fp); - patched_BO_Buf = (uint8_t*)malloc(patched_size); - if (fread(patched_BO_Buf, 1, patched_size, fp) < patched_size) + patched_bo_buf = (uint8_t*)malloc(patched_size); + if (fread(patched_bo_buf, 1, patched_size, fp) < patched_size) { perror(read_patched.str().c_str()); } @@ -239,7 +250,7 @@ namespace buzz_update{ /*Write the patched to a code struct and return*/ updater_code_t update_code = (updater_code_t)malloc(sizeof(struct updater_code_s)); - update_code->bcode = patched_BO_Buf; + update_code->bcode = patched_bo_buf; update_code->bcode_size = (uint8_t*)malloc(sizeof(uint16_t)); *(uint16_t*)(update_code->bcode_size) = patched_size; @@ -577,9 +588,6 @@ namespace buzz_update{ buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); buzzvm_pushi(VM, no_of_robot); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1)); - buzzvm_pushs(VM, buzzvm_string_register(VM, "UPDATE", 1)); - buzzvm_gstore(VM); return 1; } /*Unable to step something wrong*/ diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 82c665a..afff0c6 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -37,7 +37,7 @@ roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv) cur_pos.longitude = 0; cur_pos.latitude = 0; cur_pos.altitude = 0; - + update=0; // set stream rate - wait for the FC to be started SetStreamRate(0, 10, 1); @@ -106,7 +106,7 @@ void roscontroller::RosControllerRun() ROS_INFO("[%i] Bytecode file found and set", robot_id); std::string standby_bo = Compile_bzz(stand_by) + ".bo"; // Intialize the update monitor - buzz_update::init_update_monitor(bcfname.c_str(), standby_bo.c_str(), dbgfname.c_str(), robot_id); + update = buzz_update::init_update_monitor(bcfname.c_str(), standby_bo.c_str(), dbgfname.c_str(), robot_id); // set ROS loop rate ros::Rate loop_rate(BUZZRATE); // DEBUG @@ -119,8 +119,7 @@ void roscontroller::RosControllerRun() grid_publisher(); send_MPpayload(); // Check updater state and step code - buzz_update::update_routine(); - ROS_WARN("OUT OF UPDATE ROUTINE"); + if(update) buzz_update::update_routine(); if (time_step == BUZZRATE) { time_step = 0; @@ -152,7 +151,7 @@ void roscontroller::RosControllerRun() // Set ROBOTS variable (swarm size) get_number_of_robots(); buzz_utility::set_robot_var(no_of_robots); - buzz_update::updates_set_robots(no_of_robots); + if(update) buzz_update::updates_set_robots(no_of_robots); // get_xbee_status(); // commented out because it may slow down the node too much, to be tested ros::spinOnce(); @@ -570,7 +569,7 @@ with size......... | / delete[] out; delete[] payload_out_ptr; // Check for updater message if present send - if (buzz_update::is_msg_present()) + if (update && buzz_update::is_msg_present()) { uint8_t* buff_send = 0; uint16_t updater_msgSize = *(uint16_t*)(buzz_update::getupdate_out_msg_size());