diff --git a/CMakeLists.txt~ b/CMakeLists.txt~
deleted file mode 100644
index b9c2ec4..0000000
--- a/CMakeLists.txt~
+++ /dev/null
@@ -1,52 +0,0 @@
-cmake_minimum_required(VERSION 2.8.3)
-project(rosbuzz)
-
-if(UNIX)
-    SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=gnu++11")
-endif()
-
-## Find catkin macros and libraries
-find_package(catkin REQUIRED COMPONENTS
-  roscpp
-  std_msgs
-  #mavros_msgs
-  sensor_msgs
-)
-
-###################################
-## catkin specific configuration ##
-###################################
-
-catkin_package(
-  INCLUDE_DIRS include
-#  LIBRARIES xbee_ros_node
-  CATKIN_DEPENDS roscpp std_msgs mavros_msgs sensor_msgs
-#  DEPENDS system_lib
-)
-
-###########
-## Build ##
-###########
-
-include_directories(
-  include ${rosbuzz_INCLUDE_DIRS}
-  ${catkin_INCLUDE_DIRS}
-)
-
-# Executables 
-add_executable(rosbuzz_node src/rosbuzz.cpp
-  src/roscontroller.cpp
-  src/buzz_utility.cpp
-  src/buzzuav_closures.cpp
-  src/uav_utility.cpp)
-target_link_libraries(rosbuzz_node ${catkin_LIBRARIES} /usr/local/lib/libbuzz.so /usr/local/lib/libbuzzdbg.so -lpthread)
-
-# Executables and libraries for installation to do
-install(TARGETS rosbuzz_node
-   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-
-find_package(catkin REQUIRED COMPONENTS roslaunch)
-roslaunch_add_file_check(launch)
diff --git a/launch/rosbuzz.launch~ b/launch/rosbuzz.launch~
deleted file mode 100644
index 7d7f7ee..0000000
--- a/launch/rosbuzz.launch~
+++ /dev/null
@@ -1,11 +0,0 @@
-<!-- Launch file for ROSBuzz -->
-
-<launch>
-	<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >	
-		<param name="bzzfile_name" value="~/catkin_ws/src/rosbuzz/src/test.bzz" />
-                <param name="rcclient" value="true" /> 
-		<param name="rcservice_name" value="rc_cmd" />
-		<param name="fcclient_name" value="fc_cmd" />
-	</node>
-     		<param name="robot_id" value="1"/>
-</launch>
diff --git a/src/buzz_utility.cpp~ b/src/buzz_utility.cpp~
deleted file mode 100644
index d50725e..0000000
--- a/src/buzz_utility.cpp~
+++ /dev/null
@@ -1,361 +0,0 @@
-/** @file      buzz_utility.cpp
- *  @version   1.0 
- *  @date      27.09.2016
- *  @brief     Buzz Implementation as a node in ROS for Dji M100 Drone. 
- *  @author    Vivek Shankar Varadharajan
- *  @copyright 2016 MistLab. All rights reserved.
- */
-
-#include <buzz_utility.h>
-
-namespace buzz_utility{
-
-	/****************************************/
-	/****************************************/
-
-	static buzzvm_t     VM              = 0;
-	static char*        BO_FNAME        = 0;
-	static uint8_t*     BO_BUF          = 0;
-	static buzzdebug_t  DBG_INFO        = 0;
-	static unsigned int MSG_SIZE        = 32;
-	
-	
-	/****************************************/
-	/*adds neighbours position*/
-	void neighbour_pos_callback(std::map< int,  Pos_struct> neighbours_pos_map){
-		/* Reset neighbor information */
-    		buzzneighbors_reset(VM);
-  		/* Get robot id and update neighbor information */
-  	  	map< int, Pos_struct >::iterator it;
-    		for (it=neighbours_pos_map.begin(); it!=neighbours_pos_map.end(); ++it){
-    			buzzneighbors_add(VM,
-                        		  it->first,
-                        		  (it->second).x,
-                        		  (it->second).y,
-                        		  (it->second).z);
-    		}
-	}
-	/***************************************/
-	/*Reinterprets  uint64_t into 4 uint16_t*/
-	/***************************************/
-	uint16_t* u64_cvt_u16(uint64_t u64){
-   	uint16_t* out = new uint16_t[4];
-   	uint32_t int32_1 = u64 & 0xFFFFFFFF;
-   	uint32_t int32_2 = (u64 & 0xFFFFFFFF00000000 ) >> 32;
-   	 out[0] = int32_1 & 0xFFFF;
-   	 out[1] = (int32_1 & (0xFFFF0000) ) >> 16;
-   	 out[2] = int32_2 & 0xFFFF;
-   	 out[3] = (int32_2 & (0xFFFF0000) ) >> 16;
-   	//cout << " values " <<out[0] <<"  "<<out[1] <<"  "<<out[2] <<"  "<<out[3] <<"  ";
-	return out;
-	} 
-
-	/***************************************************/
-	/*Appends obtained messages to buzz in message Queue*/
-	/***************************************************/
-
-	void in_msg_process(uint64_t* payload){
-
-   		/* Go through messages and add them to the FIFO */
-   		uint16_t* data= u64_cvt_u16((uint64_t)payload[0]);
-   		uint16_t size=data[0]*sizeof(uint64_t);
-   		delete[] data;
-   		uint8_t* pl =(uint8_t*)malloc(size);
-   		memset(pl, 0,size);
-   		/* Copy packet into temporary buffer */
-   		memcpy(pl, payload ,size);
-
-   		size_t tot = sizeof(uint32_t);
-      
-		/* Go through the messages until there's nothing else to read */
-      		uint16_t unMsgSize;
-      			do {
-         			/* Get payload size */
-         			unMsgSize = *(uint16_t*)(pl + tot);
-   	 			tot += sizeof(uint16_t);
-         			/* Append message to the Buzz input message queue */
-         			if(unMsgSize > 0 && unMsgSize <= size*sizeof(uint64_t) - tot) {
-            			buzzinmsg_queue_append(VM->inmsgs,
-                                buzzmsg_payload_frombuffer(pl +tot, unMsgSize));
-            			tot += unMsgSize;
-         			}
-      			}while(size - tot > sizeof(uint16_t) && unMsgSize > 0);
-  
-   		/* Process messages */
-		buzzvm_process_inmsgs(VM);
-	}
-	/***************************************************/
-	/*Obtains messages from buzz out message Queue*/
-	/***************************************************/
-
-   	uint64_t* out_msg_process(){
-   		buzzvm_process_outmsgs(VM);
-   		uint8_t* buff_send =(uint8_t*)malloc(MSG_SIZE);
-   		memset(buff_send, 0, MSG_SIZE);
-   		ssize_t tot = sizeof(uint16_t);
-   		/* Send robot id */
-   		*(uint16_t*)(buff_send+tot) = (uint16_t) VM->robot;
-   		tot += sizeof(uint16_t);
-   		/* Send messages from FIFO */
-   		do {
-        		/* Are there more messages? */
-      			if(buzzoutmsg_queue_isempty(VM->outmsgs)) break;
-      			/* Get first message */
-      			buzzmsg_payload_t m = buzzoutmsg_queue_first(VM->outmsgs);
-      			/* Make sure the next message fits the data buffer */
-      			if(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)
-        			>
-        	 		MSG_SIZE) {
-        	 		buzzmsg_payload_destroy(&m);
-        	 		break;
-      			}
-       
-      			/* Add message length to data buffer */
-      			*(uint16_t*)(buff_send + tot) = (uint16_t)buzzmsg_payload_size(m);
-      			tot += sizeof(uint16_t);
-   
-      			/* Add payload to data buffer */
-      			memcpy(buff_send + tot, m->data, buzzmsg_payload_size(m));
-			tot += buzzmsg_payload_size(m);
-   
-      			/* Get rid of message */
-      			buzzoutmsg_queue_next(VM->outmsgs);
-      			buzzmsg_payload_destroy(&m);
-   		} while(1);
-
-   		int total_size =(ceil((float)tot/(float)sizeof(uint64_t))); 
-   		*(uint16_t*)buff_send = (uint16_t) total_size;   
-  
-   
-   		uint64_t* payload_64 = new uint64_t[total_size];
- 
-   		memcpy((void*)payload_64, (void*)buff_send, total_size*sizeof(uint64_t));
-  		/*for(int i=0;i<total_size;i++){
-   		cout<<" payload from out msg  "<<*(payload_64+i)<<endl;
-   		}*/
-   		/* Send message */
-		return payload_64;
-	}
-	/****************************************/
-	/*Buzz script not able to load*/
-	/****************************************/
-
-	static const char* buzz_error_info() {
-		buzzdebug_entry_t dbg = *buzzdebug_info_get_fromoffset(DBG_INFO, &VM->pc);
-   		char* msg;
-   		if(dbg != NULL) {
-      			asprintf(&msg,
-               		"%s: execution terminated abnormally at %s:%" PRIu64 ":%" PRIu64 " : %s\n\n",
-               		BO_FNAME,
-               		dbg->fname,
-               		dbg->line,
-               		dbg->col,
-               		VM->errormsg);
-   		}
-   		else {
-      			asprintf(&msg,
-               			"%s: execution terminated abnormally at bytecode offset %d: %s\n\n",
-               			 BO_FNAME,
-              			 VM->pc,
-      			         VM->errormsg);
-   		}
-  	 	return msg;
-	}
-
-	/****************************************/
-	/*Buzz hooks that can be used inside .bzz file*/
-	/****************************************/
-
-	static int buzz_register_hooks() {
-		buzzvm_pushs(VM,  buzzvm_string_register(VM, "print", 1));
-   		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
-   		buzzvm_gstore(VM);
-   		buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_goto", 1));
-   		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_goto));
-   		buzzvm_gstore(VM);
-   		buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_takeoff", 1));
-   		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_takeoff));
-   		buzzvm_gstore(VM);
-   		buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_gohome", 1));
-   		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_gohome));
-   		buzzvm_gstore(VM);
-   		buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_land", 1));
-   		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_land));
-   		buzzvm_gstore(VM);
-   	return BUZZVM_STATE_READY;
-	}
-
-	/****************************************/
-	/*Sets the .bzz and .bdbg file*/
-	/****************************************/
-
-	int buzz_script_set(const char* bo_filename,
-	                    const char* bdbg_filename, int robot_id) {
-	cout<<"bo file name"<<bo_filename;
-		/* Get hostname */
-   		char hstnm[30];
-   	gethostname(hstnm, 30);
-   	/* Make numeric id from hostname */
-   	/* NOTE: here we assume that the hostname is in the format Knn */
-   	int id = robot_id; //strtol(hstnm + 1, NULL, 10);
-   	cout << " Robot ID" << id<< endl;
-   	/* Reset the Buzz VM */
-   	if(VM) buzzvm_destroy(&VM);
-   	VM = buzzvm_new(id);
-   	/* Get rid of debug info */
-   	if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
-   	DBG_INFO = buzzdebug_new();
-   	/* Read bytecode and fill in data structure */
-   	FILE* fd = fopen(bo_filename, "rb");
-   	if(!fd) {
-      		perror(bo_filename);
-      		return 0;
-   	}
-   	fseek(fd, 0, SEEK_END);
-   	size_t bcode_size = ftell(fd);
-   	rewind(fd);
-   	BO_BUF = (uint8_t*)malloc(bcode_size);
-   	if(fread(BO_BUF, 1, bcode_size, fd) < bcode_size) {
-      		perror(bo_filename);
-      		buzzvm_destroy(&VM);
-      		buzzdebug_destroy(&DBG_INFO);
-      		fclose(fd);
-      		return 0;
-   	}
-   	fclose(fd);
-   	/* Read debug information */
-   	if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) {
-      		buzzvm_destroy(&VM);
-      		buzzdebug_destroy(&DBG_INFO);
-      		perror(bdbg_filename);
-      		return 0;
-   	}
-   	/* Set byte code */
-   	if(buzzvm_set_bcode(VM, BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
-      		buzzvm_destroy(&VM);
-      		buzzdebug_destroy(&DBG_INFO);
-      		fprintf(stdout, "%s: Error loading Buzz script\n\n", bo_filename);
-      		return 0;
-   	}
-   	/* Register hook functions */
-   	if(buzz_register_hooks() != BUZZVM_STATE_READY) {
-      		buzzvm_destroy(&VM);
-      		buzzdebug_destroy(&DBG_INFO);
-      		fprintf(stdout, "%s: Error registering hooks\n\n", bo_filename);
-      		return 0;
-   	}
-   	/* Save bytecode file name */
-   	BO_FNAME = strdup(bo_filename);
-   	/* Execute the global part of the script */
-   	buzzvm_execute_script(VM);
-   	/* Call the Init() function */
-   	buzzvm_function_call(VM, "init", 0);
-   	/* All OK */
-   	return 1;
-	}
-
-	/****************************************/
-	/*Swarm struct*/
-	/****************************************/
-
-struct buzzswarm_elem_s {
-   buzzdarray_t swarms;
-   uint16_t age;
-};
-typedef struct buzzswarm_elem_s* buzzswarm_elem_t;
-
-void check_swarm_members(const void* key, void* data, void* params) {
-   buzzswarm_elem_t e = *(buzzswarm_elem_t*)data;
-   int* status = (int*)params;
-   if(*status == 3) return;
-   if(buzzdarray_size(e->swarms) != 1) {
-      fprintf(stderr, "Swarm list size is not 1\n");
-      *status = 3;
-   }
-   else {
-      int sid = 1;
-      if(*buzzdict_get(VM->swarms, &sid, uint8_t) &&
-         buzzdarray_get(e->swarms, 0, uint16_t) != sid) {
-         fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n",
-                 sid,
-                 buzzdarray_get(e->swarms, 0, uint16_t));
-         *status = 3;
-         return;
-      }
-      sid = 2;
-      if(*buzzdict_get(VM->swarms, &sid, uint8_t) &&
-         buzzdarray_get(e->swarms, 0, uint16_t) != sid) {
-         fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n",
-                 sid,
-                 buzzdarray_get(e->swarms, 0, uint16_t));
-         *status = 3;
-         return;
-      }
-   }
-}
-/*Step through the buzz script*/
-
-void buzz_script_step() {
-   /*
-    * Update sensors
-    */
-   buzzuav_closures::buzzuav_update_battery(VM);
-   buzzuav_closures::buzzuav_update_prox(VM);
-   /*
-    * Call Buzz step() function
-    */
-   if(buzzvm_function_call(VM, "step", 0) != BUZZVM_STATE_READY) {
-      fprintf(stderr, "%s: execution terminated abnormally: %s\n\n",
-              BO_FNAME,
-              buzz_error_info());
-      buzzvm_dump(VM);
-   }
-   /* Print swarm */
-   buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
-   /* Check swarm state */
-   /*  int status = 1;
-   buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status);
-   if(status == 1 &&
-      buzzdict_size(VM->swarmmembers) < 9)
-      status = 2;
-   buzzvm_pushs(VM, buzzvm_string_register(VM, "swarm_status", 1));
-   buzzvm_pushi(VM, status);
-   buzzvm_gstore(VM);*/
-}
-
-/****************************************/
-/*Destroy the bvm and other resorces*/
-/****************************************/
-
-void buzz_script_destroy() {
-   if(VM) {
-      if(VM->state != BUZZVM_STATE_READY) {
-         fprintf(stderr, "%s: execution terminated abnormally: %s\n\n",
-                 BO_FNAME,
-                 buzz_error_info());
-         buzzvm_dump(VM);
-      }
-      buzzvm_function_call(VM, "destroy", 0);
-      buzzvm_destroy(&VM);
-      free(BO_FNAME);
-      buzzdebug_destroy(&DBG_INFO);
-   }
-   fprintf(stdout, "Script execution stopped.\n");
-}
-
-
-/****************************************/
-/****************************************/
-
-/****************************************/
-/*Execution completed*/
-/****************************************/
-
-int buzz_script_done() {
-   return VM->state != BUZZVM_STATE_READY;
-}
-
-}
-
-
-
diff --git a/src/roscontroller.cpp~ b/src/roscontroller.cpp~
deleted file mode 100644
index 2ed1fbc..0000000
--- a/src/roscontroller.cpp~
+++ /dev/null
@@ -1,275 +0,0 @@
-
-#include "roscontroller.h"
-
-
-namespace rosbzz_node{
-
-	/***Constructor***/
-	roscontroller::roscontroller()	
-	{
-		ROS_INFO("Buzz_node");
-		/*Obtain parameters from ros parameter server*/
-	  	Rosparameters_get();
-		/*Initialize publishers, subscribers and client*/
-  		Initialize_pub_sub();
-		/*Compile the .bzz file to .basm, .bo and .bdbg*/
- 		Compile_bzz();
-	}
-
-	/***Destructor***/
-	roscontroller::~roscontroller()
-	{
-		/* All done */  
-		/* Cleanup */
-		buzz_utility::buzz_script_destroy();
- 		/* Stop the robot */
-   		uav_done();
-	}
-	
-	/*rosbuzz_node run*/
-	void roscontroller::RosControllerRun(){
-		/* 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");
-			while (ros::ok() && !buzz_utility::buzz_script_done())
-  			{
-      				/*Update neighbors position inside Buzz*/
-     				buzz_utility::neighbour_pos_callback(neighbours_pos_map);
-      				/*Step buzz script */
-      				buzz_utility::buzz_script_step();
-				/*Prepare messages and publish them in respective topic*/
-		  		prepare_msg_and_publish();
-    				/*run once*/
-    				ros::spinOnce();
-				/*loop rate of ros*/
-				ros::Rate loop_rate(1);
-    				/*sleep for the mentioned loop rate*/
-    				loop_rate.sleep();
-    				timer_step+=1;
-   				maintain_pos(timer_step);
-				
-			}
-  		}
-	}
-
-	void roscontroller::Rosparameters_get(){
-		/*Obtain .bzz file name from parameter server*/
-	  	if(ros::param::get("/rosbuzz_node/bzzfile_name", bzzfile_name));
-	  	else {ROS_ERROR("Provide a .bzz file to run in Launch file"); system("rosnode kill rosbuzz_node");}  
-	  	/*Obtain rc service option from parameter server*/
-	  	if(ros::param::get("/rosbuzz_node/rcclient", rcclient)){
-		     	if(rcclient==true){
-			    	/*Service*/
-			    	if(ros::param::get("/rosbuzz_node/rcservice_name", rcservice_name)){
-				        service = n_c.advertiseService(rcservice_name, &roscontroller::rc_callback,this);
-				    	ROS_INFO("Ready to receive Mav Commands from RC client");
-			        }
-			        else{ROS_ERROR("Provide a name topic name for rc service in Launch file"); system("rosnode kill rosbuzz_node");}
-		        }
-     			else if(rcclient==false){ROS_INFO("RC service is disabled");}
-  		}
-  		else{ROS_ERROR("Provide a rc client option: yes or no in Launch file"); system("rosnode kill rosbuzz_node");} 
-  		/*Obtain fc client name from parameter server*/
-  		if(ros::param::get("/rosbuzz_node/fcclient_name", fcclient_name));
-  		else {ROS_ERROR("Provide a fc client name in Launch file"); system("rosnode kill rosbuzz_node");}  
-  		/*Obtain robot_id from parameter server*/
-  		ros::param::get("/rosbuzz_node/robot_id", robot_id);	
-
-	}
-
-	void roscontroller::Initialize_pub_sub(){
-		/*subscribers*/
-  		current_position_sub = n_c.subscribe("current_pos", 1000, &roscontroller::current_pos,this);
-  		battery_sub = n_c.subscribe("battery_state", 1000, &roscontroller::battery,this);
-  		payload_sub = n_c.subscribe("inMavlink", 1000, &roscontroller::payload_obt,this);
-  		/*publishers*/
-		payload_pub = n_c.advertise<mavros_msgs::Mavlink>("outMavlink", 1000);
-		/* Clients*/
-  		mav_client = n_c.serviceClient<mavros_msgs::CommandInt>(fcclient_name);
-
-	}
-	
-	void roscontroller::Compile_bzz(){
-		/*Compile the buzz code .bzz to .bo*/
-		stringstream bzzfile_in_compile;
-	        std::string  path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/"));
-   		bzzfile_in_compile << "bzzparse "<<bzzfile_name<<" "<<path<< "/out.basm";
-   		//system("rm ../catkin_ws/src/rosbuzz/src/out.basm ../catkin_ws/src/rosbuzz/src/out.bo ../catkin_ws/src/rosbuzz/src/out.bdbg");
-   		system(bzzfile_in_compile.str().c_str());
-		bzzfile_in_compile.str("");
-           	cout << "Found PAth name is"<< bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) <<"string stream: 			"<<bzzfile_in_compile.str() << endl;
-		bzzfile_in_compile <<"bzzasm "<<path<<"/out.basm "<<path<<"/out.bo "<<path<<"/out.bdbg";
-   		system(bzzfile_in_compile.str().c_str());
-		bzzfile_in_compile.str("");
-		bzzfile_in_compile <<path<<"/out.bo";
-		bcfname = bzzfile_in_compile.str();
-		bzzfile_in_compile.str("");
-		bzzfile_in_compile <<path<<"/out.bdbg";
-		dbgfname = bzzfile_in_compile.str();
-
-	}
-	
-	void roscontroller::prepare_msg_and_publish(){
-		/*prepare the goto publish message */
-    		double* goto_pos = buzzuav_closures::getgoto();
-		cmd_srv.request.param1 = goto_pos[0];
-    		cmd_srv.request.param2 = goto_pos[1];
-    		cmd_srv.request.param3 = goto_pos[2];
-    		cmd_srv.request.command =  buzzuav_closures::getcmd();
- 		/* flight controller client call*/	
-    		if (mav_client.call(cmd_srv)){ ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); }
-    		else{ ROS_ERROR("Failed to call service 'djicmd'"); }
-    		/*obtain Pay load to be sent*/  
-   		uint64_t* payload_out_ptr= buzz_utility::out_msg_process();
-    		uint64_t  position[3];
-  		/*Appened current position to message*/
-    		memcpy(position, cur_pos, 3*sizeof(uint64_t));
-    		mavros_msgs::Mavlink payload_out;
-    		payload_out.payload64.push_back(position[0]);
-    		payload_out.payload64.push_back(position[1]);
-    		payload_out.payload64.push_back(position[2]);
-    		/*Append Buzz message*/
-    		uint16_t* out = buzz_utility::u64_cvt_u16(payload_out_ptr[0]);  
-    		for(int i=0;i<out[0];i++){
-    			payload_out.payload64.push_back(payload_out_ptr[i]);
-    		}	
-    		/*int i=0;
-		uint64_t message_obt[payload_out.payload64.size()];
-    		for(std::vector<long unsigned int>::const_iterator it = payload_out.payload64.begin();
-			it != payload_out.payload64.end(); ++it){
-			message_obt[i] =(uint64_t) *it;
-         		cout<<" [Debug:] sent message "<<*it<<endl;
-			i++;
-        	}*/
-    		/*publish prepared messages in respective topic*/
-    		payload_pub.publish(payload_out);
-    		delete[] out;
-    		delete[] payload_out_ptr;
-		}
-
-
-	/*Refresh neighbours Position for every ten step*/
-	void roscontroller::maintain_pos(int tim_step){
-		if(timer_step >=10){
-		neighbours_pos_map.clear();
-		timer_step=0;
-		}
-	}
-
-	/*Maintain neighbours position*/
-	void roscontroller::neighbours_pos_maintain(int id, buzz_utility::Pos_struct pos_arr ){
-		map< int, buzz_utility::Pos_struct >::iterator it = neighbours_pos_map.find(id);
-		if(it!=neighbours_pos_map.end())
-		neighbours_pos_map.erase(it);
-		neighbours_pos_map.insert(make_pair(id, pos_arr));
-		}
-
-
-	/*Set the current position of the robot callback*/
-	void roscontroller::set_cur_pos(double latitude,
-			 double longitude,
-			 double altitude){
-		cur_pos [0] =latitude;
-		cur_pos [1] =longitude;
-		cur_pos [2] =altitude;
-
-	}
-
-	/*convert from catresian to spherical coordinate system callback */
-	double* roscontroller::cvt_spherical_coordinates(double neighbours_pos_payload []){
-		double latitude,longitude,altitude;
-		latitude=neighbours_pos_payload[0];
-		longitude = neighbours_pos_payload[1];
-		altitude=neighbours_pos_payload[2];
-		neighbours_pos_payload[0]=sqrt(pow(latitude,2.0)+pow(longitude,2.0)+pow(altitude,2.0));
-		neighbours_pos_payload[1]=atan(longitude/latitude);
-		neighbours_pos_payload[2]=atan((sqrt(pow(latitude,2.0)+pow(longitude,2.0)))/altitude);
-		return neighbours_pos_payload;
-	}
-
-	/*battery status callback*/ 
-	void roscontroller::battery(const mavros_msgs::BatteryStatus::ConstPtr& msg){
-		buzzuav_closures::set_battery(msg->voltage,msg->current,msg->remaining);
-	}
-
-	/*current position callback*/
-	void roscontroller::current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg){
-		set_cur_pos(msg->latitude,msg->longitude,msg->altitude);
-	}
-
-	/*payload callback callback*/
-	void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg){
-		uint64_t message_obt[msg->payload64.size()];
-		int i = 0;
-
-		/* Go throught the obtained payload*/
-		for(i=0;i < (int)msg->payload64.size();i++){
-			message_obt[i] =(uint64_t)msg->payload64[i];
-                	//cout<<"[Debug:] obtaind message "<<message_obt[i]<<endl;
-			//i++;
-        	}
-		/* Extract neighbours position from payload*/
-		double neighbours_pos_payload[3];
-		memcpy(neighbours_pos_payload, message_obt, 3*sizeof(uint64_t));
-		/*Convert obtained position to relative position*/
-		for(i=0;i<3;i++){
-			neighbours_pos_payload[i]=neighbours_pos_payload[i]-cur_pos[i];
-		}
-		double* cvt_neighbours_pos_payload = cvt_spherical_coordinates(neighbours_pos_payload);
-		/*Extract robot id of the neighbour*/
- 		uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t)*(message_obt+3));  
-		/*pass neighbour position to local maintaner*/
-		buzz_utility::Pos_struct n_pos(cvt_neighbours_pos_payload[0],cvt_neighbours_pos_payload[1],cvt_neighbours_pos_payload[2]);
-		neighbours_pos_maintain((int)out[1],n_pos);
-		delete[] out;
-		buzz_utility::in_msg_process((message_obt+3));
-   
-	}
- 
-	/* RC command service */
-	bool roscontroller::rc_callback(mavros_msgs::CommandInt::Request  &req,
-		         mavros_msgs::CommandInt::Response &res){
-		int rc_cmd;
-		if(req.command==oldcmdID)
-		return true;
-		else oldcmdID=req.command;
-   		switch(req.command){
-			case mavros_msgs::CommandCode::NAV_TAKEOFF:
-   				ROS_INFO("RC_call: TAKE OFF!!!!");
-				rc_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF;
-				buzzuav_closures::rc_call(rc_cmd);
-				res.success = true;
-				break;
-			case mavros_msgs::CommandCode::NAV_LAND:
-   				ROS_INFO("RC_Call: LAND!!!!");
-				rc_cmd=mavros_msgs::CommandCode::NAV_LAND;
-				buzzuav_closures::rc_call(rc_cmd);
-				res.success = true;
-				break;
-			case mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH:
-   				ROS_INFO("RC_Call: GO HOME!!!!");
-				rc_cmd=mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
-				buzzuav_closures::rc_call(rc_cmd);
-				res.success = true;
-				break;
-			case mavros_msgs::CommandCode::NAV_WAYPOINT:
-   				ROS_INFO("RC_Call: GO TO!!!! x = %f , y = %f , Z = %f",req.param1,req.param2,req.param3);
-				double rc_goto[3];
-   				rc_goto[0]=req.param1;
-				rc_goto[1]=req.param2;
-				rc_goto[2]=req.param3;
-				buzzuav_closures::set_goto(rc_goto);
-				rc_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
-				buzzuav_closures::rc_call(rc_cmd);
-				res.success = true;
-				break;
-			default:
-				res.success = false;
-				break;
-   		}
-   		return true;
-	}
-
-}
-
-