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

new updater with standby script loading

This commit is contained in:
vivek-shankar 2017-01-27 08:10:27 -05:00
parent c724900448
commit b5bb610043
14 changed files with 405 additions and 313 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
include/buzz_update.h
View File

@ -16,8 +16,6 @@
typedef enum {
CODE_RUNNING = 0, // Code executing
CODE_UPDATE, // Updating code
CODE_NEIGHBOUR, // Neighbour triggered an update
CODE_STANDBY, // Standing by for others to update
} code_states_e;
@ -47,33 +45,34 @@ struct updater_msgqueue_s {
struct buzz_updater_elem_s {
/* robot id */
uint16_t robotid;
/*current Bytecode content */
/*current Bytecode content */
uint8_t* bcode;
/*current bcode size*/
size_t bcode_size;
size_t* bcode_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;
/*Table with state of other neighbours*/
buzzdict_t state_dict;
/*Update number to ensure consistency*/
uint8_t update_no;
int* mode;
uint8_t* update_no;
} ;
typedef struct buzz_updater_elem_s* buzz_updater_elem_t;
/**************************************************************************/
/*Updater routine from msg processing to file checks to be called from main*/
/**************************************************************************/
int update_routine(const char* bcfname,
const char* dbgfname, int destroy);
void update_routine(const char* bcfname,
const char* dbgfname);
/************************************************/
/*Initalizes the updater */
/************************************************/
void init_update_monitor(const char* bo_filename,int barrier);
void init_update_monitor(const char* bo_filename,const char* stand_by_script,int barrier);
/*********************************************************/
@ -113,6 +112,8 @@ int get_update_mode();
/***************************************************/
void set_bzz_file(const char* in_bzz_file);
int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_size);
/****************************************************/
/*Destroys the updater*/
/***************************************************/

1
include/buzz_utility.h
View File

@ -51,4 +51,5 @@ int update_step_test();
uint16_t get_robotid();
buzzvm_t get_vm();
}

2
include/roscontroller.h
View File

@ -46,7 +46,7 @@ private:
//int oldcmdID=0;
int rc_cmd;
int barrier;
std::string bzzfile_name, fcclient_name, rcservice_name,bcfname,dbgfname,out_payload,in_payload; //, rcclient;
std::string bzzfile_name, fcclient_name, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by; //, rcclient;
bool rcclient;
ros::ServiceClient mav_client;
ros::Publisher payload_pub;

1
launch/rosbuzz_2_parallel.launch
View File

@ -11,6 +11,7 @@
<param name="out_payload" value="/outMavlink"/>
<param name="robot_id" value="1"/>
<param name="No_of_Robots" value="1"/>
<param name="stand_by" value="/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bo"/>
</node>
<!-- node name="rosbuzz_node1" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" -->
<!-- param name="bzzfile_name" value="/home/vivek/catkin_ws/src/rosbuzz/src/test1.bzz" -->

1
launch/rosbuzzm100.launch
View File

@ -10,6 +10,7 @@
<param name="out_payload" value="/outMavlink"/>
<param name="robot_id" value="3"/>
<param name="No_of_Robots" value="2"/>
<param name="stand_by" value="../ROS_WS/src/ROSBuzz/src/stand_by.bo"/>
</node>

383
src/buzz_update.cpp
View File

@ -8,7 +8,7 @@
#include <string.h>
#include <buzz/buzzdebug.h>
#include <sys/time.h>
#define MAX_BUCKETS 10
/*Temp for data collection*/
//static int neigh=-1;
@ -23,35 +23,9 @@ static buzz_updater_elem_t updater;
static int no_of_robot;
static char* dbgf_name;
static const char* bzz_file;
static int neigh=0;
uint32_t buzz_updater_hashfunp (const void* key){
return (*(uint16_t*)key);
}
int buzz_updater_key_cmp(const void* a, const void* b){
if( *(uint16_t*)a < *(uint16_t*)b ) return -1;
if( *(uint16_t*)a > *(uint16_t*)b ) return 1;
return 0;
}
void updater_entry_destroy(const void* key, void* data, void* params) {
// fprintf(stdout,"freeing element.\n");
free((void*)key);
free((void*)data);
}
void standby_barrier_test(const void* key, const void* data, void* tmp){
//fprintf(stdout,"Checking barrier for robot :%i it has barrier : %i.\n",*(uint16_t*) key,*(uint8_t*) data );
if(*(uint8_t*) data == CODE_STANDBY || *(uint8_t*) data == CODE_RUNNING ){
if(updater->update_no==*(uint8_t*) (data+sizeof(uint8_t)) )
(*(uint8_t*)tmp)+=1;
}
}
void init_update_monitor(const char* bo_filename,int barrier){
void init_update_monitor(const char* bo_filename, const char* stand_by_script,int barrier){
fprintf(stdout,"intiialized file monitor.\n");
fd=inotify_init1(IN_NONBLOCK);
if ( fd < 0 ) {
@ -80,23 +54,43 @@ void init_update_monitor(const char* bo_filename,int barrier){
//return 0;
}
fclose(fp);
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);
updater = (buzz_updater_elem_t)malloc(sizeof(struct buzz_updater_elem_s));
/* Create a new table for updater*/
updater->bcode = BO_BUF;
updater->bcode_size = bcode_size;
updater->mode=CODE_RUNNING;
updater->robotid= buzz_utility::get_robotid();
updater->state_dict=buzzdict_new(MAX_BUCKETS,
sizeof(uint16_t),
sizeof(uint16_t),
buzz_updater_hashfunp,
buzz_updater_key_cmp,
updater_entry_destroy);
updater->outmsg_queue = NULL;
updater->inmsg_queue = NULL;
updater->update_no=0;
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;
//neigh = 0;
//updater->outmsg_queue=
// update_table->barrier=nvs;
@ -143,71 +137,25 @@ int check_update(){
}
void code_message_outqueue_append(int type){
uint16_t size =0;
//fprintf(stdout,"updater queue append.\n");
//if(updater->outmsg_queue == NULL)
//updater->outmsg_queue=(updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s));
//else {
// destroy_out_msg_queue();
updater->outmsg_queue=(updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s));
// }
if(type==SEND_CODE){
updater->outmsg_queue->queue = (uint8_t*)malloc((6*sizeof(uint8_t))+updater->bcode_size);
updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t));
//*(uint16_t*)updater->outmsg_queue->size =(3*sizeof(uint8_t))+sizeof(size_t)+updater->bcode_size;
*(uint16_t*)(updater->outmsg_queue->queue+size) = (uint16_t) updater->robotid;
size+=sizeof(uint16_t);
*(uint8_t*)(updater->outmsg_queue->queue+size) = (uint8_t) updater->mode;
size+=sizeof(uint8_t);
*(uint8_t*)(updater->outmsg_queue->queue+size) = updater->update_no;
size+=sizeof(uint8_t);
*(uint16_t*)(updater->outmsg_queue->queue+size) = (uint16_t) updater->bcode_size;
size+=sizeof(uint16_t);
memcpy(updater->outmsg_queue->queue+size, updater->bcode, updater->bcode_size);
size+=updater->bcode_size;
FILE *fp;
fp=fopen("update.bo", "wb");
fwrite((updater->bcode), updater->bcode_size, 1, fp);
fclose(fp);
fprintf(stdout,"[Debug : ]updater rid = %i \n",(int)updater->robotid);
fprintf(stdout,"[Debug : ]updater mode = %i \n",(int)updater->mode);
fprintf(stdout,"[Debug : ]update number = %i \n",(int)updater->update_no);
*(uint16_t*)updater->outmsg_queue->size=size;
/*Update local dictionary*/
uint8_t* dict_update=(uint8_t*)malloc(sizeof(uint16_t));
*(uint8_t*)dict_update=updater->mode;
*(uint8_t*)(dict_update+sizeof(uint8_t))=updater->update_no;
buzzdict_set(updater->state_dict,(uint8_t*) &(updater->robotid), (uint8_t*)dict_update);
delete_p(dict_update);
}
else if(type==STATE_MSG){
updater->outmsg_queue->queue = (uint8_t*)malloc(4*sizeof(uint8_t));
updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t));
//*(uint16_t*)updater->outmsg_queue->size = 3*sizeof(uint8_t);
*(uint16_t*)(updater->outmsg_queue->queue+size) = (uint16_t) updater->robotid;
size+=sizeof(uint16_t);
*(uint8_t*)(updater->outmsg_queue->queue+size) = (uint8_t) updater->mode;
size+=sizeof(uint8_t);
*(uint8_t*)(updater->outmsg_queue->queue+size) = updater->update_no;
size+=sizeof(uint8_t);
*(uint16_t*)updater->outmsg_queue->size=size;
fprintf(stdout,"[Debug : ]updater rid = %i \n",(int)updater->robotid);
fprintf(stdout,"[Debug : ]updater mode = %i \n",(int)updater->mode);
fprintf(stdout,"[Debug : ]update number = %i \n",(int)updater->update_no);
/*Update local dictionary*/
uint8_t* dict_update=(uint8_t*)malloc(sizeof(uint16_t));
*(uint8_t*)dict_update=updater->mode;
*(uint8_t*)(dict_update+sizeof(uint8_t))=updater->update_no;
buzzdict_set(updater->state_dict,(uint8_t*) &(updater->robotid), (uint8_t*)dict_update);
delete_p(dict_update);
}
void code_message_outqueue_append(){
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)+ *(size_t*)(updater->bcode_size));
updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_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) = *(size_t*) updater->bcode_size;
size+=sizeof(uint16_t);
memcpy(updater->outmsg_queue->queue+size, updater->bcode, *(size_t*)updater->bcode_size);
size+=*(size_t*)updater->bcode_size;
/*FILE *fp;
fp=fopen("update.bo", "wb");
fwrite((updater->bcode), updater->bcode_size, 1, fp);
fclose(fp);*/
*(uint16_t*)updater->outmsg_queue->size=size;
//fprintf(stdout,"out mes append transfer code %d\n", transfer_code);
}
void code_message_inqueue_append(uint8_t* msg,uint16_t size){
@ -221,75 +169,34 @@ memcpy(updater->inmsg_queue->queue, msg, size);
void code_message_inqueue_process(){
int size=0;
//fprintf(stdout,"received state : %i from robot : %i\n",*(uint8_t*)(updater->inmsg_queue->queue+sizeof(uint16_t)),*(uint16_t*)updater->inmsg_queue->queue);
if(*(uint8_t*)(updater->inmsg_queue->queue+sizeof(uint16_t)) != CODE_RUNNING){
buzzdict_set(updater->state_dict, updater->inmsg_queue->queue,(updater->inmsg_queue->queue+sizeof(uint16_t)));
size +=3*sizeof(uint8_t);
if(*(uint8_t*)(updater->inmsg_queue->queue+size) > (updater->update_no)){
if(updater->mode==CODE_RUNNING){
uint8_t update_no_obt=*(uint8_t*)(updater->inmsg_queue->queue+size);
size+=sizeof(uint8_t);
uint16_t update_bcode_size =*(uint16_t*)(updater->inmsg_queue->queue+size);
size +=sizeof(uint16_t);
FILE *fp;
fp=fopen("update.bo", "wb");
fwrite((updater->inmsg_queue->queue+size), update_bcode_size, 1, fp);
fclose(fp);
if(buzz_utility::buzz_update_init_test((updater->inmsg_queue->queue+size), dbgf_name,(size_t)update_bcode_size)){
fprintf(stdout,"Initializtion of script test passed\n");
if(buzz_utility::update_step_test()){
/*data logging*/
//neigh=1;
//gettimeofday(&t1, NULL);
//fprintf(stdout,"start and end time in queue process of update Info : %f,%f",(double)begin,(double)end);
/*data logging*/
fprintf(stdout,"Step test passed\n");
updater->update_no=update_no_obt;
updater->mode=CODE_UPDATE;
//fprintf(stdout,"updater value = %i\n",updater->mode);
free(updater->bcode);
updater->bcode = (uint8_t*)malloc(update_bcode_size);
memcpy(updater->bcode, updater->inmsg_queue->queue+size, update_bcode_size);
//updater->bcode = BO_BUF;
updater->bcode_size = (size_t)update_bcode_size;
code_message_outqueue_append(STATE_MSG);
updater->mode=CODE_STANDBY;
//return updater->mode;
//return 0;
}
/*Unable to step something wrong*/
else{
fprintf(stdout,"Step test failed, stick to old script\n");
buzz_utility::buzz_update_set((updater)->bcode, dbgf_name, (updater)->bcode_size);
//return updater->mode;
//return 1;
}
}
else {
fprintf(stdout,"Initialiation test failed, stick to old script\n");
buzz_utility::buzz_update_set((updater)->bcode, dbgf_name, (updater)->bcode_size);
//return updater->mode;
//return 1;
}
}
if(*(int*)updater->mode==CODE_RUNNING){
if(*(uint16_t*)(updater->inmsg_queue->queue+size) > *(uint16_t*) updater->update_no){
uint16_t update_no=*(uint16_t*)(updater->inmsg_queue->queue+size);
size +=sizeof(uint16_t);
uint16_t update_bcode_size =*(uint16_t*)(updater->inmsg_queue->queue+size);
size +=sizeof(uint16_t);
//FILE *fp;
//fp=fopen("update.bo", "wb");
//fwrite((updater->inmsg_queue->queue+size), update_bcode_size, 1, fp);
//fclose(fp);
if(test_set_code((updater->inmsg_queue->queue+size),
(char*) dbgf_name,(size_t)update_bcode_size)) {
*(uint16_t*)updater->update_no=update_no;
neigh=1;
}
}
}
}
else {
buzzdict_set(updater->state_dict,updater->inmsg_queue->queue, updater->inmsg_queue->queue+sizeof(uint16_t));
}
//fprintf(stdout,"in queue freed\n");
delete_p(updater->inmsg_queue->queue);
delete_p(updater->inmsg_queue->size);
delete_p(updater->inmsg_queue);
}
int update_routine(const char* bcfname,
const char* dbgfname, int destroy){
dbgf_name=(char*)dbgfname;
void update_routine(const char* bcfname,
const char* dbgfname){
//dbgf_name=(char*)dbgfname;
//fprintf(stdout,"[Debug : ]updater value = %i \n",updater->mode);
if(updater->mode==CODE_RUNNING){
if(*(int*)updater->mode==CODE_RUNNING){
if(check_update()){
std::string bzzfile_name(bzz_file);
stringstream bzzfile_in_compile;
@ -347,86 +254,38 @@ dbgf_name=(char*)dbgfname;
}
fclose(fp);
if(buzz_utility::buzz_update_init_test(BO_BUF, dbgfname,bcode_size)){
fprintf(stdout,"Initializtion of script test passed\n");
if(buzz_utility::update_step_test()){
/*data logging*/
//neigh=0;
//gettimeofday(&t1, NULL);
//fprintf(stdout,"start and end time in code running state Info : %f,%f",(double)begin,(double)end);
/*data logging*/
fprintf(stdout,"Step test passed\n");
updater->update_no+=1;
updater->mode=CODE_UPDATE;
//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);
updater->bcode_size = bcode_size;
code_message_outqueue_append(SEND_CODE);
delete_p(BO_BUF);
return updater->mode;
//return 0;
}
/*Unable to step something wrong*/
else{
fprintf(stdout,"step test failed, stick to old script\n");
buzz_utility::buzz_update_set((updater)->bcode, dbgfname, (updater)->bcode_size);
code_message_outqueue_append(STATE_MSG);
delete_p(BO_BUF);
return updater->mode;
//return 1;
}
}
else {
fprintf(stdout,"unable to set new script to switch to old new script\n");
delete_p(BO_BUF);
buzz_utility::buzz_update_set((updater)->bcode, dbgfname, (updater)->bcode_size);
code_message_outqueue_append(STATE_MSG);
return updater->mode;
//return 1;
}
if(test_set_code(BO_BUF, dbgfname,bcode_size)){
uint16_t update_no =*(uint16_t*)(updater->update_no);
*(uint16_t*)(updater->update_no) =update_no +1;
code_message_outqueue_append();
fprintf(stdout,"Update no %d\n", update_no);
}
neigh=0;
delete_p(BO_BUF);
}
}
code_message_outqueue_append(STATE_MSG);
}
else if (updater->mode==CODE_UPDATE){
code_message_outqueue_append(SEND_CODE);
updater->mode=CODE_STANDBY;
else{
}
else if (updater->mode==CODE_NEIGHBOUR){
updater->mode=CODE_STANDBY;
}
else if (updater->mode==CODE_STANDBY){
uint8_t* tmp =(uint8_t*)malloc(sizeof(uint8_t));
*(uint8_t*)tmp=0;
buzzdict_foreach( updater->state_dict,reinterpret_cast<buzzdict_elem_funp>(standby_barrier_test), tmp);
fprintf(stdout,"Standby barrier ................... %i\n",*(uint8_t*)tmp);
code_message_outqueue_append(SEND_CODE);
if(*(uint8_t*)tmp==no_of_robot) {
updater->mode=CODE_RUNNING;
//gettimeofday(&t2, NULL);
//fprintf(stdout,"start and end time in standby state Info : %f,%f",(double)begin,(double)end);
if(neigh==0) code_message_outqueue_append();
buzzvm_t VM = buzz_utility::get_vm();
buzzvm_pushs(VM, buzzvm_string_register(VM, "barrier_val",1));
buzzvm_gload(VM);
buzzobj_t tObj = buzzvm_stack_at(VM, 1);
buzzvm_pop(VM);
fprintf(stdout,"Barrier ..................... %i \n",tObj->i.value);
if(tObj->i.value==no_of_robot) {
*(int*)updater->mode=CODE_RUNNING;
//collect_data();
buzz_utility::buzz_update_set((updater)->bcode, dbgf_name, (updater)->bcode_size);
buzz_utility::buzz_update_init_test((updater)->bcode, (char*)dbgfname, *(size_t*)(updater->bcode_size));
//buzzvm_function_call(m_tBuzzVM, "updated", 0);
}
delete_p(tmp);
//fprintf(stdout,"freed tmp\n");
}
if(destroy){
destroy_updater();
fprintf(stdout,"updater destoryed.\n");
}
return updater->mode;
}
@ -439,7 +298,44 @@ return (uint8_t*)updater->outmsg_queue->queue;
uint8_t* getupdate_out_msg_size(){
//fprintf(stdout,"[Debug : ]size = %i \n",*(uint16_t*)updater->outmsg_queue->size);
return 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)){
fprintf(stdout,"Initializtion of script test passed\n");
if(buzz_utility::update_step_test()){
/*data logging*/
//start =1;
/*data logging*/
fprintf(stdout,"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*)(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 1;
}
/*Unable to step something wrong*/
else{
fprintf(stdout,"step test failed, stick to old script\n");
buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, *(size_t*)(updater->bcode_size));
return 0;
}
}
else {
fprintf(stdout,"Initialization test failed\n");
buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname,*(size_t*) (updater->bcode_size));
return 0;
}
}
void destroy_out_msg_queue(){
@ -450,19 +346,16 @@ delete_p(updater->outmsg_queue);
}
int get_update_mode(){
return updater->mode;
return *(int*)updater->mode;
}
void destroy_updater(){
buzz_utility::buzz_script_destroy();
//dictonary
//fprintf(stdout,"freeing dict.\n");
buzzdict_t* updater_dict_ptr=&(updater->state_dict);
buzzdict_destroy((updater_dict_ptr));
//fprintf(stdout,"freeing complete dict.\n");
delete_p(updater->bcode);
//fprintf(stdout,"freeing complete bcode.\n");
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);

106
src/buzz_utility.cpp
View File

@ -84,39 +84,35 @@ namespace buzz_utility{
}*/
/* Go through the messages until there's nothing else to read */
uint16_t unMsgSize;
uint8_t is_msg_pres=*(uint8_t*)(pl + tot);
tot+=sizeof(uint8_t);
if(is_msg_pres){
unMsgSize = *(uint16_t*)(pl + tot);
fprintf(stdout,"received Msg size : %i\n",(int) *(uint16_t*)(pl + tot));
/*Xbee seems to send a lots of unknown message check later, added to avoid it, safe anyways*/
if(*(uint16_t*)(pl + tot) >= 4){
tot += sizeof(uint16_t);
code_message_inqueue_append(pl+tot,unMsgSize);
fprintf(stdout,"Msg 1 : %i , and Msg 2: %i\n",(int) *(uint16_t*)(pl+tot),(int) *(uint8_t*)(pl+tot+sizeof(uint16_t)));
tot += unMsgSize;
code_message_inqueue_process();
unMsgSize=0;
/*Check for Buzz messages only when the code is running*/
if(get_update_mode()==CODE_RUNNING){
uint8_t buzz_msg_pre=*(uint8_t*)(pl + tot);
tot+= sizeof(uint8_t);
/*Obtain Buzz messages only when they are present*/
if(buzz_msg_pre){
do {
/* Get payload size */
unMsgSize = *(uint16_t*)(pl + tot);
tot += sizeof(uint16_t);
/* Append message to the Buzz input message queue */
if(unMsgSize > 0 && unMsgSize <= size - tot ) {
buzzinmsg_queue_append(VM,
buzzmsg_payload_frombuffer(pl +tot, unMsgSize));
tot += unMsgSize;
}
}while(size - tot > sizeof(uint16_t) && unMsgSize > 0);
}
tot+=sizeof(uint16_t);
// fprintf(stdout,"%u : read msg size : %u \n",m_unRobotId,unMsgSize);
if(unMsgSize>0){
code_message_inqueue_append((uint8_t*)(pl + tot),unMsgSize);
tot+=unMsgSize;
code_message_inqueue_process();
}
/* Process messages */
buzzvm_process_inmsgs(VM);
}
unMsgSize=0;
/*Obtain Buzz messages only when they are present*/
do {
/* Get payload size */
unMsgSize = *(uint16_t*)(pl + tot);
tot += sizeof(uint16_t);
/* Append message to the Buzz input message queue */
if(unMsgSize > 0 && unMsgSize <= size - tot ) {
buzzinmsg_queue_append(VM,
buzzmsg_payload_frombuffer(pl +tot, unMsgSize));
tot += unMsgSize;
}
}while(size - tot > sizeof(uint16_t) && unMsgSize > 0);
/* Process messages */
buzzvm_process_inmsgs(VM);
}
/***************************************************/
/*Obtains messages from buzz out message Queue*/
@ -130,20 +126,28 @@ namespace buzz_utility{
/* Send robot id */
*(uint16_t*)(buff_send+tot) = (uint16_t) VM->robot;
tot += sizeof(uint16_t);
/*Append updater msg size*/
memcpy(buff_send + tot, (uint8_t*)getupdate_out_msg_size(), sizeof(uint16_t));
fprintf(stdout,"Msg size : %i\n",(int) *(uint16_t*)(buff_send + tot));
tot+= sizeof(uint16_t);
/*Append updater msgs*/
memcpy(buff_send + tot, (uint8_t*) getupdater_out_msg(),*(uint16_t*)getupdate_out_msg_size());
fprintf(stdout,"Msg 1 : %i , and Msg 2: %i\n",(int) *(uint16_t*)(buff_send + tot),(int) *(uint8_t*)(buff_send + tot+sizeof(uint16_t)));
tot+= *(uint16_t*)getupdate_out_msg_size();
/*destroy the updater out msg queue*/
destroy_out_msg_queue();
if(get_update_mode()==CODE_RUNNING){
*(uint8_t*)(buff_send + tot) = 1;
tot+= sizeof(uint8_t);
/* Send messages from FIFO */
uint8_t updater_msg_pre = 0;
uint16_t updater_msgSize= 0;
if((int)get_update_mode()!=CODE_RUNNING){
//fprintf(stdout,"transfer code %d\n", transfer_code);
updater_msg_pre =1;
//transfer_code=0;
*(uint8_t*)(buff_send + tot) = (uint8_t)updater_msg_pre;
tot += sizeof(uint8_t);
/*Append updater msg size*/
*(uint16_t*)(buff_send + tot) = *(uint16_t*) (getupdate_out_msg_size());
tot += sizeof(uint16_t);
/*Append updater msgs*/
memcpy(buff_send + tot, (uint8_t*)(getupdater_out_msg()), updater_msgSize);
tot += updater_msgSize;
/*destroy the updater out msg queue*/
destroy_out_msg_queue();
}
else{
*(uint8_t*)(buff_send + tot) = (uint8_t)updater_msg_pre;
tot += sizeof(uint8_t);
}
/* Send messages from FIFO */
do {
/* Are there more messages? */
if(buzzoutmsg_queue_isempty(VM)) break;
@ -169,11 +173,7 @@ namespace buzz_utility{
buzzoutmsg_queue_next(VM);
buzzmsg_payload_destroy(&m);
} while(1);
}
else{
*(uint8_t*)(buff_send + tot) = 0;
tot+= sizeof(uint8_t);
}
uint16_t total_size =(ceil((float)tot/(float)sizeof(uint64_t)));
*(uint16_t*)buff_send = (uint16_t) total_size;
@ -489,7 +489,10 @@ namespace buzz_utility{
buzz_error_info());
buzzvm_dump(VM);
}
/* Print swarm */
/*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/
/* look into this currently we don't have swarm feature at all */
/*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/
/*Print swarm*/
//buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
/* Check swarm state */
/* int status = 1;
@ -560,6 +563,9 @@ uint16_t get_robotid(){
return (uint16_t)id;
}
buzzvm_t get_vm(){
return VM;
}
}

12
src/roscontroller.cpp
View File

@ -33,7 +33,7 @@ namespace rosbzz_node{
/* Set the Buzz bytecode */
if(buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(),robot_id)) {
fprintf(stdout, "Bytecode file found and set\n");
init_update_monitor(bcfname.c_str(),barrier);
init_update_monitor(bcfname.c_str(),stand_by.c_str(),barrier);
while (ros::ok() && !buzz_utility::buzz_script_done())
{
/*Update neighbors position inside Buzz*/
@ -56,12 +56,14 @@ namespace rosbzz_node{
//std::cout<<"long obt"<<neigh_tmp.longitude<<endl;
}
neigh_pos_pub.publish(neigh_pos_array);
fprintf(stdout, "before update routine\n");
/*Check updater state and step code*/
if(update_routine(bcfname.c_str(), dbgfname.c_str(),0)==CODE_RUNNING)
update_routine(bcfname.c_str(), dbgfname.c_str());
/*Step buzz script */
fprintf(stdout, "before code step\n");
buzz_utility::buzz_script_step();
/*Prepare messages and publish them in respective topic*/
fprintf(stdout, "before publish msg\n");
prepare_msg_and_publish();
/*run once*/
ros::spinOnce();
@ -74,7 +76,7 @@ namespace rosbzz_node{
}
/* Destroy updater and Cleanup */
update_routine(bcfname.c_str(), dbgfname.c_str(),1);
//update_routine(bcfname.c_str(), dbgfname.c_str(),1);
}
}
@ -107,6 +109,8 @@ namespace rosbzz_node{
n_c.getParam("in_payload", in_payload);
/*Obtain Number of robots for barrier*/
n_c.getParam("No_of_Robots", barrier);
/*Obtain standby script to run during update*/
n_c.getParam("stand_by", stand_by);
}

144
src/stand_by.basm Normal file
View File

@ -0,0 +1,144 @@
!19
'updated
'update_ack
'init
'barrier
'stigmergy
'create
'put
'id
'barrier_val
'onconflict
'data
'stand_by
'get
'size
'neighbors
'listen
'update_no
'ROBOTS
'step
pushs 2
pushcn @__label_1
gstore
pushs 11
pushcn @__label_5
gstore
pushs 18
pushcn @__label_9
gstore
nop
@__label_0
pushs 0 |1,7,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
pushs 1 |1,8,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
gstore |1,20,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
@__exitpoint
done
@__label_1
pushs 3 |4,8,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
pushs 4 |4,19,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
gload |4,19,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
pushs 5 |4,20,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
tget |4,26,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
pushi 101 |4,27,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
pushi 1 |4,31,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
callc |4,31,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
gstore |4,31,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
pushs 3 |5,7,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
gload |5,7,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
pushs 6 |5,8,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
tget |5,11,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
pushs 7 |5,14,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
gload |5,14,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
pushi 1 |5,15,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
pushi 2 |5,17,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
callc |5,17,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
pushs 8 |6,11,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
pushi 0 |6,12,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
gstore |6,13,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
pushs 3 |7,7,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
gload |7,7,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
pushs 9 |7,8,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
tget |7,18,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
pushl @__label_2 |7,19,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
pushi 1 |10,2,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
callc |10,2,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
ret0 |12,1,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
@__label_2
lload 3 |8,4,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
pushs 10 |8,6,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
tget |8,11,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
lload 2 |8,14,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
pushs 10 |8,16,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
tget |8,21,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
lt |8,21,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
lload 3 |8,26,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
pushs 10 |8,28,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
tget |8,33,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
lload 2 |8,37,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
pushs 10 |8,39,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
tget |8,44,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
eq |8,44,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
or |8,45,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
jumpz @__label_3 |8,47,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
lload 2 |8,55,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
ret1 |8,55,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
jump @__label_4 |9,5,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
@__label_3 |9,5,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
lload 3 |9,13,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
ret1 |9,13,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
@__label_4 |9,13,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
ret0 |10,1,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
@__label_5
pushs 3 |16,7,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
gload |16,7,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
pushs 12 |16,8,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
tget |16,11,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
pushs 7 |16,14,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
gload |16,14,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
pushi 1 |16,15,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
callc |16,15,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
pushs 8 |17,12,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
pushs 3 |17,21,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
gload |17,21,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
pushs 13 |17,22,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
tget |17,26,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
pushi 0 |17,28,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
callc |17,28,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
gstore |17,28,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
pushs 14 |19,9,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
gload |19,9,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
pushs 15 |19,10,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
tget |19,16,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
pushs 0 |19,24,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
gload |19,24,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
pushl @__label_6 |20,3,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
pushi 2 |23,1,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
callc |23,1,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
ret0 |25,1,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
@__label_6
lload 2 |21,14,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
pushs 16 |21,25,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
gload |21,25,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
eq |21,25,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
jumpz @__label_7 |21,27,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
pushs 8 |21,38,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
pushs 17 |21,45,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
gload |21,45,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
gstore |21,45,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
@__label_7 |22,3,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
ret0 |22,4,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
@__label_9
pushs 11 |30,8,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
gload |30,8,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
pushi 0 |30,10,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
callc |30,10,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
ret0 |32,1,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz

BIN
src/stand_by.bdb Normal file
View File

Binary file not shown.

BIN
src/stand_by.bdbg Normal file
View File

Binary file not shown.

BIN
src/stand_by.bo Normal file
View File

Binary file not shown.

32
src/stand_by.bzz Normal file
View File

@ -0,0 +1,32 @@
updated="update_ack"
function init(){
barrier = stigmergy.create(101)
barrier.put(id,1)
barrier_val=0
barrier.onconflict(function (k,l,r) {
if(r. data < l. data or (r. data == l. data )) return l
else return r
})
}
function stand_by(){
barrier.get(id)
barrier_val = barrier.size()
neighbors.listen(updated,
function(vid, value, rid) {
if(value==update_no) barrier_val=ROBOTS
}
)
}
function step() {
stand_by()
}

11
src/test1.bzz
View File

@ -2,6 +2,15 @@
# We need this for 2D vectors
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
include "/home/ubuntu/buzz/src/include/vec2.bzz"
####################################################################################################
# Updater related
# This should be here for the updater to work, changing position of code will crash the updater
####################################################################################################
updated="update_ack"
function updated_neigh(){
neighbors.broadcast(updated, update_no)
}
TARGET_ALTITUDE = 10.1
@ -46,7 +55,7 @@ function hexagon() {
# Constants
#
BARRIER_VSTIG = 1
ROBOTS = 3 # number of robots in the swarm
#ROBOTS = 3 # number of robots in the swarm
#
# Sets a barrier