From 9b25ad53bd5752c1b7905127ee9df3c45e8964b2 Mon Sep 17 00:00:00 2001
From: Vivek Shankar Varadharajan <vivek-shankar.varadharajan@polymtl.ca>
Date: Mon, 10 Oct 2016 20:22:17 -0400
Subject: [PATCH] Class Implementaion and code clean commit

---
 CMakeLists.txt                 |  13 +-
 include/buzz_utility.h         |  44 +++
 include/buzzuav_closures.h     |  58 ++++
 include/roscontroller.h        | 103 +++++++
 {src => include}/uav_utility.h |   0
 package.xml                    |   8 +
 src/buzz_utility.cpp           | 475 ++++++++++++++++-----------------
 src/buzz_utility.h             |  18 +-
 src/buzzasm.h                  |  56 ----
 src/buzzuav_closures.cpp       |  30 ++-
 src/buzzuav_closures.h         |  15 +-
 src/rosbuzz.cpp                | 318 +---------------------
 src/roscontroller.cpp          | 264 ++++++++++++++++++
 src/roscontroller.h            | 101 +++++++
 src/test.bzz                   |   2 +-
 15 files changed, 875 insertions(+), 630 deletions(-)
 create mode 100644 include/buzz_utility.h
 create mode 100644 include/buzzuav_closures.h
 create mode 100644 include/roscontroller.h
 rename {src => include}/uav_utility.h (100%)
 delete mode 100644 src/buzzasm.h
 create mode 100644 src/roscontroller.cpp
 create mode 100644 src/roscontroller.h

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 84f05b5..1e8ead7 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,5 +1,10 @@
 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
@@ -13,18 +18,24 @@ find_package(catkin REQUIRED COMPONENTS
 ###################################
 
 catkin_package(
-#CATKIN_DEPENDS message_runtime
+  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)
diff --git a/include/buzz_utility.h b/include/buzz_utility.h
new file mode 100644
index 0000000..ee85a0f
--- /dev/null
+++ b/include/buzz_utility.h
@@ -0,0 +1,44 @@
+#pragma once
+#include <stdio.h>
+#include "buzz_utility.h"
+#include "buzzuav_closures.h"
+#include <buzz/buzzdebug.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <iostream>
+#include <stdint.h>
+#include <map>
+
+using namespace std;
+namespace buzz_utility{
+
+struct pos_struct
+{
+  double x,y,z;
+  pos_struct(double x,double y,double z):x(x),y(y),z(z){};
+  pos_struct(){}
+};
+
+typedef struct pos_struct Pos_struct ;
+
+uint16_t* u64_cvt_u16(uint64_t u64);
+
+int buzz_listen(const char* type,
+                       int msg_size);
+
+void neighbour_pos_callback(std::map< int,  Pos_struct> neighbours_pos_map);
+
+void in_msg_process(uint64_t* payload);
+
+uint64_t* out_msg_process();
+
+int buzz_script_set(const char* bo_filename,
+                           const char* bdbg_filename, int robot_id);
+void buzz_script_step();
+
+void buzz_script_destroy();
+
+int buzz_script_done();
+
+}
diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h
new file mode 100644
index 0000000..4e24c6b
--- /dev/null
+++ b/include/buzzuav_closures.h
@@ -0,0 +1,58 @@
+#pragma once
+//#ifndef BUZZUAV_CLOSURES_H
+//#define BUZZUAV_CLOSURES_H
+#include <buzz/buzzvm.h>
+#include <stdio.h>
+#include "uav_utility.h"
+#include "mavros_msgs/CommandCode.h"
+#include "ros/ros.h"
+
+namespace buzzuav_closures{
+/*
+ * prextern int() function in Buzz
+ * This function is used to print data from buzz
+ * The command to use in Buzz is buzzros_print takes any available datatype in Buzz
+ */
+int buzzros_print(buzzvm_t vm);
+
+/*
+ * buzzuav_goto(latitude,longitude,altitude) function in Buzz
+ * commands the UAV to go to a position supplied
+ */
+int buzzuav_goto(buzzvm_t vm);
+/* Returns the current command from local variable*/
+int getcmd();
+/*Sets goto position could be used for bypassing*/
+void set_goto(double pos[]);
+/*sets rc requested command */
+void rc_call(int rc_cmd);
+/* sets the battery state */
+void set_battery(float voltage,float current,float remaining);
+/*retuns the current go to position */
+double* getgoto();
+/*
+ * Commands the UAV to takeoff
+ */
+int buzzuav_takeoff(buzzvm_t vm);
+
+/* Commands the UAV to land
+ */
+int buzzuav_land(buzzvm_t vm);
+
+/* Command the UAV to go to home location
+ */
+int buzzuav_gohome(buzzvm_t vm);
+
+/*
+ * Updates battery information in Buzz
+ */
+int buzzuav_update_battery(buzzvm_t vm);
+
+/*
+ * Updates IR information in Buzz
+ * Proximity and ground sensors to do !!!!
+ */
+int buzzuav_update_prox(buzzvm_t vm);
+
+//#endif
+}
diff --git a/include/roscontroller.h b/include/roscontroller.h
new file mode 100644
index 0000000..9b821c6
--- /dev/null
+++ b/include/roscontroller.h
@@ -0,0 +1,103 @@
+#pragma once
+#include "ros/ros.h"
+#include "sensor_msgs/NavSatFix.h"
+#include "mavros_msgs/GlobalPositionTarget.h"
+#include "mavros_msgs/CommandCode.h"
+#include "mavros_msgs/CommandInt.h"
+#include "mavros_msgs/State.h"
+#include "mavros_msgs/BatteryStatus.h"
+#include "mavros_msgs/Mavlink.h"
+#include "sensor_msgs/NavSatStatus.h"
+#include <sstream>
+#include <buzz/buzzasm.h>
+#include "buzzuav_closures.h"
+#include "buzz_utility.h"
+#include "uav_utility.h"
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+#include <signal.h>
+#include <ostream>
+#include <map>
+
+using namespace std;
+
+namespace rosbzz_node{
+
+
+class roscontroller{
+
+public:
+	roscontroller();
+	~roscontroller();
+	//void RosControllerInit();
+	void RosControllerRun();
+
+private:
+        
+ 	double cur_pos[3];
+	uint64_t payload;
+	std::map< int,  buzz_utility::Pos_struct> neighbours_pos_map;
+	int timer_step=0;
+	int robot_id=0;
+        int oldcmdID=0;
+	int rc_cmd;
+	std::string bzzfile_name, fcclient_name, rcservice_name; //, rcclient;
+	bool rcclient;
+	ros::ServiceClient mav_client;
+	ros::Publisher payload_pub;
+	ros::ServiceServer service;
+	ros::Subscriber current_position_sub;
+	ros::Subscriber battery_sub;
+	ros::Subscriber payload_sub;
+	/*Create node Handler*/
+		ros::NodeHandle n_c;
+	/*Commands for flight controller*/
+  		mavros_msgs::CommandInt cmd_srv;	
+  	/* The bytecode filename */
+   		char* bcfname = (char*)"../catkin_ws/src/rosbuzz/src/out.bo"; //argv[1];
+   		/* The debugging information file name */
+   		char* dbgfname = (char*)"../catkin_ws/src/rosbuzz/src/out.bdbg"; //argv[2];
+
+	void Initialize_pub_sub();
+
+	/*Obtain data from ros parameter server*/	
+	void Rosparameters_get();
+
+	void Compile_bzz();
+
+	/*Prepare messages and publish*/
+	inline void prepare_msg_and_publish();
+
+	  	
+	/*Refresh neighbours Position for every ten step*/
+	inline void maintain_pos(int tim_step);
+
+	/*Maintain neighbours position*/
+	inline void neighbours_pos_maintain(int id, buzz_utility::Pos_struct pos_arr );
+
+        /*Set the current position of the robot callback*/
+	inline void set_cur_pos(double latitude,
+			 double longitude,
+			 double altitude);
+	/*convert from catresian to spherical coordinate system callback */
+	inline double* cvt_spherical_coordinates(double neighbours_pos_payload []);
+	
+	/*battery status callback*/ 
+	inline void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg);
+
+	/*current position callback*/
+	inline void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg);
+
+	/*payload callback callback*/
+	inline void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg);
+
+	/* RC commands service */
+	inline bool rc_callback(mavros_msgs::CommandInt::Request  &req,
+		         mavros_msgs::CommandInt::Response &res);
+
+
+
+};
+
+}
diff --git a/src/uav_utility.h b/include/uav_utility.h
similarity index 100%
rename from src/uav_utility.h
rename to include/uav_utility.h
diff --git a/package.xml b/package.xml
index 2a425c7..cd839c4 100644
--- a/package.xml
+++ b/package.xml
@@ -40,6 +40,14 @@
   <!-- Use test_depend for packages you need only for testing: -->
   <!--   <test_depend>gtest</test_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <run_depend>roscpp</run_depend>
+  <build_depend>std_msgs</build_depend>
+  <run_depend>std_msgs</run_depend>
+  <build_depend>mavros_msgs</build_depend>
+  <run_depend>mavros_msgs</run_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <run_depend>sensor_msgs</run_depend>
   <!-- <build_depend>message_generation</build_depend> -->
   <!-- <run_depend>message_runtime</run_depend> -->
 
diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp
index 61697d7..198e5c5 100644
--- a/src/buzz_utility.cpp
+++ b/src/buzz_utility.cpp
@@ -5,266 +5,257 @@
  *  @author    Vivek Shankar Varadharajan
  *  @copyright 2016 MistLab. All rights reserved.
  */
-#define _GNU_SOURCE
-#include <stdio.h>
+
 #include "buzz_utility.h"
-#include "buzzuav_closures.h"
-#include <buzz/buzzdebug.h>
-#include <stdlib.h>
-#include <string.h>
-#include <unistd.h>
-#include <iostream>
-#include <map>
-using namespace std;
-/****************************************/
-/****************************************/
 
-static buzzvm_t    VM              = 0;
-static char*       BO_FNAME        = 0;
-static uint8_t*    BO_BUF          = 0;
-static buzzdebug_t DBG_INFO        = 0;
-static int         MSG_SIZE        = 32;
-static int         TCP_LIST_STREAM = -1;
-static int         TCP_COMM_STREAM = -1;
-static uint8_t*    STREAM_SEND_BUF = NULL;
+namespace buzz_utility{
 
+	/****************************************/
+	/****************************************/
 
-/****************************************/
-/*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;
-} 
+	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*/
-/***************************************************/
+	/***************************************************/
+	/*Appends obtained messages to buzz in message Queue*/
+	/***************************************************/
 
-void in_msg_process(uint64_t* payload){
+	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);
+   		/* 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);
+   		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);
+		/* 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*/
-/***************************************************/
+   		/* 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;
-      }
+   	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 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);
+      			/* 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);
+      			/* 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;   
+   		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];
+   		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*/
-/****************************************/
+   		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;
-}
+	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*/
-/****************************************/
+	/****************************************/
+	/*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, buzzros_print));
-   buzzvm_gstore(VM);
-   buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_goto", 1));
-   buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_goto));
-   buzzvm_gstore(VM);
-   buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_takeoff", 1));
-   buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_takeoff));
-   buzzvm_gstore(VM);
-   buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_gohome", 1));
-   buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_gohome));
-   buzzvm_gstore(VM);
-   buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_land", 1));
-   buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_land));
-   buzzvm_gstore(VM);
-   return BUZZVM_STATE_READY;
-}
+	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*/
-/****************************************/
+	/****************************************/
+	/*Sets the .bzz and .bdbg file*/
+	/****************************************/
 
-int buzz_script_set(const char* bo_filename,
-                    const char* bdbg_filename, int robot_id) {
-   /* 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;
-}
+	int buzz_script_set(const char* bo_filename,
+	                    const char* bdbg_filename, int robot_id) {
+		/* 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*/
-/****************************************/
+	/****************************************/
+	/*Swarm struct*/
+	/****************************************/
 
 struct buzzswarm_elem_s {
    buzzdarray_t swarms;
@@ -307,8 +298,8 @@ void buzz_script_step() {
    /*
     * Update sensors
     */
-   buzzuav_update_battery(VM);
-   buzzuav_update_prox(VM);
+   buzzuav_closures::buzzuav_update_battery(VM);
+   buzzuav_closures::buzzuav_update_prox(VM);
    /*
     * Call Buzz step() function
     */
@@ -351,6 +342,10 @@ void buzz_script_destroy() {
    fprintf(stdout, "Script execution stopped.\n");
 }
 
+
+/****************************************/
+/****************************************/
+
 /****************************************/
 /*Execution completed*/
 /****************************************/
@@ -359,7 +354,7 @@ int buzz_script_done() {
    return VM->state != BUZZVM_STATE_READY;
 }
 
-/****************************************/
-/****************************************/
+}
+
 
 
diff --git a/src/buzz_utility.h b/src/buzz_utility.h
index 256f7d3..ee85a0f 100644
--- a/src/buzz_utility.h
+++ b/src/buzz_utility.h
@@ -1,13 +1,25 @@
-#ifndef BUZZ_UTILITY_H
-#define BUZZ_UTILITY_H
+#pragma once
+#include <stdio.h>
+#include "buzz_utility.h"
+#include "buzzuav_closures.h"
+#include <buzz/buzzdebug.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <iostream>
 #include <stdint.h>
 #include <map>
+
+using namespace std;
+namespace buzz_utility{
+
 struct pos_struct
 {
   double x,y,z;
   pos_struct(double x,double y,double z):x(x),y(y),z(z){};
   pos_struct(){}
 };
+
 typedef struct pos_struct Pos_struct ;
 
 uint16_t* u64_cvt_u16(uint64_t u64);
@@ -29,4 +41,4 @@ void buzz_script_destroy();
 
 int buzz_script_done();
 
-#endif
+}
diff --git a/src/buzzasm.h b/src/buzzasm.h
deleted file mode 100644
index 69e71f6..0000000
--- a/src/buzzasm.h
+++ /dev/null
@@ -1,56 +0,0 @@
-#ifndef BUZZASM_H
-#define BUZZASM_H
-
-#include <buzz/buzzvm.h>
-#include <buzz/buzzdebug.h>
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-   /*
-    * Compiles an assembly file into bytecode.
-    * @param fname The file name where the code is located.
-    * @param buf The buffer in which the bytecode will be stored. Created internally.
-    * @param size The size of the bytecode buffer.
-    * @param dbg The debug data structure to fill into. Created internally.
-    * @return 0 if no error occurred, 1 for I/O error, 2 for compilation error.
-    */
-   extern int buzz_asm(const char* fname,
-                       uint8_t** buf,
-                       uint32_t* size,
-                       buzzdebug_t* dbg);
-
-   /*
-    * Decompiles bytecode into an assembly file.
-    * @param buf The buffer in which the bytecode is stored.
-    * @param size The size of the bytecode buffer.
-    * @param dbg The debug data structure.
-    * @param fname The file name where the assembly will be written.
-    * @return 0 if no error occurred, 1 for I/O error, 2 for decompilation error.
-    */
-   extern int buzz_deasm(const uint8_t* buf,
-                         uint32_t size,
-                         buzzdebug_t dbg,
-                         const char* fname);
-
-   /*
-    * Decompiles the bytecode of a single instruction.
-    * This function writes the decompiled instruction into a new string pointed to by
-    * *buf. When you're done with it, you must free it.
-    * NOTE: no sanity check is performed to make sure that this function does not
-    * read beyond the bcode buffer size.
-    * @param bcode The buffer in which the bytecode is stored.
-    * @param off The offset at which decompilation must occur.
-    * @param buf The bytecode where the 
-    * @return 1 if no error occurred, 0 for decompilation error.
-    */
-   extern int buzz_instruction_deasm(const uint8_t* bcode,
-                                     uint32_t off,
-                                     char** buf);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif
diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp
index 7cf051f..033edc7 100644
--- a/src/buzzuav_closures.cpp
+++ b/src/buzzuav_closures.cpp
@@ -6,15 +6,12 @@
  *  @copyright 2016 MistLab. All rights reserved.
  */
 //#define _GNU_SOURCE
-#include <stdio.h>
 #include "buzzuav_closures.h"
-#include "uav_utility.h"
-#include "mavros_msgs/CommandCode.h"
-#include "ros/ros.h"
-
-double goto_pos[3];
-float batt[3];
-int cur_cmd;
+namespace buzzuav_closures{
+static double goto_pos[3];
+static float batt[3];
+static int cur_cmd;
+static int rc_cmd=0;
 /****************************************/
 /****************************************/
 
@@ -84,7 +81,12 @@ return goto_pos;
 }
 /******************************/
 int getcmd(){
+if(rc_cmd==0) return cur_cmd;
+else {
+cur_cmd=rc_cmd;
+rc_cmd=0;
 return cur_cmd;
+} 
 }
 
 void set_goto(double pos[]){
@@ -94,8 +96,8 @@ goto_pos[2]=pos[2];
     
 }
 
-void rc_call(int rc_cmd){
-cur_cmd=rc_cmd;
+void rc_call(int rc_cmd_in){
+rc_cmd=rc_cmd_in;
 }
 
 /****************************************/
@@ -128,7 +130,7 @@ void set_battery(float voltage,float current,float remaining){
 /****************************************/
 
 int buzzuav_update_battery(buzzvm_t vm) {
-   static char BATTERY_BUF[256];
+   //static char BATTERY_BUF[256];
    buzzvm_pushs(vm, buzzvm_string_register(vm, "battery", 1));
    buzzvm_pusht(vm);
    buzzvm_dup(vm);
@@ -172,9 +174,11 @@ int buzzuav_update_prox(buzzvm_t vm) {
       buzzvm_pushi(vm, (PROXIMITY_BUF[i*2] | PROXIMITY_BUF[i*2+1] << 8));
       buzzvm_tput(vm);
    }
-   buzzvm_gstore(vm);
-   return vm->state;*/
+   buzzvm_gstore(vm);*/
+   return vm->state;
 }
 
 /****************************************/
 /****************************************/
+
+}
diff --git a/src/buzzuav_closures.h b/src/buzzuav_closures.h
index 33671db..6e3f805 100644
--- a/src/buzzuav_closures.h
+++ b/src/buzzuav_closures.h
@@ -1,7 +1,13 @@
-#ifndef BUZZUAV_CLOSURES_H
-#define BUZZUAV_CLOSURES_H
-
+#pragma once
+//#ifndef BUZZUAV_CLOSURES_H
+//#define BUZZUAV_CLOSURES_H
 #include <buzz/buzzvm.h>
+#include <stdio.h>
+#include "uav_utility.h"
+#include "mavros_msgs/CommandCode.h"
+#include "ros/ros.h"
+
+namespace buzzuav_closures{
 
 /*
  * prextern int() function in Buzz
@@ -49,4 +55,5 @@ int buzzuav_update_battery(buzzvm_t vm);
  */
 int buzzuav_update_prox(buzzvm_t vm);
 
-#endif
+//#endif
+}
diff --git a/src/rosbuzz.cpp b/src/rosbuzz.cpp
index 1877067..efa02dd 100644
--- a/src/rosbuzz.cpp
+++ b/src/rosbuzz.cpp
@@ -6,325 +6,19 @@
  *  @copyright 2016 MistLab. All rights reserved.
  */
 
-#include "ros/ros.h"
-#include "sensor_msgs/NavSatFix.h"
-#include "mavros_msgs/GlobalPositionTarget.h"
-#include "mavros_msgs/CommandCode.h"
-#include "mavros_msgs/CommandInt.h"
-#include "mavros_msgs/State.h"
-#include "mavros_msgs/BatteryStatus.h"
-#include "mavros_msgs/Mavlink.h"
-#include "sensor_msgs/NavSatStatus.h"
-#include <sstream>
-#include <buzz/buzzasm.h>
-#include "buzzuav_closures.h"
-#include "buzz_utility.h"
-#include "uav_utility.h"
-#include <stdlib.h>
-#include <string.h>
-#include <math.h>
-#include <signal.h>
-#include <ostream>
-#include <map>
-using namespace std;
-
+#include "roscontroller.h"
 
 /**
  * This program implements Buzz node in ros using mavros_msgs.
  */
-
-static int done = 0;
-static double cur_pos[3];
-static uint64_t payload;
-static std::map< int,  Pos_struct> neighbours_pos_map;
-static int timer_step=0;
-static int robot_id=0;
-
-/*Refresh neighbours Position for every ten step*/
-void maintain_pos(int tim_step){
-if(timer_step >=10){
-neighbours_pos_map.clear();
-timer_step=0;
-}
-}
-
-/*Maintain neighbours position*/
-void neighbours_pos_maintain(int id, Pos_struct pos_arr ){
-map< int, 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));
-}
-
-/*print usage information not needed at the moment*/
-void usage(const char* path, int status) {
-  
-}
-
-/*Set the current position of the robot callback*/
-void 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* 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 battery(const mavros_msgs::BatteryStatus::ConstPtr& msg)
-{
-set_battery(msg->voltage,msg->current,msg->remaining);
-}
-
-/*current position callback*/
-void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg)
-{
-set_cur_pos(msg->latitude,msg->longitude,msg->altitude);
-}
-
-/*payload callback callback*/
-void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg)
-{
-uint64_t message_obt[msg->payload64.size()];
-int i = 0;
-
-	/* Go throught the obtained payload*/
-	for(std::vector<long unsigned int>::const_iterator it = msg->payload64.begin(); it != msg->payload64.end(); ++it)
-	{
-		message_obt[i] =(uint64_t) *it;
-                //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 = u64_cvt_u16((uint64_t)*(message_obt+3));  
-/*pass neighbour position to local maintaner*/
-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;
-in_msg_process((message_obt+3));
-   
-}
- 
-/*Dji RC commands service */
-int oldcmdID=0;
-int rc_cmd;
-bool rc_callback(mavros_msgs::CommandInt::Request  &req,
-         mavros_msgs::CommandInt::Response &res){
-
-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;
-		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;
-		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;
-		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;
-                
-		set_goto(rc_goto);
-		rc_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
-		rc_call(rc_cmd);
-		res.success = true;
-		break;
-	default:
-		res.success = false;
-		break;
-   }
-   return true;
-}
-
-
-/*control c handler callback*/
-static void ctrlc_handler(int sig) {
-   done = 1;
-}
-
-
-
+	
 int main(int argc, char **argv)
 {
-
-  ros::ServiceServer service;
-  /*initiate rosBuzz*/
+  /*Initialize rosBuzz node*/
   ros::init(argc, argv, "rosBuzz"); 
-  ROS_INFO("Buzz_node");
- 
-  /*Create node Handler*/
-  ros::NodeHandle n_c;
-  std::string bzzfile_name, fcclient_name, rcservice_name; //, rcclient;
-  bool rcclient;
-  /*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, rc_callback);
-    	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);
-
-  /*subscribers*/
-  ros::Subscriber current_position_sub = n_c.subscribe("current_pos", 1000, current_pos);
-
-  ros::Subscriber battery_sub = n_c.subscribe("battery_state", 1000, battery);
-
-  ros::Subscriber payload_sub = n_c.subscribe("inMavlink", 1000, payload_obt);
-
-
-  /*publishers*/
-  ros::Publisher payload_pub = n_c.advertise<mavros_msgs::Mavlink>("outMavlink", 1000);
-
-  /* Clients*/
-  ros::ServiceClient mav_client = n_c.serviceClient<mavros_msgs::CommandInt>(fcclient_name);
-  cout<< " rc client name"<<rcservice_name;
-  
-  
-  /*loop rate of ros*/
-  ros::Rate loop_rate(1);
-
-   /*Compile the buzz code .bzz to .bo*/
-   stringstream bzzfile_in_compile;
-   bzzfile_in_compile << "bzzparse "<<bzzfile_name<<" ../catkin_ws/src/rosbuzz/src/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());
-   system("bzzasm ../catkin_ws/src/rosbuzz/src/out.basm ../catkin_ws/src/rosbuzz/src/out.bo ../catkin_ws/src/rosbuzz/src/out.bdbg");
-
-   /* The bytecode filename */
-   char* bcfname = (char*)"../catkin_ws/src/rosbuzz/src/out.bo"; //argv[1];
-   /* The debugging information file name */
-   char* dbgfname = (char*)"../catkin_ws/src/rosbuzz/src/out.bdbg"; //argv[2];
-   /* Set CTRL-C handler */
-   signal(SIGTERM, ctrlc_handler);
-   signal(SIGINT, ctrlc_handler);
-
-   
-   /* Set the Buzz bytecode */
-   if(buzz_script_set(bcfname, dbgfname,robot_id)) {
-   fprintf(stdout, "Bytecode file found and set\n");
-
-  /*Commands for dji flight controller*/
-  mavros_msgs::CommandInt cmd_srv;
-  
-  int count = 0;
-  while (ros::ok() && !done && !buzz_script_done())
-  {
-
-      /*Update neighbors position inside Buzz*/
-      neighbour_pos_callback(neighbours_pos_map);
-  
-      /* Main loop */
-      buzz_script_step();
- 
-    /*prepare the goto publish message */
-    
-    double* goto_pos = 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 =  getcmd();
-    /* diji 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= 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 = 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;
-    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;
-    /*run once*/
-    ros::spinOnce();
-    /*sleep for the mentioned loop rate*/
-    loop_rate.sleep();
-    ++count;
-    timer_step+=1;
-    maintain_pos(timer_step);
-   
-  }
-  /* Cleanup */
-   buzz_script_destroy();
-
- /* Stop the robot */
-   uav_done();
-   
-}
-  /* All done */
-   return 0;
-
+  rosbzz_node::roscontroller RosController;
+  RosController.RosControllerRun(); 
+  return 0;
 }
 
 
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
new file mode 100644
index 0000000..ac38094
--- /dev/null
+++ b/src/roscontroller.cpp
@@ -0,0 +1,264 @@
+
+#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, dbgfname,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;
+   		bzzfile_in_compile << "bzzparse "<<bzzfile_name<<" ../catkin_ws/src/rosbuzz/src/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());
+   		system("bzzasm ../catkin_ws/src/rosbuzz/src/out.basm ../catkin_ws/src/rosbuzz/src/out.bo ../catkin_ws/src/rosbuzz/src/out.bdbg");
+	}
+	
+	inline 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*/
+	inline void roscontroller::maintain_pos(int tim_step){
+		if(timer_step >=10){
+		neighbours_pos_map.clear();
+		timer_step=0;
+		}
+	}
+
+	/*Maintain neighbours position*/
+	inline 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*/
+	inline 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 */
+	inline 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*/ 
+	inline void roscontroller::battery(const mavros_msgs::BatteryStatus::ConstPtr& msg){
+		buzzuav_closures::set_battery(msg->voltage,msg->current,msg->remaining);
+	}
+
+	/*current position callback*/
+	inline void roscontroller::current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg){
+		set_cur_pos(msg->latitude,msg->longitude,msg->altitude);
+	}
+
+	/*payload callback callback*/
+	inline 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(std::vector<long unsigned int>::const_iterator it = msg->payload64.begin(); it != msg->payload64.end(); ++it){
+			message_obt[i] =(uint64_t) *it;
+                	//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 */
+	inline 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;
+	}
+
+}
+
+
diff --git a/src/roscontroller.h b/src/roscontroller.h
new file mode 100644
index 0000000..804d2b6
--- /dev/null
+++ b/src/roscontroller.h
@@ -0,0 +1,101 @@
+#pragma once
+#include "ros/ros.h"
+#include "sensor_msgs/NavSatFix.h"
+#include "mavros_msgs/GlobalPositionTarget.h"
+#include "mavros_msgs/CommandCode.h"
+#include "mavros_msgs/CommandInt.h"
+#include "mavros_msgs/State.h"
+#include "mavros_msgs/BatteryStatus.h"
+#include "mavros_msgs/Mavlink.h"
+#include "sensor_msgs/NavSatStatus.h"
+#include <sstream>
+#include <buzz/buzzasm.h>
+#include "buzzuav_closures.h"
+#include "buzz_utility.h"
+#include "uav_utility.h"
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+#include <signal.h>
+#include <ostream>
+#include <map>
+
+using namespace std;
+
+namespace rosbzz_node{
+
+
+class roscontroller{
+
+public:
+	roscontroller();
+	~roscontroller();
+	void RosControllerRun();
+
+private:
+        
+ 	double cur_pos[3];
+	uint64_t payload;
+	std::map< int,  buzz_utility::Pos_struct> neighbours_pos_map;
+	int timer_step=0;
+	int robot_id=0;
+        int oldcmdID=0;
+	std::string bzzfile_name, fcclient_name, rcservice_name; //, rcclient;
+	bool rcclient;
+	ros::ServiceClient mav_client;
+	ros::Publisher payload_pub;
+	ros::ServiceServer service;
+	ros::Subscriber current_position_sub;
+	ros::Subscriber battery_sub;
+	ros::Subscriber payload_sub;
+	/*Create node Handler*/
+		ros::NodeHandle n_c;
+	/*Commands for flight controller*/
+  		mavros_msgs::CommandInt cmd_srv;	
+  	/* The bytecode filename */
+   		char* bcfname = (char*)"../catkin_ws/src/rosbuzz/src/out.bo"; //argv[1];
+   		/* The debugging information file name */
+   		char* dbgfname = (char*)"../catkin_ws/src/rosbuzz/src/out.bdbg"; //argv[2];
+
+	void Initialize_pub_sub();
+
+	/*Obtain data from ros parameter server*/	
+	void Rosparameters_get();
+
+	void Compile_bzz();
+
+	/*Prepare messages and publish*/
+	inline void prepare_msg_and_publish();
+
+	  	
+	/*Refresh neighbours Position for every ten step*/
+	inline void maintain_pos(int tim_step);
+
+	/*Maintain neighbours position*/
+	inline void neighbours_pos_maintain(int id, buzz_utility::Pos_struct pos_arr );
+
+        /*Set the current position of the robot callback*/
+	inline void set_cur_pos(double latitude,
+			 double longitude,
+			 double altitude);
+	/*convert from catresian to spherical coordinate system callback */
+	inline double* cvt_spherical_coordinates(double neighbours_pos_payload []);
+	
+	/*battery status callback*/ 
+	inline void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg);
+
+	/*current position callback*/
+	inline void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg);
+
+	/*payload callback callback*/
+	inline void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg);
+
+	/* RC commands service */
+	inline bool rc_callback(mavros_msgs::CommandInt::Request  &req,
+		         mavros_msgs::CommandInt::Response &res);
+
+
+
+};
+
+}
diff --git a/src/test.bzz b/src/test.bzz
index cf807b2..c24846e 100644
--- a/src/test.bzz
+++ b/src/test.bzz
@@ -17,7 +17,7 @@ neighbors.foreach(
         "azimuth   = ", data.azimuth, ", ",
         "elevation = ", data.elevation) })
 if(i==1){
-s.exec(function() {uav_takeoff()})
+s.exec(function() {uav_gohome()})
 }
 else{
 s.exec(function() {uav_goto(1.1234,2.2345,3.3456)})