diff --git a/include/buzz_update.h b/include/buzz_update.h index 9c51601..bee36ef 100644 --- a/include/buzz_update.h +++ b/include/buzz_update.h @@ -6,7 +6,7 @@ #include #include #include - +#include #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 collect_data(std::ofstream &logger); #endif diff --git a/include/roscontroller.h b/include/roscontroller.h index ce4e67d..006d78b 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -121,6 +121,8 @@ private: int setpoint_counter; double my_x = 0, my_y = 0; + + std::ofstream log; /*Commands for flight controller*/ //mavros_msgs::CommandInt cmd_srv; diff --git a/script/stand_by.bzz b/script/stand_by.bzz index d51e99e..1349089 100644 --- a/script/stand_by.bzz +++ b/script/stand_by.bzz @@ -21,7 +21,7 @@ barrier_val = barrier.size() neighbors.listen(updated, function(vid, value, rid) { print(" got from",vid," ", " value = ",value," ","rid"," " ) - if(value==update_no) barrier_val=ROBOTS + if(value==update_no) barrier.put(rid,1) } ) diff --git a/src/buzz_update.cpp b/src/buzz_update.cpp index 69d9ec8..86919df 100644 --- a/src/buzz_update.cpp +++ b/src/buzz_update.cpp @@ -15,7 +15,7 @@ static struct timeval t1, t2; static int timer_steps=0; //static clock_t end; -void collect_data(); + /*Temp end */ static int fd,wd =0; static int old_update =0; @@ -24,7 +24,7 @@ static int no_of_robot; static char* dbgf_name; static const char* bzz_file; static int neigh=-1; -static int updater_msg_ready ; +static int updater_msg_ready ; static int updated=0; /*Initialize updater*/ @@ -89,7 +89,8 @@ void init_update_monitor(const char* bo_filename, const char* stand_by_script){ //neigh = 0; //updater->outmsg_queue= // update_table->barrier=nvs; - + // open logger + } /*Check for .bzz file chages*/ int check_update(){ @@ -178,6 +179,7 @@ void code_message_inqueue_process(){ (char*) dbgf_name,(size_t)update_bcode_size) ) { *(uint16_t*)(updater->update_no)=update_no; neigh=1; + //gettimeofday(&t1, NULL); } } } @@ -241,11 +243,11 @@ void update_routine(const char* bcfname, } else{ - //gettimeofday(&t1, NULL); + if(neigh==0 && (!is_msg_present())){ ROS_INFO("Sending code... \n"); code_message_outqueue_append(); - + } timer_steps++; 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; gettimeofday(&t2, NULL); //collect_data(); - timer_steps=0; - neigh=0; buzz_utility::buzz_update_set((updater)->bcode, (char*)dbgfname, *(size_t*)(updater->bcode_size)); //buzzvm_function_call(m_tBuzzVM, "updated", 0); 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, (char*)dbgfname,(size_t) *(size_t*)(updater->standby_bcode_size)); buzzvm_t VM = buzz_utility::get_vm(); + gettimeofday(&t1, NULL); buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); buzzvm_pushi(VM, no_of_robot); buzzvm_gstore(VM); @@ -385,7 +386,7 @@ void destroy_updater(){ // inotify_rm_watch(fd,wd); close(fd); - } +} void set_bzz_file(const char* 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()); 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); 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; //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<<","<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; //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); diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 91f3dd7..5cb21e0 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -708,7 +708,7 @@ int create_stig_tables() { //buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot); //int SwarmSize = buzzdict_size(VM->swarmmembers)+1; //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 */ //int status = 1; diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index cc36c5c..d21fe9c 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -45,6 +45,9 @@ namespace rosbzz_node{ } else { 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(); /* Stop the robot */ uav_done(); + log.close(); } void roscontroller::GetRobotId() @@ -106,19 +110,29 @@ namespace rosbzz_node{ if(get_update_status()){ set_read_update_status(); multi_msg=true; + log<::iterator it = neighbours_pos_map.begin(); + log<<","<second.x<<","<<(double)it->second.y<<","<<(double)it->second.z; + } + log<0) no_of_robots =neighbours_pos_map.size()+1; //buzz_utility::set_robot_var(no_of_robots); /*Set no of robots for updates TODO only when not updating*/ - if(multi_msg) + //if(multi_msg) 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*/ ros::spinOnce(); /*loop rate of ros*/ - ros::Rate loop_rate(BUZZRATE); + ros::Rate loop_rate(BUZZRATE); loop_rate.sleep(); if(fcu_timeout<=0) buzzuav_closures::rc_call(mavros_msgs::CommandCode::NAV_LAND); @@ -911,21 +925,21 @@ namespace rosbzz_node{ }*/ void roscontroller::get_number_of_robots(){ - + int cur_robots=(int)buzzdict_size(buzz_utility::get_vm()->swarmmembers)+1; if(no_of_robots==0){ - no_of_robots=neighbours_pos_map.size()+1; + no_of_robots=cur_robots; } else{ - if(no_of_robots!=neighbours_pos_map.size()+1 && no_cnt==0){ + if(no_of_robots!=cur_robots && no_cnt==0){ 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++; - if(no_cnt>=4){ - no_of_robots=neighbours_pos_map.size()+1; + if(no_cnt>=150 || cur_robots > no_of_robots){ + no_of_robots=cur_robots; no_cnt=0; } }