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

updates addition and swarm table correction

This commit is contained in:
vivek-shankar 2017-05-06 23:07:52 -04:00
parent 50327cb6b6
commit a921c3819c
5 changed files with 333 additions and 376 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
include/buzz_utility.h
View File

@ -32,9 +32,11 @@ void neighbour_pos_callback(std::map< int, Pos_struct> neighbours_pos_map);
int buzz_update_users_stigmergy(buzzobj_t tbl); int buzz_update_users_stigmergy(buzzobj_t tbl);
void in_msg_process(uint64_t* payload); void in_msg_append(uint64_t* payload);
uint64_t* out_msg_process(); uint64_t* obt_out_msg();
void update_sensors();
int buzz_script_set(const char* bo_filename, int buzz_script_set(const char* bo_filename,
const char* bdbg_filename, int robot_id); const char* bdbg_filename, int robot_id);

2
launch/rosbuzzm100.launch
View File

@ -13,7 +13,7 @@
<param name="xbee_plugged" value="true"/> <param name="xbee_plugged" value="true"/>
<!--param name="robot_id" value="3"--> <!--param name="robot_id" value="3"-->
<!--param name="No_of_Robots" value="3"--> <!--param name="No_of_Robots" value="3"-->
<param name="stand_by" value="$(env ROS_WS)/src/ROSBuzz/src/stand_by.bo"/> <param name="stand_by" value="$(env ROS_WS)/src/stand_by.bo"/>
</node> </node>

361
src/buzz_update.cpp
View File

@ -27,80 +27,72 @@ static int neigh=-1;
static int updater_msg_ready ; static int updater_msg_ready ;
static int updated=0; static int updated=0;
/*Initialize updater*/
void init_update_monitor(const char* bo_filename, const char* stand_by_script){ void init_update_monitor(const char* bo_filename, const char* stand_by_script){
fprintf(stdout,"intiialized file monitor.\n"); fprintf(stdout,"intiialized file monitor.\n");
fd=inotify_init1(IN_NONBLOCK); fd=inotify_init1(IN_NONBLOCK);
if ( fd < 0 ) { if ( fd < 0 ) {
perror( "inotify_init error" ); perror( "inotify_init error" );
} }
/* watch /.bzz file for any activity and report it back to me */ /* watch /.bzz file for any activity and report it back to update */
wd=inotify_add_watch(fd, bzz_file,IN_ALL_EVENTS ); wd=inotify_add_watch(fd, bzz_file,IN_ALL_EVENTS );
/*load the .bo under execution into the updater*/
uint8_t* BO_BUF = 0; uint8_t* BO_BUF = 0;
FILE* fp = fopen(bo_filename, "rb"); FILE* fp = fopen(bo_filename, "rb");
if(!fp) {
if(!fp) { perror(bo_filename);
perror(bo_filename); }
fseek(fp, 0, SEEK_END);
} size_t bcode_size = ftell(fp);
fseek(fp, 0, SEEK_END); rewind(fp);
BO_BUF = (uint8_t*)malloc(bcode_size);
size_t bcode_size = ftell(fp); if(fread(BO_BUF, 1, bcode_size, fp) < bcode_size) {
rewind(fp); perror(bo_filename);
fclose(fp);
BO_BUF = (uint8_t*)malloc(bcode_size); //return 0;
if(fread(BO_BUF, 1, bcode_size, fp) < bcode_size) { }
perror(bo_filename); fclose(fp);
/*Load stand_by .bo file into the updater*/
fclose(fp);
//return 0;
}
fclose(fp);
uint8_t* STD_BO_BUF = 0; uint8_t* STD_BO_BUF = 0;
fp = fopen(stand_by_script, "rb"); fp = fopen(stand_by_script, "rb");
if(!fp) {
perror(stand_by_script);
if(!fp) { }
perror(stand_by_script); fseek(fp, 0, SEEK_END);
size_t stdby_bcode_size = ftell(fp);
} rewind(fp);
fseek(fp, 0, SEEK_END); STD_BO_BUF = (uint8_t*)malloc(stdby_bcode_size);
if(fread(STD_BO_BUF, 1, stdby_bcode_size, fp) < stdby_bcode_size) {
size_t stdby_bcode_size = ftell(fp); perror(stand_by_script);
rewind(fp); fclose(fp);
//return 0;
STD_BO_BUF = (uint8_t*)malloc(stdby_bcode_size); }
if(fread(STD_BO_BUF, 1, stdby_bcode_size, fp) < stdby_bcode_size) { fclose(fp);
perror(stand_by_script); /*Create the updater*/
updater = (buzz_updater_elem_t)malloc(sizeof(struct buzz_updater_elem_s));
fclose(fp); /*Intialize the updater with the required data*/
//return 0; updater->bcode = BO_BUF;
} updater->outmsg_queue = NULL;
fclose(fp); updater->inmsg_queue = NULL;
updater->bcode_size = (size_t*) malloc(sizeof(size_t));
updater = (buzz_updater_elem_t)malloc(sizeof(struct buzz_updater_elem_s)); updater->update_no = (uint8_t*) malloc(sizeof(uint16_t));
/* Create a new table for updater*/ *(uint16_t*)(updater->update_no) =0;
updater->bcode = BO_BUF; *(size_t*)(updater->bcode_size)=bcode_size;
updater->outmsg_queue = NULL; updater->standby_bcode = STD_BO_BUF;
updater->inmsg_queue = NULL; updater->standby_bcode_size = (size_t*)malloc(sizeof(size_t));
updater->bcode_size = (size_t*) malloc(sizeof(size_t)); *(size_t*)(updater->standby_bcode_size)=stdby_bcode_size;
updater->update_no = (uint8_t*) malloc(sizeof(uint16_t)); updater->mode=(int*)malloc(sizeof(int));
*(uint16_t*)(updater->update_no) =0; *(int*)(updater->mode)=CODE_RUNNING;
*(size_t*)(updater->bcode_size)=bcode_size; //no_of_robot=barrier;
updater->standby_bcode = STD_BO_BUF; updater_msg_ready=0;
updater->standby_bcode_size = (size_t*)malloc(sizeof(size_t)); //neigh = 0;
*(size_t*)(updater->standby_bcode_size)=stdby_bcode_size; //updater->outmsg_queue=
updater->mode=(int*)malloc(sizeof(int)); // update_table->barrier=nvs;
*(int*)(updater->mode)=CODE_RUNNING;
//no_of_robot=barrier;
updater_msg_ready=0;
//neigh = 0;
//updater->outmsg_queue=
// update_table->barrier=nvs;
} }
/*Check for .bzz file chages*/
int check_update(){ int check_update(){
struct inotify_event *event; struct inotify_event *event;
char buf[1024]; char buf[1024];
int check =0; int check =0;
@ -108,7 +100,6 @@ int check_update(){
int len=read(fd,buf,1024); int len=read(fd,buf,1024);
while(i<len){ while(i<len){
event=(struct inotify_event *) &buf[i]; event=(struct inotify_event *) &buf[i];
/* file was modified this flag is true in nano and self delet in gedit and other editors */ /* file was modified this flag is true in nano and self delet in gedit and other editors */
//fprintf(stdout,"inside file monitor.\n"); //fprintf(stdout,"inside file monitor.\n");
if(event->mask & (IN_MODIFY| IN_DELETE_SELF)){ if(event->mask & (IN_MODIFY| IN_DELETE_SELF)){
@ -123,20 +114,12 @@ int check_update(){
check=1; check=1;
old_update =1; old_update =1;
} }
} }
/* update index to start of next event */ /* update index to start of next event */
i+=sizeof(struct inotify_event)+event->len; i+=sizeof(struct inotify_event)+event->len;
} }
if (!check) old_update=0; if (!check) old_update=0;
/*if(update){ return check;
buzz_script_set(update_bo, update_bdbg);
update = 0;
}*/
return check;
} }
@ -163,57 +146,56 @@ void code_message_outqueue_append(){
} }
void code_message_inqueue_append(uint8_t* msg,uint16_t size){ void code_message_inqueue_append(uint8_t* msg,uint16_t size){
updater->inmsg_queue=(updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); updater->inmsg_queue=(updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s));
fprintf(stdout,"in ms append code size %d\n", (int) size); fprintf(stdout,"in ms append code size %d\n", (int) size);
updater->inmsg_queue->queue = (uint8_t*)malloc(size); updater->inmsg_queue->queue = (uint8_t*)malloc(size);
updater->inmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); updater->inmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t));
memcpy(updater->inmsg_queue->queue, msg, size); memcpy(updater->inmsg_queue->queue, msg, size);
*(uint16_t*)(updater->inmsg_queue->size) = size; *(uint16_t*)(updater->inmsg_queue->size) = size;
} }
void code_message_inqueue_process(){ void code_message_inqueue_process(){
int size=0; int size=0;
fprintf(stdout,"[debug]Updater mode %d \n", *(int*)(updater->mode) ); fprintf(stdout,"[debug]Updater mode %d \n", *(int*)(updater->mode) );
fprintf(stdout,"[debug] %u : current update number, %u : received update no \n",( *(uint16_t*) (updater->update_no) ), (*(uint16_t*)(updater->inmsg_queue->queue)) ); fprintf(stdout,"[debug] %u : current update number, %u : received update no \n",( *(uint16_t*) (updater->update_no) ), (*(uint16_t*)(updater->inmsg_queue->queue)) );
fprintf(stdout,"[debug]Updater code size %u \n",(*(uint16_t*)(updater->inmsg_queue->queue+sizeof(uint16_t)) ) ); fprintf(stdout,"[debug]Updater code size %u \n",(*(uint16_t*)(updater->inmsg_queue->queue+sizeof(uint16_t)) ) );
if( *(int*) (updater->mode) == CODE_RUNNING){ if( *(int*) (updater->mode) == CODE_RUNNING){
//fprintf(stdout,"[debug]Inside inmsg code running"); //fprintf(stdout,"[debug]Inside inmsg code running");
if( *(uint16_t*)(updater->inmsg_queue->queue) > *(uint16_t*) (updater->update_no) ){ if( *(uint16_t*)(updater->inmsg_queue->queue) > *(uint16_t*) (updater->update_no) ){
//fprintf(stdout,"[debug]Inside update number comparision"); //fprintf(stdout,"[debug]Inside update number comparision");
uint16_t update_no=*(uint16_t*)(updater->inmsg_queue->queue); uint16_t update_no=*(uint16_t*)(updater->inmsg_queue->queue);
size +=sizeof(uint16_t); size +=sizeof(uint16_t);
uint16_t update_bcode_size =*(uint16_t*)(updater->inmsg_queue->queue+size); uint16_t update_bcode_size =*(uint16_t*)(updater->inmsg_queue->queue+size);
size +=sizeof(uint16_t); size +=sizeof(uint16_t);
//fprintf(stdout,"in queue process Update no %d\n", (int) update_no); //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); //fprintf(stdout,"in queue process bcode size %d\n", (int) update_bcode_size);
//FILE *fp; //FILE *fp;
//fp=fopen("update.bo", "wb"); //fp=fopen("update.bo", "wb");
//fwrite((updater->inmsg_queue->queue+size), update_bcode_size, 1, fp); //fwrite((updater->inmsg_queue->queue+size), update_bcode_size, 1, fp);
//fclose(fp); //fclose(fp);
if( test_set_code((uint8_t*)(updater->inmsg_queue->queue+size), if( test_set_code((uint8_t*)(updater->inmsg_queue->queue+size),
(char*) dbgf_name,(size_t)update_bcode_size) ) { (char*) dbgf_name,(size_t)update_bcode_size) ) {
*(uint16_t*)(updater->update_no)=update_no; *(uint16_t*)(updater->update_no)=update_no;
neigh=1; neigh=1;
}
} }
} }
}
//fprintf(stdout,"in queue freed\n"); //fprintf(stdout,"in queue freed\n");
delete_p(updater->inmsg_queue->queue); delete_p(updater->inmsg_queue->queue);
delete_p(updater->inmsg_queue->size); delete_p(updater->inmsg_queue->size);
delete_p(updater->inmsg_queue); delete_p(updater->inmsg_queue);
} }
void update_routine(const char* bcfname, void update_routine(const char* bcfname,
const char* dbgfname){ const char* dbgfname){
dbgf_name=(char*)dbgfname; dbgf_name=(char*)dbgfname;
buzzvm_t VM = buzz_utility::get_vm(); buzzvm_t VM = buzz_utility::get_vm();
buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1));
buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); buzzvm_pushi(VM, *(uint16_t*)(updater->update_no));
buzzvm_gstore(VM); buzzvm_gstore(VM);
//fprintf(stdout,"[Debug : ]updater value = %i \n",updater->mode); //fprintf(stdout,"[Debug : ]updater value = %i \n",updater->mode);
if(*(int*)updater->mode==CODE_RUNNING){ if(*(int*)updater->mode==CODE_RUNNING){
buzzvm_function_call(VM, "updated_neigh", 0); buzzvm_function_call(VM, "updated_neigh", 0);
if(check_update()){ if(check_update()){
@ -226,65 +208,55 @@ buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1));
std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
name = name.substr(0,name.find_last_of(".")); name = name.substr(0,name.find_last_of("."));
bzzfile_in_compile << "bzzparse "<<bzzfile_name<<" "<<path<< name<<".basm"; bzzfile_in_compile << "bzzparse "<<bzzfile_name<<" "<<path<< name<<".basm";
FILE *fp; FILE *fp;
int comp=0; int comp=0;
char buf[128]; char buf[128];
fprintf(stdout,"Update found \nUpdating script ...\n"); fprintf(stdout,"Update found \nUpdating script ...\n");
if ((fp = popen(bzzfile_in_compile.str().c_str(), "r")) == NULL) { // to change file edit if ((fp = popen(bzzfile_in_compile.str().c_str(), "r")) == NULL) { // to change file edit
fprintf(stdout,"Error opening pipe!\n"); fprintf(stdout,"Error opening pipe!\n");
} }
while (fgets(buf, 128, fp) != NULL) {
while (fgets(buf, 128, fp) != NULL) { fprintf(stdout,"OUTPUT: %s \n", buf);
fprintf(stdout,"OUTPUT: %s \n", buf); comp=1;
comp=1; }
}
bzzfile_in_compile.str(""); bzzfile_in_compile.str("");
bzzfile_in_compile <<"bzzasm "<<path<<name<<".basm "<<path<<name<<".bo "<<path<<name<<".bdbg"; bzzfile_in_compile <<"bzzasm "<<path<<name<<".basm "<<path<<name<<".bo "<<path<<name<<".bdbg";
if ((fp = popen(bzzfile_in_compile.str().c_str(), "r")) == NULL) { // to change file edit if ((fp = popen(bzzfile_in_compile.str().c_str(), "r")) == NULL) { // to change file edit
fprintf(stdout,"Error opening pipe!\n"); fprintf(stdout,"Error opening pipe!\n");
} }
while (fgets(buf, 128, fp) != NULL) { while (fgets(buf, 128, fp) != NULL) {
fprintf(stdout,"OUTPUT: %s \n", buf); fprintf(stdout,"OUTPUT: %s \n", buf);
} }
if(pclose(fp) || comp) {
if(pclose(fp) || comp) { fprintf(stdout,"Errors in comipilg script so staying on old script\n");
fprintf(stdout,"Errors in comipilg script so staying on old script\n"); }
// return -1; else {
}
else {
uint8_t* BO_BUF = 0; uint8_t* BO_BUF = 0;
FILE* fp = fopen(bcfname, "rb"); // to change file edit FILE* fp = fopen(bcfname, "rb"); // to change file edit
if(!fp) { if(!fp) {
perror(bcfname); perror(bcfname);
}
} fseek(fp, 0, SEEK_END);
fseek(fp, 0, SEEK_END); size_t bcode_size = ftell(fp);
size_t bcode_size = ftell(fp); rewind(fp);
rewind(fp); BO_BUF = (uint8_t*)malloc(bcode_size);
BO_BUF = (uint8_t*)malloc(bcode_size); if(fread(BO_BUF, 1, bcode_size, fp) < bcode_size) {
if(fread(BO_BUF, 1, bcode_size, fp) < bcode_size) { perror(bcfname);
perror(bcfname); fclose(fp);
}
fclose(fp); fclose(fp);
}
fclose(fp);
if(test_set_code(BO_BUF, dbgfname,bcode_size)){ if(test_set_code(BO_BUF, dbgfname,bcode_size)){
uint16_t update_no =*(uint16_t*)(updater->update_no); uint16_t update_no =*(uint16_t*)(updater->update_no);
*(uint16_t*)(updater->update_no) =update_no +1; *(uint16_t*)(updater->update_no) =update_no +1;
code_message_outqueue_append(); code_message_outqueue_append();
VM = buzz_utility::get_vm(); VM = buzz_utility::get_vm();
fprintf(stdout,"Update no %d\n", *(uint16_t*)(updater->update_no)); fprintf(stdout,"Update no %d\n", *(uint16_t*)(updater->update_no));
buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1));
buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); buzzvm_pushi(VM, *(uint16_t*)(updater->update_no));
buzzvm_gstore(VM); buzzvm_gstore(VM);
neigh=-1; neigh=-1;
fprintf(stdout,"Sending code... \n"); fprintf(stdout,"Sending code... \n");
code_message_outqueue_append(); code_message_outqueue_append();
} }
delete_p(BO_BUF); delete_p(BO_BUF);
} }
@ -330,7 +302,6 @@ 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); //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; return (uint8_t*)updater->outmsg_queue->size;
} }
@ -397,17 +368,17 @@ int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_size ){
} }
void destroy_out_msg_queue(){ void destroy_out_msg_queue(){
//fprintf(stdout,"out queue freed\n"); //fprintf(stdout,"out queue freed\n");
delete_p(updater->outmsg_queue->queue); delete_p(updater->outmsg_queue->queue);
delete_p(updater->outmsg_queue->size); delete_p(updater->outmsg_queue->size);
delete_p(updater->outmsg_queue); delete_p(updater->outmsg_queue);
updater_msg_ready=0; updater_msg_ready=0;
} }
int get_update_status(){ int get_update_status(){
return updated; return updated;
} }
void set_read_update_status(){ void set_read_update_status(){
updated=0; updated=0;
} }
int get_update_mode(){ int get_update_mode(){
return (int)*(int*)(updater->mode); return (int)*(int*)(updater->mode);
@ -420,44 +391,44 @@ buzz_updater_elem_t get_updater(){
return updater; return updater;
} }
void destroy_updater(){ void destroy_updater(){
delete_p(updater->bcode); delete_p(updater->bcode);
delete_p(updater->bcode_size); delete_p(updater->bcode_size);
delete_p(updater->standby_bcode); delete_p(updater->standby_bcode);
delete_p(updater->standby_bcode_size); delete_p(updater->standby_bcode_size);
delete_p(updater->mode); delete_p(updater->mode);
delete_p(updater->update_no); delete_p(updater->update_no);
if(updater->outmsg_queue){ if(updater->outmsg_queue){
delete_p(updater->outmsg_queue->queue); delete_p(updater->outmsg_queue->queue);
delete_p(updater->outmsg_queue->size); delete_p(updater->outmsg_queue->size);
delete_p(updater->outmsg_queue); delete_p(updater->outmsg_queue);
} }
if(updater->inmsg_queue){ if(updater->inmsg_queue){
delete_p(updater->inmsg_queue->queue); delete_p(updater->inmsg_queue->queue);
delete_p(updater->inmsg_queue->size); delete_p(updater->inmsg_queue->size);
delete_p(updater->inmsg_queue); delete_p(updater->inmsg_queue);
} }
// //
inotify_rm_watch(fd,wd); inotify_rm_watch(fd,wd);
close(fd); close(fd);
} }
void set_bzz_file(const char* in_bzz_file){ void set_bzz_file(const char* in_bzz_file){
bzz_file=in_bzz_file; bzz_file=in_bzz_file;
} }
void updates_set_robots(int robots){ void updates_set_robots(int robots){
no_of_robot=robots; no_of_robot=robots;
} }
void collect_data(){ void collect_data(){
//fprintf(stdout,"start and end time in data collection Info : %f,%f",(double)begin,(double)end); //fprintf(stdout,"start and end time in data collection Info : %f,%f",(double)begin,(double)end);
double time_spent = (t2.tv_sec - t1.tv_sec) * 1000.0; //(double)(end - begin) / CLOCKS_PER_SEC; 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; time_spent += (t2.tv_usec - t1.tv_usec) / 1000.0;
//int bytecodesize=(int); //int bytecodesize=(int);
fprintf(stdout,"Data logger Info : %i , %i , %f , %ld , %i , %d \n",(int)no_of_robot,neigh,time_spent,*(size_t*)updater->bcode_size,(int)no_of_robot,*(uint8_t*)updater->update_no); fprintf(stdout,"Data logger Info : %i , %i , %f , %ld , %i , %d \n",(int)no_of_robot,neigh,time_spent,*(size_t*)updater->bcode_size,(int)no_of_robot,*(uint8_t*)updater->update_no);
//FILE *Fileptr; //FILE *Fileptr;
//Fileptr=fopen("/home/ubuntu/ROS_WS/update_logger.csv", "a"); //Fileptr=fopen("/home/ubuntu/ROS_WS/update_logger.csv", "a");
//fprintf(Fileptr,"%i,%i,%i,%i,%i,%u\n",(int)buzz_utility::get_robotid(),neigh,timer_steps,(int)*(size_t*)updater->bcode_size,(int)no_of_robot, *(uint8_t*)updater->update_no); //fprintf(Fileptr,"%i,%i,%i,%i,%i,%u\n",(int)buzz_utility::get_robotid(),neigh,timer_steps,(int)*(size_t*)updater->bcode_size,(int)no_of_robot, *(uint8_t*)updater->update_no);
//fclose(Fileptr); //fclose(Fileptr);
} }

320
src/buzz_utility.cpp
View File

@ -45,10 +45,10 @@ namespace buzz_utility{
uint16_t* out = new uint16_t[4]; uint16_t* out = new uint16_t[4];
uint32_t int32_1 = u64 & 0xFFFFFFFF; uint32_t int32_1 = u64 & 0xFFFFFFFF;
uint32_t int32_2 = (u64 & 0xFFFFFFFF00000000 ) >> 32; uint32_t int32_2 = (u64 & 0xFFFFFFFF00000000 ) >> 32;
out[0] = int32_1 & 0xFFFF; out[0] = int32_1 & 0xFFFF;
out[1] = (int32_1 & (0xFFFF0000) ) >> 16; out[1] = (int32_1 & (0xFFFF0000) ) >> 16;
out[2] = int32_2 & 0xFFFF; out[2] = int32_2 & 0xFFFF;
out[3] = (int32_2 & (0xFFFF0000) ) >> 16; out[3] = (int32_2 & (0xFFFF0000) ) >> 16;
//cout << " values " <<out[0] <<" "<<out[1] <<" "<<out[2] <<" "<<out[3] <<" "; //cout << " values " <<out[0] <<" "<<out[1] <<" "<<out[2] <<" "<<out[3] <<" ";
return out; return out;
} }
@ -68,7 +68,7 @@ namespace buzz_utility{
/*|__________________________________________________________________________|____________________________________|*/ /*|__________________________________________________________________________|____________________________________|*/
/*******************************************************************************************************************/ /*******************************************************************************************************************/
void in_msg_process(uint64_t* payload){ void in_msg_append(uint64_t* payload){
/* Go through messages and add them to the FIFO */ /* Go through messages and add them to the FIFO */
uint16_t* data= u64_cvt_u16((uint64_t)payload[0]); uint16_t* data= u64_cvt_u16((uint64_t)payload[0]);
@ -97,15 +97,14 @@ namespace buzz_utility{
} }
}while(size - tot > sizeof(uint16_t) && unMsgSize > 0); }while(size - tot > sizeof(uint16_t) && unMsgSize > 0);
free(pl); free(pl);
/* Process messages */
buzzvm_process_inmsgs(VM);
} }
/***************************************************/ /***************************************************/
/*Obtains messages from buzz out message Queue*/ /*Obtains messages from buzz out message Queue*/
/***************************************************/ /***************************************************/
uint64_t* out_msg_process(){ uint64_t* obt_out_msg(){
buzzvm_process_outmsgs(VM);
uint8_t* buff_send =(uint8_t*)malloc(MAX_MSG_SIZE); uint8_t* buff_send =(uint8_t*)malloc(MAX_MSG_SIZE);
memset(buff_send, 0, MAX_MSG_SIZE); memset(buff_send, 0, MAX_MSG_SIZE);
/*Taking into consideration the sizes included at the end*/ /*Taking into consideration the sizes included at the end*/
@ -151,8 +150,8 @@ namespace buzz_utility{
cout<<" payload from out msg "<<*(payload_64+i)<<endl; cout<<" payload from out msg "<<*(payload_64+i)<<endl;
}*/ }*/
/* Send message */ /* Send message */
return payload_64; return payload_64;
} }
/****************************************/ /****************************************/
/*Buzz script not able to load*/ /*Buzz script not able to load*/
@ -177,7 +176,7 @@ namespace buzz_utility{
VM->pc, VM->pc,
VM->errormsg); VM->errormsg);
} }
return msg; return msg;
} }
/****************************************/ /****************************************/
@ -254,87 +253,86 @@ namespace buzz_utility{
/****************************************/ /****************************************/
/*Sets the .bzz and .bdbg file*/ /*Sets the .bzz and .bdbg file*/
/****************************************/ /****************************************/
int buzz_script_set(const char* bo_filename, int buzz_script_set(const char* bo_filename,
const char* bdbg_filename, int robot_id) { const char* bdbg_filename, int robot_id) {
cout << " Robot ID: " <<robot_id<< endl; cout << " Robot ID: " <<robot_id<< endl;
/* Reset the Buzz VM */ /* Reset the Buzz VM */
if(VM) buzzvm_destroy(&VM); if(VM) buzzvm_destroy(&VM);
Robot_id = robot_id; Robot_id = robot_id;
VM = buzzvm_new((int)robot_id); VM = buzzvm_new((int)robot_id);
/* Get rid of debug info */ /* Get rid of debug info */
if(DBG_INFO) buzzdebug_destroy(&DBG_INFO); if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
DBG_INFO = buzzdebug_new(); DBG_INFO = buzzdebug_new();
/* Read bytecode and fill in data structure */ /* Read bytecode and fill in data structure */
FILE* fd = fopen(bo_filename, "rb"); FILE* fd = fopen(bo_filename, "rb");
if(!fd) { if(!fd) {
perror(bo_filename); perror(bo_filename);
return 0; return 0;
} }
fseek(fd, 0, SEEK_END); fseek(fd, 0, SEEK_END);
size_t bcode_size = ftell(fd); size_t bcode_size = ftell(fd);
rewind(fd); rewind(fd);
BO_BUF = (uint8_t*)malloc(bcode_size); BO_BUF = (uint8_t*)malloc(bcode_size);
if(fread(BO_BUF, 1, bcode_size, fd) < bcode_size) { if(fread(BO_BUF, 1, bcode_size, fd) < bcode_size) {
perror(bo_filename); perror(bo_filename);
buzzvm_destroy(&VM); buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO); buzzdebug_destroy(&DBG_INFO);
fclose(fd); fclose(fd);
return 0; return 0;
} }
fclose(fd); fclose(fd);
/* Read debug information */ /* Read debug information */
if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) { if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) {
buzzvm_destroy(&VM); buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO); buzzdebug_destroy(&DBG_INFO);
perror(bdbg_filename); perror(bdbg_filename);
return 0; return 0;
} }
/* Set byte code */ /* Set byte code */
if(buzzvm_set_bcode(VM, BO_BUF, bcode_size) != BUZZVM_STATE_READY) { if(buzzvm_set_bcode(VM, BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM); buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO); buzzdebug_destroy(&DBG_INFO);
fprintf(stderr, "%s: Error loading Buzz script\n\n", bo_filename); fprintf(stderr, "%s: Error loading Buzz script\n\n", bo_filename);
return 0; return 0;
} }
/* Register hook functions */ /* Register hook functions */
if(buzz_register_hooks() != BUZZVM_STATE_READY) { if(buzz_register_hooks() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM); buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO); buzzdebug_destroy(&DBG_INFO);
fprintf(stderr, "%s: Error registering hooks\n\n", bo_filename); fprintf(stderr, "%s: Error registering hooks\n\n", bo_filename);
return 0; return 0;
} }
buzzvm_dup(VM); buzzvm_dup(VM);
// usersvstig = stigmergy.create(123) // usersvstig = stigmergy.create(123)
buzzvm_pushs(VM, buzzvm_string_register(VM, "v", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "v", 1));
// value of the stigmergy id // value of the stigmergy id
buzzvm_pushi(VM, 5); buzzvm_pushi(VM, 5);
// get the stigmergy table from the global scope // get the stigmergy table from the global scope
// buzzvm_pushs(VM, buzzvm_string_register(VM, "stigmergy", 1)); // buzzvm_pushs(VM, buzzvm_string_register(VM, "stigmergy", 1));
// buzzvm_gload(VM); // buzzvm_gload(VM);
// get the create method from the stigmergy table // get the create method from the stigmergy table
// buzzvm_pushs(VM, buzzvm_string_register(VM, "create", 1)); // buzzvm_pushs(VM, buzzvm_string_register(VM, "create", 1));
// buzzvm_tget(VM); // buzzvm_tget(VM);
// call the stigmergy.create() method // call the stigmergy.create() method
// buzzvm_closure_call(VM, 1); // buzzvm_closure_call(VM, 1);
// now the stigmergy is at the top of the stack - save it for future reference // now the stigmergy is at the top of the stack - save it for future reference
// usersvstig = buzzvm_stack_at(VM, 0); // usersvstig = buzzvm_stack_at(VM, 0);
//buzzvm_pop(VM); //buzzvm_pop(VM);
// assign the virtual stigmergy to the global symbol v // assign the virtual stigmergy to the global symbol v
// create also a global variable for it, so the garbage collector does not remove it // create also a global variable for it, so the garbage collector does not remove it
//buzzvm_pushs(VM, buzzvm_string_register(VM, "v", 1)); //buzzvm_pushs(VM, buzzvm_string_register(VM, "v", 1));
//buzzvm_push(VM, usersvstig); //buzzvm_push(VM, usersvstig);
buzzvm_gstore(VM); buzzvm_gstore(VM);
/* Save bytecode file name */ /* Save bytecode file name */
BO_FNAME = strdup(bo_filename); BO_FNAME = strdup(bo_filename);
/* Execute the global part of the script */ /* Execute the global part of the script */
buzzvm_execute_script(VM); buzzvm_execute_script(VM);
/* Call the Init() function */ /* Call the Init() function */
buzzvm_function_call(VM, "init", 0); buzzvm_function_call(VM, "init", 0);
/* All OK */ /* All OK */
return 1;//buzz_update_set(BO_BUF, bdbg_filename, bcode_size); return 1;//buzz_update_set(BO_BUF, bdbg_filename, bcode_size);
} }
@ -358,7 +356,7 @@ buzzvm_dup(VM);
/*Sets a new update */ /*Sets a new update */
/****************************************/ /****************************************/
int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){ int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){
/* // Reset the Buzz VM // Reset the Buzz VM
if(VM) buzzvm_destroy(&VM); if(VM) buzzvm_destroy(&VM);
VM = buzzvm_new(Robot_id); VM = buzzvm_new(Robot_id);
// Get rid of debug info // Get rid of debug info
@ -392,14 +390,14 @@ buzzvm_dup(VM);
// Call the Init() function // Call the Init() function
buzzvm_function_call(VM, "init", 0); buzzvm_function_call(VM, "init", 0);
// All OK // All OK
*/ return 1; return 1;
} }
/****************************************/ /****************************************/
/*Performs a initialization test */ /*Performs a initialization test */
/****************************************/ /****************************************/
int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){ int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){
/* // Reset the Buzz VM // Reset the Buzz VM
if(VM) buzzvm_destroy(&VM); if(VM) buzzvm_destroy(&VM);
VM = buzzvm_new(Robot_id); VM = buzzvm_new(Robot_id);
// Get rid of debug info // Get rid of debug info
@ -433,7 +431,7 @@ buzzvm_dup(VM);
// Call the Init() function // Call the Init() function
buzzvm_function_call(VM, "init", 0); buzzvm_function_call(VM, "init", 0);
// All OK // All OK
*/ return 1; return 1;
} }
/****************************************/ /****************************************/
@ -447,75 +445,72 @@ buzzvm_dup(VM);
typedef struct buzzswarm_elem_s* buzzswarm_elem_t; typedef struct buzzswarm_elem_s* buzzswarm_elem_t;
void check_swarm_members(const void* key, void* data, void* params) { void check_swarm_members(const void* key, void* data, void* params) {
buzzswarm_elem_t e = *(buzzswarm_elem_t*)data; buzzswarm_elem_t e = *(buzzswarm_elem_t*)data;
int* status = (int*)params; int* status = (int*)params;
if(*status == 3) return; if(*status == 3) return;
fprintf(stderr, "CHECKING SWARM :%i, memeber: %i, age: %i \n",buzzdarray_get(e->swarms, 0, uint16_t), *(uint16_t*)key, e->age); fprintf(stderr, "CHECKING SWARM :%i, memeber: %i, age: %i \n",
if(buzzdarray_size(e->swarms) != 1) { buzzdarray_get(e->swarms, 0, uint16_t), *(uint16_t*)key, e->age);
fprintf(stderr, "Swarm list size is not 1\n"); if(buzzdarray_size(e->swarms) != 1) {
*status = 3; fprintf(stderr, "Swarm list size is not 1\n");
} *status = 3;
else { }
int sid = 1; else {
if(!buzzdict_isempty(VM->swarms)) { int sid = 1;
if(*buzzdict_get(VM->swarms, &sid, uint8_t) && if(!buzzdict_isempty(VM->swarms)) {
buzzdarray_get(e->swarms, 0, uint16_t) != sid) { if(*buzzdict_get(VM->swarms, &sid, uint8_t) &&
fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n", buzzdarray_get(e->swarms, 0, uint16_t) != sid) {
sid, fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n",
buzzdarray_get(e->swarms, 0, uint16_t)); sid,
*status = 3; buzzdarray_get(e->swarms, 0, uint16_t));
return; *status = 3;
} return;
} }
if(buzzdict_size(VM->swarms)>1) { }
sid = 2; if(buzzdict_size(VM->swarms)>1) {
if(*buzzdict_get(VM->swarms, &sid, uint8_t) && sid = 2;
buzzdarray_get(e->swarms, 0, uint16_t) != sid) { if(*buzzdict_get(VM->swarms, &sid, uint8_t) &&
fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n", buzzdarray_get(e->swarms, 0, uint16_t) != sid) {
sid, fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n",
buzzdarray_get(e->swarms, 0, uint16_t)); sid,
*status = 3; buzzdarray_get(e->swarms, 0, uint16_t));
return; *status = 3;
} return;
} }
} }
}
} }
/*Step through the buzz script*/ /*Step through the buzz script*/
void update_sensors(){
/* Update sensors*/
buzzuav_closures::buzzuav_update_battery(VM);
buzzuav_closures::buzzuav_update_prox(VM);
buzzuav_closures::buzzuav_update_currentpos(VM);
buzzobj_t tbl = buzzuav_closures::buzzuav_update_userspos(VM);
buzzuav_closures::buzzuav_update_flight_status(VM);
//buzz_update_users_stigmergy(tbl);
}
void buzz_script_step() { void buzz_script_step() {
/* /* Process messages */
* Update sensors buzzvm_process_inmsgs(VM);
*/ /*Update sensors*/
buzzuav_closures::buzzuav_update_battery(VM); update_sensors();
buzzuav_closures::buzzuav_update_prox(VM); /* Call Buzz step() function */
buzzuav_closures::buzzuav_update_currentpos(VM); if(buzzvm_function_call(VM, "step", 0) != BUZZVM_STATE_READY) {
buzzobj_t tbl = buzzuav_closures::buzzuav_update_userspos(VM); fprintf(stderr, "%s: execution terminated abnormally: %s\n\n",
buzzuav_closures::buzzuav_update_flight_status(VM);
//buzz_update_users_stigmergy(tbl);
/*
* Call Buzz step() function
*/
if(buzzvm_function_call(VM, "step", 0) != BUZZVM_STATE_READY) {
fprintf(stderr, "%s: execution terminated abnormally: %s\n\n",
BO_FNAME, BO_FNAME,
buzz_error_info()); buzz_error_info());
buzzvm_dump(VM); buzzvm_dump(VM);
} }
/* Update swarm membership */ /* Process out messages */
buzzswarm_members_update(VM->swarmmembers); buzzvm_process_outmsgs(VM);
//buzzvm_process_outmsgs(VM); //--> done in out_msg_process() function called each step /*Print swarm*/
//usleep(10000); buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
/*Print swarm*/ //int SwarmSize = buzzdict_size(VM->swarmmembers)+1;
buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot); //fprintf(stderr, "Real Swarm Size: %i\n",SwarmSize);
//int SwarmSize = buzzdict_size(VM->swarmmembers)+1;
//fprintf(stderr, "Real Swarm Size: %i\n",SwarmSize);
/* Check swarm state -- Not crashing thanks to test added in check_swarm_members */ /* Check swarm state -- Not crashing thanks to test added in check_swarm_members */
int status = 1; int status = 1;
buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status); buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status);
} }
/****************************************/ /****************************************/
@ -565,26 +560,15 @@ buzzvm_dup(VM);
return a == BUZZVM_STATE_READY; return a == BUZZVM_STATE_READY;
} }
/* uint16_t get_robotid() {
// Get hostname
char hstnm[30];
gethostname(hstnm, 30);
// Make numeric id from hostname
// NOTE: here we assume that the hostname is in the format Knn
int id = strtol(hstnm + 4, NULL, 10);
//fprintf(stdout, "Robot id from get rid buzz util: %i\n",id);
return (uint16_t)id;
}*/
buzzvm_t get_vm() { buzzvm_t get_vm() {
return VM; return VM;
} }
void set_robot_var(int ROBOTS){ void set_robot_var(int ROBOTS){
buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1));
buzzvm_pushi(VM, ROBOTS); buzzvm_pushi(VM, ROBOTS);
buzzvm_gstore(VM); buzzvm_gstore(VM);
} }
} }

20
src/roscontroller.cpp
View File

@ -22,11 +22,11 @@ namespace rosbzz_node{
multi_msg = true; multi_msg = true;
// set stream rate - wait for the FC to be started // set stream rate - wait for the FC to be started
SetStreamRate(0, 10, 1); SetStreamRate(0, 10, 1);
/// Get Robot Id - wait for Xbee to be started // Get Robot Id - wait for Xbee to be started
if(xbeeplugged) if(xbeeplugged)
GetRobotId(); GetRobotId();
else else
robot_id= strtol(robot_name.c_str() + 5, NULL, 10);; robot_id= strtol(robot_name.c_str() + 5, NULL, 10);
setpoint_counter = 0; setpoint_counter = 0;
fcu_timeout = TIMEOUT; fcu_timeout = TIMEOUT;
home[0]=0.0;home[1]=0.0;home[2]=0.0; home[0]=0.0;home[1]=0.0;home[2]=0.0;
@ -67,7 +67,7 @@ namespace rosbzz_node{
/* Set the Buzz bytecode */ /* Set the Buzz bytecode */
if(buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(),robot_id)) { if(buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(),robot_id)) {
fprintf(stdout, "Bytecode file found and set\n"); fprintf(stdout, "Bytecode file found and set\n");
//init_update_monitor(bcfname.c_str(),stand_by.c_str()); init_update_monitor(bcfname.c_str(),stand_by.c_str());
/////////////////////////////////////////////////////// ///////////////////////////////////////////////////////
// MAIN LOOP // MAIN LOOP
////////////////////////////////////////////////////// //////////////////////////////////////////////////////
@ -78,7 +78,7 @@ namespace rosbzz_node{
/*Neighbours of the robot published with id in respective topic*/ /*Neighbours of the robot published with id in respective topic*/
neighbours_pos_publisher(); neighbours_pos_publisher();
/*Check updater state and step code*/ /*Check updater state and step code*/
//update_routine(bcfname.c_str(), dbgfname.c_str()); update_routine(bcfname.c_str(), dbgfname.c_str());
/*Step buzz script */ /*Step buzz script */
buzz_utility::buzz_script_step(); buzz_utility::buzz_script_step();
/*Prepare messages and publish them in respective topic*/ /*Prepare messages and publish them in respective topic*/
@ -86,10 +86,10 @@ namespace rosbzz_node{
/*call flight controler service to set command long*/ /*call flight controler service to set command long*/
flight_controller_service_call(); flight_controller_service_call();
/*Set multi message available after update*/ /*Set multi message available after update*/
/*if(get_update_status()){ if(get_update_status()){
set_read_update_status(); set_read_update_status();
multi_msg=true; multi_msg=true;
}*/ }
/*Set ROBOTS variable for barrier in .bzz from neighbours count*/ /*Set ROBOTS variable for barrier in .bzz from neighbours count*/
//no_of_robots=get_number_of_robots(); //no_of_robots=get_number_of_robots();
get_number_of_robots(); get_number_of_robots();
@ -334,7 +334,7 @@ namespace rosbzz_node{
/*----------------------------------------------------------------------------------------------------*/ /*----------------------------------------------------------------------------------------------------*/
void roscontroller::prepare_msg_and_publish(){ void roscontroller::prepare_msg_and_publish(){
/*obtain Pay load to be sent*/ /*obtain Pay load to be sent*/
uint64_t* payload_out_ptr= buzz_utility::out_msg_process(); uint64_t* payload_out_ptr= buzz_utility::obt_out_msg();
uint64_t position[3]; uint64_t position[3];
/*Appened current position to message*/ /*Appened current position to message*/
memcpy(position, cur_pos, 3*sizeof(uint64_t)); memcpy(position, cur_pos, 3*sizeof(uint64_t));
@ -358,7 +358,7 @@ namespace rosbzz_node{
delete[] out; delete[] out;
delete[] payload_out_ptr; delete[] payload_out_ptr;
/*Check for updater message if present send*/ /*Check for updater message if present send*/
/*if((int)get_update_mode()!=CODE_RUNNING && is_msg_present()==1 && multi_msg){ if((int)get_update_mode()!=CODE_RUNNING && is_msg_present()==1 && multi_msg){
uint8_t* buff_send = 0; uint8_t* buff_send = 0;
uint16_t updater_msgSize=*(uint16_t*) (getupdate_out_msg_size());; uint16_t updater_msgSize=*(uint16_t*) (getupdate_out_msg_size());;
int tot=0; int tot=0;
@ -394,7 +394,7 @@ namespace rosbzz_node{
payload_pub.publish(update_packets); payload_pub.publish(update_packets);
multi_msg=false; multi_msg=false;
delete[] payload_64; delete[] payload_64;
}*/ }
} }
/*--------------------------------------------------------------------------------- /*---------------------------------------------------------------------------------
@ -855,7 +855,7 @@ namespace rosbzz_node{
raw_neighbours_pos_put((int)out[1],raw_neigh_pos); raw_neighbours_pos_put((int)out[1],raw_neigh_pos);
neighbours_pos_put((int)out[1],n_pos); neighbours_pos_put((int)out[1],n_pos);
delete[] out; delete[] out;
buzz_utility::in_msg_process((message_obt+3)); buzz_utility::in_msg_append((message_obt+3));
} }
} }