From a9038601aef3393634d79149056d273bf4e56e4f Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Thu, 14 Jun 2018 13:37:00 -0400 Subject: [PATCH] added update changes and neighbour pos clear on dev --- include/buzz_update.h | 248 +++---- include/buzzuav_closures.h | 4 + include/roscontroller.h | 1 + src/buzz_update.cpp | 1293 ++++++++++++++++++------------------ src/buzzuav_closures.cpp | 5 + src/roscontroller.cpp | 28 +- 6 files changed, 799 insertions(+), 780 deletions(-) diff --git a/include/buzz_update.h b/include/buzz_update.h index 5680f85..4b43e86 100644 --- a/include/buzz_update.h +++ b/include/buzz_update.h @@ -1,161 +1,171 @@ #ifndef BUZZ_UPDATE_H #define BUZZ_UPDATE_H -/*Simulation or robot check*/ -//#define SIMULATION 1 // set in CMAKELIST #include #include +#include +#include +#include +#include +#include #include #include #include #include +#include #include + #define delete_p(p) \ do \ { \ free(p); \ p = NULL; \ } while (0) - -static const uint16_t CODE_REQUEST_PADDING = 250; -static const uint16_t MIN_UPDATE_PACKET = 251; -static const uint16_t UPDATE_CODE_HEADER_SIZE = 5; -static const uint16_t TIMEOUT_FOR_ROLLBACK = 50; -/*********************/ -/* Updater states */ -/********************/ - -typedef enum { - CODE_RUNNING = 0, // Code executing - CODE_STANDBY, // Standing by for others to update -} code_states_e; - -/*********************/ -/*Message types */ -/********************/ - -typedef enum { - SENT_CODE = 0, // Broadcast code - RESEND_CODE, // ReBroadcast request -} code_message_e; - -/*************************/ -/*Updater message queue */ -/*************************/ - -struct updater_msgqueue_s +namespace buzz_update { - uint8_t* queue; - uint8_t* size; -}; -typedef struct updater_msgqueue_s* updater_msgqueue_t; + static const uint16_t CODE_REQUEST_PADDING = 250; + static const uint16_t MIN_UPDATE_PACKET = 251; + static const uint16_t UPDATE_CODE_HEADER_SIZE = 5; + static const uint16_t TIMEOUT_FOR_ROLLBACK = 50; + /*********************/ + /* Updater states */ + /********************/ -struct updater_code_s -{ - uint8_t* bcode; - uint8_t* bcode_size; -}; -typedef struct updater_code_s* updater_code_t; + typedef enum { + CODE_RUNNING = 0, // Code executing + CODE_STANDBY, // Standing by for others to update + } code_states_e; -/**************************/ -/*Updater data*/ -/**************************/ + /*********************/ + /*Message types */ + /********************/ -struct buzz_updater_elem_s -{ - /* robot id */ - // uint16_t robotid; - /*current Bytecode content */ - uint8_t* bcode; - /*old Bytecode name */ - const char* old_bcode; - /*current bcode size*/ - size_t* bcode_size; - /*Update patch*/ - uint8_t* patch; - /* Update patch size*/ - size_t* patch_size; - /*current Bytecode content */ - uint8_t* standby_bcode; - /*current bcode size*/ - size_t* standby_bcode_size; - /*updater out msg queue */ - updater_msgqueue_t outmsg_queue; - /*updater in msg queue*/ - updater_msgqueue_t inmsg_queue; - /*Current state of the updater one in code_states_e ENUM*/ - int* mode; - uint8_t* update_no; -}; -typedef struct buzz_updater_elem_s* buzz_updater_elem_t; + typedef enum { + SENT_CODE = 0, // Broadcast code + RESEND_CODE, // ReBroadcast request + } code_message_e; -/**************************************************************************/ -/*Updater routine from msg processing to file checks to be called from main*/ -/**************************************************************************/ -void update_routine(); + /*************************/ + /*Updater message queue */ + /*************************/ -/************************************************/ -/*Initalizes the updater */ -/************************************************/ -void init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id); + struct updater_msgqueue_s + { + uint8_t* queue; + uint8_t* size; + }; + typedef struct updater_msgqueue_s* updater_msgqueue_t; -/*********************************************************/ -/*Appends buffer of given size to in msg queue of updater*/ -/*********************************************************/ + struct updater_code_s + { + uint8_t* bcode; + uint8_t* bcode_size; + }; + typedef struct updater_code_s* updater_code_t; -void code_message_inqueue_append(uint8_t* msg, uint16_t size); + /**************************/ + /*Updater data*/ + /**************************/ -/*********************************************************/ -/*Processes messages inside the queue of the updater*/ -/*********************************************************/ + struct buzz_updater_elem_s + { + /* robot id */ + // uint16_t robotid; + /*current Bytecode content */ + uint8_t* bcode; + /*old Bytecode name */ + const char* old_bcode; + /*current bcode size*/ + size_t* bcode_size; + /*Update patch*/ + uint8_t* patch; + /* Update patch size*/ + size_t* patch_size; + /*current Bytecode content */ + uint8_t* standby_bcode; + /*current bcode size*/ + size_t* standby_bcode_size; + /*updater out msg queue */ + updater_msgqueue_t outmsg_queue; + /*updater in msg queue*/ + updater_msgqueue_t inmsg_queue; + /*Current state of the updater one in code_states_e ENUM*/ + int* mode; + uint8_t* update_no; + }; + typedef struct buzz_updater_elem_s* buzz_updater_elem_t; -void code_message_inqueue_process(); + /**************************************************************************/ + /*Updater routine from msg processing to file checks to be called from main*/ + /**************************************************************************/ + void update_routine(); -/*****************************************************/ -/* obtains messages from out msgs queue of the updater*/ -/*******************************************************/ -uint8_t* getupdater_out_msg(); + /************************************************/ + /*Initalizes the updater */ + /************************************************/ + int init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id); -/******************************************************/ -/*obtains out msg queue size*/ -/*****************************************************/ -uint8_t* getupdate_out_msg_size(); + /*********************************************************/ + /*Appends buffer of given size to in msg queue of updater*/ + /*********************************************************/ -/**************************************************/ -/*destroys the out msg queue*/ -/*************************************************/ -void destroy_out_msg_queue(); + void code_message_inqueue_append(uint8_t* msg, uint16_t size); -/***************************************************/ -/*obatins updater state*/ -/***************************************************/ -// int get_update_mode(); + /*********************************************************/ + /*Processes messages inside the queue of the updater*/ + /*********************************************************/ -// buzz_updater_elem_t get_updater(); -/***************************************************/ -/*sets bzz file name*/ -/***************************************************/ -void set_bzz_file(const char* in_bzz_file); + void code_message_inqueue_process(); -int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size); + /*****************************************************/ + /*Obtains messages from out msgs queue of the updater*/ + /*******************************************************/ + uint8_t* getupdater_out_msg(); -/****************************************************/ -/*Destroys the updater*/ -/***************************************************/ + /******************************************************/ + /*Obtains out msg queue size*/ + /*****************************************************/ + uint8_t* getupdate_out_msg_size(); -void destroy_updater(); + /**************************************************/ + /*Destroys the out msg queue*/ + /*************************************************/ + void destroy_out_msg_queue(); -int is_msg_present(); + // buzz_updater_elem_t get_updater(); + /***************************************************/ + /*Sets bzz file name*/ + /***************************************************/ + void set_bzz_file(const char* in_bzz_file, bool dbg); -int get_update_status(); + /****************************************************/ + /*Tests the code from a buffer*/ + /***************************************************/ -void set_read_update_status(); + int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size); -int compile_bzz(std::string bzz_file); + /****************************************************/ + /*Destroys the updater*/ + /***************************************************/ -void updates_set_robots(int robots); + void destroy_updater(); -// void set_packet_id(int packet_id); + /****************************************************/ + /*Checks for updater message*/ + /***************************************************/ -// void collect_data(std::ofstream& logger); + int is_msg_present(); + + /****************************************************/ + /*Compiles a bzz script to bo and bdbg*/ + /***************************************************/ + + int compile_bzz(std::string bzz_file); + + /****************************************************/ + /*Set number of robots in the updater*/ + /***************************************************/ + + void updates_set_robots(int robots); +} #endif diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index e6c3f48..7de79e1 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -118,6 +118,10 @@ void neighbour_pos_callback(int id, float range, float bearing, float elevation) * update neighbors from in msgs */ void update_neighbors(buzzvm_t vm); +/* + *Clear neighbours struct + */ +void clear_neighbours_pos(); /* * closure to add a neighbor status */ diff --git a/include/roscontroller.h b/include/roscontroller.h index 5259343..a366bf4 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -97,6 +97,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 1d169f9..8624c3a 100644 --- a/src/buzz_update.cpp +++ b/src/buzz_update.cpp @@ -1,599 +1,633 @@ -#include -#include -#include -#include -#include +/** @file buzz_utility.cpp + * @version 1.0 + * @date 27.09.2016 + * @brief Buzz Implementation as a node in ROS. + * @author Vivek Shankar Varadharajan and David St-Onge + * @copyright 2016 MistLab. All rights reserved. + */ + #include "buzz_update.h" -#include -#include -#include -#include -/*Temp for data collection*/ -// static int neigh=-1; -static struct timeval t1, t2; -static int timer_steps = 0; -// static clock_t end; +namespace buzz_update{ + /*Temp for data collection*/ + // static int neigh=-1; + static struct timeval t1, t2; + static int timer_steps = 0; + // static clock_t end; -/*Temp end */ -static int fd, wd = 0; -static int old_update = 0; -static buzz_updater_elem_t updater; -static int no_of_robot; -static const char* dbgf_name; -static const char* bcfname; -static const char* old_bcfname = "old_bcode.bo"; -static const char* bzz_file; -static int Robot_id = 0; -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; + /*Temp end */ + static int fd, wd = 0; + static int old_update = 0; + static buzz_updater_elem_t updater; + static int no_of_robot; + static const char* dbgf_name; + static const char* bcfname; + static const char* old_bcfname = "old_bcode.bo"; + static const char* bzz_file; + static int Robot_id = 0; + static int neigh = -1; + static int updater_msg_ready; + static uint16_t update_try_counter = 0; + static const uint16_t MAX_UPDATE_TRY = 5; + static size_t old_byte_code_size = 0; + static bool debug = false; -/*Initialize updater*/ -void init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id) -{ - Robot_id = robot_id; - dbgf_name = dbgfname; - bcfname = bo_filename; - ROS_INFO("Initializing file monitor..."); - fd = inotify_init1(IN_NONBLOCK); - if (fd < 0) + /*Initialize updater*/ + int init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id) { - perror("inotify_init error"); - } - /*If simulation set the file monitor only for robot 1*/ - 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); -} -/*Check for .bzz file chages*/ -int check_update() -{ - char buf[1024]; - int check = 0; - int i = 0; - int len = read(fd, buf, 1024); - 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"); - if (event->mask & (IN_MODIFY | IN_DELETE_SELF)) - { - /*respawn watch if the file is self deleted */ - inotify_rm_watch(fd, wd); - close(fd); + buzzvm_t VM = buzz_utility::get_vm(); + 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)"); + if(debug) ROS_INFO("Initializing file monitor..."); fd = inotify_init1(IN_NONBLOCK); - wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS); - /* To mask multiple writes from editors*/ - if (!old_update) + if (fd < 0) { - check = 1; - old_update = 1; + 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); + 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() + { + char buf[1024]; + int check = 0; + int i = 0; + int len = read(fd, buf, 1024); + while (i < len) + { + struct inotify_event* event = (struct inotify_event*)&buf[i]; + /*Report file changes and self deletes*/ + if (event->mask & (IN_MODIFY | IN_DELETE_SELF)) + { + /*Respawn watch if the file is self deleted */ + inotify_rm_watch(fd, wd); + close(fd); + fd = inotify_init1(IN_NONBLOCK); + wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS); + /*To mask multiple writes from editors*/ + if (!old_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; + return check; + } + + int test_patch(std::string& path, std::string& name1, size_t update_patch_size, uint8_t* patch) + { + if (SIMULATION == 1) + { + return 1; + } + else + { + /*Patch the old bo with new patch*/ + std::stringstream patch_writefile; + patch_writefile << path << name1 << "tmp_patch.bo"; + /*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"; + if(debug) ROS_WARN("Launching bspatch command: %s", patch_exsisting.str().c_str()); + if (system(patch_exsisting.str().c_str())) + return 0; + else + return 1; + } + } + + updater_code_t obtain_patched_bo(std::string& path, std::string& name1) + { + if (SIMULATION == 1) + { + /*Read the exsisting file to simulate the patching*/ + std::stringstream read_patched; + read_patched << path << name1 << ".bo"; + 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) + { + perror(read_patched.str().c_str()); + } + 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) + { + 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_size = (uint8_t*)malloc(sizeof(uint16_t)); + *(uint16_t*)(update_code->bcode_size) = patched_size; + + return update_code; + } + + else + { + /*Read the new patched file*/ + std::stringstream read_patched; + read_patched << path << name1 << "-patched.bo"; + 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) + { + perror(read_patched.str().c_str()); + } + 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) + { + perror(read_patched.str().c_str()); + } + fclose(fp); + + /* delete old bo file & rename new bo file */ + remove((path + name1 + ".bo").c_str()); + rename((path + name1 + "-patched.bo").c_str(), (path + name1 + ".bo").c_str()); + + /*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_size = (uint8_t*)malloc(sizeof(uint16_t)); + *(uint16_t*)(update_code->bcode_size) = patched_size; + + return update_code; + } + } + + void code_message_outqueue_append() + { + updater->outmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); + /* if size less than 250 Pad with zeors to assure transmission*/ + uint16_t size = UPDATE_CODE_HEADER_SIZE + *(size_t*)(updater->patch_size); + uint16_t padding_size = (size > MIN_UPDATE_PACKET) ? 0 : MIN_UPDATE_PACKET - size; + size += padding_size; + updater->outmsg_queue->queue = (uint8_t*)malloc(size); + memset(updater->outmsg_queue->queue, 0, size); + updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); + *(uint16_t*)(updater->outmsg_queue->size) = size; + size = 0; + /*Append message type*/ + *(uint8_t*)(updater->outmsg_queue->queue + size) = SENT_CODE; + size += sizeof(uint8_t); + /*Append the update no, code size and code to out msg*/ + *(uint16_t*)(updater->outmsg_queue->queue + size) = *(uint16_t*)(updater->update_no); + size += sizeof(uint16_t); + *(uint16_t*)(updater->outmsg_queue->queue + size) = (uint16_t) * (size_t*)(updater->patch_size); + size += sizeof(uint16_t); + memcpy(updater->outmsg_queue->queue + size, updater->patch, *(size_t*)(updater->patch_size)); + // size += (uint16_t) * (size_t*)(updater->patch_size); + updater_msg_ready = 1; + } + + void outqueue_append_code_request(uint16_t update_no) + { + updater->outmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); + uint16_t size = 0; + updater->outmsg_queue->queue = (uint8_t*)malloc(2 * sizeof(uint16_t) + sizeof(uint8_t) + CODE_REQUEST_PADDING); + updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); + memset(updater->outmsg_queue->queue, 0, sizeof(uint16_t) + sizeof(uint8_t) + CODE_REQUEST_PADDING); + /*Append message type*/ + *(uint8_t*)(updater->outmsg_queue->queue + size) = RESEND_CODE; + size += sizeof(uint8_t); + /*Append the update no, code size and code to out msg*/ + *(uint16_t*)(updater->outmsg_queue->queue + size) = update_no; + size += sizeof(uint16_t); + *(uint16_t*)(updater->outmsg_queue->queue + size) = update_try_counter; + size += sizeof(uint16_t); + *(uint16_t*)(updater->outmsg_queue->size) = size + CODE_REQUEST_PADDING; + updater_msg_ready = 1; + 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", (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; + } + /*Used for data logging*/ + /*void set_packet_id(int packet_id) + { + packet_id_ = packet_id; + }*/ + void code_message_inqueue_process() + { + int size = 0; + updater_code_t out_code = NULL; + 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_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) + { + size += sizeof(uint8_t); + if (*(uint16_t*)(updater->inmsg_queue->queue + size) > *(uint16_t*)(updater->update_no)) + { + // fprintf(stdout,"[debug]Inside update number comparision"); + uint16_t update_no = *(uint16_t*)(updater->inmsg_queue->queue + size); + size += sizeof(uint16_t); + uint16_t update_patch_size = *(uint16_t*)(updater->inmsg_queue->queue + size); + size += sizeof(uint16_t); + /*Generate patch*/ + std::string bzzfile_name(bzz_file); + std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; + std::string name1 = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); + name1 = name1.substr(0, name1.find_last_of(".")); + if (test_patch(path, name1, update_patch_size, (updater->inmsg_queue->queue + size))) + { + out_code = obtain_patched_bo(path, name1); + + // 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((out_code->bcode), (char*)dbgf_name, (size_t)(*(uint16_t*)out_code->bcode_size))) + { + if(debug) ROS_WARN("Received patch PASSED tests!"); + *(uint16_t*)updater->update_no = update_no; + /*Clear exisiting patch if any*/ + delete_p(updater->patch); + /*copy the patch into the updater*/ + updater->patch = (uint8_t*)malloc(update_patch_size); + memcpy(updater->patch, (updater->inmsg_queue->queue + size), update_patch_size); + *(size_t*)(updater->patch_size) = update_patch_size; + // code_message_outqueue_append(); + neigh = 1; + } + /*clear the temp code buff*/ + delete_p(out_code->bcode); + delete_p(out_code->bcode_size); + delete_p(out_code); + } + else + { + ROS_ERROR("Patching the .bo file failed"); + update_try_counter++; + outqueue_append_code_request(update_no); + } + } } } - /* update index to start of next event */ - i += sizeof(struct inotify_event) + event->len; - } - if (!check) - old_update = 0; - return check; -} - -int test_patch(std::string& path, std::string& name1, size_t update_patch_size, uint8_t* patch) -{ - if (SIMULATION == 1) - { - return 1; - } - else - { - /*Patch the old bo with new patch*/ - std::stringstream patch_writefile; - patch_writefile << path << name1 << "tmp_patch.bo"; - /*Write the patch to a file*/ - 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 (system(patch_exsisting.str().c_str())) - return 0; - else - return 1; - } -} - -updater_code_t obtain_patched_bo(std::string& path, std::string& name1) -{ - if (SIMULATION == 1) - { - /*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()); - uint8_t* patched_BO_Buf = 0; - FILE* fp = fopen(read_patched.str().c_str(), "rb"); - if (!fp) - { - perror(read_patched.str().c_str()); - } - 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) - { - 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_size = (uint8_t*)malloc(sizeof(uint16_t)); - *(uint16_t*)(update_code->bcode_size) = patched_size; - - return update_code; - } - - else - { - /*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()); - uint8_t* patched_BO_Buf = 0; - FILE* fp = fopen(read_patched.str().c_str(), "rb"); - if (!fp) - { - perror(read_patched.str().c_str()); - } - 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) - { - perror(read_patched.str().c_str()); - } - fclose(fp); - - /* delete old bo file & rename new bo file */ - remove((path + name1 + ".bo").c_str()); - rename((path + name1 + "-patched.bo").c_str(), (path + name1 + ".bo").c_str()); - - /*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_size = (uint8_t*)malloc(sizeof(uint16_t)); - *(uint16_t*)(update_code->bcode_size) = patched_size; - - return update_code; - } -} - -void code_message_outqueue_append() -{ - updater->outmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); - /* if size less than 250 Pad with zeors to assure transmission*/ - uint16_t size = UPDATE_CODE_HEADER_SIZE + *(size_t*)(updater->patch_size); - uint16_t padding_size = (size > MIN_UPDATE_PACKET) ? 0 : MIN_UPDATE_PACKET - size; - size += padding_size; - updater->outmsg_queue->queue = (uint8_t*)malloc(size); - memset(updater->outmsg_queue->queue, 0, size); - updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); - *(uint16_t*)(updater->outmsg_queue->size) = size; - size = 0; - /*Append message type*/ - *(uint8_t*)(updater->outmsg_queue->queue + size) = SENT_CODE; - size += sizeof(uint8_t); - /*Append the update no, code size and code to out msg*/ - *(uint16_t*)(updater->outmsg_queue->queue + size) = *(uint16_t*)(updater->update_no); - size += sizeof(uint16_t); - *(uint16_t*)(updater->outmsg_queue->queue + size) = (uint16_t) * (size_t*)(updater->patch_size); - size += sizeof(uint16_t); - memcpy(updater->outmsg_queue->queue + size, updater->patch, *(size_t*)(updater->patch_size)); - // size += (uint16_t) * (size_t*)(updater->patch_size); - updater_msg_ready = 1; -} - -void outqueue_append_code_request(uint16_t update_no) -{ - updater->outmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); - uint16_t size = 0; - updater->outmsg_queue->queue = (uint8_t*)malloc(2 * sizeof(uint16_t) + sizeof(uint8_t) + CODE_REQUEST_PADDING); - updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); - memset(updater->outmsg_queue->queue, 0, sizeof(uint16_t) + sizeof(uint8_t) + CODE_REQUEST_PADDING); - /*Append message type*/ - *(uint8_t*)(updater->outmsg_queue->queue + size) = RESEND_CODE; - size += sizeof(uint8_t); - /*Append the update no, code size and code to out msg*/ - *(uint16_t*)(updater->outmsg_queue->queue + size) = update_no; - size += sizeof(uint16_t); - *(uint16_t*)(updater->outmsg_queue->queue + size) = update_try_counter; - 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); -} - -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); - 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; -} -/*Used for data logging*/ -/*void set_packet_id(int packet_id) -{ - packet_id_ = packet_id; -}*/ -void code_message_inqueue_process() -{ - 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)), - (*(uint16_t*)(updater->inmsg_queue->queue + sizeof(uint8_t)))); - ROS_INFO("[Debug] Updater received patch of size %u \n", - (*(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) + 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)) + if (*(uint16_t*)(updater->inmsg_queue->queue + size) == *(uint16_t*)(updater->update_no)) { - // fprintf(stdout,"[debug]Inside update number comparision"); - uint16_t update_no = *(uint16_t*)(updater->inmsg_queue->queue + size); size += sizeof(uint16_t); - uint16_t update_patch_size = *(uint16_t*)(updater->inmsg_queue->queue + size); - size += sizeof(uint16_t); - /*Generate patch*/ - std::string bzzfile_name(bzz_file); - std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; - std::string name1 = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); - name1 = name1.substr(0, name1.find_last_of(".")); - if (test_patch(path, name1, update_patch_size, (updater->inmsg_queue->queue + size))) + if (*(uint16_t*)(updater->inmsg_queue->queue + size) > update_try_counter) { - out_code = obtain_patched_bo(path, name1); + update_try_counter = *(uint16_t*)(updater->inmsg_queue->queue + size); + 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: Max rebroadcast retry reached, ROLL BACK !!"); + } + } + // 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 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); + void create_update_patch() + { + std::stringstream genpatch; - if (test_set_code((out_code->bcode), (char*)dbgf_name, (size_t)(*(uint16_t*)out_code->bcode_size))) - { - printf("TEST PASSED!\n"); - *(uint16_t*)updater->update_no = update_no; - /*Clear exisiting patch if any*/ - delete_p(updater->patch); - /*copy the patch into the updater*/ - updater->patch = (uint8_t*)malloc(update_patch_size); - memcpy(updater->patch, (updater->inmsg_queue->queue + size), update_patch_size); - *(size_t*)(updater->patch_size) = update_patch_size; - // code_message_outqueue_append(); - neigh = 1; - } - /*clear the temp code buff*/ - delete_p(out_code->bcode); - delete_p(out_code->bcode_size); - delete_p(out_code); + std::string bzzfile_name(bzz_file); + + std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; + + std::string name1 = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); + name1 = name1.substr(0, name1.find_last_of(".")); + + std::string name2 = name1 + "-update"; + + /*Launch bsdiff and create a patch*/ + genpatch << "bsdiff " << path << name1 << ".bo " << path << name2 << ".bo " << path << "diff.bo"; + if(debug) ROS_WARN("Launching bsdiff: %s", genpatch.str().c_str()); + system(genpatch.str().c_str()); + + /*Delete old files & rename new files*/ + + remove((path + name1 + ".bo").c_str()); + remove((path + name1 + ".bdb").c_str()); + + rename((path + name2 + ".bo").c_str(), (path + name1 + ".bo").c_str()); + rename((path + name2 + ".bdb").c_str(), (path + name1 + ".bdb").c_str()); + + /*Read the diff file */ + std::stringstream patchfileName; + patchfileName << path << "diff.bo"; + + uint8_t* patch_buff = 0; + FILE* fp = fopen(patchfileName.str().c_str(), "rb"); + if (!fp) + { + perror(patchfileName.str().c_str()); + } + fseek(fp, 0, SEEK_END); + size_t patch_size = ftell(fp); + rewind(fp); + patch_buff = (uint8_t*)malloc(patch_size); + if (fread(patch_buff, 1, patch_size, fp) < patch_size) + { + perror(patchfileName.str().c_str()); + } + fclose(fp); + delete_p(updater->patch); + updater->patch = patch_buff; + *(size_t*)(updater->patch_size) = patch_size; + + /* Delete the diff file */ + remove(patchfileName.str().c_str()); + } + + void update_routine() + { + 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); + if (*(int*)updater->mode == CODE_RUNNING) + { + buzzvm_function_call(VM, "updated_no_bct", 0); + // Check for update + if (check_update()) + { + ROS_INFO("Update found \tUpdating script ..."); + + if (compile_bzz(bzz_file)) + { + ROS_ERROR("Error in compiling script, resuming old script."); } else { - ROS_ERROR("Patching the .bo file failed, could be corrupt patch\n"); - update_try_counter++; - outqueue_append_code_request(update_no); + std::string bzzfile_name(bzz_file); + stringstream bzzfile_in_compile; + std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; + std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); + name = name.substr(0, name.find_last_of(".")); + bzzfile_in_compile << path << name << "-update.bo"; + uint8_t* BO_BUF = 0; + FILE* fp = fopen(bzzfile_in_compile.str().c_str(), "rb"); + if (!fp) + { + perror(bzzfile_in_compile.str().c_str()); + } + 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(bcfname); + } + fclose(fp); + if (test_set_code(BO_BUF, dbgf_name, bcode_size)) + { + uint16_t update_no = *(uint16_t*)(updater->update_no); + *(uint16_t*)(updater->update_no) = update_no + 1; + + create_update_patch(); + VM = buzz_utility::get_vm(); + 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; + if(debug) ROS_INFO("Broadcasting patch ..."); + code_message_outqueue_append(); + } + delete_p(BO_BUF); } } } - } - size = 0; - if (*(uint8_t*)(updater->inmsg_queue->queue) == RESEND_CODE) - { - size += sizeof(uint8_t); - if (*(uint16_t*)(updater->inmsg_queue->queue + size) == *(uint16_t*)(updater->update_no)) + + else { - size += sizeof(uint16_t); - if (*(uint16_t*)(updater->inmsg_queue->queue + size) > update_try_counter) + timer_steps++; + buzzvm_pushs(VM, buzzvm_string_register(VM, "barrier_val", 1)); + buzzvm_gload(VM); + buzzobj_t tObj = buzzvm_stack_at(VM, 1); + buzzvm_pop(VM); + ROS_INFO("Barrier ............. No. of robots deployed: %i", tObj->i.value); + if (tObj->i.value == no_of_robot) { - update_try_counter = *(uint16_t*)(updater->inmsg_queue->queue + size); - ROS_ERROR("Code appended! update try : %u \n", update_try_counter); - code_message_outqueue_append(); + ROS_WARN("Patch deployment successful, rolling forward"); + *(int*)(updater->mode) = CODE_RUNNING; + gettimeofday(&t2, NULL); + // collect_data(); + buzz_utility::buzz_update_set((updater)->bcode, (char*)dbgf_name, *(size_t*)(updater->bcode_size)); + // buzzvm_function_call(m_tBuzzVM, "updated", 0); + update_try_counter = 0; + timer_steps = 0; + } + else if (timer_steps > TIMEOUT_FOR_ROLLBACK) + { + 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); + update_try_counter = 0; + timer_steps = 0; } - if (update_try_counter > MAX_UPDATE_TRY) - ROS_ERROR("TODO: ROLL BACK !!"); } } - // fprintf(stdout,"in queue freed\n"); - delete_p(updater->inmsg_queue->queue); - delete_p(updater->inmsg_queue->size); - delete_p(updater->inmsg_queue); -} -void create_update_patch() -{ - std::stringstream genpatch; - - std::string bzzfile_name(bzz_file); - - std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; - - std::string name1 = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); - name1 = name1.substr(0, name1.find_last_of(".")); - - std::string name2 = name1 + "-update"; - - // CALL BSDIFF CMD - genpatch << "bsdiff " << path << name1 << ".bo " << path << name2 << ".bo " << path << "diff.bo"; - fprintf(stdout, "Launching bsdiff command: %s \n", genpatch.str().c_str()); - system(genpatch.str().c_str()); - - // BETTER: CALL THE FUNCTION IN BSDIFF.CPP - // bsdiff_do(path + name1 + ".bo", path + name2 + ".bo", path + "diff.bo"); - - /* delete old files & rename new files */ - - remove((path + name1 + ".bo").c_str()); - remove((path + name1 + ".bdb").c_str()); - - rename((path + name2 + ".bo").c_str(), (path + name1 + ".bo").c_str()); - rename((path + name2 + ".bdb").c_str(), (path + name1 + ".bdb").c_str()); - - /*Read the diff file */ - std::stringstream patchfileName; - patchfileName << path << "diff.bo"; - - uint8_t* patch_buff = 0; - FILE* fp = fopen(patchfileName.str().c_str(), "rb"); - if (!fp) + uint8_t* getupdater_out_msg() { - perror(patchfileName.str().c_str()); + return (uint8_t*)updater->outmsg_queue->queue; } - fseek(fp, 0, SEEK_END); - size_t patch_size = ftell(fp); - rewind(fp); - patch_buff = (uint8_t*)malloc(patch_size); - if (fread(patch_buff, 1, patch_size, fp) < patch_size) + + uint8_t* getupdate_out_msg_size() { - perror(patchfileName.str().c_str()); + // 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; } - fclose(fp); - delete_p(updater->patch); - updater->patch = patch_buff; - *(size_t*)(updater->patch_size) = patch_size; - /* Delete the diff file */ - remove(patchfileName.str().c_str()); -} - -void update_routine() -{ - 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); - if (*(int*)updater->mode == CODE_RUNNING) + int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size) { - buzzvm_function_call(VM, "updated_neigh", 0); - // Check for update - if (check_update()) + if (buzz_utility::buzz_update_init_test(BO_BUF, dbgfname, bcode_size)) { - ROS_INFO("Update found \nUpdating script ...\n"); - - if (compile_bzz(bzz_file)) + if(debug) ROS_WARN("Initializtion test passed"); + if (buzz_utility::update_step_test()) { - ROS_WARN("Errors in comipilg script so staying on old script\n"); + /*data logging*/ + old_byte_code_size = *(size_t*)updater->bcode_size; + /*data logging*/ + if(debug) ROS_WARN("Step test passed"); + *(int*)(updater->mode) = CODE_STANDBY; + // fprintf(stdout,"updater value = %i\n",updater->mode); + delete_p(updater->bcode); + updater->bcode = (uint8_t*)malloc(bcode_size); + memcpy(updater->bcode, BO_BUF, bcode_size); + *(size_t*)updater->bcode_size = bcode_size; + 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(); + gettimeofday(&t1, NULL); + buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); + buzzvm_pushi(VM, no_of_robot); + buzzvm_gstore(VM); + return 1; } + /*Unable to step something wrong*/ else { - std::string bzzfile_name(bzz_file); - stringstream bzzfile_in_compile; - std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; - std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); - name = name.substr(0, name.find_last_of(".")); - bzzfile_in_compile << path << name << "-update.bo"; - uint8_t* BO_BUF = 0; - FILE* fp = fopen(bzzfile_in_compile.str().c_str(), "rb"); // to change file edit - if (!fp) + if (*(int*)(updater->mode) == CODE_RUNNING) { - perror(bzzfile_in_compile.str().c_str()); + ROS_ERROR("Step test failed, resuming old script"); + buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (size_t) * (size_t*)(updater->bcode_size)); } - 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) + else { - perror(bcfname); - } - fclose(fp); - if (test_set_code(BO_BUF, dbgf_name, bcode_size)) - { - uint16_t update_no = *(uint16_t*)(updater->update_no); - *(uint16_t*)(updater->update_no) = update_no + 1; - - create_update_patch(); - VM = buzz_utility::get_vm(); - ROS_INFO("Current 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)); + /*You will never reach here*/ + 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(); + buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); + buzzvm_pushi(VM, no_of_robot); buzzvm_gstore(VM); - neigh = -1; - ROS_INFO("Sending code... \n"); - code_message_outqueue_append(); } - delete_p(BO_BUF); + return 0; } } - } - - else - { - timer_steps++; - buzzvm_pushs(VM, buzzvm_string_register(VM, "barrier_val", 1)); - buzzvm_gload(VM); - buzzobj_t tObj = buzzvm_stack_at(VM, 1); - buzzvm_pop(VM); - ROS_INFO("Barrier ..................... %i \n", tObj->i.value); - if (tObj->i.value == no_of_robot) - { - *(int*)(updater->mode) = CODE_RUNNING; - gettimeofday(&t2, NULL); - // collect_data(); - buzz_utility::buzz_update_set((updater)->bcode, (char*)dbgf_name, *(size_t*)(updater->bcode_size)); - // buzzvm_function_call(m_tBuzzVM, "updated", 0); - updated = 1; - update_try_counter = 0; - timer_steps = 0; - } - else if (timer_steps > TIMEOUT_FOR_ROLLBACK) - { - ROS_ERROR("TIME OUT Reached, decided to roll 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); - updated = 1; - update_try_counter = 0; - timer_steps = 0; - } - } -} - -uint8_t* getupdater_out_msg() -{ - 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; -} - -int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size) -{ - if (buzz_utility::buzz_update_init_test(BO_BUF, dbgfname, bcode_size)) - { - ROS_WARN("Initializtion of script test passed\n"); - 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"); - *(int*)(updater->mode) = CODE_STANDBY; - // fprintf(stdout,"updater value = %i\n",updater->mode); - delete_p(updater->bcode); - updater->bcode = (uint8_t*)malloc(bcode_size); - memcpy(updater->bcode, BO_BUF, bcode_size); - *(size_t*)updater->bcode_size = bcode_size; - 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(); - gettimeofday(&t1, NULL); - buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); - buzzvm_pushi(VM, no_of_robot); - buzzvm_gstore(VM); - return 1; - } - /*Unable to step something wrong*/ else { if (*(int*)(updater->mode) == CODE_RUNNING) { - ROS_ERROR("step test failed, stick to old script\n"); - buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (size_t) * (size_t*)(updater->bcode_size)); + 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("step 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(); @@ -604,118 +638,87 @@ int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size) return 0; } } - else - { - if (*(int*)(updater->mode) == CODE_RUNNING) - { - ROS_ERROR("Initialization test failed, stick to old script\n"); - 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"); - 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(); - buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); - buzzvm_pushi(VM, no_of_robot); - buzzvm_gstore(VM); - } - return 0; - } -} -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; -} -int get_update_status() -{ - return updated; -} -void set_read_update_status() -{ - updated = 0; -} -/*int get_update_mode() -{ - return (int)*(int*)(updater->mode); -}*/ - -int is_msg_present() -{ - return updater_msg_ready; -} -/*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) + 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; } - if (updater->inmsg_queue) + + int is_msg_present() { - delete_p(updater->inmsg_queue->queue); - delete_p(updater->inmsg_queue->size); - delete_p(updater->inmsg_queue); + return updater_msg_ready; + } + /*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); } - inotify_rm_watch(fd, wd); - close(fd); -} -void set_bzz_file(const char* in_bzz_file) -{ - bzz_file = in_bzz_file; -} + void set_bzz_file(const char* in_bzz_file, bool dbg) + { + debug=dbg; + bzz_file = in_bzz_file; + } -void updates_set_robots(int robots) -{ - no_of_robot = robots; -} + void updates_set_robots(int robots) + { + no_of_robot = robots; + } -/*-------------------------------------------------------- -/ Create Buzz bytecode from the bzz script inputed -/-------------------------------------------------------*/ -int compile_bzz(std::string bzz_file) -{ - /*Compile the buzz code .bzz to .bo*/ - std::string bzzfile_name(bzz_file); - stringstream bzzfile_in_compile; - std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; - std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); - name = name.substr(0, name.find_last_of(".")); - bzzfile_in_compile << "bzzc -I " << path << "include/"; //<<" "<bcode_size << "," << *(size_t*)updater->patch_size << "," - << (int)*(uint8_t*)updater->update_no << "," << (int)packet_id_; -}*/ + /*void collect_data(std::ofstream& logger) + { + 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; + // RID,update trigger,time steps taken,old byte code size, new bytecode size, patch size,update number, + // + Patch_packets_received_counter,Patch_request_packets_received,Patch_packets_sent_counter,Patch_request_packets_sent_counter + logger << (int)no_of_robot << "," << neigh << "," << (double)time_spent << "," << (int)timer_steps << "," + << old_byte_code_size << "," << *(size_t*)updater->bcode_size << "," << *(size_t*)updater->patch_size << "," + << (int)*(uint8_t*)updater->update_no << "," << (int)packet_id_; + }*/ +} \ No newline at end of file diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 364f2f8..6631ba1 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -678,6 +678,11 @@ void update_neighbors(buzzvm_t vm) } } +// Clear neighbours pos +void clear_neighbours_pos(){ + neighbors_map.clear(); +} + int buzzuav_update_currentpos(buzzvm_t vm) /* / Update the BVM position table diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 9dc1374..e84025a 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -27,7 +27,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"; - 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); @@ -107,7 +107,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 - 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 @@ -120,7 +120,7 @@ void roscontroller::RosControllerRun() grid_publisher(); send_MPpayload(); // Check updater state and step code - update_routine(); + if(update) buzz_update::update_routine(); if (time_step == BUZZRATE) { time_step = 0; @@ -144,15 +144,10 @@ void roscontroller::RosControllerRun() prepare_msg_and_publish(); // Call the flight controler service flight_controller_service_call(); - // Set multi message available after update - if (get_update_status()) - { - set_read_update_status(); - } // Set ROBOTS variable (swarm size) get_number_of_robots(); buzz_utility::set_robot_var(no_of_robots); - 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(); @@ -561,11 +556,11 @@ with size......... | / payload_pub.publish(payload_out); delete[] out; delete[] payload_out_ptr; - // Check for updater message if present send - if (is_msg_present()) + /// Check for updater message if present send + if (update && buzz_update::is_msg_present()) { uint8_t* buff_send = 0; - uint16_t updater_msgSize = *(uint16_t*)(getupdate_out_msg_size()); + uint16_t updater_msgSize = *(uint16_t*)(buzz_update::getupdate_out_msg_size()); ; int tot = 0; mavros_msgs::Mavlink update_packets; @@ -578,10 +573,10 @@ with size......... | / *(uint16_t*)(buff_send + tot) = updater_msgSize; tot += sizeof(uint16_t); // Append updater msgs - memcpy(buff_send + tot, (uint8_t*)(getupdater_out_msg()), updater_msgSize); + memcpy(buff_send + tot, (uint8_t*)(buzz_update::getupdater_out_msg()), updater_msgSize); tot += updater_msgSize; // Destroy the updater out msg queue - destroy_out_msg_queue(); + buzz_update::destroy_out_msg_queue(); uint16_t total_size = (ceil((float)(float)tot / (float)sizeof(uint64_t))); uint64_t* payload_64 = new uint64_t[total_size]; memcpy((void*)payload_64, (void*)buff_send, total_size * sizeof(uint64_t)); @@ -750,6 +745,7 @@ void roscontroller::maintain_pos(int tim_step) if (timer_step >= BUZZRATE) { neighbours_pos_map.clear(); + buzzuav_closures::clear_neighbours_pos(); // raw_neighbours_pos_map.clear(); // TODO: currently not a problem, but // have to clear ! timer_step = 0; @@ -1031,8 +1027,8 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) fprintf(stdout, "Update packet received, read msg size : %u \n", unMsgSize); if (unMsgSize > 0) { - code_message_inqueue_append((uint8_t*)(pl + sizeof(uint16_t)), unMsgSize); - code_message_inqueue_process(); + buzz_update::code_message_inqueue_append((uint8_t*)(pl + sizeof(uint16_t)), unMsgSize); + buzz_update::code_message_inqueue_process(); } free(pl); }