From 2647f9dc9a61c406c397681351695034d3c0366e Mon Sep 17 00:00:00 2001
From: vivek-shankar <robovivek@gmail.com>
Date: Sun, 2 Apr 2017 22:46:48 -0400
Subject: [PATCH] inertia for no.of robots

---
 include/roscontroller.h | 13 ++++++++++++-
 src/roscontroller.cpp   | 38 +++++++++++++++++++++++++++++++++++++-
 2 files changed, 49 insertions(+), 2 deletions(-)

diff --git a/include/roscontroller.h b/include/roscontroller.h
index 370fcf0..aba888a 100644
--- a/include/roscontroller.h
+++ b/include/roscontroller.h
@@ -46,7 +46,15 @@ public:
 	void RosControllerRun();
 	
 private:
-        
+        struct num_robot_count
+	{
+		uint8_t history[10];
+		uint8_t index=0;
+	  	uint8_t current=0;
+  		num_robot_count(){}
+	  
+	}; typedef struct num_robot_count Num_robot_count ;
+
  	double cur_pos[3];
 	uint64_t payload;
 	std::map< int,  buzz_utility::Pos_struct> neighbours_pos_map;
@@ -63,6 +71,7 @@ private:
 	std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by,xbeesrv_name;
 	bool rcclient;
 	bool multi_msg;
+	Num_robot_count count_robots;
 	ros::ServiceClient mav_client;
 	ros::ServiceClient xbeestatus_srv;
 	ros::Publisher payload_pub;
@@ -157,6 +166,8 @@ private:
 
 	/*Robot independent subscribers*/
 	void Subscribe(ros::NodeHandle n_c);
+	
+	int get_number_of_robots();
 };
 
 }
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index da7c8a7..dd92d64 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -73,7 +73,8 @@ namespace rosbzz_node{
 					multi_msg=true;
 				}
 				/*Set ROBOTS variable for barrier in .bzz from neighbours count*/
-				if(neighbours_pos_map.size() >0) no_of_robots =neighbours_pos_map.size()+1;
+				no_of_robots=get_number_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*/
 				updates_set_robots(no_of_robots);
@@ -654,6 +655,41 @@ namespace rosbzz_node{
 	
 	
 	}*/
+	int roscontroller::get_number_of_robots(){
+		if(count_robots.current !=0){
+			uint8_t current_count,odd_count,odd_val=0;
+			count_robots.history[count_robots.index]=neighbours_pos_map.size()+1;
+			//count_robots.current=neighbours_pos_map.size()+1;
+			count_robots.index++;	
+			if(count_robots.index >9) count_robots.index =0;	
+			for(int i=0;i<10;i++){
+				if(count_robots.history[i]==count_robots.current){
+					current_count++;	
+				}
+				else{
+					if(count_robots.history[i]!=0){
+						odd_count++;
+						odd_val=count_robots.history[i];
+					}
+				}
+			}
+			if(odd_count>count_robots.current){
+				count_robots.current=odd_val;
+			}	
+		}
+		else{
+			if(neighbours_pos_map.size()!=0){
+				count_robots.history[count_robots.index]=neighbours_pos_map.size()+1;
+				//count_robots.current=neighbours_pos_map.size()+1;
+				count_robots.index++;
+				if(count_robots.index >9){ 
+					count_robots.index =0;
+					count_robots.current=neighbours_pos_map.size()+1;
+				} 
+			}
+		}
+	return count_robots.current;
+	}
 	
 }