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

include/buzz_update.h

@@ -6,7 +6,7 @@
 #include <buzz/buzzdict.h>
 #include <buzz/buzzdarray.h>
 #include <buzz/buzzvstig.h>
-
+#include <fstream>
 #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

include/roscontroller.h

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

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

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,6 +89,7 @@ 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*/
@@ -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,7 +243,7 @@ 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();
@@ -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<<","<<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;
 	//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);

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;

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<<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*/
 				//no_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;
 				//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;
 				}
 			}