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

Merge branch 'test' of https://github.com/MISTLab/ROSBuzz into dev

This commit is contained in:
dave 2017-05-23 23:32:29 -04:00
parent 0810963359 95f80f5e9d
commit 79282cc2ab
6 changed files with 43 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
include/buzz_update.h
View File

@ -6,7 +6,7 @@
#include <buzz/buzzdict.h> #include <buzz/buzzdict.h>
#include <buzz/buzzdarray.h> #include <buzz/buzzdarray.h>
#include <buzz/buzzvstig.h> #include <buzz/buzzvstig.h>
#include <fstream>
#define delete_p(p) do { free(p); p = NULL; } while(0) #define delete_p(p) do { free(p); p = NULL; } while(0)
@ -133,4 +133,5 @@ int compile_bzz();
void updates_set_robots(int robots); void updates_set_robots(int robots);
void collect_data(std::ofstream &logger);
#endif #endif

2
include/roscontroller.h
View File

@ -121,6 +121,8 @@ private:
int setpoint_counter; int setpoint_counter;
double my_x = 0, my_y = 0; double my_x = 0, my_y = 0;
std::ofstream log;
/*Commands for flight controller*/ /*Commands for flight controller*/
//mavros_msgs::CommandInt cmd_srv; //mavros_msgs::CommandInt cmd_srv;

2
script/stand_by.bzz
View File

@ -21,7 +21,7 @@ barrier_val = barrier.size()
neighbors.listen(updated, neighbors.listen(updated,
function(vid, value, rid) { function(vid, value, rid) {
print(" got from",vid," ", " value = ",value," ","rid"," " ) print(" got from",vid," ", " value = ",value," ","rid"," " )
if(value==update_no) barrier_val=ROBOTS if(value==update_no) barrier.put(rid,1)
} }
) )

24
src/buzz_update.cpp
View File

@ -15,7 +15,7 @@
static struct timeval t1, t2; static struct timeval t1, t2;
static int timer_steps=0; static int timer_steps=0;
//static clock_t end; //static clock_t end;
void collect_data();
/*Temp end */ /*Temp end */
static int fd,wd =0; static int fd,wd =0;
static int old_update =0; static int old_update =0;
@ -24,7 +24,7 @@ static int no_of_robot;
static char* dbgf_name; static char* dbgf_name;
static const char* bzz_file; static const char* bzz_file;
static int neigh=-1; static int neigh=-1;
static int updater_msg_ready ; static int updater_msg_ready ;
static int updated=0; static int updated=0;
/*Initialize updater*/ /*Initialize updater*/
@ -89,7 +89,8 @@ void init_update_monitor(const char* bo_filename, const char* stand_by_script){
//neigh = 0; //neigh = 0;
//updater->outmsg_queue= //updater->outmsg_queue=
// update_table->barrier=nvs; // update_table->barrier=nvs;
// open logger
} }
/*Check for .bzz file chages*/ /*Check for .bzz file chages*/
int check_update(){ int check_update(){
@ -178,6 +179,7 @@ void code_message_inqueue_process(){
(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;
//gettimeofday(&t1, NULL);
} }
} }
} }
@ -241,11 +243,11 @@ void update_routine(const char* bcfname,
} }
else{ else{
//gettimeofday(&t1, NULL);
if(neigh==0 && (!is_msg_present())){ if(neigh==0 && (!is_msg_present())){
ROS_INFO("Sending code... \n"); ROS_INFO("Sending code... \n");
code_message_outqueue_append(); code_message_outqueue_append();
} }
timer_steps++; timer_steps++;
buzzvm_pushs(VM, buzzvm_string_register(VM, "barrier_val",1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "barrier_val",1));
@ -257,8 +259,6 @@ void update_routine(const char* bcfname,
*(int*)(updater->mode) = CODE_RUNNING; *(int*)(updater->mode) = CODE_RUNNING;
gettimeofday(&t2, NULL); gettimeofday(&t2, NULL);
//collect_data(); //collect_data();
timer_steps=0;
neigh=0;
buzz_utility::buzz_update_set((updater)->bcode, (char*)dbgfname, *(size_t*)(updater->bcode_size)); buzz_utility::buzz_update_set((updater)->bcode, (char*)dbgfname, *(size_t*)(updater->bcode_size));
//buzzvm_function_call(m_tBuzzVM, "updated", 0); //buzzvm_function_call(m_tBuzzVM, "updated", 0);
updated=1; updated=1;
@ -297,6 +297,7 @@ int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_size ){
buzz_utility::buzz_update_init_test((updater)->standby_bcode, buzz_utility::buzz_update_init_test((updater)->standby_bcode,
(char*)dbgfname,(size_t) *(size_t*)(updater->standby_bcode_size)); (char*)dbgfname,(size_t) *(size_t*)(updater->standby_bcode_size));
buzzvm_t VM = buzz_utility::get_vm(); buzzvm_t VM = buzz_utility::get_vm();
gettimeofday(&t1, NULL);
buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1));
buzzvm_pushi(VM, no_of_robot); buzzvm_pushi(VM, no_of_robot);
buzzvm_gstore(VM); buzzvm_gstore(VM);
@ -385,7 +386,7 @@ void destroy_updater(){
// //
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;
@ -412,12 +413,15 @@ int compile_bzz(){
ROS_WARN("Launching buzz compilation for update: %s", bzzfile_in_compile.str().c_str()); ROS_WARN("Launching buzz compilation for update: %s", bzzfile_in_compile.str().c_str());
return system(bzzfile_in_compile.str().c_str()); return system(bzzfile_in_compile.str().c_str());
} }
void collect_data(){ void collect_data(std::ofstream &logger){
//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); logger<<(int)no_of_robot<<","<<neigh<<","<<(double)time_spent<<","<<(int)timer_steps<<","<<*(size_t*)updater->bcode_size<<","<<(int)*(uint8_t*)updater->update_no;
timer_steps=0;
neigh=0;
//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);

2
src/buzz_utility.cpp
View File

@ -708,7 +708,7 @@ int create_stig_tables() {
//buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot); //buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
//int SwarmSize = buzzdict_size(VM->swarmmembers)+1; //int SwarmSize = buzzdict_size(VM->swarmmembers)+1;
//fprintf(stderr, "Real Swarm Size: %i\n",SwarmSize); //fprintf(stderr, "Real Swarm Size: %i\n",SwarmSize);
set_robot_var(buzzdict_size(VM->swarmmembers)+1);
/* 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;

32
src/roscontroller.cpp
View File

@ -45,6 +45,9 @@ namespace rosbzz_node{
} else { } else {
robot_id= strtol(robot_name.c_str() + 5, NULL, 10); robot_id= strtol(robot_name.c_str() + 5, NULL, 10);
} }
std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
path+="Update.log";
log.open(path.c_str(), std::ios_base::trunc | std::ios_base::out);
} }
/*--------------------- /*---------------------
@ -57,6 +60,7 @@ namespace rosbzz_node{
buzz_utility::buzz_script_destroy(); buzz_utility::buzz_script_destroy();
/* Stop the robot */ /* Stop the robot */
uav_done(); uav_done();
log.close();
} }
void roscontroller::GetRobotId() void roscontroller::GetRobotId()
@ -106,19 +110,29 @@ namespace rosbzz_node{
if(get_update_status()){ if(get_update_status()){
set_read_update_status(); set_read_update_status();
multi_msg=true; multi_msg=true;
log<<cur_pos.latitude<<","<<cur_pos.longitude<<","<<cur_pos.altitude<<",";
collect_data(log);
map< int, buzz_utility::Pos_struct >::iterator it = neighbours_pos_map.begin();
log<<","<<neighbours_pos_map.size();
for(;it != neighbours_pos_map.end();++it){
log<<","<<(double)it->second.x<<","<<(double)it->second.y<<","<<(double)it->second.z;
}
log<<std::endl;
} }
/*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();
buzz_utility::set_robot_var(no_of_robots);
//if(neighbours_pos_map.size() >0) no_of_robots =neighbours_pos_map.size()+1; //if(neighbours_pos_map.size() >0) no_of_robots =neighbours_pos_map.size()+1;
//buzz_utility::set_robot_var(no_of_robots); //buzz_utility::set_robot_var(no_of_robots);
/*Set no of robots for updates TODO only when not updating*/ /*Set no of robots for updates TODO only when not updating*/
if(multi_msg) //if(multi_msg)
updates_set_robots(no_of_robots); updates_set_robots(no_of_robots);
ROS_INFO("ROBOTS: %i , acutal : %i",(int)no_of_robots,(int)buzzdict_size(buzz_utility::get_vm()->swarmmembers)+1);
/*run once*/ /*run once*/
ros::spinOnce(); ros::spinOnce();
/*loop rate of ros*/ /*loop rate of ros*/
ros::Rate loop_rate(BUZZRATE); ros::Rate loop_rate(BUZZRATE);
loop_rate.sleep(); loop_rate.sleep();
if(fcu_timeout<=0) if(fcu_timeout<=0)
buzzuav_closures::rc_call(mavros_msgs::CommandCode::NAV_LAND); buzzuav_closures::rc_call(mavros_msgs::CommandCode::NAV_LAND);
@ -911,21 +925,21 @@ namespace rosbzz_node{
}*/ }*/
void roscontroller::get_number_of_robots(){ void roscontroller::get_number_of_robots(){
int cur_robots=(int)buzzdict_size(buzz_utility::get_vm()->swarmmembers)+1;
if(no_of_robots==0){ if(no_of_robots==0){
no_of_robots=neighbours_pos_map.size()+1; no_of_robots=cur_robots;
} }
else{ else{
if(no_of_robots!=neighbours_pos_map.size()+1 && no_cnt==0){ if(no_of_robots!=cur_robots && no_cnt==0){
no_cnt++; no_cnt++;
old_val=neighbours_pos_map.size()+1; old_val=cur_robots;
} }
else if(no_cnt!=0 && old_val==neighbours_pos_map.size()+1){ else if(no_cnt!=0 && old_val==cur_robots){
no_cnt++; no_cnt++;
if(no_cnt>=4){ if(no_cnt>=150 || cur_robots > no_of_robots){
no_of_robots=neighbours_pos_map.size()+1; no_of_robots=cur_robots;
no_cnt=0; no_cnt=0;
} }
} }