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
else return r
})
BVMSTATE = "UPDATESTANDBY"
init_swarm()
# start the swarm command listener
nei_cmd_listen()
@ -27,7 +25,6 @@ function stand_by(){
neighbors.listen(updated,
function(vid, value, rid) {
print(" got from",vid," ", " value = ",value," ","rid"," " )
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*/
/***************************************************/
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*/

86
src/buzz_update.cpp
View File

@ -28,10 +28,9 @@ namespace buzz_update{
static int neigh = -1;
static int updater_msg_ready;
static uint16_t update_try_counter = 0;
static int updated = 0;
static const uint16_t MAX_UPDATE_TRY = 5;
static int packet_id_ = 0;
static size_t old_byte_code_size = 0;
static bool debug = false;
/*Initialize updater*/
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;
bcfname = bo_filename;
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);
if (fd < 0)
{
@ -140,11 +139,10 @@ namespace buzz_update{
while (i < len)
{
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 */
// fprintf(stdout,"inside file monitor.\n");
/*Report file changes and self deletes*/
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);
close(fd);
fd = inotify_init1(IN_NONBLOCK);
@ -156,7 +154,7 @@ namespace buzz_update{
old_update = 1;
}
}
/* update index to start of next event */
/*Update index to start of next event*/
i += sizeof(struct inotify_event) + event->len;
}
if (!check)
@ -175,14 +173,14 @@ namespace buzz_update{
/*Patch the old bo with new patch*/
std::stringstream patch_writefile;
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");
fwrite(patch, update_patch_size, 1, fp);
fclose(fp);
std::stringstream patch_exsisting;
patch_exsisting << "bspatch " << path << name1 << ".bo " << path << name1 << "-patched.bo " << path << name1
<< "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()))
return 0;
else
@ -197,7 +195,10 @@ namespace buzz_update{
/*Read the exsisting file to simulate the patching*/
std::stringstream read_patched;
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;
FILE* fp = fopen(read_patched.str().c_str(), "rb");
if (!fp)
@ -227,7 +228,10 @@ namespace buzz_update{
/*Read the new patched file*/
std::stringstream read_patched;
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;
FILE* fp = fopen(read_patched.str().c_str(), "rb");
if (!fp)
@ -300,13 +304,13 @@ namespace buzz_update{
size += sizeof(uint16_t);
*(uint16_t*)(updater->outmsg_queue->size) = size + CODE_REQUEST_PADDING;
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)
{
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->size = (uint8_t*)malloc(sizeof(uint16_t));
memcpy(updater->inmsg_queue->queue, msg, size);
@ -321,19 +325,18 @@ namespace buzz_update{
{
int size = 0;
updater_code_t out_code = NULL;
ROS_INFO("[Debug] Updater processing in msg with mode %d \n", *(int*)(updater->mode));
ROS_INFO("[Debug] %u : Current update number, %u : Received update no \n", (*(uint16_t*)(updater->update_no)),
if(debug) {
ROS_WARN("[Debug] Updater processing in msg with mode %d", *(int*)(updater->mode));
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))));
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))));
}
if (*(int*)(updater->mode) == CODE_RUNNING)
{
// fprintf(stdout,"[debug]Inside inmsg code running");
if (*(uint8_t*)(updater->inmsg_queue->queue) == SENT_CODE)
{
updated = 1;
size += sizeof(uint8_t);
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)))
{
printf("TEST PASSED!\n");
if(debug) ROS_WARN("Received patch PASSED tests!");
*(uint16_t*)updater->update_no = update_no;
/*Clear exisiting patch if any*/
delete_p(updater->patch);
@ -378,7 +381,7 @@ namespace buzz_update{
}
else
{
ROS_ERROR("Patching the .bo file failed, could be corrupt patch\n");
ROS_ERROR("Patching the .bo file failed");
update_try_counter++;
outqueue_append_code_request(update_no);
}
@ -388,6 +391,7 @@ namespace buzz_update{
size = 0;
if (*(uint8_t*)(updater->inmsg_queue->queue) == RESEND_CODE)
{
if(debug) ROS_WARN("Patch rebroadcast request received.");
size += sizeof(uint8_t);
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)
{
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();
}
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");
@ -421,12 +425,12 @@ namespace buzz_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";
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());
/* delete old files & rename new files */
/*Delete old files & rename new files*/
remove((path + name1 + ".bo").c_str());
remove((path + name1 + ".bdb").c_str());
@ -474,11 +478,11 @@ namespace buzz_update{
// Check for update
if (check_update())
{
ROS_INFO("Update found \nUpdating script ...\n");
ROS_INFO("Update found \tUpdating script ...");
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
{
@ -510,12 +514,12 @@ namespace buzz_update{
create_update_patch();
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_pushi(VM, *(uint16_t*)(updater->update_no));
buzzvm_gstore(VM);
neigh = -1;
ROS_INFO("Sending code... \n");
if(debug) ROS_INFO("Broadcasting patch ...");
code_message_outqueue_append();
}
delete_p(BO_BUF);
@ -530,9 +534,10 @@ namespace buzz_update{
buzzvm_gload(VM);
buzzobj_t tObj = buzzvm_stack_at(VM, 1);
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)
{
ROS_WARN("Patch deployment successful, rolling forward");
*(int*)(updater->mode) = CODE_RUNNING;
gettimeofday(&t2, NULL);
// collect_data();
@ -543,7 +548,7 @@ namespace buzz_update{
}
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*/
*(int*)(updater->mode) = CODE_RUNNING;
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))
{
ROS_WARN("Initializtion of script test passed\n");
if(debug) ROS_WARN("Initializtion test passed");
if (buzz_utility::update_step_test())
{
/*data logging*/
old_byte_code_size = *(size_t*)updater->bcode_size;
/*data logging*/
ROS_WARN("Step test passed\n");
if(debug) ROS_WARN("Step test passed");
*(int*)(updater->mode) = CODE_STANDBY;
// fprintf(stdout,"updater value = %i\n",updater->mode);
delete_p(updater->bcode);
@ -595,13 +600,13 @@ namespace buzz_update{
{
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));
}
else
{
/*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,
(size_t) * (size_t*)(updater->standby_bcode_size));
buzzvm_t VM = buzz_utility::get_vm();
@ -616,13 +621,13 @@ namespace buzz_update{
{
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));
}
else
{
/*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,
(size_t) * (size_t*)(updater->standby_bcode_size));
buzzvm_t VM = buzz_utility::get_vm();
@ -675,8 +680,9 @@ namespace buzz_update{
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;
}
@ -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)
{

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);
bcfname = fname + ".bo";
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("\\/")) + "/");
// Initialize variables
SetMode("LOITER", 0);
@ -576,8 +576,7 @@ with size......... | /
;
int tot = 0;
mavros_msgs::Mavlink update_packets;
fprintf(stdout, "Appending code into message ...\n");
fprintf(stdout, "Sent Update packet Size: %u \n", updater_msgSize);
if(debug) ROS_INFO("Broadcasted Update packet Size: %u", updater_msgSize);
// allocate mem and clear it
buff_send = (uint8_t*)malloc(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
memcpy((void*)pl, (void*)(message_obt + 1), obt_msg_size);
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)
{
buzz_update::code_message_inqueue_append((uint8_t*)(pl + sizeof(uint16_t)), unMsgSize);