Merge remote-tracking branch 'refs/remotes/origin/dev'

Conflicts:
	include/roscontroller.h
	launch/rosbuzz.launch
	src/buzz_utility.cpp
	src/roscontroller.cpp

include/buzz_update.h

@@ -73,7 +73,7 @@ void update_routine(const char* bcfname,
 /************************************************/
 /*Initalizes the updater */
 /************************************************/
-void init_update_monitor(const char* bo_filename,const char* stand_by_script,int barrier);
+void init_update_monitor(const char* bo_filename,const char* stand_by_script);
 
 
 /*********************************************************/
@@ -129,4 +129,6 @@ int get_update_status();
 
 void set_read_update_status();
 
+void updates_set_robots(int robots);
+
 #endif

include/buzz_utility.h

@@ -52,4 +52,6 @@ int update_step_test();
 uint16_t get_robotid();
 
 buzzvm_t get_vm();
+
+void set_robot_var(int ROBOTS);
 }

include/buzzuav_closures.h

@@ -10,6 +10,17 @@
 //#include "roscontroller.h"
 
 namespace buzzuav_closures{
+	typedef enum {
+      		COMMAND_NIL = 0,     // Dummy command
+      		COMMAND_TAKEOFF,     // Take off
+		COMMAND_LAND,
+		COMMAND_GOHOME,
+		COMMAND_ARM,
+		COMMAND_DISARM,
+		COMMAND_GOTO,
+		COMMAND_MOVETO,
+	 } Custom_CommandCode;
+
 /*
  * prextern int() function in Buzz
  * This function is used to print data from buzz

include/roscontroller.h

@@ -48,7 +48,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;
@@ -61,10 +69,17 @@ private:
 	int armstate;
 	int barrier;
 	int message_number=0;
-	std::string bzzfile_name, fcclient_name, armclient, stream_client_name, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by;
+	int no_of_robots=0;
+	/*tmp to be corrected*/
+	int no_cnt=0;
+	int old_val=0;	
+	std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by,xbeesrv_name;
+	std::string stream_client_name;
 	bool rcclient;
 	bool multi_msg;
+	Num_robot_count count_robots;
 	ros::ServiceClient mav_client;
+	ros::ServiceClient xbeestatus_srv;
 	ros::Publisher payload_pub;
 	ros::Publisher neigh_pos_pub;
 	ros::Publisher localsetpoint_pub;
@@ -80,7 +95,6 @@ private:
 	/*Commands for flight controller*/
   	//mavros_msgs::CommandInt cmd_srv;
   	mavros_msgs::CommandLong cmd_srv;
-
   	std::vector<std::string> m_sMySubscriptions;
   	std::map<std::string, std::string> m_smTopic_infos;
 
@@ -170,6 +184,8 @@ private:
 	void SetLocalPosition(float x, float y, float z, float yaw);
 
 	void SetStreamRate(int id, int rate, int on_off);
+	
+	void get_number_of_robots();
 };
 
 }

launch/rosbuzz.launch

@@ -8,8 +8,8 @@
 		<param name="rcservice_name" value="/buzzcmd" />
 		<param name="in_payload" value="/inMavlink"/>
 		<param name="out_payload" value="/outMavlink"/>
-		<param name="robot_id" value="3"/>
+		<param name="xbee_status_srv" value="/xbee_status"/>
-		<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="/home/ivan/catkin_ws/src/rosbuzz/src/stand_by.bo"/>
+		</node>
-		</node>	
+		
 </launch>

launch/rosbuzzm100.launch

@@ -8,6 +8,7 @@
 		<param name="fcclient_name" value="/dji_mavcmd" />
 		<param name="in_payload" value="/inMavlink"/>
 		<param name="out_payload" value="/outMavlink"/>
+		<param name="xbee_status_srv" value="/xbee_status"/>
 		<param name="robot_id" value="3"/>
 		<param name="No_of_Robots" value="3"/>
 		<param name="stand_by" value="$(env ROS_WS)/src/ROSBuzz/src/stand_by.bo"/>

src/buzz_update.cpp

@@ -27,7 +27,7 @@ static int neigh=-1;
 static int 	    updater_msg_ready ;
 static int updated=0;
 
-void init_update_monitor(const char* bo_filename, const char* stand_by_script,int barrier){
+void init_update_monitor(const char* bo_filename, const char* stand_by_script){
 	fprintf(stdout,"intiialized file monitor.\n");
 	fd=inotify_init1(IN_NONBLOCK);
 	if ( fd < 0 ) {
@@ -91,7 +91,7 @@ void init_update_monitor(const char* bo_filename, const char* stand_by_script,in
 	  *(size_t*)(updater->standby_bcode_size)=stdby_bcode_size;
 	  updater->mode=(int*)malloc(sizeof(int));
 	  *(int*)(updater->mode)=CODE_RUNNING;
-	  no_of_robot=barrier;
+	  //no_of_robot=barrier;
 	  updater_msg_ready=0;
 	  //neigh = 0;
 	  //updater->outmsg_queue=
@@ -444,6 +444,11 @@ close(fd);
 void set_bzz_file(const char* in_bzz_file){
 bzz_file=in_bzz_file;
 }
+
+void updates_set_robots(int robots){
+no_of_robot=robots;
+}
+
 void collect_data(){
 //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;

src/buzz_utility.cpp

@@ -18,9 +18,9 @@ namespace buzz_utility{
 	static uint8_t*     BO_BUF          = 0;
 	static buzzdebug_t  DBG_INFO        = 0;
 	static uint8_t      MSG_SIZE        = 50;   // Only 100 bytes of Buzz messages every step
-	static int          MAX_MSG_SIZE    = 10000; // Maximum Msg size for sending update packets
+	static int          MAX_MSG_SIZE    = 10000; // Maximum Msg size for sending update packets 
-
+	static int 	    Robot_id        = 0;
-
+	
 	/****************************************/
 	/*adds neighbours position*/
 	void neighbour_pos_callback(std::map< int,  Pos_struct> neighbours_pos_map){
@@ -254,16 +254,17 @@ namespace buzz_utility{
 	                    const char* bdbg_filename, int robot_id) {
 	//cout<<"bo file name"<<bo_filename;
 	/* Get hostname */
-   	char hstnm[30];
+   	//char hstnm[30];
         //char* hstnm ="M1003";
-   	gethostname(hstnm, 30);
+   	//gethostname(hstnm, 30);
    	/* Make numeric id from hostname */
-   	/* NOTE: here we assume that the hostname is in the format Knn */
+   	/* NOTE: here we assume that the hostname is in the format Mnn */
-   	int id = strtol(hstnm+4, NULL, 10);
+   	//int id = strtol(hstnm+4, NULL, 10); 
-   	cout << " Robot ID: " <<id<< endl;
+   	cout << " Robot ID: " <<robot_id<< endl;
    	/* Reset the Buzz VM */
    	if(VM) buzzvm_destroy(&VM);
-   	VM = buzzvm_new((int)id);
+	Robot_id =robot_id;
+   	VM = buzzvm_new((int)robot_id);
    	/* Get rid of debug info */
    	if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
    	DBG_INFO = buzzdebug_new();
@@ -321,15 +322,15 @@ namespace buzz_utility{
 	/****************************************/
 	int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){
 	/* Get hostname */
-   	char hstnm[30];
+   	//char hstnm[30];
-   	gethostname(hstnm, 30);
+   	//gethostname(hstnm, 30);
    	/* Make numeric id from hostname */
-   	/* NOTE: here we assume that the hostname is in the format Knn */
+   	/* NOTE: here we assume that the hostname is in the format Mnn */
-   	int id = strtol(hstnm + 4, NULL, 10);
+   	//int id = strtol(hstnm + 4, NULL, 10);
-
+	
 	/* Reset the Buzz VM */
    	if(VM) buzzvm_destroy(&VM);
-   	VM = buzzvm_new(id);
+   	VM = buzzvm_new(Robot_id);
    	/* Get rid of debug info */
    	if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
   	DBG_INFO = buzzdebug_new();
@@ -369,14 +370,14 @@ namespace buzz_utility{
 	/****************************************/
 	int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){
 	/* Get hostname */
-   	char hstnm[30];
+   	//char hstnm[30];
-   	gethostname(hstnm, 30);
+   	//gethostname(hstnm, 30);
    	/* Make numeric id from hostname */
-   	/* NOTE: here we assume that the hostname is in the format Knn */
+   	/* NOTE: here we assume that the hostname is in the format Mnn */
-   	int id = strtol(hstnm + 4, NULL, 10);
+   	//int id = strtol(hstnm + 4, NULL, 10);
 	/* Reset the Buzz VM */
    	if(VM) buzzvm_destroy(&VM);
-   	VM = buzzvm_new(id);
+   	VM = buzzvm_new(Robot_id);
    	/* Get rid of debug info */
    	if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
   	DBG_INFO = buzzdebug_new();
@@ -547,6 +548,11 @@ namespace buzz_utility{
 		return VM;
 	}
 
+void set_robot_var(int ROBOTS){
+	buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1));
+	buzzvm_pushi(VM, ROBOTS);
+	buzzvm_gstore(VM);
+}
 }
 
 

src/buzzuav_closures.cpp

@@ -29,40 +29,42 @@ namespace buzzuav_closures{
 
 	int buzzros_print(buzzvm_t vm) {
 	int i;
+	char buffer [50] = "";
 	   for(i = 1; i < buzzdarray_size(vm->lsyms->syms); ++i) {
 	      buzzvm_lload(vm, i);
 	      buzzobj_t o = buzzvm_stack_at(vm, 1);
 	      buzzvm_pop(vm);
 	      switch(o->o.type) {
 		 case BUZZTYPE_NIL:
-		    ROS_INFO("BUZZ - [nil]");
+		    sprintf(buffer,"%s BUZZ - [nil]", buffer);
 		    break;
 		 case BUZZTYPE_INT:
-		    ROS_INFO("%d", o->i.value);
+		    sprintf(buffer,"%s %d", buffer, o->i.value);
 		    //fprintf(stdout, "%d", o->i.value);
 		    break;
 		 case BUZZTYPE_FLOAT:
-		    ROS_INFO("%f", o->f.value);
+		    sprintf(buffer,"%s %f", buffer, o->f.value);
 		    break;
 		 case BUZZTYPE_TABLE:
-		    ROS_INFO("[table with %d elems]", (buzzdict_size(o->t.value)));
+		    sprintf(buffer,"%s [table with %d elems]", buffer, (buzzdict_size(o->t.value)));
 		    break;
 		 case BUZZTYPE_CLOSURE:
 		    if(o->c.value.isnative)
-			ROS_INFO("[n-closure @%d]", o->c.value.ref);
+		    	sprintf(buffer,"%s [n-closure @%d]", buffer, o->c.value.ref);
 		    else
-			ROS_INFO("[c-closure @%d]", o->c.value.ref);
+		    	sprintf(buffer,"%s [c-closure @%d]", buffer, o->c.value.ref);
 		    break;
 		 case BUZZTYPE_STRING:
-		    ROS_INFO("%s", o->s.value.str);
+		    sprintf(buffer,"%s %s", buffer, o->s.value.str);
 		    break;
 		 case BUZZTYPE_USERDATA:
-		    ROS_INFO("[userdata @%p]", o->u.value);
+		    sprintf(buffer,"%s [userdata @%p]", buffer, o->u.value);
 		    break;
 		 default:
 		    break;
 	      }
 	   }
+	   ROS_INFO(buffer);
 	   //fprintf(stdout, "\n");
 	   return buzzvm_ret0(vm);
 	}
@@ -108,7 +110,7 @@ namespace buzzuav_closures{
 	   gps_from_rb(d, b, goto_pos);
 	   cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;*/
 	   printf(" Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f  \n",goto_pos[0], goto_pos[1], goto_pos[2]);
-	   buzz_cmd=5;
+	   buzz_cmd= COMMAND_MOVETO; // TO DO what should we use
 	   return buzzvm_ret0(vm);
 	}
 
@@ -120,7 +122,7 @@ namespace buzzuav_closures{
 	   set_goto(rc_goto_pos);
 	   cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
 	   printf(" Buzz requested Go To, to Latitude: %.7f , Longitude: %.7f, Altitude: %.7f  \n",goto_pos[0],goto_pos[1],goto_pos[2]);
-	   buzz_cmd=2;
+	   buzz_cmd=COMMAND_GOTO;
 	   return buzzvm_ret0(vm);
 	}
 
@@ -130,13 +132,13 @@ namespace buzzuav_closures{
 	int buzzuav_arm(buzzvm_t vm) {
 	   cur_cmd=mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM;
 	   printf(" Buzz requested Arm \n");
-	   buzz_cmd=3;
+	   buzz_cmd=COMMAND_ARM;
 	   return buzzvm_ret0(vm);
 	}
 	int buzzuav_disarm(buzzvm_t vm) {
 	   cur_cmd=mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM + 1;
 	   printf(" Buzz requested Disarm  \n");
-	   buzz_cmd=4;
+	   buzz_cmd=COMMAND_DISARM;
 	   return buzzvm_ret0(vm);
 	}
 
@@ -151,21 +153,21 @@ namespace buzzuav_closures{
 	   height = goto_pos[2];
 	   cur_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF;
 	   printf(" Buzz requested Take off !!! \n");
-	   buzz_cmd = 1;
+	   buzz_cmd = COMMAND_TAKEOFF;
 	   return buzzvm_ret0(vm);
 	}
 
 	int buzzuav_land(buzzvm_t vm) {
 	   cur_cmd=mavros_msgs::CommandCode::NAV_LAND;
 	   printf(" Buzz requested Land !!! \n");
-	   buzz_cmd = 1;
+	   buzz_cmd = COMMAND_LAND;
 	   return buzzvm_ret0(vm);
 	}
 
 	int buzzuav_gohome(buzzvm_t vm) {
 	   cur_cmd=mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
 	   printf(" Buzz requested gohome !!! \n");
-	   buzz_cmd = 1;
+	   buzz_cmd = COMMAND_GOHOME;
 	   return buzzvm_ret0(vm);
 	}
 
@@ -306,9 +308,65 @@ namespace buzzuav_closures{
 
 	/******************************************************/
 	/*Create an obstacle Buzz table from proximity sensors*/
+	/*  Acessing proximity in buzz script 
+	    proximity[0].angle and proximity[0].value - front
+		""          ""          "" 	      - right and back
+	    proximity[3].angle and proximity[3].value - left
+	    proximity[4].angle = -1 and proximity[4].value -bottom */
 	/******************************************************/
 
 	int buzzuav_update_prox(buzzvm_t vm) {
+
+	buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity", 1));
+        buzzvm_pusht(vm);
+        buzzobj_t tProxTable = buzzvm_stack_at(vm, 1);
+        buzzvm_gstore(vm);
+      
+        /* Fill into the proximity table */
+        buzzobj_t tProxRead;
+        float angle =0;
+        for(size_t i = 0; i < 4; ++i) {
+           /* Create table for i-th read */
+           buzzvm_pusht(vm);
+           tProxRead = buzzvm_stack_at(vm, 1);
+           buzzvm_pop(vm);
+           /* Fill in the read */
+	   buzzvm_push(vm, tProxRead);
+   	   buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0));
+   	   buzzvm_pushf(vm, obst[i+1]);
+   	   buzzvm_tput(vm);
+	   buzzvm_push(vm, tProxRead);
+   	   buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0));
+   	   buzzvm_pushf(vm, angle);
+   	   buzzvm_tput(vm);
+           /* Store read table in the proximity table */
+          buzzvm_push(vm, tProxTable);
+   	  buzzvm_pushi(vm, i);
+  	  buzzvm_push(vm, tProxRead);
+ 	  buzzvm_tput(vm);
+	   angle+=1.5708;
+        }
+	/* Create table for bottom read */
+	 angle =-1;
+         buzzvm_pusht(vm);
+         tProxRead = buzzvm_stack_at(vm, 1);
+         buzzvm_pop(vm);
+         /* Fill in the read */
+	 buzzvm_push(vm, tProxRead);
+   	 buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0));
+   	 buzzvm_pushf(vm, obst[0]);
+   	 buzzvm_tput(vm);
+	 buzzvm_push(vm, tProxRead);
+   	 buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0));
+   	 buzzvm_pushf(vm, angle);
+   	 buzzvm_tput(vm);
+	 /*Store read table in the proximity table*/
+	 buzzvm_push(vm, tProxTable);
+   	 buzzvm_pushi(vm, 4);
+  	 buzzvm_push(vm, tProxRead);
+ 	 buzzvm_tput(vm);
+
+	 /*
 	  buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity", 1));
 	   buzzvm_pusht(vm);
 	   buzzvm_dup(vm);
@@ -331,7 +389,7 @@ namespace buzzuav_closures{
 	   buzzvm_pushs(vm, buzzvm_string_register(vm, "left", 1));
 	   buzzvm_pushf(vm, obst[4]);
 	   buzzvm_tput(vm);
-	   buzzvm_gstore(vm);
+	   buzzvm_gstore(vm);*/
 	   return vm->state;
 	}
 

src/roscontroller.cpp

@@ -40,10 +40,23 @@ namespace rosbzz_node{
 	/rosbuzz_node loop method executed once every step
 	/--------------------------------------------------*/
 	void roscontroller::RosControllerRun(){
+		mavros_msgs::ParamGet::Request robot_id_srv_request; robot_id_srv_request.param_id="id";
+        	mavros_msgs::ParamGet::Response robot_id_srv_response;
+		while(!xbeestatus_srv.call(robot_id_srv_request,robot_id_srv_response)){
+			/*run once*/
+    			ros::spinOnce();
+			/*loop rate of ros*/
+			ros::Rate loop_rate(10);
+			loop_rate.sleep();
+ 			/*sleep for the mentioned loop rate*/
+			ROS_ERROR("Waiting for Xbee to respond to get device ID");
+		}
+		robot_id=robot_id_srv_response.value.integer;
+			
 		/* Set the Buzz bytecode */
 		if(buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(),robot_id)) {
 			fprintf(stdout, "Bytecode file found and set\n");
-			init_update_monitor(bcfname.c_str(),stand_by.c_str(),barrier);
+			init_update_monitor(bcfname.c_str(),stand_by.c_str());
 			while (ros::ok() && !buzz_utility::buzz_script_done())
   			{
       				/*Update neighbors position inside Buzz*/
@@ -63,6 +76,13 @@ namespace rosbzz_node{
 					set_read_update_status();
 					multi_msg=true;
 				}
+				/*Set ROBOTS variable for barrier in .bzz from neighbours count*/
+				//no_of_robots=get_number_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);
     				/*run once*/
     				ros::spinOnce();
 				/*loop rate of ros*/
@@ -71,9 +91,8 @@ namespace rosbzz_node{
  				/*sleep for the mentioned loop rate*/
     				timer_step+=1;
    				maintain_pos(timer_step);
-
+				
-
+				
-
 			}
 			/* Destroy updater and Cleanup */
     			//update_routine(bcfname.c_str(), dbgfname.c_str(),1);
@@ -112,6 +131,7 @@ namespace rosbzz_node{
 		n_c.getParam("No_of_Robots", barrier);
 		/*Obtain standby script to run during update*/
 		n_c.getParam("stand_by", stand_by);
+		n_c.getParam("xbee_status_srv", xbeesrv_name);
 		
 		GetSubscriptionParameters(n_c);
 		// initialize topics to null?
@@ -161,7 +181,7 @@ namespace rosbzz_node{
   		//battery_sub = n_c.subscribe("/power_status", 1000, &roscontroller::battery,this);
   		payload_sub = n_c.subscribe(in_payload, 1000, &roscontroller::payload_obt,this);
 		//flight_status_sub =n_c.subscribe("/flight_status",100, &roscontroller::flight_extended_status_update,this);
-		Robot_id_sub = n_c.subscribe("/device_id_xbee_", 1000, &roscontroller::set_robot_id,this);
+		//Robot_id_sub = n_c.subscribe("/device_id_xbee_", 1000, &roscontroller::set_robot_id,this);
   		obstacle_sub= n_c.subscribe("/guidance/obstacle_distance",100, &roscontroller::obstacle_dist,this);
   		/*publishers*/
 		payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 1000);
@@ -171,6 +191,7 @@ namespace rosbzz_node{
 		arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient);
 		mode_client =  n_c.serviceClient<mavros_msgs::SetMode>(modeclient);
 		mav_client = n_c.serviceClient<mavros_msgs::CommandLong>(fcclient_name);
+		xbeestatus_srv = n_c.serviceClient<mavros_msgs::ParamGet>(xbeesrv_name);
 		stream_client = n_c.serviceClient<mavros_msgs::StreamRate>(stream_client_name);
 
 		multi_msg=true;
@@ -444,8 +465,16 @@ namespace rosbzz_node{
 	/ from GPS coordinates
 	----------------------------------------------------------- */
 	#define EARTH_RADIUS (double) 6371000.0
+	#define DEG2RAD(DEG) ((DEG)*((M_PI)/(180.0)))
 	void roscontroller::cvt_rangebearing_coordinates(double nei[], double out[], double cur[]){
-		double lat1 = cur[0]*M_PI/180.0;
+ 		double d_lon = nei[1] - cur[1];
+        	double d_lat = nei[0] - cur[0];
+        	double ned_x = DEG2RAD(d_lat) * EARTH_RADIUS;
+        	double ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0]));
+		out[0] = sqrt(ned_x*ned_x+ned_y*ned_y);
+		out[1] = atan2(ned_y,ned_x);
+		out[2] = 0.0;
+/*		double lat1 = cur[0]*M_PI/180.0;
 		double lon1 = cur[1]*M_PI/180.0;
 		double lat2 = nei[0]*M_PI/180.0;
 		double lon2 = nei[1]*M_PI/180.0;
@@ -453,7 +482,7 @@ namespace rosbzz_node{
 		double y = sin(lon1-lon2)*cos(lat1);
 		double x =  cos(lat2)*sin(lat1) - sin(lat2)*cos(lat1)*cos(lon1-lon2);
 		out[1] = atan2(y,x)+M_PI;
-		out[2] = 0.0;
+		out[2] = 0.0;*/
 	}
 
 	/*------------------------------------------------------
@@ -633,10 +662,66 @@ namespace rosbzz_node{
 	/Obtain robot id by subscribing to xbee robot id topic 
 	/ TODO: check for integrity of this subscriber call back
 	/----------------------------------------------------*/
-	void roscontroller::set_robot_id(const std_msgs::UInt8::ConstPtr& msg){
+	/*void roscontroller::set_robot_id(const std_msgs::UInt8::ConstPtr& msg){
-		robot_id=(int)msg->data;
-	}
 	
+	
+	}*/
+	void roscontroller::get_number_of_robots(){
+		
+		if(no_of_robots==0){
+			no_of_robots=neighbours_pos_map.size()+1;
+						
+		}
+		else{
+			if(no_of_robots!=neighbours_pos_map.size()+1 && no_cnt==0){
+				no_cnt++;
+				old_val=neighbours_pos_map.size()+1;
+			
+			}			
+			else if(old_val==neighbours_pos_map.size()+1){
+				no_cnt++;
+				if(no_cnt==3){
+					no_of_robots=neighbours_pos_map.size()+1;
+					no_cnt=0;
+				}
+			}
+			else{
+				no_cnt=0;
+			}
+		}
+		//if(count_robots.current !=0){
+			/*std::map< int,  int> count_count;
+			uint8_t index=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>current_count){
+				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;
+				} 
+			}
+		}*/
 	/*
 	 * SOLO SPECIFIC FUNCTIONS
 	 */

src/testflockfev.bzz

@@ -16,7 +16,7 @@ CURSTATE = "TURNEDOFF"
  
 # Lennard-Jones parameters
 TARGET     = 10.0	#0.000001001
-EPSILON    = 10.0	#0.001
+EPSILON    = 18.0	#0.001
  
 # Lennard-Jones interaction magnitude
 function lj_magnitude(dist, target, epsilon) {
@@ -56,7 +56,7 @@ function hexagon() {
 # Constants
 #
 BARRIER_VSTIG = 1
-ROBOTS = 4 # number of robots in the swarm
+# ROBOTS = 3 # number of robots in the swarm
 
 #
 # Sets a barrier
@@ -86,12 +86,12 @@ function barrier_wait(threshold, transf) {
   if(barrier.size() >= threshold) {
     barrier = nil
     transf()
-   } else if(timeW>=WAIT_TIMEOUT) {
+  } else if(timeW>=WAIT_TIMEOUT) {
-     barrier = nil
+    barrier = nil
-     statef=hexagon #idle
+    statef=land
-     timeW=0
+    timeW=0
-   }
+  }
-   timeW = timeW+1
+  timeW = timeW+1
 }
 
 # flight status
@@ -99,25 +99,12 @@ function barrier_wait(threshold, transf) {
 function idle() {
 statef=idle
 CURSTATE = "IDLE"
-neighbors.listen("cmd",
-   function(vid, value, rid) {
-      print("Got (", vid, ",", value, ") from robot #", rid)
-	if(value==22) {
-		statef=takeoff
-	} else if(value==21) {
-		statef=land
-	} else if(value==400) {
-		uav_arm()
-	} else if(value==401){
-		uav_disarm()
-	}
-   }
    
-
-)
 }
 
 function takeoff() {
+	CURSTATE = "TAKEOFF"
+	statef=takeoff
 	log("TakeOff: ", flight.status)
 	if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
 		barrier_set(ROBOTS,hexagon)
@@ -131,6 +118,8 @@ function takeoff() {
 	}
 }
 function land() {
+	CURSTATE = "LAND"
+	statef=land
 	log("Land: ", flight.status)
 	if(flight.status == 2 or flight.status == 1){
 		neighbors.broadcast("cmd", 21)
@@ -165,6 +154,7 @@ function step() {
 		neighbors.broadcast("cmd", 21)
 	} else if(flight.rc_cmd==16) {
 		flight.rc_cmd=0
+		statef = idle
 		uav_goto()
 	} else if(flight.rc_cmd==400) {
 		flight.rc_cmd=0
@@ -175,8 +165,24 @@ function step() {
 		uav_disarm()
 		neighbors.broadcast("cmd", 401)
 	}
+neighbors.listen("cmd",
+   function(vid, value, rid) {
+      print("Got (", vid, ",", value, ") from robot #", rid)
+	if(value==22 and CURSTATE=="IDLE") {
+		statef=takeoff
+	} else if(value==21) {
+		statef=land
+	} else if(value==400 and CURSTATE=="IDLE") {
+		uav_arm()
+	} else if(value==401 and CURSTATE=="IDLE"){
+		uav_disarm()
+	}
+   }
+
+)
 	statef()
 	log("Current state: ", CURSTATE)
+	log("Swarm size: ",ROBOTS)
 }
  
 # Executed once when the robot (or the simulator) is reset.