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

Added debug variable for prints within update files, corrected 

prints, beautified update files and incorporate a part of safe
deployment state machine with standby.bzz.
This commit is contained in:
vivek-shankar 2018-06-07 01:29:42 -04:00
parent 8fa58e5e5b
commit 3154976e8a
4 changed files with 53 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
buzz_scripts/stand_by.bzz
View File

@ -14,8 +14,6 @@ function init(){
if(r. data < l. data or (r. data == l. data )) return l if(r. data < l. data or (r. data == l. data )) return l
else return r else return r
}) })
BVMSTATE = "UPDATESTANDBY"
init_swarm() init_swarm()
# start the swarm command listener # start the swarm command listener
nei_cmd_listen() nei_cmd_listen()
@ -27,7 +25,6 @@ function stand_by(){
neighbors.listen(updated, neighbors.listen(updated,
function(vid, value, rid) { function(vid, value, rid) {
print(" got from",vid," ", " value = ",value," ","rid"," " )
if(value==update_no) barrier.put(rid,1) if(value==update_no) barrier.put(rid,1)
} }
) )

2
include/buzz_update.h
View File

@ -136,7 +136,7 @@ namespace buzz_update
/***************************************************/ /***************************************************/
/*Sets bzz file name*/ /*Sets bzz file name*/
/***************************************************/ /***************************************************/
void set_bzz_file(const char* in_bzz_file); void set_bzz_file(const char* in_bzz_file, bool dbg);
/****************************************************/ /****************************************************/
/*Tests the code from a buffer*/ /*Tests the code from a buffer*/

88
src/buzz_update.cpp
View File

@ -28,10 +28,9 @@ namespace buzz_update{
static int neigh = -1; static int neigh = -1;
static int updater_msg_ready; static int updater_msg_ready;
static uint16_t update_try_counter = 0; static uint16_t update_try_counter = 0;
static int updated = 0;
static const uint16_t MAX_UPDATE_TRY = 5; static const uint16_t MAX_UPDATE_TRY = 5;
static int packet_id_ = 0;
static size_t old_byte_code_size = 0; static size_t old_byte_code_size = 0;
static bool debug = false;
/*Initialize updater*/ /*Initialize updater*/
int 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)
@ -45,7 +44,7 @@ namespace buzz_update{
dbgf_name = dbgfname; dbgf_name = dbgfname;
bcfname = bo_filename; bcfname = bo_filename;
ROS_WARN("UPDATES TURNED ON (modifying .bzz script file will update all robots)"); ROS_WARN("UPDATES TURNED ON (modifying .bzz script file will update all robots)");
ROS_INFO("Initializing file monitor..."); if(debug) ROS_INFO("Initializing file monitor...");
fd = inotify_init1(IN_NONBLOCK); fd = inotify_init1(IN_NONBLOCK);
if (fd < 0) if (fd < 0)
{ {
@ -140,23 +139,22 @@ namespace buzz_update{
while (i < len) while (i < len)
{ {
struct inotify_event* event = (struct inotify_event*)&buf[i]; struct inotify_event* event = (struct inotify_event*)&buf[i];
/* file was modified this flag is true in nano and self delet in gedit and other editors */ /*Report file changes and self deletes*/
// fprintf(stdout,"inside file monitor.\n");
if (event->mask & (IN_MODIFY | IN_DELETE_SELF)) if (event->mask & (IN_MODIFY | IN_DELETE_SELF))
{ {
/*respawn watch if the file is self deleted */ /*Respawn watch if the file is self deleted */
inotify_rm_watch(fd, wd); inotify_rm_watch(fd, wd);
close(fd); close(fd);
fd = inotify_init1(IN_NONBLOCK); fd = inotify_init1(IN_NONBLOCK);
wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS); wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS);
/* To mask multiple writes from editors*/ /*To mask multiple writes from editors*/
if (!old_update) if (!old_update)
{ {
check = 1; check = 1;
old_update = 1; old_update = 1;
} }
} }
/* update index to start of next event */ /*Update index to start of next event*/
i += sizeof(struct inotify_event) + event->len; i += sizeof(struct inotify_event) + event->len;
} }
if (!check) if (!check)
@ -175,14 +173,14 @@ namespace buzz_update{
/*Patch the old bo with new patch*/ /*Patch the old bo with new patch*/
std::stringstream patch_writefile; std::stringstream patch_writefile;
patch_writefile << path << name1 << "tmp_patch.bo"; patch_writefile << path << name1 << "tmp_patch.bo";
/*Write the patch to a file*/ /*Write the patch to a file and call bsdiff to patch*/
FILE* fp = fopen(patch_writefile.str().c_str(), "wb"); FILE* fp = fopen(patch_writefile.str().c_str(), "wb");
fwrite(patch, update_patch_size, 1, fp); fwrite(patch, update_patch_size, 1, fp);
fclose(fp); fclose(fp);
std::stringstream patch_exsisting; std::stringstream patch_exsisting;
patch_exsisting << "bspatch " << path << name1 << ".bo " << path << name1 << "-patched.bo " << path << name1 patch_exsisting << "bspatch " << path << name1 << ".bo " << path << name1 << "-patched.bo " << path << name1
<< "tmp_patch.bo"; << "tmp_patch.bo";
fprintf(stdout, "Launching bspatch command: %s \n", patch_exsisting.str().c_str()); if(debug) ROS_WARN("Launching bspatch command: %s", patch_exsisting.str().c_str());
if (system(patch_exsisting.str().c_str())) if (system(patch_exsisting.str().c_str()))
return 0; return 0;
else else
@ -197,7 +195,10 @@ namespace buzz_update{
/*Read the exsisting file to simulate the patching*/ /*Read the exsisting file to simulate the patching*/
std::stringstream read_patched; std::stringstream read_patched;
read_patched << path << name1 << ".bo"; read_patched << path << name1 << ".bo";
fprintf(stdout, "read file name %s \n", read_patched.str().c_str()); if(debug){
ROS_WARN("Simulation patching ...");
ROS_WARN("Read file for patching %s", 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"); FILE* fp = fopen(read_patched.str().c_str(), "rb");
if (!fp) if (!fp)
@ -227,7 +228,10 @@ namespace buzz_update{
/*Read the new patched file*/ /*Read the new patched file*/
std::stringstream read_patched; std::stringstream read_patched;
read_patched << path << name1 << "-patched.bo"; read_patched << path << name1 << "-patched.bo";
fprintf(stdout, "read file name %s \n", read_patched.str().c_str()); if(debug) {
ROS_WARN("Robot patching ...");
ROS_WARN("Read file for patching %s", 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"); FILE* fp = fopen(read_patched.str().c_str(), "rb");
if (!fp) if (!fp)
@ -300,13 +304,13 @@ namespace buzz_update{
size += sizeof(uint16_t); size += sizeof(uint16_t);
*(uint16_t*)(updater->outmsg_queue->size) = size + CODE_REQUEST_PADDING; *(uint16_t*)(updater->outmsg_queue->size) = size + CODE_REQUEST_PADDING;
updater_msg_ready = 1; updater_msg_ready = 1;
ROS_INFO("[Debug] Requested update no. %u with try: %u \n", update_no, update_try_counter); if(debug) ROS_WARN("[Debug] Requesting update no. %u for rebroadcast, try: %u", update_no, update_try_counter);
} }
void code_message_inqueue_append(uint8_t* msg, uint16_t size) void code_message_inqueue_append(uint8_t* msg, uint16_t size)
{ {
updater->inmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); updater->inmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s));
// ROS_INFO("[DEBUG] Updater append code of size %d\n", (int) size); // ROS_INFO("[DEBUG] Updater append code of size %d", (int) size);
updater->inmsg_queue->queue = (uint8_t*)malloc(size); updater->inmsg_queue->queue = (uint8_t*)malloc(size);
updater->inmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); updater->inmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t));
memcpy(updater->inmsg_queue->queue, msg, size); memcpy(updater->inmsg_queue->queue, msg, size);
@ -321,19 +325,18 @@ namespace buzz_update{
{ {
int size = 0; int size = 0;
updater_code_t out_code = NULL; updater_code_t out_code = NULL;
if(debug) {
ROS_INFO("[Debug] Updater processing in msg with mode %d \n", *(int*)(updater->mode)); ROS_WARN("[Debug] Updater processing in msg with mode %d", *(int*)(updater->mode));
ROS_INFO("[Debug] %u : Current update number, %u : Received update no \n", (*(uint16_t*)(updater->update_no)), ROS_WARN("[Debug] Current update no: %u, Received update no: %u", (*(uint16_t*)(updater->update_no)),
(*(uint16_t*)(updater->inmsg_queue->queue + sizeof(uint8_t)))); (*(uint16_t*)(updater->inmsg_queue->queue + sizeof(uint8_t))));
ROS_INFO("[Debug] Updater received patch of size %u \n", ROS_WARN("[Debug] Updater received patch of size %u",
(*(uint16_t*)(updater->inmsg_queue->queue + sizeof(uint16_t) + sizeof(uint8_t)))); (*(uint16_t*)(updater->inmsg_queue->queue + sizeof(uint16_t) + sizeof(uint8_t))));
}
if (*(int*)(updater->mode) == CODE_RUNNING) if (*(int*)(updater->mode) == CODE_RUNNING)
{ {
// fprintf(stdout,"[debug]Inside inmsg code running"); // fprintf(stdout,"[debug]Inside inmsg code running");
if (*(uint8_t*)(updater->inmsg_queue->queue) == SENT_CODE) if (*(uint8_t*)(updater->inmsg_queue->queue) == SENT_CODE)
{ {
updated = 1;
size += sizeof(uint8_t); size += sizeof(uint8_t);
if (*(uint16_t*)(updater->inmsg_queue->queue + size) > *(uint16_t*)(updater->update_no)) if (*(uint16_t*)(updater->inmsg_queue->queue + size) > *(uint16_t*)(updater->update_no))
{ {
@ -360,7 +363,7 @@ namespace buzz_update{
if (test_set_code((out_code->bcode), (char*)dbgf_name, (size_t)(*(uint16_t*)out_code->bcode_size))) if (test_set_code((out_code->bcode), (char*)dbgf_name, (size_t)(*(uint16_t*)out_code->bcode_size)))
{ {
printf("TEST PASSED!\n"); if(debug) ROS_WARN("Received patch PASSED tests!");
*(uint16_t*)updater->update_no = update_no; *(uint16_t*)updater->update_no = update_no;
/*Clear exisiting patch if any*/ /*Clear exisiting patch if any*/
delete_p(updater->patch); delete_p(updater->patch);
@ -378,7 +381,7 @@ namespace buzz_update{
} }
else else
{ {
ROS_ERROR("Patching the .bo file failed, could be corrupt patch\n"); ROS_ERROR("Patching the .bo file failed");
update_try_counter++; update_try_counter++;
outqueue_append_code_request(update_no); outqueue_append_code_request(update_no);
} }
@ -388,6 +391,7 @@ namespace buzz_update{
size = 0; size = 0;
if (*(uint8_t*)(updater->inmsg_queue->queue) == RESEND_CODE) if (*(uint8_t*)(updater->inmsg_queue->queue) == RESEND_CODE)
{ {
if(debug) ROS_WARN("Patch rebroadcast request received.");
size += sizeof(uint8_t); size += sizeof(uint8_t);
if (*(uint16_t*)(updater->inmsg_queue->queue + size) == *(uint16_t*)(updater->update_no)) if (*(uint16_t*)(updater->inmsg_queue->queue + size) == *(uint16_t*)(updater->update_no))
{ {
@ -395,11 +399,11 @@ namespace buzz_update{
if (*(uint16_t*)(updater->inmsg_queue->queue + size) > update_try_counter) if (*(uint16_t*)(updater->inmsg_queue->queue + size) > update_try_counter)
{ {
update_try_counter = *(uint16_t*)(updater->inmsg_queue->queue + size); update_try_counter = *(uint16_t*)(updater->inmsg_queue->queue + size);
ROS_ERROR("Code appended! update try : %u \n", update_try_counter); if(debug) ROS_WARN("Rebroadcasting patch, try : %u", update_try_counter);
code_message_outqueue_append(); code_message_outqueue_append();
} }
if (update_try_counter > MAX_UPDATE_TRY) if (update_try_counter > MAX_UPDATE_TRY)
ROS_ERROR("TODO: ROLL BACK !!"); ROS_ERROR("TODO: Max rebroadcast retry reached, ROLL BACK !!");
} }
} }
// fprintf(stdout,"in queue freed\n"); // fprintf(stdout,"in queue freed\n");
@ -421,12 +425,12 @@ namespace buzz_update{
std::string name2 = name1 + "-update"; std::string name2 = name1 + "-update";
// CALL BSDIFF CMD /*Launch bsdiff and create a patch*/
genpatch << "bsdiff " << path << name1 << ".bo " << path << name2 << ".bo " << path << "diff.bo"; genpatch << "bsdiff " << path << name1 << ".bo " << path << name2 << ".bo " << path << "diff.bo";
fprintf(stdout, "Launching bsdiff command: %s \n", genpatch.str().c_str()); if(debug) ROS_WARN("Launching bsdiff: %s", genpatch.str().c_str());
system(genpatch.str().c_str()); system(genpatch.str().c_str());
/* delete old files & rename new files */ /*Delete old files & rename new files*/
remove((path + name1 + ".bo").c_str()); remove((path + name1 + ".bo").c_str());
remove((path + name1 + ".bdb").c_str()); remove((path + name1 + ".bdb").c_str());
@ -474,11 +478,11 @@ namespace buzz_update{
// Check for update // Check for update
if (check_update()) if (check_update())
{ {
ROS_INFO("Update found \nUpdating script ...\n"); ROS_INFO("Update found \tUpdating script ...");
if (compile_bzz(bzz_file)) if (compile_bzz(bzz_file))
{ {
ROS_WARN("Errors in comipilg script so staying on old script\n"); ROS_ERROR("Error in compiling script, resuming old script.");
} }
else else
{ {
@ -510,12 +514,12 @@ namespace buzz_update{
create_update_patch(); create_update_patch();
VM = buzz_utility::get_vm(); VM = buzz_utility::get_vm();
ROS_INFO("Current Update no %d\n", *(uint16_t*)(updater->update_no)); if(debug) ROS_INFO("Current Update no %d", *(uint16_t*)(updater->update_no));
buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1));
buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); buzzvm_pushi(VM, *(uint16_t*)(updater->update_no));
buzzvm_gstore(VM); buzzvm_gstore(VM);
neigh = -1; neigh = -1;
ROS_INFO("Sending code... \n"); if(debug) ROS_INFO("Broadcasting patch ...");
code_message_outqueue_append(); code_message_outqueue_append();
} }
delete_p(BO_BUF); delete_p(BO_BUF);
@ -530,9 +534,10 @@ namespace buzz_update{
buzzvm_gload(VM); buzzvm_gload(VM);
buzzobj_t tObj = buzzvm_stack_at(VM, 1); buzzobj_t tObj = buzzvm_stack_at(VM, 1);
buzzvm_pop(VM); buzzvm_pop(VM);
ROS_INFO("Barrier ..................... %i \n", tObj->i.value); ROS_INFO("Barrier ............. No. of robots deployed: %i", tObj->i.value);
if (tObj->i.value == no_of_robot) if (tObj->i.value == no_of_robot)
{ {
ROS_WARN("Patch deployment successful, rolling forward");
*(int*)(updater->mode) = CODE_RUNNING; *(int*)(updater->mode) = CODE_RUNNING;
gettimeofday(&t2, NULL); gettimeofday(&t2, NULL);
// collect_data(); // collect_data();
@ -543,7 +548,7 @@ namespace buzz_update{
} }
else if (timer_steps > TIMEOUT_FOR_ROLLBACK) else if (timer_steps > TIMEOUT_FOR_ROLLBACK)
{ {
ROS_ERROR("TIME OUT Reached, decided to roll back"); ROS_ERROR("TIME OUT Reached, rolling back");
/*Time out hit deceided to roll back*/ /*Time out hit deceided to roll back*/
*(int*)(updater->mode) = CODE_RUNNING; *(int*)(updater->mode) = CODE_RUNNING;
buzz_utility::buzz_script_set(old_bcfname, dbgf_name, (int)VM->robot); buzz_utility::buzz_script_set(old_bcfname, dbgf_name, (int)VM->robot);
@ -568,13 +573,13 @@ namespace buzz_update{
{ {
if (buzz_utility::buzz_update_init_test(BO_BUF, dbgfname, bcode_size)) if (buzz_utility::buzz_update_init_test(BO_BUF, dbgfname, bcode_size))
{ {
ROS_WARN("Initializtion of script test passed\n"); if(debug) ROS_WARN("Initializtion test passed");
if (buzz_utility::update_step_test()) if (buzz_utility::update_step_test())
{ {
/*data logging*/ /*data logging*/
old_byte_code_size = *(size_t*)updater->bcode_size; old_byte_code_size = *(size_t*)updater->bcode_size;
/*data logging*/ /*data logging*/
ROS_WARN("Step test passed\n"); if(debug) ROS_WARN("Step test passed");
*(int*)(updater->mode) = CODE_STANDBY; *(int*)(updater->mode) = CODE_STANDBY;
// fprintf(stdout,"updater value = %i\n",updater->mode); // fprintf(stdout,"updater value = %i\n",updater->mode);
delete_p(updater->bcode); delete_p(updater->bcode);
@ -595,13 +600,13 @@ namespace buzz_update{
{ {
if (*(int*)(updater->mode) == CODE_RUNNING) if (*(int*)(updater->mode) == CODE_RUNNING)
{ {
ROS_ERROR("step test failed, stick to old script\n"); ROS_ERROR("Step test failed, resuming old script");
buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (size_t) * (size_t*)(updater->bcode_size)); buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (size_t) * (size_t*)(updater->bcode_size));
} }
else else
{ {
/*You will never reach here*/ /*You will never reach here*/
ROS_ERROR("step test failed, Return to stand by\n"); ROS_ERROR("Step test failed, returning to standby");
buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname, buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname,
(size_t) * (size_t*)(updater->standby_bcode_size)); (size_t) * (size_t*)(updater->standby_bcode_size));
buzzvm_t VM = buzz_utility::get_vm(); buzzvm_t VM = buzz_utility::get_vm();
@ -616,13 +621,13 @@ namespace buzz_update{
{ {
if (*(int*)(updater->mode) == CODE_RUNNING) if (*(int*)(updater->mode) == CODE_RUNNING)
{ {
ROS_ERROR("Initialization test failed, stick to old script\n"); ROS_ERROR("Initialization test failed, resuming old script");
buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (int)*(size_t*)(updater->bcode_size)); buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (int)*(size_t*)(updater->bcode_size));
} }
else else
{ {
/*You will never reach here*/ /*You will never reach here*/
ROS_ERROR("Initialization test failed, Return to stand by\n"); ROS_ERROR("Initialization test failed, returning to standby");
buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname, buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname,
(size_t) * (size_t*)(updater->standby_bcode_size)); (size_t) * (size_t*)(updater->standby_bcode_size));
buzzvm_t VM = buzz_utility::get_vm(); buzzvm_t VM = buzz_utility::get_vm();
@ -675,8 +680,9 @@ namespace buzz_update{
close(fd); close(fd);
} }
void set_bzz_file(const char* in_bzz_file) void set_bzz_file(const char* in_bzz_file, bool dbg)
{ {
debug=dbg;
bzz_file = in_bzz_file; bzz_file = in_bzz_file;
} }
@ -686,7 +692,7 @@ namespace buzz_update{
} }
/*-------------------------------------------------------- /*--------------------------------------------------------
/ Create Buzz bytecode from the bzz script inputed / Create Buzz bytecode from the bzz script input
/-------------------------------------------------------*/ /-------------------------------------------------------*/
int compile_bzz(std::string bzz_file) int compile_bzz(std::string bzz_file)
{ {

7
src/roscontroller.cpp
View File

@ -26,7 +26,7 @@ roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv)
std::string fname = Compile_bzz(bzzfile_name); std::string fname = Compile_bzz(bzzfile_name);
bcfname = fname + ".bo"; bcfname = fname + ".bo";
dbgfname = fname + ".bdb"; dbgfname = fname + ".bdb";
buzz_update::set_bzz_file(bzzfile_name.c_str()); buzz_update::set_bzz_file(bzzfile_name.c_str(),debug);
buzzuav_closures::setWPlist(bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"); buzzuav_closures::setWPlist(bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/");
// Initialize variables // Initialize variables
SetMode("LOITER", 0); SetMode("LOITER", 0);
@ -576,8 +576,7 @@ with size......... | /
; ;
int tot = 0; int tot = 0;
mavros_msgs::Mavlink update_packets; mavros_msgs::Mavlink update_packets;
fprintf(stdout, "Appending code into message ...\n"); if(debug) ROS_INFO("Broadcasted Update packet Size: %u", updater_msgSize);
fprintf(stdout, "Sent Update packet Size: %u \n", updater_msgSize);
// allocate mem and clear it // allocate mem and clear it
buff_send = (uint8_t*)malloc(sizeof(uint16_t) + updater_msgSize); buff_send = (uint8_t*)malloc(sizeof(uint16_t) + updater_msgSize);
memset(buff_send, 0, sizeof(uint16_t) + updater_msgSize); memset(buff_send, 0, sizeof(uint16_t) + updater_msgSize);
@ -1008,7 +1007,7 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg)
// Copy packet into temporary buffer neglecting update constant // Copy packet into temporary buffer neglecting update constant
memcpy((void*)pl, (void*)(message_obt + 1), obt_msg_size); memcpy((void*)pl, (void*)(message_obt + 1), obt_msg_size);
uint16_t unMsgSize = *(uint16_t*)(pl); uint16_t unMsgSize = *(uint16_t*)(pl);
fprintf(stdout, "Update packet received, read msg size : %u \n", unMsgSize); if(debug) ROS_INFO("Update packet received, size : %u", unMsgSize);
if (unMsgSize > 0) if (unMsgSize > 0)
{ {
buzz_update::code_message_inqueue_append((uint8_t*)(pl + sizeof(uint16_t)), unMsgSize); buzz_update::code_message_inqueue_append((uint8_t*)(pl + sizeof(uint16_t)), unMsgSize);