Spiri
/
ROSBuzz_MISTLab
12
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Made updates optional: including update.bzz will turnon update and 

not including it will disable updates.
This commit is contained in:
vivek-shankar 2018-06-06 23:58:38 -04:00
parent e953de6191
commit 8fa58e5e5b
5 changed files with 108 additions and 99 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
buzz_scripts/include/update.bzz
View File

@ -4,6 +4,7 @@
####################################################################################################
updated="update_ack"
update_no=0
updates_active = 1
function updated_no_bct(){
neighbors.broadcast(updated, update_no)
}

2
include/buzz_update.h
View File

@ -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*/

1
include/roscontroller.h
View File

@ -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;

192
src/buzz_update.cpp
View File

@ -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*/

11
src/roscontroller.cpp
View File

@ -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());