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

added update changes and neighbour pos clear on dev

This commit is contained in:
vivek-shankar 2018-06-14 13:37:00 -04:00
parent 6d52a57dcd
commit a9038601ae
6 changed files with 799 additions and 780 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

180
include/buzz_update.h
View File

@ -1,68 +1,74 @@
#ifndef BUZZ_UPDATE_H
#define BUZZ_UPDATE_H
/*Simulation or robot check*/
//#define SIMULATION 1 // set in CMAKELIST
#include <stdlib.h>
#include <stdio.h>
#include <fcntl.h>
#include <unistd.h>
#include <string.h>
#include <sys/time.h>
#include <sys/inotify.h>
#include <buzz/buzztype.h>
#include <buzz/buzzdict.h>
#include <buzz/buzzdarray.h>
#include <buzz/buzzvstig.h>
#include <buzz_utility.h>
#include <fstream>
#define delete_p(p) \
do \
{ \
free(p); \
p = NULL; \
} while (0)
namespace buzz_update
{
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 */
/********************/
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 {
typedef enum {
CODE_RUNNING = 0, // Code executing
CODE_STANDBY, // Standing by for others to update
} code_states_e;
} code_states_e;
/*********************/
/*Message types */
/********************/
/*********************/
/*Message types */
/********************/
typedef enum {
typedef enum {
SENT_CODE = 0, // Broadcast code
RESEND_CODE, // ReBroadcast request
} code_message_e;
} code_message_e;
/*************************/
/*Updater message queue */
/*************************/
/*************************/
/*Updater message queue */
/*************************/
struct updater_msgqueue_s
{
struct updater_msgqueue_s
{
uint8_t* queue;
uint8_t* size;
};
typedef struct updater_msgqueue_s* updater_msgqueue_t;
};
typedef struct updater_msgqueue_s* updater_msgqueue_t;
struct updater_code_s
{
struct updater_code_s
{
uint8_t* bcode;
uint8_t* bcode_size;
};
typedef struct updater_code_s* updater_code_t;
};
typedef struct updater_code_s* updater_code_t;
/**************************/
/*Updater data*/
/**************************/
/**************************/
/*Updater data*/
/**************************/
struct buzz_updater_elem_s
{
struct buzz_updater_elem_s
{
/* robot id */
// uint16_t robotid;
/*current Bytecode content */
@ -86,76 +92,80 @@ struct buzz_updater_elem_s
/*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 struct buzz_updater_elem_s* buzz_updater_elem_t;
/**************************************************************************/
/*Updater routine from msg processing to file checks to be called from main*/
/**************************************************************************/
void update_routine();
/**************************************************************************/
/*Updater routine from msg processing to file checks to be called from main*/
/**************************************************************************/
void update_routine();
/************************************************/
/*Initalizes the updater */
/************************************************/
void init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id);
/************************************************/
/*Initalizes the updater */
/************************************************/
int init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id);
/*********************************************************/
/*Appends buffer of given size to in msg queue of updater*/
/*********************************************************/
/*********************************************************/
/*Appends buffer of given size to in msg queue of updater*/
/*********************************************************/
void code_message_inqueue_append(uint8_t* msg, uint16_t size);
void code_message_inqueue_append(uint8_t* msg, uint16_t size);
/*********************************************************/
/*Processes messages inside the queue of the updater*/
/*********************************************************/
/*********************************************************/
/*Processes messages inside the queue of the updater*/
/*********************************************************/
void code_message_inqueue_process();
void code_message_inqueue_process();
/*****************************************************/
/* obtains messages from out msgs queue of the updater*/
/*******************************************************/
uint8_t* getupdater_out_msg();
/*****************************************************/
/*Obtains messages from out msgs queue of the updater*/
/*******************************************************/
uint8_t* getupdater_out_msg();
/******************************************************/
/*obtains out msg queue size*/
/*****************************************************/
uint8_t* getupdate_out_msg_size();
/******************************************************/
/*Obtains out msg queue size*/
/*****************************************************/
uint8_t* getupdate_out_msg_size();
/**************************************************/
/*destroys the out msg queue*/
/*************************************************/
void destroy_out_msg_queue();
/**************************************************/
/*Destroys the out msg queue*/
/*************************************************/
void destroy_out_msg_queue();
/***************************************************/
/*obatins updater state*/
/***************************************************/
// int get_update_mode();
// buzz_updater_elem_t get_updater();
/***************************************************/
/*Sets bzz file name*/
/***************************************************/
void set_bzz_file(const char* in_bzz_file, bool dbg);
// buzz_updater_elem_t get_updater();
/***************************************************/
/*sets bzz file name*/
/***************************************************/
void set_bzz_file(const char* in_bzz_file);
/****************************************************/
/*Tests the code from a buffer*/
/***************************************************/
int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size);
int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size);
/****************************************************/
/*Destroys the updater*/
/***************************************************/
/****************************************************/
/*Destroys the updater*/
/***************************************************/
void destroy_updater();
void destroy_updater();
int is_msg_present();
/****************************************************/
/*Checks for updater message*/
/***************************************************/
int get_update_status();
int is_msg_present();
void set_read_update_status();
/****************************************************/
/*Compiles a bzz script to bo and bdbg*/
/***************************************************/
int compile_bzz(std::string bzz_file);
int compile_bzz(std::string bzz_file);
void updates_set_robots(int robots);
/****************************************************/
/*Set number of robots in the updater*/
/***************************************************/
// void set_packet_id(int packet_id);
// void collect_data(std::ofstream& logger);
void updates_set_robots(int robots);
}
#endif

4
include/buzzuav_closures.h
View File

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

1
include/roscontroller.h
View File

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

339
src/buzz_update.cpp
View File

@ -1,51 +1,56 @@
#include <stdio.h>
#include <stdlib.h>
#include <fcntl.h>
#include <unistd.h>
#include <sys/inotify.h>
/** @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 <buzz_utility.h>
#include <string.h>
#include <buzz/buzzdebug.h>
#include <sys/time.h>
/*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)
{
/*Initialize updater*/
int init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id)
{
buzzvm_t VM = buzz_utility::get_vm();
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_INFO("Initializing file monitor...");
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);
if (fd < 0)
{
perror("inotify_init error");
}
/*If simulation set the file monitor only for robot 1*/
/*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 */
@ -117,10 +122,16 @@ void init_update_monitor(const char* bo_filename, const char* stand_by_script, c
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()
{
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;
@ -128,32 +139,31 @@ int check_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);
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)
{
check = 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;
}
if (!check)
old_update = 0;
return check;
}
}
int test_patch(std::string& path, std::string& name1, size_t update_patch_size, uint8_t* patch)
{
int test_patch(std::string& path, std::string& name1, size_t update_patch_size, uint8_t* patch)
{
if (SIMULATION == 1)
{
return 1;
@ -163,30 +173,33 @@ int test_patch(std::string& path, std::string& name1, size_t update_patch_size,
/*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
return 1;
}
}
}
updater_code_t obtain_patched_bo(std::string& path, std::string& name1)
{
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;
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)
{
@ -195,15 +208,15 @@ updater_code_t obtain_patched_bo(std::string& path, std::string& name1)
fseek(fp, 0, SEEK_END);
size_t patched_size = ftell(fp);
rewind(fp);
patched_BO_Buf = (uint8_t*)malloc(patched_size);
if (fread(patched_BO_Buf, 1, patched_size, fp) < patched_size)
patched_bo_buf = (uint8_t*)malloc(patched_size);
if (fread(patched_bo_buf, 1, patched_size, fp) < patched_size)
{
perror(read_patched.str().c_str());
}
fclose(fp);
/*Write the patched to a code struct and return*/
updater_code_t update_code = (updater_code_t)malloc(sizeof(struct updater_code_s));
update_code->bcode = patched_BO_Buf;
update_code->bcode = patched_bo_buf;
update_code->bcode_size = (uint8_t*)malloc(sizeof(uint16_t));
*(uint16_t*)(update_code->bcode_size) = patched_size;
@ -215,8 +228,11 @@ updater_code_t obtain_patched_bo(std::string& path, std::string& name1)
/*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;
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)
{
@ -225,8 +241,8 @@ updater_code_t obtain_patched_bo(std::string& path, std::string& name1)
fseek(fp, 0, SEEK_END);
size_t patched_size = ftell(fp);
rewind(fp);
patched_BO_Buf = (uint8_t*)malloc(patched_size);
if (fread(patched_BO_Buf, 1, patched_size, fp) < patched_size)
patched_bo_buf = (uint8_t*)malloc(patched_size);
if (fread(patched_bo_buf, 1, patched_size, fp) < patched_size)
{
perror(read_patched.str().c_str());
}
@ -238,16 +254,16 @@ updater_code_t obtain_patched_bo(std::string& path, std::string& name1)
/*Write the patched to a code struct and return*/
updater_code_t update_code = (updater_code_t)malloc(sizeof(struct updater_code_s));
update_code->bcode = patched_BO_Buf;
update_code->bcode = patched_bo_buf;
update_code->bcode_size = (uint8_t*)malloc(sizeof(uint16_t));
*(uint16_t*)(update_code->bcode_size) = patched_size;
return update_code;
}
}
}
void code_message_outqueue_append()
{
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);
@ -269,10 +285,10 @@ void code_message_outqueue_append()
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)
{
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);
@ -288,34 +304,34 @@ void outqueue_append_code_request(uint16_t update_no)
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)
{
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);
*(uint16_t*)(updater->inmsg_queue->size) = size;
}
/*Used for data logging*/
/*void set_packet_id(int packet_id)
{
}
/*Used for data logging*/
/*void set_packet_id(int packet_id)
{
packet_id_ = packet_id;
}*/
void code_message_inqueue_process()
{
}*/
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)),
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");
@ -347,7 +363,7 @@ void code_message_inqueue_process()
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);
@ -365,7 +381,7 @@ void code_message_inqueue_process()
}
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);
}
@ -375,6 +391,7 @@ void code_message_inqueue_process()
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))
{
@ -382,21 +399,21 @@ void code_message_inqueue_process()
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");
delete_p(updater->inmsg_queue->queue);
delete_p(updater->inmsg_queue->size);
delete_p(updater->inmsg_queue);
}
}
void create_update_patch()
{
void create_update_patch()
{
std::stringstream genpatch;
std::string bzzfile_name(bzz_file);
@ -408,15 +425,12 @@ void create_update_patch()
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());
// BETTER: CALL THE FUNCTION IN BSDIFF.CPP
// bsdiff_do(path + name1 + ".bo", path + name2 + ".bo", path + "diff.bo");
/* delete old files & rename new files */
/*Delete old files & rename new files*/
remove((path + name1 + ".bo").c_str());
remove((path + name1 + ".bdb").c_str());
@ -449,10 +463,10 @@ void create_update_patch()
/* Delete the diff file */
remove(patchfileName.str().c_str());
}
}
void update_routine()
{
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));
@ -460,15 +474,15 @@ void update_routine()
// fprintf(stdout,"[Debug : ]updater value = %i \n",updater->mode);
if (*(int*)updater->mode == CODE_RUNNING)
{
buzzvm_function_call(VM, "updated_neigh", 0);
buzzvm_function_call(VM, "updated_no_bct", 0);
// 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
{
@ -479,7 +493,7 @@ void update_routine()
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
FILE* fp = fopen(bzzfile_in_compile.str().c_str(), "rb");
if (!fp)
{
perror(bzzfile_in_compile.str().c_str());
@ -500,12 +514,12 @@ void update_routine()
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);
@ -520,53 +534,52 @@ void update_routine()
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();
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");
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);
updated = 1;
update_try_counter = 0;
timer_steps = 0;
}
}
}
}
uint8_t* getupdater_out_msg()
{
uint8_t* getupdater_out_msg()
{
return (uint8_t*)updater->outmsg_queue->queue;
}
}
uint8_t* getupdate_out_msg_size()
{
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)
{
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(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);
@ -587,13 +600,13 @@ int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size)
{
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();
@ -608,13 +621,13 @@ int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size)
{
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();
@ -624,39 +637,27 @@ int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size)
}
return 0;
}
}
}
void destroy_out_msg_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;
}
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()
{
int is_msg_present()
{
return updater_msg_ready;
}
/*buzz_updater_elem_t get_updater()
{
}
/*buzz_updater_elem_t get_updater()
{
return updater;
}*/
void destroy_updater()
{
}*/
void destroy_updater()
{
delete_p(updater->bcode);
delete_p(updater->bcode_size);
delete_p(updater->standby_bcode);
@ -677,23 +678,24 @@ void destroy_updater()
}
inotify_rm_watch(fd, wd);
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;
}
}
void updates_set_robots(int 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)
{
/*--------------------------------------------------------
/ Create Buzz bytecode from the bzz script input
/-------------------------------------------------------*/
int compile_bzz(std::string bzz_file)
{
/*Compile the buzz code .bzz to .bo*/
std::string bzzfile_name(bzz_file);
stringstream bzzfile_in_compile;
@ -706,16 +708,17 @@ int compile_bzz(std::string bzz_file)
bzzfile_in_compile << bzzfile_name;
ROS_WARN("Launching buzz compilation for update: %s", bzzfile_in_compile.str().c_str());
return system(bzzfile_in_compile.str().c_str());
}
}
/*void collect_data(std::ofstream& logger)
{
/*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
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_;
}*/
}*/
}

5
src/buzzuav_closures.cpp
View File

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

28
src/roscontroller.cpp
View File

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