From b5b0d18a73d352941051d7af401e05ae044eaab2 Mon Sep 17 00:00:00 2001
From: isvogor <isvogor@gmail.com>
Date: Mon, 20 Feb 2017 17:59:35 -0500
Subject: [PATCH 01/12] minor updates in controller

---
 include/roscontroller.h |  7 ++++++-
 src/roscontroller.cpp   | 20 +++++++++++++++++---
 src/test1.bzz           |  2 +-
 3 files changed, 24 insertions(+), 5 deletions(-)

diff --git a/include/roscontroller.h b/include/roscontroller.h
index 1a1c30a..7ce69d8 100644
--- a/include/roscontroller.h
+++ b/include/roscontroller.h
@@ -148,9 +148,14 @@ private:
 
 	void Arm();
 
-	void SetMode();
+	void SetMode(std::string mode, int delay_miliseconds);
+
+	void SetModeAsync(std::string mode, int delay_miliseconds);
+
+	//void SetModeAsync(std::string mode, int delay);
 
 	void Subscribe(ros::NodeHandle n_c);
+
 };
 
 }
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index 1b19a30..3160f09 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -1,4 +1,5 @@
 #include "roscontroller.h"
+#include <thread>
 
 namespace rosbzz_node{
 
@@ -236,10 +237,15 @@ namespace rosbzz_node{
 		}
 	}
 
-	void roscontroller::SetMode(){
+	void roscontroller::SetMode(std::string mode, int delay_miliseconds){
+		// wait if necessary
+		if (delay_miliseconds > 0){
+			std::this_thread::sleep_for( std::chrono::milliseconds ( delay_miliseconds ) );
+		}
+		// set mode
 		mavros_msgs::SetMode set_mode_message;
 		set_mode_message.request.base_mode = 0;
-		set_mode_message.request.custom_mode = "GUIDED"; // shit!
+		set_mode_message.request.custom_mode = mode;
 		if(mode_client.call(set_mode_message)) {
 			ROS_INFO("Service called!");
 		} else {
@@ -247,6 +253,12 @@ namespace rosbzz_node{
 		}
 	}
 
+	//todo: this
+	void roscontroller::SetModeAsync(std::string mode, int delay_miliseconds){
+		std::thread([&](){SetMode(mode, delay_miliseconds);}).detach();
+		cout << "Async call called... " <<endl;
+	}
+
 
 	/*Prepare messages for each step and publish*/
 	/*******************************************************************************************************/
@@ -695,7 +707,9 @@ namespace rosbzz_node{
    				ROS_INFO("RC_call: TAKE OFF!!!!");
 				rc_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF;
 				/* arming */
-				SetMode();
+				SetMode("GUIDED", 0);
+				cout<< "this..."<<endl;
+				SetModeAsync("LAND", 5000);
 				Arm();
 				buzzuav_closures::rc_call(rc_cmd);
 				res.success = true;
diff --git a/src/test1.bzz b/src/test1.bzz
index 3e9d1fb..8edac26 100644
--- a/src/test1.bzz
+++ b/src/test1.bzz
@@ -1,7 +1,7 @@
 
 # We need this for 2D vectors
 # Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
-include "/home/ubuntu/buzz/src/include/vec2.bzz"
+include "/home/ivan/dev/buzz/src/include/vec2.bzz"
 ####################################################################################################
 # Updater related
 # This should be here for the updater to work, changing position of code will crash the updater

From f01804b5a32e8a1fe6d81ecba47bb161a8d9c4d5 Mon Sep 17 00:00:00 2001
From: isvogor <isvogor@gmail.com>
Date: Tue, 21 Feb 2017 16:09:16 -0500
Subject: [PATCH 02/12] first waypoint implementation - sitl works

---
 include/roscontroller.h |  6 ++++++
 launch/rosbuzz.launch   |  2 +-
 src/roscontroller.cpp   | 37 ++++++++++++++++++++++++++++++++-----
 src/test1.bzz           |  2 +-
 4 files changed, 40 insertions(+), 7 deletions(-)

diff --git a/include/roscontroller.h b/include/roscontroller.h
index 7ce69d8..fbf3d6d 100644
--- a/include/roscontroller.h
+++ b/include/roscontroller.h
@@ -11,6 +11,8 @@
 #include "mavros_msgs/State.h"
 #include "mavros_msgs/BatteryStatus.h"
 #include "mavros_msgs/Mavlink.h"
+#include "mavros_msgs/WaypointPush.h"
+#include "mavros_msgs/Waypoint.h"
 #include "sensor_msgs/NavSatStatus.h"
 #include <sensor_msgs/LaserScan.h>
 #include <rosbuzz/neigh_pos.h>
@@ -81,6 +83,8 @@ private:
   	mavros_msgs::SetMode m_cmdSetMode;
   	ros::ServiceClient mode_client;
 
+  	ros::ServiceClient mission_client;
+
 	void Initialize_pub_sub(ros::NodeHandle n_c);
 
 	/*Obtain data from ros parameter server*/	
@@ -156,6 +160,8 @@ private:
 
 	void Subscribe(ros::NodeHandle n_c);
 
+	void WaypointMissionSetup();
+
 };
 
 }
diff --git a/launch/rosbuzz.launch b/launch/rosbuzz.launch
index eb901e7..47f5a0f 100644
--- a/launch/rosbuzz.launch
+++ b/launch/rosbuzz.launch
@@ -3,7 +3,7 @@
 <launch>
 		<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >	
 		<rosparam file="$(env ROS_WS)/src/ROSBuzz/launch/launch_config/$(env ROBOT).yaml"/>
-		<param name="bzzfile_name" value="$(env ROS_WS)/src/ROSBuzz/src/test1.bzz" />
+		<param name="bzzfile_name" value="$(env ROS_WS)/src/ROSBuzz/src/testalone.bzz" />
         <param name="rcclient" value="true" /> 
 		<param name="rcservice_name" value="/buzzcmd" />
 		<param name="in_payload" value="/inMavlink"/>
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index 3160f09..e0647df 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -177,6 +177,7 @@ namespace rosbzz_node{
 		neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("/neighbours_pos",1000);	
 		/* Service Clients*/
 		arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient);
+		mission_client = n_c.serviceClient<mavros_msgs::WaypointPush>("/mavros/mission/push");
 		mode_client =  n_c.serviceClient<mavros_msgs::SetMode>(modeclient);
 		mav_client = n_c.serviceClient<mavros_msgs::CommandLong>(fcclient_name);
 
@@ -237,6 +238,29 @@ namespace rosbzz_node{
 		}
 	}
 
+	void roscontroller::WaypointMissionSetup(){
+		mavros_msgs::WaypointPush srv;
+		mavros_msgs::Waypoint waypoint;
+
+		// prepare waypoint mission package
+		waypoint.frame = mavros_msgs::Waypoint::FRAME_GLOBAL;
+		waypoint.command = mavros_msgs::CommandCode::NAV_WAYPOINT;
+		waypoint.is_current = 2;	// IMPORTANT: goto is no longer used, so instead, use magic number 2 for is_current parameter
+		waypoint.autocontinue = 0;
+		waypoint.x_lat = 45.507730f;
+		waypoint.y_long = -73.613961f;
+		//45.507730, -73.613961
+		waypoint.z_alt = 10;
+
+		srv.request.waypoints.push_back(waypoint);
+		if(mission_client.call(srv)){
+			ROS_INFO("Mission service called!");
+		} else {
+			ROS_INFO("Mission service failed!");
+		}
+
+	}
+
 	void roscontroller::SetMode(std::string mode, int delay_miliseconds){
 		// wait if necessary
 		if (delay_miliseconds > 0){
@@ -588,7 +612,7 @@ namespace rosbzz_node{
 			buzzuav_closures::flight_status_update(1);
 		else if (msg->mode == "LAND")
 			buzzuav_closures::flight_status_update(4);
-		else // ground standby = LOITER?
+		else
 			buzzuav_closures::flight_status_update(1);//?
 	}
 
@@ -708,8 +732,9 @@ namespace rosbzz_node{
 				rc_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF;
 				/* arming */
 				SetMode("GUIDED", 0);
-				cout<< "this..."<<endl;
-				SetModeAsync("LAND", 5000);
+				//SetMode("LOITER", 0);
+				cout << "this..." << endl;
+				//SetModeAsync("GUIDED", 5000);
 				Arm();
 				buzzuav_closures::rc_call(rc_cmd);
 				res.success = true;
@@ -728,11 +753,13 @@ namespace rosbzz_node{
 				break;
 			case mavros_msgs::CommandCode::NAV_WAYPOINT:
    				ROS_INFO("RC_Call: GO TO!!!! ");
-				double rc_goto[3];
+
+   				WaypointMissionSetup();
+
+   				double rc_goto[3];
 				rc_goto[0] = req.param5;
 				rc_goto[1] = req.param6;
 				rc_goto[2] = req.param7;
-
 				buzzuav_closures::rc_set_goto(rc_goto);
 				rc_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
 				buzzuav_closures::rc_call(rc_cmd);
diff --git a/src/test1.bzz b/src/test1.bzz
index 8edac26..3e9d1fb 100644
--- a/src/test1.bzz
+++ b/src/test1.bzz
@@ -1,7 +1,7 @@
 
 # We need this for 2D vectors
 # Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
-include "/home/ivan/dev/buzz/src/include/vec2.bzz"
+include "/home/ubuntu/buzz/src/include/vec2.bzz"
 ####################################################################################################
 # Updater related
 # This should be here for the updater to work, changing position of code will crash the updater

From 24defade59acbe58ffeabc79d6e8f472ccf568f0 Mon Sep 17 00:00:00 2001
From: isvogor <isvogor@gmail.com>
Date: Tue, 21 Feb 2017 19:20:38 -0500
Subject: [PATCH 03/12] for testing #2 - waypoints included

---
 include/roscontroller.h |    2 +-
 src/buzz_utility.cpp    | 1062 +++++++++++++++++++--------------------
 src/roscontroller.cpp   |   31 +-
 3 files changed, 549 insertions(+), 546 deletions(-)

diff --git a/include/roscontroller.h b/include/roscontroller.h
index fbf3d6d..c9352ba 100644
--- a/include/roscontroller.h
+++ b/include/roscontroller.h
@@ -160,7 +160,7 @@ private:
 
 	void Subscribe(ros::NodeHandle n_c);
 
-	void WaypointMissionSetup();
+	void WaypointMissionSetup(float lat, float lng, float alg);
 
 };
 
diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp
index 741d24b..4ec0ff3 100644
--- a/src/buzz_utility.cpp
+++ b/src/buzz_utility.cpp
@@ -8,539 +8,533 @@
 
 #include <buzz_utility.h>
 
-namespace buzz_utility{
+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 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 
-	
-	
-	/****************************************/
-	/*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);
-    		}
+static buzzvm_t VM = 0;
+static char* BO_FNAME = 0;
+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
+
+
+/****************************************/
+/*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);
 	}
-	/**************************************************************************/
-	/*Deserializes uint64_t into 4 uint16_t, freeing out is left to the user  */
-	/**************************************************************************/
-	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] <<"  ";
+}
+/**************************************************************************/
+/*Deserializes uint64_t into 4 uint16_t, freeing out is left to the user  */
+/**************************************************************************/
+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*/
+/***************************************************/
 
-	/*******************************************************************************************************************/
-	/* Message format of payload (Each slot is uint64_t)						                   */
-	/* _______________________________________________________________________________________________________________ */
-	/*|					        		             |			                  |*/
-	/*|Size in Uint64_t(but size is Uint16_t)|robot_id|Update msg size|Update msg|Update msgs+Buzz_msgs with size.....|*/
-	/*|__________________________________________________________________________|____________________________________|*/
-	/*******************************************************************************************************************/
+/*******************************************************************************************************************/
+/* Message format of payload (Each slot is uint64_t)						                   */
+/* _______________________________________________________________________________________________________________ */
+/*|					        		             |			                  |*/
+/*|Size in Uint64_t(but size is Uint16_t)|robot_id|Update msg size|Update msg|Update msgs+Buzz_msgs with size.....|*/
+/*|__________________________________________________________________________|____________________________________|*/
+/*******************************************************************************************************************/
 
-	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]);
-		/*Size is at first 2 bytes*/
-   		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 and robot id read*/
-   		size_t tot = sizeof(uint32_t);
-      		/*for(int i=0;i<data[0];i++){
-			uint16_t* out = u64_cvt_u16(payload[i]);
-			for(int k=0;k<4;k++){
-				cout<<" [Debug inside buzz util: obt msg:] "<<out[k]<<endl;
-			}
-		}*/
-		/* Go through the messages until there's nothing else to read */
-		//fprintf(stdout,"Total data size  : utils : %d\n",(int)size);
-      		uint16_t unMsgSize=0;
-		//uint8_t is_msg_pres=*(uint8_t*)(pl + tot);
-		//tot+=sizeof(uint8_t);
-		 //fprintf(stdout,"is_updater msg present : %i\n",(int)is_msg_pres);
-		//if(is_msg_pres){
-		//unMsgSize = *(uint16_t*)(pl + tot);
-		//tot+=sizeof(uint16_t);
-     		// fprintf(stdout,"%u : read msg size : %u \n",m_unRobotId,unMsgSize);	
-		//	if(unMsgSize>0){
-		//		code_message_inqueue_append((uint8_t*)(pl + tot),unMsgSize);
-		//		tot+=unMsgSize;
-				//fprintf(stdout,"before in queue process : utils\n");
-		//	      	code_message_inqueue_process();
-				//fprintf(stdout,"after in queue process : utils\n");
-		//	}
-		//}
-	      	//unMsgSize=0;
-		
-			/*Obtain Buzz messages only when they are present*/
-	      			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 - tot ) {
-		    			buzzinmsg_queue_append(VM,
-		                        buzzmsg_payload_frombuffer(pl +tot, unMsgSize));
-		    			tot += unMsgSize;
-		 			}
-	      			}while(size - tot > sizeof(uint16_t) && unMsgSize > 0);
-		delete[] pl;
-   		/* Process messages */
-		buzzvm_process_inmsgs(VM);
-	}
-	/***************************************************/
-	/*Obtains messages from buzz out message Queue*/
-	/***************************************************/
+	/* Go through messages and add them to the FIFO */
+	uint16_t* data = u64_cvt_u16((uint64_t) payload[0]);
+	/*Size is at first 2 bytes*/
+	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 and robot id read*/
+	size_t tot = sizeof(uint32_t);
+	/*for(int i=0;i<data[0];i++){
+	 uint16_t* out = u64_cvt_u16(payload[i]);
+	 for(int k=0;k<4;k++){
+	 cout<<" [Debug inside buzz util: obt msg:] "<<out[k]<<endl;
+	 }
+	 }*/
+	/* Go through the messages until there's nothing else to read */
+	//fprintf(stdout,"Total data size  : utils : %d\n",(int)size);
+	uint16_t unMsgSize = 0;
+	//uint8_t is_msg_pres=*(uint8_t*)(pl + tot);
+	//tot+=sizeof(uint8_t);
+	//fprintf(stdout,"is_updater msg present : %i\n",(int)is_msg_pres);
+	//if(is_msg_pres){
+	//unMsgSize = *(uint16_t*)(pl + tot);
+	//tot+=sizeof(uint16_t);
+	// fprintf(stdout,"%u : read msg size : %u \n",m_unRobotId,unMsgSize);
+	//	if(unMsgSize>0){
+	//		code_message_inqueue_append((uint8_t*)(pl + tot),unMsgSize);
+	//		tot+=unMsgSize;
+	//fprintf(stdout,"before in queue process : utils\n");
+	//	      	code_message_inqueue_process();
+	//fprintf(stdout,"after in queue process : utils\n");
+	//	}
+	//}
+	//unMsgSize=0;
 
-   	uint64_t* out_msg_process(){
-   		buzzvm_process_outmsgs(VM);
-   		uint8_t* buff_send =(uint8_t*)malloc(MAX_MSG_SIZE);
-   		memset(buff_send, 0, MAX_MSG_SIZE);
-		/*Taking into consideration the sizes included at the end*/
-   		ssize_t tot = sizeof(uint16_t);
-   		/* Send robot id */
-   		*(uint16_t*)(buff_send+tot) = (uint16_t) VM->robot;
-   		tot += sizeof(uint16_t);
-		//uint8_t updater_msg_pre = 0;
-   		//uint16_t updater_msgSize= 0;
-		//if((int)get_update_mode()!=CODE_RUNNING && is_msg_present()==1){
-		  // fprintf(stdout,"transfer code \n");
-		 //  updater_msg_pre =1;
-		   //transfer_code=0;
-		 //  *(uint8_t*)(buff_send + tot) = (uint8_t)updater_msg_pre;
-      		 //  tot += sizeof(uint8_t);
-		   /*Append updater msg size*/
-		   //*(uint16_t*)(buff_send + tot) = *(uint16_t*) (getupdate_out_msg_size());
-		//	updater_msgSize=*(uint16_t*) (getupdate_out_msg_size());
-		//	*(uint16_t*)(buff_send + tot)=updater_msgSize;
-			//fprintf(stdout,"Updater sent msg size : %i \n", (int)updater_msgSize);
-      		//   tot += sizeof(uint16_t);
-		   /*Append updater msgs*/   	
-    		//   memcpy(buff_send + tot, (uint8_t*)(getupdater_out_msg()), updater_msgSize);
-		//   tot += updater_msgSize;
-		   /*destroy the updater out msg queue*/
-		//   destroy_out_msg_queue();
-		//}
-		//else{
-		//   *(uint8_t*)(buff_send + tot) = (uint8_t)updater_msg_pre;
-      		//   tot += sizeof(uint8_t);
-		//}
- 			/* Send messages from FIFO */
-	   		do {
-				/* Are there more messages? */
-	      			if(buzzoutmsg_queue_isempty(VM)) break;
-	      			/* Get first message */
-	      			buzzmsg_payload_t m = buzzoutmsg_queue_first(VM);
-	      			/* Make sure the next message makes the data buffer with buzz messages to be less than 100 Bytes */
-	      			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);
-      			buzzmsg_payload_destroy(&m);
-	   		} while(1);
-		
-   		uint16_t 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));
-		delete[] buff_send;
-  		/*for(int i=0;i<total_size;i++){
-   		cout<<" payload from out msg  "<<*(payload_64+i)<<endl;
-   		}*/
-   		/* Send message */
-		return payload_64;
+	/*Obtain Buzz messages only when they are present*/
+	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 - tot) {
+			buzzinmsg_queue_append(VM,
+					buzzmsg_payload_frombuffer(pl + tot, unMsgSize));
+			tot += unMsgSize;
 		}
-		
-	/****************************************/
-	/*Buzz script not able to load*/
-	/****************************************/
+	} while (size - tot > sizeof(uint16_t) && unMsgSize > 0);
+	delete[] pl;
+	/* Process messages */
+	buzzvm_process_inmsgs(VM);
+}
+/***************************************************/
+/*Obtains messages from buzz out message Queue*/
+/***************************************************/
 
-	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;
+uint64_t* out_msg_process() {
+	buzzvm_process_outmsgs(VM);
+	uint8_t* buff_send = (uint8_t*) malloc(MAX_MSG_SIZE);
+	memset(buff_send, 0, MAX_MSG_SIZE);
+	/*Taking into consideration the sizes included at the end*/
+	ssize_t tot = sizeof(uint16_t);
+	/* Send robot id */
+	*(uint16_t*) (buff_send + tot) = (uint16_t) VM->robot;
+	tot += sizeof(uint16_t);
+	//uint8_t updater_msg_pre = 0;
+	//uint16_t updater_msgSize= 0;
+	//if((int)get_update_mode()!=CODE_RUNNING && is_msg_present()==1){
+	// fprintf(stdout,"transfer code \n");
+	//  updater_msg_pre =1;
+	//transfer_code=0;
+	//  *(uint8_t*)(buff_send + tot) = (uint8_t)updater_msg_pre;
+	//  tot += sizeof(uint8_t);
+	/*Append updater msg size*/
+	//*(uint16_t*)(buff_send + tot) = *(uint16_t*) (getupdate_out_msg_size());
+	//	updater_msgSize=*(uint16_t*) (getupdate_out_msg_size());
+	//	*(uint16_t*)(buff_send + tot)=updater_msgSize;
+	//fprintf(stdout,"Updater sent msg size : %i \n", (int)updater_msgSize);
+	//   tot += sizeof(uint16_t);
+	/*Append updater msgs*/
+	//   memcpy(buff_send + tot, (uint8_t*)(getupdater_out_msg()), updater_msgSize);
+	//   tot += updater_msgSize;
+	/*destroy the updater out msg queue*/
+	//   destroy_out_msg_queue();
+	//}
+	//else{
+	//   *(uint8_t*)(buff_send + tot) = (uint8_t)updater_msg_pre;
+	//   tot += sizeof(uint8_t);
+	//}
+	/* Send messages from FIFO */
+	do {
+		/* Are there more messages? */
+		if (buzzoutmsg_queue_isempty(VM))
+			break;
+		/* Get first message */
+		buzzmsg_payload_t m = buzzoutmsg_queue_first(VM);
+		/* Make sure the next message makes the data buffer with buzz messages to be less than 100 Bytes */
+		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);
+		buzzmsg_payload_destroy(&m);
+	} while (1);
+
+	uint16_t 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));
+	delete[] buff_send;
+	/*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, "log", 1));
+buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
+buzzvm_gstore(VM);
+buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1));
+buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_moveto));
+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;
+}
+
+/**************************************************/
+/*Register dummy Buzz hooks for test during update*/
+/**************************************************/
+
+static int testing_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, "log", 1));
+buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
+buzzvm_gstore(VM);
+buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1));
+buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
+buzzvm_gstore(VM);
+buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_goto", 1));
+buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
+buzzvm_gstore(VM);
+buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1));
+buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
+buzzvm_gstore(VM);
+buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_gohome", 1));
+buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
+buzzvm_gstore(VM);
+buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_land", 1));
+buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
+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];
+//char* hstnm ="M1003";
+gethostname(hstnm, 30);
+/* Make numeric id from hostname */
+/* NOTE: here we assume that the hostname is in the format Knn */
+int id = strtol(hstnm+4, NULL, 10);
+cout << " Robot ID: " <<id<< endl;
+/* Reset the Buzz VM */
+if(VM) buzzvm_destroy(&VM);
+VM = buzzvm_new((int)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 buzz_update_set(BO_BUF, bdbg_filename, bcode_size);
+}
+
+/****************************************/
+/*Sets a new update                     */
+/****************************************/
+int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size) {
+/* 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 = strtol(hstnm + 4, NULL, 10);
+
+/* 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 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, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
+	buzzvm_destroy(&VM);
+	buzzdebug_destroy(&DBG_INFO);
+	fprintf(stdout, "%s: Error loading Buzz script\n\n", BO_FNAME);
+	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_FNAME);
+	return 0;
+}
+
+/* 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;
+}
+
+/****************************************/
+/*Performs a initialization test        */
+/****************************************/
+int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size) {
+/* 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 = strtol(hstnm + 4, NULL, 10);
+/* 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 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, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
+	buzzvm_destroy(&VM);
+	buzzdebug_destroy(&DBG_INFO);
+	fprintf(stdout, "%s: Error loading Buzz script\n\n", BO_FNAME);
+	return 0;
+}
+/* Register hook functions */
+if(testing_buzz_register_hooks() != BUZZVM_STATE_READY) {
+	buzzvm_destroy(&VM);
+	buzzdebug_destroy(&DBG_INFO);
+	fprintf(stdout, "%s: Error registering hooks\n\n", BO_FNAME);
+	return 0;
+}
+
+/* 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;
 	}
-
-	/****************************************/
-	/*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, "log", 1));
-                buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
-                buzzvm_gstore(VM);
-                buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_moveto", 1));
-   		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_moveto));
-   		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;
+	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*/
 
-	/**************************************************/
-	/*Register dummy Buzz hooks for test during update*/
-	/**************************************************/
-
-	static int testing_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, "log", 1));
-                buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
-                buzzvm_gstore(VM);
-                buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_moveto", 1));
-   		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
-   		buzzvm_gstore(VM);
-                buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_goto", 1));
-   		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
-   		buzzvm_gstore(VM);
-   		buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_takeoff", 1));
-   		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
-   		buzzvm_gstore(VM);
-   		buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_gohome", 1));
-   		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
-   		buzzvm_gstore(VM);
-   		buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_land", 1));
-   		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
-   		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];
-        //char* hstnm ="M1003";
-   	gethostname(hstnm, 30);
-   	/* Make numeric id from hostname */
-   	/* NOTE: here we assume that the hostname is in the format Knn */
-   	int id = strtol(hstnm+4, NULL, 10); 
-   	cout << " Robot ID: " <<id<< endl;
-   	/* Reset the Buzz VM */
-   	if(VM) buzzvm_destroy(&VM);
-   	VM = buzzvm_new((int)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 buzz_update_set(BO_BUF, bdbg_filename, bcode_size);
-	}
-
-	/****************************************/
-	/*Sets a new update                     */
-	/****************************************/
-	int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){
-	/* 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 = strtol(hstnm + 4, NULL, 10);
-	
-	/* 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 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, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
-      		buzzvm_destroy(&VM);
-      		buzzdebug_destroy(&DBG_INFO);
-      		fprintf(stdout, "%s: Error loading Buzz script\n\n", BO_FNAME);
-      		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_FNAME);
-        	return 0;
-   	}
- 
-   	/* 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;
-	}
-
-	/****************************************/
-	/*Performs a initialization test        */
-	/****************************************/
-	int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){
-	/* 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 = strtol(hstnm + 4, NULL, 10);
-	/* 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 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, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
-      		buzzvm_destroy(&VM);
-      		buzzdebug_destroy(&DBG_INFO);
-      		fprintf(stdout, "%s: Error loading Buzz script\n\n", BO_FNAME);
-      		return 0;
-   	 }
-   	/* Register hook functions */
-   	if(testing_buzz_register_hooks() != BUZZVM_STATE_READY) {
-      		buzzvm_destroy(&VM);
-      		buzzdebug_destroy(&DBG_INFO);
-      		fprintf(stdout, "%s: Error registering hooks\n\n", BO_FNAME);
-        	return 0;
-   	}
- 
-   	/* 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);
-	   buzzuav_closures::buzzuav_update_currentpos(VM);
-	   buzzuav_closures::buzzuav_update_flight_status(VM);
-	   buzzuav_closures::buzzuav_update_obstacle(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);
-	   }
-	   /*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/
-	   /* look into this currently we don't have swarm feature at all */
-	   /*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/
-	   /*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);*/
-	}
+void buzz_script_step() {
+/*
+ * Update sensors
+ */
+buzzuav_closures::buzzuav_update_battery(VM);
+buzzuav_closures::buzzuav_update_prox(VM);
+buzzuav_closures::buzzuav_update_currentpos(VM);
+buzzuav_closures::buzzuav_update_flight_status(VM);
+buzzuav_closures::buzzuav_update_obstacle(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);
+}
+/*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/
+/* look into this currently we don't have swarm feature at all */
+/*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/
+/*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");
+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");
 }
-
 
 /****************************************/
 /****************************************/
@@ -550,41 +544,39 @@ void buzz_script_destroy() {
 /****************************************/
 
 int buzz_script_done() {
-   return VM->state != BUZZVM_STATE_READY;
+return VM->state != BUZZVM_STATE_READY;
 }
 
-int update_step_test(){
+int update_step_test() {
 
-  buzzuav_closures::buzzuav_update_battery(VM);
-  buzzuav_closures::buzzuav_update_prox(VM);
-  buzzuav_closures::buzzuav_update_currentpos(VM);
-  buzzuav_closures::buzzuav_update_flight_status(VM);
-  buzzuav_closures::buzzuav_update_obstacle(VM);
+	buzzuav_closures::buzzuav_update_battery(VM);
+	buzzuav_closures::buzzuav_update_prox(VM);
+	buzzuav_closures::buzzuav_update_currentpos(VM);
+	buzzuav_closures::buzzuav_update_flight_status(VM);
+	buzzuav_closures::buzzuav_update_obstacle(VM);
 
- int a = buzzvm_function_call(VM, "step", 0);
-if(a != BUZZVM_STATE_READY){
- fprintf(stdout, "step test VM state %i\n",a);
-  fprintf(stdout, " execution terminated abnormally\n\n");
-}
- return a == BUZZVM_STATE_READY;
+	int a = buzzvm_function_call(VM, "step", 0);
+	if(a != BUZZVM_STATE_READY) {
+		fprintf(stdout, "step test VM state %i\n",a);
+		fprintf(stdout, " execution terminated abnormally\n\n");
+	}
+	return a == BUZZVM_STATE_READY;
 }
 
-uint16_t get_robotid(){
+uint16_t get_robotid() {
 /* 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 = strtol(hstnm + 4, NULL, 10);
-	//fprintf(stdout, "Robot id from get rid buzz util:  %i\n",id);
+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 = strtol(hstnm + 4, NULL, 10);
+//fprintf(stdout, "Robot id from get rid buzz util:  %i\n",id);
 return (uint16_t)id;
 }
 
-buzzvm_t get_vm(){
+buzzvm_t get_vm() {
 return VM;
 }
 
 }
 
-
-
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index e0647df..1982f97 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -238,7 +238,7 @@ namespace rosbzz_node{
 		}
 	}
 
-	void roscontroller::WaypointMissionSetup(){
+	void roscontroller::WaypointMissionSetup(float lat, float lng, float alt){
 		mavros_msgs::WaypointPush srv;
 		mavros_msgs::Waypoint waypoint;
 
@@ -247,10 +247,9 @@ namespace rosbzz_node{
 		waypoint.command = mavros_msgs::CommandCode::NAV_WAYPOINT;
 		waypoint.is_current = 2;	// IMPORTANT: goto is no longer used, so instead, use magic number 2 for is_current parameter
 		waypoint.autocontinue = 0;
-		waypoint.x_lat = 45.507730f;
-		waypoint.y_long = -73.613961f;
-		//45.507730, -73.613961
-		waypoint.z_alt = 10;
+		waypoint.x_lat = lat;
+		waypoint.y_long = lng;
+		waypoint.z_alt = alt;
 
 		srv.request.waypoints.push_back(waypoint);
 		if(mission_client.call(srv)){
@@ -731,10 +730,10 @@ namespace rosbzz_node{
    				ROS_INFO("RC_call: TAKE OFF!!!!");
 				rc_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF;
 				/* arming */
-				SetMode("GUIDED", 0);
-				//SetMode("LOITER", 0);
+				SetMode("LOITER", 0);
+				//SetMode("GUIDED", 0);
 				cout << "this..." << endl;
-				//SetModeAsync("GUIDED", 5000);
+				SetModeAsync("GUIDED", 2000);
 				Arm();
 				buzzuav_closures::rc_call(rc_cmd);
 				res.success = true;
@@ -754,12 +753,24 @@ namespace rosbzz_node{
 			case mavros_msgs::CommandCode::NAV_WAYPOINT:
    				ROS_INFO("RC_Call: GO TO!!!! ");
 
-   				WaypointMissionSetup();
-
+   				//WaypointMissionSetup();
    				double rc_goto[3];
 				rc_goto[0] = req.param5;
 				rc_goto[1] = req.param6;
 				rc_goto[2] = req.param7;
+
+				WaypointMissionSetup(req.param5, req.param6, req.param7);
+				/*
+				WaypointMissionSetup(45.505293f, -73.614722f, 2.0f);
+					std::this_thread::sleep_for( std::chrono::milliseconds ( 100 ) );
+				WaypointMissionSetup(45.505324f, -73.614502f, 2.0f);
+					std::this_thread::sleep_for( std::chrono::milliseconds ( 100 ) );
+				WaypointMissionSetup(45.505452f, -73.614655f, 2.0f);
+					std::this_thread::sleep_for( std::chrono::milliseconds ( 100 ) );
+				WaypointMissionSetup(45.505463f, -73.614389f, 2.0f);
+					std::this_thread::sleep_for( std::chrono::milliseconds ( 100 ) );
+				*/
+
 				buzzuav_closures::rc_set_goto(rc_goto);
 				rc_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
 				buzzuav_closures::rc_call(rc_cmd);

From d2c9ee0417aad69a636be98901d3c013ee9068af Mon Sep 17 00:00:00 2001
From: isvogor <isvogor@gmail.com>
Date: Tue, 21 Feb 2017 19:27:57 -0500
Subject: [PATCH 04/12] launch

---
 launch/rosbuzz-testing.launch | 32 ++++++++++++++++++++++++++++++++
 1 file changed, 32 insertions(+)
 create mode 100644 launch/rosbuzz-testing.launch

diff --git a/launch/rosbuzz-testing.launch b/launch/rosbuzz-testing.launch
new file mode 100644
index 0000000..4baca4f
--- /dev/null
+++ b/launch/rosbuzz-testing.launch
@@ -0,0 +1,32 @@
+<launch>
+	<arg name="path_save"   default="/home/pi/bags"/>
+	<arg name="file_name"  	default="test.bag"/>
+
+	<group ns="record">
+		<param name="path_save" type="str" value="$(arg path_save)"	/>
+		<param name="file_name" type="str" value="$(arg file_name)"	/>
+		<node name="record_ros" pkg="record_ros" type="record_ros" output="screen" respawn="true"/>
+	</group>
+
+	<node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" />
+	<param name="Xbee_In_From_Buzz" type="str" value="outMavlink" />
+	<param name="Xbee_Out_To_Buzz" type="str" value="inMavlink" />
+	<param name="Xbee_In_From_Controller" type="str" value="xbee_cmd" />
+	<param name="No_of_dev" type="int" value="3" />
+	<param name="Xbee_Out_To_Controller" type="str" value="mav_dji_cmd" />
+
+	
+	<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >	
+		<rosparam file="/home/pi/ros_catkin_ws/src/ROSBuzz/launch/launch_config/solo.yaml"/>
+		<param name="bzzfile_name" value="/home/pi/ros_catkin_ws/src/ROSBuzz/src/testalone.bzz" />
+	    <param name="rcclient" value="true" /> 
+		<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="No_of_Robots" value="3"/>
+		<param name="stand_by" value="$(env ROS_WS)/src/ROSBuzz/src/stand_by.bo"/>
+	</node>
+
+</launch>
+

From aafa037c22e4648cbe78f1992769906fc8a34b55 Mon Sep 17 00:00:00 2001
From: isvogor <isvogor@gmail.com>
Date: Wed, 22 Feb 2017 08:13:23 -0500
Subject: [PATCH 05/12] :wrench:

---
 launch/rosbuzz-testing.launch | 34 +++++++++++++++++++++++-----------
 misc/cmdlinectr.sh            | 20 ++++++++++++++++++++
 2 files changed, 43 insertions(+), 11 deletions(-)
 create mode 100644 misc/cmdlinectr.sh

diff --git a/launch/rosbuzz-testing.launch b/launch/rosbuzz-testing.launch
index 4baca4f..d674c14 100644
--- a/launch/rosbuzz-testing.launch
+++ b/launch/rosbuzz-testing.launch
@@ -1,13 +1,26 @@
 <launch>
-	<arg name="path_save"   default="/home/pi/bags"/>
-	<arg name="file_name"  	default="test.bag"/>
+	<!-- RUN mavros -->
+	<arg name="fcu_url" default="/dev/ttyAMA0:115200" />
+	<arg name="gcs_url" default="" />
+	<arg name="tgt_system" default="1" />
+	<arg name="tgt_component" default="1" />
+	<arg name="log_output" default="screen" />
 
-	<group ns="record">
-		<param name="path_save" type="str" value="$(arg path_save)"	/>
-		<param name="file_name" type="str" value="$(arg file_name)"	/>
-		<node name="record_ros" pkg="record_ros" type="record_ros" output="screen" respawn="true"/>
-	</group>
+	<include file="$(find mavros)/launch/node.launch">
+		<arg name="pluginlists_yaml" value="$(find mavros)/launch/apm_pluginlists.yaml" />
+		<arg name="config_yaml" value="$(find mavros)/launch/apm_config.yaml" />
 
+		<arg name="fcu_url" value="$(arg fcu_url)" />
+		<arg name="gcs_url" value="$(arg gcs_url)" />
+		<arg name="tgt_system" value="$(arg tgt_system)" />
+		<arg name="tgt_component" value="$(arg tgt_component)" />
+		<arg name="log_output" value="$(arg log_output)" />
+	</include>
+
+	<!-- set streaming rate -->
+	<node pkg="rosservice" type="rosservice" name="freq" args="call /mavros/set_stream_rate 0 10 1" output="screen" />
+
+	<!-- run xbee -->
 	<node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" />
 	<param name="Xbee_In_From_Buzz" type="str" value="outMavlink" />
 	<param name="Xbee_Out_To_Buzz" type="str" value="inMavlink" />
@@ -15,18 +28,17 @@
 	<param name="No_of_dev" type="int" value="3" />
 	<param name="Xbee_Out_To_Controller" type="str" value="mav_dji_cmd" />
 
-	
+	<!-- run rosbuzz -->
 	<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >	
 		<rosparam file="/home/pi/ros_catkin_ws/src/ROSBuzz/launch/launch_config/solo.yaml"/>
 		<param name="bzzfile_name" value="/home/pi/ros_catkin_ws/src/ROSBuzz/src/testalone.bzz" />
-	    <param name="rcclient" value="true" /> 
+		<param name="rcclient" value="true" /> 
 		<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="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/pi/ros_catkin_ws/src/ROSBuzz/src/stand_by.bo"/>
 	</node>
 
 </launch>
-
diff --git a/misc/cmdlinectr.sh b/misc/cmdlinectr.sh
new file mode 100644
index 0000000..e00b73b
--- /dev/null
+++ b/misc/cmdlinectr.sh
@@ -0,0 +1,20 @@
+#! /bin/bash
+function takeoff {
+	rosservice call /buzzcmd 0 22 0 0 0 0 0 0 0 0
+}
+function land {
+	rosservice call /buzzcmd 0 21 0 0 0 0 0 0 0 0
+}
+function record {
+	rosbag record /flight_status /global_position /dji_sdk/local_position /neighbours_pos /power_status /guidance/obstacle_distance /guidance/front_depth_image/compressedDepth /guidance/right_depth_image/compressedDepth /guidance/left_depth_image/compressedDepth /guidance/bottom_depth_image/compressedDepth /guidance/back_depth_image/compressedDepth
+}
+function clean {
+	sudo rm /var/log/upstart/robot*
+	sudo rm /var/log/upstart/x3s*
+}
+function startrobot {
+	sudo service robot start
+}
+function stoprobot {
+	sudo service robot stop
+}

From e855c8c78aecd6c7272ac396683eb6a394528586 Mon Sep 17 00:00:00 2001
From: isvogor <isvogor@gmail.com>
Date: Thu, 23 Feb 2017 10:57:05 -0500
Subject: [PATCH 06/12] ...

---
 CMakeLists.txt                     |  1 -
 launch/rosbuzz-testing-sitl.launch | 47 ++++++++++++++++++++++++++++++
 launch/rosbuzz.launch              |  9 +++---
 3 files changed, 51 insertions(+), 6 deletions(-)
 create mode 100644 launch/rosbuzz-testing-sitl.launch

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 0df4d4c..385dec8 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -15,7 +15,6 @@ find_package(catkin REQUIRED COMPONENTS
 )
 
 ##############################
-#Generate messages#
 ##############################
 
 add_message_files(
diff --git a/launch/rosbuzz-testing-sitl.launch b/launch/rosbuzz-testing-sitl.launch
new file mode 100644
index 0000000..d832e25
--- /dev/null
+++ b/launch/rosbuzz-testing-sitl.launch
@@ -0,0 +1,47 @@
+<launch>
+	<!-- RUN mavros -->
+	<arg name="fcu_url" default="tcp://127.0.0.1:5760" />
+	<arg name="gcs_url" default="" />
+	<arg name="tgt_system" default="1" />
+	<arg name="tgt_component" default="1" />
+	<arg name="log_output" default="screen" />
+
+	<include file="$(find mavros)/launch/node.launch">
+		<arg name="pluginlists_yaml" value="$(find mavros)/launch/apm_pluginlists.yaml" />
+		<arg name="config_yaml" value="$(find mavros)/launch/apm_config.yaml" />
+		<arg name="fcu_url" value="$(arg fcu_url)" />
+		<arg name="gcs_url" value="$(arg gcs_url)" />
+		<arg name="tgt_system" value="$(arg tgt_system)" />
+		<arg name="tgt_component" value="$(arg tgt_component)" />
+		<arg name="log_output" value="$(arg log_output)" />
+	</include>
+
+	<!-- run xbee -->
+	<node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" />
+	<param name="Xbee_In_From_Buzz" type="str" value="outMavlink" />
+	<param name="Xbee_Out_To_Buzz" type="str" value="inMavlink" />
+	<param name="Xbee_In_From_Controller" type="str" value="xbee_cmd" />
+	<param name="No_of_dev" type="int" value="3" />
+
+	<!-- run rosbuzz -->
+	<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >	
+		<rosparam file="/home/ivan/catkin_ws/src/rosbuzz/launch/launch_config/solo.yaml"/>
+		<param name="bzzfile_name" value="/home/ivan/catkin_ws/src/rosbuzz/src/testalone.bzz" />
+		<param name="rcclient" value="true" /> 
+		<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="No_of_Robots" value="3"/>
+		<param name="stand_by" value="/home/ivan/catkin_ws/src/rosbuzz/src/stand_by.bo"/>
+	</node>
+		
+	<node pkg="rosloader" type="rosloader" name="rosloader" output="screen"/> 
+		
+	<!-- set streaming rate 
+	/mavros/cmd/arming
+	<node pkg="rosservice" type="rosservice" name="stream_rate" args="call /mavros/set_stream_rate 0 10 1" />
+	-->
+	
+
+</launch>
diff --git a/launch/rosbuzz.launch b/launch/rosbuzz.launch
index 47f5a0f..a187ab4 100644
--- a/launch/rosbuzz.launch
+++ b/launch/rosbuzz.launch
@@ -2,15 +2,14 @@
 
 <launch>
 		<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >	
-		<rosparam file="$(env ROS_WS)/src/ROSBuzz/launch/launch_config/$(env ROBOT).yaml"/>
-		<param name="bzzfile_name" value="$(env ROS_WS)/src/ROSBuzz/src/testalone.bzz" />
+		<rosparam file="/home/ivan/catkin_ws/src/rosbuzz/launch/launch_config/solo.yaml"/>
+		<param name="bzzfile_name" value="/home/ivan/catkin_ws/src/rosbuzz/src/testalone.bzz" />
         <param name="rcclient" value="true" /> 
 		<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="No_of_Robots" value="3"/>
-		<param name="stand_by" value="$(env ROS_WS)/src/ROSBuzz/src/stand_by.bo"/>
-		</node>
-		
+		<param name="stand_by" value="/home/ivan/catkin_ws/src/rosbuzz/src/stand_by.bo"/>
+		</node>	
 </launch>

From 4b0e18b76201265a90531b2e368d32108597ba4e Mon Sep 17 00:00:00 2001
From: isvogor <isvogor@gmail.com>
Date: Mon, 20 Feb 2017 17:59:35 -0500
Subject: [PATCH 07/12] minor updates in controller

---
 include/roscontroller.h |  7 ++++++-
 src/roscontroller.cpp   | 20 +++++++++++++++++---
 src/test1.bzz           |  2 +-
 3 files changed, 24 insertions(+), 5 deletions(-)

diff --git a/include/roscontroller.h b/include/roscontroller.h
index 1a1c30a..7ce69d8 100644
--- a/include/roscontroller.h
+++ b/include/roscontroller.h
@@ -148,9 +148,14 @@ private:
 
 	void Arm();
 
-	void SetMode();
+	void SetMode(std::string mode, int delay_miliseconds);
+
+	void SetModeAsync(std::string mode, int delay_miliseconds);
+
+	//void SetModeAsync(std::string mode, int delay);
 
 	void Subscribe(ros::NodeHandle n_c);
+
 };
 
 }
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index 389b0f8..30ac680 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -1,4 +1,5 @@
 #include "roscontroller.h"
+#include <thread>
 
 namespace rosbzz_node{
 
@@ -236,10 +237,15 @@ namespace rosbzz_node{
 		}
 	}
 
-	void roscontroller::SetMode(){
+	void roscontroller::SetMode(std::string mode, int delay_miliseconds){
+		// wait if necessary
+		if (delay_miliseconds > 0){
+			std::this_thread::sleep_for( std::chrono::milliseconds ( delay_miliseconds ) );
+		}
+		// set mode
 		mavros_msgs::SetMode set_mode_message;
 		set_mode_message.request.base_mode = 0;
-		set_mode_message.request.custom_mode = "GUIDED"; // shit!
+		set_mode_message.request.custom_mode = mode;
 		if(mode_client.call(set_mode_message)) {
 			ROS_INFO("Service called!");
 		} else {
@@ -247,6 +253,12 @@ namespace rosbzz_node{
 		}
 	}
 
+	//todo: this
+	void roscontroller::SetModeAsync(std::string mode, int delay_miliseconds){
+		std::thread([&](){SetMode(mode, delay_miliseconds);}).detach();
+		cout << "Async call called... " <<endl;
+	}
+
 
 	/*Prepare messages for each step and publish*/
 	/*******************************************************************************************************/
@@ -695,7 +707,9 @@ namespace rosbzz_node{
    				ROS_INFO("RC_call: TAKE OFF!!!!");
 				rc_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF;
 				/* arming */
-				SetMode();
+				SetMode("GUIDED", 0);
+				cout<< "this..."<<endl;
+				SetModeAsync("LAND", 5000);
 				Arm();
 				buzzuav_closures::rc_call(rc_cmd);
 				res.success = true;
diff --git a/src/test1.bzz b/src/test1.bzz
index 3e9d1fb..8edac26 100644
--- a/src/test1.bzz
+++ b/src/test1.bzz
@@ -1,7 +1,7 @@
 
 # We need this for 2D vectors
 # Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
-include "/home/ubuntu/buzz/src/include/vec2.bzz"
+include "/home/ivan/dev/buzz/src/include/vec2.bzz"
 ####################################################################################################
 # Updater related
 # This should be here for the updater to work, changing position of code will crash the updater

From 321a1c84a428b3c4e2fb0f35ecdc27e9ff01473d Mon Sep 17 00:00:00 2001
From: isvogor <isvogor@gmail.com>
Date: Tue, 21 Feb 2017 16:09:16 -0500
Subject: [PATCH 08/12] first waypoint implementation - sitl works

---
 include/roscontroller.h |  6 ++++++
 launch/rosbuzz.launch   |  2 +-
 src/roscontroller.cpp   | 37 ++++++++++++++++++++++++++++++++-----
 src/test1.bzz           |  2 +-
 4 files changed, 40 insertions(+), 7 deletions(-)

diff --git a/include/roscontroller.h b/include/roscontroller.h
index 7ce69d8..fbf3d6d 100644
--- a/include/roscontroller.h
+++ b/include/roscontroller.h
@@ -11,6 +11,8 @@
 #include "mavros_msgs/State.h"
 #include "mavros_msgs/BatteryStatus.h"
 #include "mavros_msgs/Mavlink.h"
+#include "mavros_msgs/WaypointPush.h"
+#include "mavros_msgs/Waypoint.h"
 #include "sensor_msgs/NavSatStatus.h"
 #include <sensor_msgs/LaserScan.h>
 #include <rosbuzz/neigh_pos.h>
@@ -81,6 +83,8 @@ private:
   	mavros_msgs::SetMode m_cmdSetMode;
   	ros::ServiceClient mode_client;
 
+  	ros::ServiceClient mission_client;
+
 	void Initialize_pub_sub(ros::NodeHandle n_c);
 
 	/*Obtain data from ros parameter server*/	
@@ -156,6 +160,8 @@ private:
 
 	void Subscribe(ros::NodeHandle n_c);
 
+	void WaypointMissionSetup();
+
 };
 
 }
diff --git a/launch/rosbuzz.launch b/launch/rosbuzz.launch
index eb901e7..47f5a0f 100644
--- a/launch/rosbuzz.launch
+++ b/launch/rosbuzz.launch
@@ -3,7 +3,7 @@
 <launch>
 		<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >	
 		<rosparam file="$(env ROS_WS)/src/ROSBuzz/launch/launch_config/$(env ROBOT).yaml"/>
-		<param name="bzzfile_name" value="$(env ROS_WS)/src/ROSBuzz/src/test1.bzz" />
+		<param name="bzzfile_name" value="$(env ROS_WS)/src/ROSBuzz/src/testalone.bzz" />
         <param name="rcclient" value="true" /> 
 		<param name="rcservice_name" value="/buzzcmd" />
 		<param name="in_payload" value="/inMavlink"/>
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index 30ac680..07d337f 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -177,6 +177,7 @@ namespace rosbzz_node{
 		neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("/neighbours_pos",1000);	
 		/* Service Clients*/
 		arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient);
+		mission_client = n_c.serviceClient<mavros_msgs::WaypointPush>("/mavros/mission/push");
 		mode_client =  n_c.serviceClient<mavros_msgs::SetMode>(modeclient);
 		mav_client = n_c.serviceClient<mavros_msgs::CommandLong>(fcclient_name);
 
@@ -237,6 +238,29 @@ namespace rosbzz_node{
 		}
 	}
 
+	void roscontroller::WaypointMissionSetup(){
+		mavros_msgs::WaypointPush srv;
+		mavros_msgs::Waypoint waypoint;
+
+		// prepare waypoint mission package
+		waypoint.frame = mavros_msgs::Waypoint::FRAME_GLOBAL;
+		waypoint.command = mavros_msgs::CommandCode::NAV_WAYPOINT;
+		waypoint.is_current = 2;	// IMPORTANT: goto is no longer used, so instead, use magic number 2 for is_current parameter
+		waypoint.autocontinue = 0;
+		waypoint.x_lat = 45.507730f;
+		waypoint.y_long = -73.613961f;
+		//45.507730, -73.613961
+		waypoint.z_alt = 10;
+
+		srv.request.waypoints.push_back(waypoint);
+		if(mission_client.call(srv)){
+			ROS_INFO("Mission service called!");
+		} else {
+			ROS_INFO("Mission service failed!");
+		}
+
+	}
+
 	void roscontroller::SetMode(std::string mode, int delay_miliseconds){
 		// wait if necessary
 		if (delay_miliseconds > 0){
@@ -588,7 +612,7 @@ namespace rosbzz_node{
 			buzzuav_closures::flight_status_update(1);
 		else if (msg->mode == "LAND")
 			buzzuav_closures::flight_status_update(4);
-		else // ground standby = LOITER?
+		else
 			buzzuav_closures::flight_status_update(1);//?
 	}
 
@@ -708,8 +732,9 @@ namespace rosbzz_node{
 				rc_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF;
 				/* arming */
 				SetMode("GUIDED", 0);
-				cout<< "this..."<<endl;
-				SetModeAsync("LAND", 5000);
+				//SetMode("LOITER", 0);
+				cout << "this..." << endl;
+				//SetModeAsync("GUIDED", 5000);
 				Arm();
 				buzzuav_closures::rc_call(rc_cmd);
 				res.success = true;
@@ -728,11 +753,13 @@ namespace rosbzz_node{
 				break;
 			case mavros_msgs::CommandCode::NAV_WAYPOINT:
    				ROS_INFO("RC_Call: GO TO!!!! ");
-				double rc_goto[3];
+
+   				WaypointMissionSetup();
+
+   				double rc_goto[3];
 				rc_goto[0] = req.param5;
 				rc_goto[1] = req.param6;
 				rc_goto[2] = req.param7;
-
 				buzzuav_closures::rc_set_goto(rc_goto);
 				rc_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
 				buzzuav_closures::rc_call(rc_cmd);
diff --git a/src/test1.bzz b/src/test1.bzz
index 8edac26..3e9d1fb 100644
--- a/src/test1.bzz
+++ b/src/test1.bzz
@@ -1,7 +1,7 @@
 
 # We need this for 2D vectors
 # Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
-include "/home/ivan/dev/buzz/src/include/vec2.bzz"
+include "/home/ubuntu/buzz/src/include/vec2.bzz"
 ####################################################################################################
 # Updater related
 # This should be here for the updater to work, changing position of code will crash the updater

From 4f9a891216ce088e49ebb557023fc8a105edd527 Mon Sep 17 00:00:00 2001
From: isvogor <isvogor@gmail.com>
Date: Tue, 21 Feb 2017 19:20:38 -0500
Subject: [PATCH 09/12] for testing #2 - waypoints included

---
 include/roscontroller.h |    2 +-
 src/buzz_utility.cpp    | 1062 +++++++++++++++++++--------------------
 src/roscontroller.cpp   |   31 +-
 3 files changed, 549 insertions(+), 546 deletions(-)

diff --git a/include/roscontroller.h b/include/roscontroller.h
index fbf3d6d..c9352ba 100644
--- a/include/roscontroller.h
+++ b/include/roscontroller.h
@@ -160,7 +160,7 @@ private:
 
 	void Subscribe(ros::NodeHandle n_c);
 
-	void WaypointMissionSetup();
+	void WaypointMissionSetup(float lat, float lng, float alg);
 
 };
 
diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp
index 741d24b..4ec0ff3 100644
--- a/src/buzz_utility.cpp
+++ b/src/buzz_utility.cpp
@@ -8,539 +8,533 @@
 
 #include <buzz_utility.h>
 
-namespace buzz_utility{
+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 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 
-	
-	
-	/****************************************/
-	/*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);
-    		}
+static buzzvm_t VM = 0;
+static char* BO_FNAME = 0;
+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
+
+
+/****************************************/
+/*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);
 	}
-	/**************************************************************************/
-	/*Deserializes uint64_t into 4 uint16_t, freeing out is left to the user  */
-	/**************************************************************************/
-	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] <<"  ";
+}
+/**************************************************************************/
+/*Deserializes uint64_t into 4 uint16_t, freeing out is left to the user  */
+/**************************************************************************/
+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*/
+/***************************************************/
 
-	/*******************************************************************************************************************/
-	/* Message format of payload (Each slot is uint64_t)						                   */
-	/* _______________________________________________________________________________________________________________ */
-	/*|					        		             |			                  |*/
-	/*|Size in Uint64_t(but size is Uint16_t)|robot_id|Update msg size|Update msg|Update msgs+Buzz_msgs with size.....|*/
-	/*|__________________________________________________________________________|____________________________________|*/
-	/*******************************************************************************************************************/
+/*******************************************************************************************************************/
+/* Message format of payload (Each slot is uint64_t)						                   */
+/* _______________________________________________________________________________________________________________ */
+/*|					        		             |			                  |*/
+/*|Size in Uint64_t(but size is Uint16_t)|robot_id|Update msg size|Update msg|Update msgs+Buzz_msgs with size.....|*/
+/*|__________________________________________________________________________|____________________________________|*/
+/*******************************************************************************************************************/
 
-	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]);
-		/*Size is at first 2 bytes*/
-   		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 and robot id read*/
-   		size_t tot = sizeof(uint32_t);
-      		/*for(int i=0;i<data[0];i++){
-			uint16_t* out = u64_cvt_u16(payload[i]);
-			for(int k=0;k<4;k++){
-				cout<<" [Debug inside buzz util: obt msg:] "<<out[k]<<endl;
-			}
-		}*/
-		/* Go through the messages until there's nothing else to read */
-		//fprintf(stdout,"Total data size  : utils : %d\n",(int)size);
-      		uint16_t unMsgSize=0;
-		//uint8_t is_msg_pres=*(uint8_t*)(pl + tot);
-		//tot+=sizeof(uint8_t);
-		 //fprintf(stdout,"is_updater msg present : %i\n",(int)is_msg_pres);
-		//if(is_msg_pres){
-		//unMsgSize = *(uint16_t*)(pl + tot);
-		//tot+=sizeof(uint16_t);
-     		// fprintf(stdout,"%u : read msg size : %u \n",m_unRobotId,unMsgSize);	
-		//	if(unMsgSize>0){
-		//		code_message_inqueue_append((uint8_t*)(pl + tot),unMsgSize);
-		//		tot+=unMsgSize;
-				//fprintf(stdout,"before in queue process : utils\n");
-		//	      	code_message_inqueue_process();
-				//fprintf(stdout,"after in queue process : utils\n");
-		//	}
-		//}
-	      	//unMsgSize=0;
-		
-			/*Obtain Buzz messages only when they are present*/
-	      			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 - tot ) {
-		    			buzzinmsg_queue_append(VM,
-		                        buzzmsg_payload_frombuffer(pl +tot, unMsgSize));
-		    			tot += unMsgSize;
-		 			}
-	      			}while(size - tot > sizeof(uint16_t) && unMsgSize > 0);
-		delete[] pl;
-   		/* Process messages */
-		buzzvm_process_inmsgs(VM);
-	}
-	/***************************************************/
-	/*Obtains messages from buzz out message Queue*/
-	/***************************************************/
+	/* Go through messages and add them to the FIFO */
+	uint16_t* data = u64_cvt_u16((uint64_t) payload[0]);
+	/*Size is at first 2 bytes*/
+	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 and robot id read*/
+	size_t tot = sizeof(uint32_t);
+	/*for(int i=0;i<data[0];i++){
+	 uint16_t* out = u64_cvt_u16(payload[i]);
+	 for(int k=0;k<4;k++){
+	 cout<<" [Debug inside buzz util: obt msg:] "<<out[k]<<endl;
+	 }
+	 }*/
+	/* Go through the messages until there's nothing else to read */
+	//fprintf(stdout,"Total data size  : utils : %d\n",(int)size);
+	uint16_t unMsgSize = 0;
+	//uint8_t is_msg_pres=*(uint8_t*)(pl + tot);
+	//tot+=sizeof(uint8_t);
+	//fprintf(stdout,"is_updater msg present : %i\n",(int)is_msg_pres);
+	//if(is_msg_pres){
+	//unMsgSize = *(uint16_t*)(pl + tot);
+	//tot+=sizeof(uint16_t);
+	// fprintf(stdout,"%u : read msg size : %u \n",m_unRobotId,unMsgSize);
+	//	if(unMsgSize>0){
+	//		code_message_inqueue_append((uint8_t*)(pl + tot),unMsgSize);
+	//		tot+=unMsgSize;
+	//fprintf(stdout,"before in queue process : utils\n");
+	//	      	code_message_inqueue_process();
+	//fprintf(stdout,"after in queue process : utils\n");
+	//	}
+	//}
+	//unMsgSize=0;
 
-   	uint64_t* out_msg_process(){
-   		buzzvm_process_outmsgs(VM);
-   		uint8_t* buff_send =(uint8_t*)malloc(MAX_MSG_SIZE);
-   		memset(buff_send, 0, MAX_MSG_SIZE);
-		/*Taking into consideration the sizes included at the end*/
-   		ssize_t tot = sizeof(uint16_t);
-   		/* Send robot id */
-   		*(uint16_t*)(buff_send+tot) = (uint16_t) VM->robot;
-   		tot += sizeof(uint16_t);
-		//uint8_t updater_msg_pre = 0;
-   		//uint16_t updater_msgSize= 0;
-		//if((int)get_update_mode()!=CODE_RUNNING && is_msg_present()==1){
-		  // fprintf(stdout,"transfer code \n");
-		 //  updater_msg_pre =1;
-		   //transfer_code=0;
-		 //  *(uint8_t*)(buff_send + tot) = (uint8_t)updater_msg_pre;
-      		 //  tot += sizeof(uint8_t);
-		   /*Append updater msg size*/
-		   //*(uint16_t*)(buff_send + tot) = *(uint16_t*) (getupdate_out_msg_size());
-		//	updater_msgSize=*(uint16_t*) (getupdate_out_msg_size());
-		//	*(uint16_t*)(buff_send + tot)=updater_msgSize;
-			//fprintf(stdout,"Updater sent msg size : %i \n", (int)updater_msgSize);
-      		//   tot += sizeof(uint16_t);
-		   /*Append updater msgs*/   	
-    		//   memcpy(buff_send + tot, (uint8_t*)(getupdater_out_msg()), updater_msgSize);
-		//   tot += updater_msgSize;
-		   /*destroy the updater out msg queue*/
-		//   destroy_out_msg_queue();
-		//}
-		//else{
-		//   *(uint8_t*)(buff_send + tot) = (uint8_t)updater_msg_pre;
-      		//   tot += sizeof(uint8_t);
-		//}
- 			/* Send messages from FIFO */
-	   		do {
-				/* Are there more messages? */
-	      			if(buzzoutmsg_queue_isempty(VM)) break;
-	      			/* Get first message */
-	      			buzzmsg_payload_t m = buzzoutmsg_queue_first(VM);
-	      			/* Make sure the next message makes the data buffer with buzz messages to be less than 100 Bytes */
-	      			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);
-      			buzzmsg_payload_destroy(&m);
-	   		} while(1);
-		
-   		uint16_t 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));
-		delete[] buff_send;
-  		/*for(int i=0;i<total_size;i++){
-   		cout<<" payload from out msg  "<<*(payload_64+i)<<endl;
-   		}*/
-   		/* Send message */
-		return payload_64;
+	/*Obtain Buzz messages only when they are present*/
+	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 - tot) {
+			buzzinmsg_queue_append(VM,
+					buzzmsg_payload_frombuffer(pl + tot, unMsgSize));
+			tot += unMsgSize;
 		}
-		
-	/****************************************/
-	/*Buzz script not able to load*/
-	/****************************************/
+	} while (size - tot > sizeof(uint16_t) && unMsgSize > 0);
+	delete[] pl;
+	/* Process messages */
+	buzzvm_process_inmsgs(VM);
+}
+/***************************************************/
+/*Obtains messages from buzz out message Queue*/
+/***************************************************/
 
-	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;
+uint64_t* out_msg_process() {
+	buzzvm_process_outmsgs(VM);
+	uint8_t* buff_send = (uint8_t*) malloc(MAX_MSG_SIZE);
+	memset(buff_send, 0, MAX_MSG_SIZE);
+	/*Taking into consideration the sizes included at the end*/
+	ssize_t tot = sizeof(uint16_t);
+	/* Send robot id */
+	*(uint16_t*) (buff_send + tot) = (uint16_t) VM->robot;
+	tot += sizeof(uint16_t);
+	//uint8_t updater_msg_pre = 0;
+	//uint16_t updater_msgSize= 0;
+	//if((int)get_update_mode()!=CODE_RUNNING && is_msg_present()==1){
+	// fprintf(stdout,"transfer code \n");
+	//  updater_msg_pre =1;
+	//transfer_code=0;
+	//  *(uint8_t*)(buff_send + tot) = (uint8_t)updater_msg_pre;
+	//  tot += sizeof(uint8_t);
+	/*Append updater msg size*/
+	//*(uint16_t*)(buff_send + tot) = *(uint16_t*) (getupdate_out_msg_size());
+	//	updater_msgSize=*(uint16_t*) (getupdate_out_msg_size());
+	//	*(uint16_t*)(buff_send + tot)=updater_msgSize;
+	//fprintf(stdout,"Updater sent msg size : %i \n", (int)updater_msgSize);
+	//   tot += sizeof(uint16_t);
+	/*Append updater msgs*/
+	//   memcpy(buff_send + tot, (uint8_t*)(getupdater_out_msg()), updater_msgSize);
+	//   tot += updater_msgSize;
+	/*destroy the updater out msg queue*/
+	//   destroy_out_msg_queue();
+	//}
+	//else{
+	//   *(uint8_t*)(buff_send + tot) = (uint8_t)updater_msg_pre;
+	//   tot += sizeof(uint8_t);
+	//}
+	/* Send messages from FIFO */
+	do {
+		/* Are there more messages? */
+		if (buzzoutmsg_queue_isempty(VM))
+			break;
+		/* Get first message */
+		buzzmsg_payload_t m = buzzoutmsg_queue_first(VM);
+		/* Make sure the next message makes the data buffer with buzz messages to be less than 100 Bytes */
+		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);
+		buzzmsg_payload_destroy(&m);
+	} while (1);
+
+	uint16_t 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));
+	delete[] buff_send;
+	/*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, "log", 1));
+buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
+buzzvm_gstore(VM);
+buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1));
+buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_moveto));
+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;
+}
+
+/**************************************************/
+/*Register dummy Buzz hooks for test during update*/
+/**************************************************/
+
+static int testing_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, "log", 1));
+buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
+buzzvm_gstore(VM);
+buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1));
+buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
+buzzvm_gstore(VM);
+buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_goto", 1));
+buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
+buzzvm_gstore(VM);
+buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1));
+buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
+buzzvm_gstore(VM);
+buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_gohome", 1));
+buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
+buzzvm_gstore(VM);
+buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_land", 1));
+buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
+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];
+//char* hstnm ="M1003";
+gethostname(hstnm, 30);
+/* Make numeric id from hostname */
+/* NOTE: here we assume that the hostname is in the format Knn */
+int id = strtol(hstnm+4, NULL, 10);
+cout << " Robot ID: " <<id<< endl;
+/* Reset the Buzz VM */
+if(VM) buzzvm_destroy(&VM);
+VM = buzzvm_new((int)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 buzz_update_set(BO_BUF, bdbg_filename, bcode_size);
+}
+
+/****************************************/
+/*Sets a new update                     */
+/****************************************/
+int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size) {
+/* 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 = strtol(hstnm + 4, NULL, 10);
+
+/* 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 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, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
+	buzzvm_destroy(&VM);
+	buzzdebug_destroy(&DBG_INFO);
+	fprintf(stdout, "%s: Error loading Buzz script\n\n", BO_FNAME);
+	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_FNAME);
+	return 0;
+}
+
+/* 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;
+}
+
+/****************************************/
+/*Performs a initialization test        */
+/****************************************/
+int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size) {
+/* 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 = strtol(hstnm + 4, NULL, 10);
+/* 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 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, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
+	buzzvm_destroy(&VM);
+	buzzdebug_destroy(&DBG_INFO);
+	fprintf(stdout, "%s: Error loading Buzz script\n\n", BO_FNAME);
+	return 0;
+}
+/* Register hook functions */
+if(testing_buzz_register_hooks() != BUZZVM_STATE_READY) {
+	buzzvm_destroy(&VM);
+	buzzdebug_destroy(&DBG_INFO);
+	fprintf(stdout, "%s: Error registering hooks\n\n", BO_FNAME);
+	return 0;
+}
+
+/* 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;
 	}
-
-	/****************************************/
-	/*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, "log", 1));
-                buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
-                buzzvm_gstore(VM);
-                buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_moveto", 1));
-   		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_moveto));
-   		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;
+	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*/
 
-	/**************************************************/
-	/*Register dummy Buzz hooks for test during update*/
-	/**************************************************/
-
-	static int testing_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, "log", 1));
-                buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
-                buzzvm_gstore(VM);
-                buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_moveto", 1));
-   		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
-   		buzzvm_gstore(VM);
-                buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_goto", 1));
-   		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
-   		buzzvm_gstore(VM);
-   		buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_takeoff", 1));
-   		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
-   		buzzvm_gstore(VM);
-   		buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_gohome", 1));
-   		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
-   		buzzvm_gstore(VM);
-   		buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_land", 1));
-   		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
-   		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];
-        //char* hstnm ="M1003";
-   	gethostname(hstnm, 30);
-   	/* Make numeric id from hostname */
-   	/* NOTE: here we assume that the hostname is in the format Knn */
-   	int id = strtol(hstnm+4, NULL, 10); 
-   	cout << " Robot ID: " <<id<< endl;
-   	/* Reset the Buzz VM */
-   	if(VM) buzzvm_destroy(&VM);
-   	VM = buzzvm_new((int)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 buzz_update_set(BO_BUF, bdbg_filename, bcode_size);
-	}
-
-	/****************************************/
-	/*Sets a new update                     */
-	/****************************************/
-	int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){
-	/* 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 = strtol(hstnm + 4, NULL, 10);
-	
-	/* 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 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, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
-      		buzzvm_destroy(&VM);
-      		buzzdebug_destroy(&DBG_INFO);
-      		fprintf(stdout, "%s: Error loading Buzz script\n\n", BO_FNAME);
-      		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_FNAME);
-        	return 0;
-   	}
- 
-   	/* 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;
-	}
-
-	/****************************************/
-	/*Performs a initialization test        */
-	/****************************************/
-	int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){
-	/* 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 = strtol(hstnm + 4, NULL, 10);
-	/* 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 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, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
-      		buzzvm_destroy(&VM);
-      		buzzdebug_destroy(&DBG_INFO);
-      		fprintf(stdout, "%s: Error loading Buzz script\n\n", BO_FNAME);
-      		return 0;
-   	 }
-   	/* Register hook functions */
-   	if(testing_buzz_register_hooks() != BUZZVM_STATE_READY) {
-      		buzzvm_destroy(&VM);
-      		buzzdebug_destroy(&DBG_INFO);
-      		fprintf(stdout, "%s: Error registering hooks\n\n", BO_FNAME);
-        	return 0;
-   	}
- 
-   	/* 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);
-	   buzzuav_closures::buzzuav_update_currentpos(VM);
-	   buzzuav_closures::buzzuav_update_flight_status(VM);
-	   buzzuav_closures::buzzuav_update_obstacle(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);
-	   }
-	   /*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/
-	   /* look into this currently we don't have swarm feature at all */
-	   /*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/
-	   /*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);*/
-	}
+void buzz_script_step() {
+/*
+ * Update sensors
+ */
+buzzuav_closures::buzzuav_update_battery(VM);
+buzzuav_closures::buzzuav_update_prox(VM);
+buzzuav_closures::buzzuav_update_currentpos(VM);
+buzzuav_closures::buzzuav_update_flight_status(VM);
+buzzuav_closures::buzzuav_update_obstacle(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);
+}
+/*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/
+/* look into this currently we don't have swarm feature at all */
+/*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/
+/*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");
+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");
 }
-
 
 /****************************************/
 /****************************************/
@@ -550,41 +544,39 @@ void buzz_script_destroy() {
 /****************************************/
 
 int buzz_script_done() {
-   return VM->state != BUZZVM_STATE_READY;
+return VM->state != BUZZVM_STATE_READY;
 }
 
-int update_step_test(){
+int update_step_test() {
 
-  buzzuav_closures::buzzuav_update_battery(VM);
-  buzzuav_closures::buzzuav_update_prox(VM);
-  buzzuav_closures::buzzuav_update_currentpos(VM);
-  buzzuav_closures::buzzuav_update_flight_status(VM);
-  buzzuav_closures::buzzuav_update_obstacle(VM);
+	buzzuav_closures::buzzuav_update_battery(VM);
+	buzzuav_closures::buzzuav_update_prox(VM);
+	buzzuav_closures::buzzuav_update_currentpos(VM);
+	buzzuav_closures::buzzuav_update_flight_status(VM);
+	buzzuav_closures::buzzuav_update_obstacle(VM);
 
- int a = buzzvm_function_call(VM, "step", 0);
-if(a != BUZZVM_STATE_READY){
- fprintf(stdout, "step test VM state %i\n",a);
-  fprintf(stdout, " execution terminated abnormally\n\n");
-}
- return a == BUZZVM_STATE_READY;
+	int a = buzzvm_function_call(VM, "step", 0);
+	if(a != BUZZVM_STATE_READY) {
+		fprintf(stdout, "step test VM state %i\n",a);
+		fprintf(stdout, " execution terminated abnormally\n\n");
+	}
+	return a == BUZZVM_STATE_READY;
 }
 
-uint16_t get_robotid(){
+uint16_t get_robotid() {
 /* 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 = strtol(hstnm + 4, NULL, 10);
-	//fprintf(stdout, "Robot id from get rid buzz util:  %i\n",id);
+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 = strtol(hstnm + 4, NULL, 10);
+//fprintf(stdout, "Robot id from get rid buzz util:  %i\n",id);
 return (uint16_t)id;
 }
 
-buzzvm_t get_vm(){
+buzzvm_t get_vm() {
 return VM;
 }
 
 }
 
-
-
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index 07d337f..6f4fb3a 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -238,7 +238,7 @@ namespace rosbzz_node{
 		}
 	}
 
-	void roscontroller::WaypointMissionSetup(){
+	void roscontroller::WaypointMissionSetup(float lat, float lng, float alt){
 		mavros_msgs::WaypointPush srv;
 		mavros_msgs::Waypoint waypoint;
 
@@ -247,10 +247,9 @@ namespace rosbzz_node{
 		waypoint.command = mavros_msgs::CommandCode::NAV_WAYPOINT;
 		waypoint.is_current = 2;	// IMPORTANT: goto is no longer used, so instead, use magic number 2 for is_current parameter
 		waypoint.autocontinue = 0;
-		waypoint.x_lat = 45.507730f;
-		waypoint.y_long = -73.613961f;
-		//45.507730, -73.613961
-		waypoint.z_alt = 10;
+		waypoint.x_lat = lat;
+		waypoint.y_long = lng;
+		waypoint.z_alt = alt;
 
 		srv.request.waypoints.push_back(waypoint);
 		if(mission_client.call(srv)){
@@ -731,10 +730,10 @@ namespace rosbzz_node{
    				ROS_INFO("RC_call: TAKE OFF!!!!");
 				rc_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF;
 				/* arming */
-				SetMode("GUIDED", 0);
-				//SetMode("LOITER", 0);
+				SetMode("LOITER", 0);
+				//SetMode("GUIDED", 0);
 				cout << "this..." << endl;
-				//SetModeAsync("GUIDED", 5000);
+				SetModeAsync("GUIDED", 2000);
 				Arm();
 				buzzuav_closures::rc_call(rc_cmd);
 				res.success = true;
@@ -754,12 +753,24 @@ namespace rosbzz_node{
 			case mavros_msgs::CommandCode::NAV_WAYPOINT:
    				ROS_INFO("RC_Call: GO TO!!!! ");
 
-   				WaypointMissionSetup();
-
+   				//WaypointMissionSetup();
    				double rc_goto[3];
 				rc_goto[0] = req.param5;
 				rc_goto[1] = req.param6;
 				rc_goto[2] = req.param7;
+
+				WaypointMissionSetup(req.param5, req.param6, req.param7);
+				/*
+				WaypointMissionSetup(45.505293f, -73.614722f, 2.0f);
+					std::this_thread::sleep_for( std::chrono::milliseconds ( 100 ) );
+				WaypointMissionSetup(45.505324f, -73.614502f, 2.0f);
+					std::this_thread::sleep_for( std::chrono::milliseconds ( 100 ) );
+				WaypointMissionSetup(45.505452f, -73.614655f, 2.0f);
+					std::this_thread::sleep_for( std::chrono::milliseconds ( 100 ) );
+				WaypointMissionSetup(45.505463f, -73.614389f, 2.0f);
+					std::this_thread::sleep_for( std::chrono::milliseconds ( 100 ) );
+				*/
+
 				buzzuav_closures::rc_set_goto(rc_goto);
 				rc_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
 				buzzuav_closures::rc_call(rc_cmd);

From 23532118c4fc7ddeaff18c9800e834958234458a Mon Sep 17 00:00:00 2001
From: isvogor <isvogor@gmail.com>
Date: Tue, 21 Feb 2017 19:27:57 -0500
Subject: [PATCH 10/12] launch

---
 launch/rosbuzz-testing.launch | 32 ++++++++++++++++++++++++++++++++
 1 file changed, 32 insertions(+)
 create mode 100644 launch/rosbuzz-testing.launch

diff --git a/launch/rosbuzz-testing.launch b/launch/rosbuzz-testing.launch
new file mode 100644
index 0000000..4baca4f
--- /dev/null
+++ b/launch/rosbuzz-testing.launch
@@ -0,0 +1,32 @@
+<launch>
+	<arg name="path_save"   default="/home/pi/bags"/>
+	<arg name="file_name"  	default="test.bag"/>
+
+	<group ns="record">
+		<param name="path_save" type="str" value="$(arg path_save)"	/>
+		<param name="file_name" type="str" value="$(arg file_name)"	/>
+		<node name="record_ros" pkg="record_ros" type="record_ros" output="screen" respawn="true"/>
+	</group>
+
+	<node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" />
+	<param name="Xbee_In_From_Buzz" type="str" value="outMavlink" />
+	<param name="Xbee_Out_To_Buzz" type="str" value="inMavlink" />
+	<param name="Xbee_In_From_Controller" type="str" value="xbee_cmd" />
+	<param name="No_of_dev" type="int" value="3" />
+	<param name="Xbee_Out_To_Controller" type="str" value="mav_dji_cmd" />
+
+	
+	<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >	
+		<rosparam file="/home/pi/ros_catkin_ws/src/ROSBuzz/launch/launch_config/solo.yaml"/>
+		<param name="bzzfile_name" value="/home/pi/ros_catkin_ws/src/ROSBuzz/src/testalone.bzz" />
+	    <param name="rcclient" value="true" /> 
+		<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="No_of_Robots" value="3"/>
+		<param name="stand_by" value="$(env ROS_WS)/src/ROSBuzz/src/stand_by.bo"/>
+	</node>
+
+</launch>
+

From 76027ffc091b8054cc480446fb639546be17f31b Mon Sep 17 00:00:00 2001
From: isvogor <isvogor@gmail.com>
Date: Wed, 22 Feb 2017 08:13:23 -0500
Subject: [PATCH 11/12] :wrench:

---
 launch/rosbuzz-testing.launch | 34 +++++++++++++++++++++++-----------
 misc/cmdlinectr.sh            | 20 ++++++++++++++++++++
 2 files changed, 43 insertions(+), 11 deletions(-)
 create mode 100644 misc/cmdlinectr.sh

diff --git a/launch/rosbuzz-testing.launch b/launch/rosbuzz-testing.launch
index 4baca4f..d674c14 100644
--- a/launch/rosbuzz-testing.launch
+++ b/launch/rosbuzz-testing.launch
@@ -1,13 +1,26 @@
 <launch>
-	<arg name="path_save"   default="/home/pi/bags"/>
-	<arg name="file_name"  	default="test.bag"/>
+	<!-- RUN mavros -->
+	<arg name="fcu_url" default="/dev/ttyAMA0:115200" />
+	<arg name="gcs_url" default="" />
+	<arg name="tgt_system" default="1" />
+	<arg name="tgt_component" default="1" />
+	<arg name="log_output" default="screen" />
 
-	<group ns="record">
-		<param name="path_save" type="str" value="$(arg path_save)"	/>
-		<param name="file_name" type="str" value="$(arg file_name)"	/>
-		<node name="record_ros" pkg="record_ros" type="record_ros" output="screen" respawn="true"/>
-	</group>
+	<include file="$(find mavros)/launch/node.launch">
+		<arg name="pluginlists_yaml" value="$(find mavros)/launch/apm_pluginlists.yaml" />
+		<arg name="config_yaml" value="$(find mavros)/launch/apm_config.yaml" />
 
+		<arg name="fcu_url" value="$(arg fcu_url)" />
+		<arg name="gcs_url" value="$(arg gcs_url)" />
+		<arg name="tgt_system" value="$(arg tgt_system)" />
+		<arg name="tgt_component" value="$(arg tgt_component)" />
+		<arg name="log_output" value="$(arg log_output)" />
+	</include>
+
+	<!-- set streaming rate -->
+	<node pkg="rosservice" type="rosservice" name="freq" args="call /mavros/set_stream_rate 0 10 1" output="screen" />
+
+	<!-- run xbee -->
 	<node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" />
 	<param name="Xbee_In_From_Buzz" type="str" value="outMavlink" />
 	<param name="Xbee_Out_To_Buzz" type="str" value="inMavlink" />
@@ -15,18 +28,17 @@
 	<param name="No_of_dev" type="int" value="3" />
 	<param name="Xbee_Out_To_Controller" type="str" value="mav_dji_cmd" />
 
-	
+	<!-- run rosbuzz -->
 	<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >	
 		<rosparam file="/home/pi/ros_catkin_ws/src/ROSBuzz/launch/launch_config/solo.yaml"/>
 		<param name="bzzfile_name" value="/home/pi/ros_catkin_ws/src/ROSBuzz/src/testalone.bzz" />
-	    <param name="rcclient" value="true" /> 
+		<param name="rcclient" value="true" /> 
 		<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="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/pi/ros_catkin_ws/src/ROSBuzz/src/stand_by.bo"/>
 	</node>
 
 </launch>
-
diff --git a/misc/cmdlinectr.sh b/misc/cmdlinectr.sh
new file mode 100644
index 0000000..e00b73b
--- /dev/null
+++ b/misc/cmdlinectr.sh
@@ -0,0 +1,20 @@
+#! /bin/bash
+function takeoff {
+	rosservice call /buzzcmd 0 22 0 0 0 0 0 0 0 0
+}
+function land {
+	rosservice call /buzzcmd 0 21 0 0 0 0 0 0 0 0
+}
+function record {
+	rosbag record /flight_status /global_position /dji_sdk/local_position /neighbours_pos /power_status /guidance/obstacle_distance /guidance/front_depth_image/compressedDepth /guidance/right_depth_image/compressedDepth /guidance/left_depth_image/compressedDepth /guidance/bottom_depth_image/compressedDepth /guidance/back_depth_image/compressedDepth
+}
+function clean {
+	sudo rm /var/log/upstart/robot*
+	sudo rm /var/log/upstart/x3s*
+}
+function startrobot {
+	sudo service robot start
+}
+function stoprobot {
+	sudo service robot stop
+}

From 94b2f4f4ac4448cab0f9168cb1743e57a8b21c62 Mon Sep 17 00:00:00 2001
From: isvogor <isvogor@gmail.com>
Date: Thu, 23 Feb 2017 10:57:05 -0500
Subject: [PATCH 12/12] ...

---
 CMakeLists.txt                     |  1 -
 launch/rosbuzz-testing-sitl.launch | 47 ++++++++++++++++++++++++++++++
 launch/rosbuzz.launch              |  9 +++---
 3 files changed, 51 insertions(+), 6 deletions(-)
 create mode 100644 launch/rosbuzz-testing-sitl.launch

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 0df4d4c..385dec8 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -15,7 +15,6 @@ find_package(catkin REQUIRED COMPONENTS
 )
 
 ##############################
-#Generate messages#
 ##############################
 
 add_message_files(
diff --git a/launch/rosbuzz-testing-sitl.launch b/launch/rosbuzz-testing-sitl.launch
new file mode 100644
index 0000000..d832e25
--- /dev/null
+++ b/launch/rosbuzz-testing-sitl.launch
@@ -0,0 +1,47 @@
+<launch>
+	<!-- RUN mavros -->
+	<arg name="fcu_url" default="tcp://127.0.0.1:5760" />
+	<arg name="gcs_url" default="" />
+	<arg name="tgt_system" default="1" />
+	<arg name="tgt_component" default="1" />
+	<arg name="log_output" default="screen" />
+
+	<include file="$(find mavros)/launch/node.launch">
+		<arg name="pluginlists_yaml" value="$(find mavros)/launch/apm_pluginlists.yaml" />
+		<arg name="config_yaml" value="$(find mavros)/launch/apm_config.yaml" />
+		<arg name="fcu_url" value="$(arg fcu_url)" />
+		<arg name="gcs_url" value="$(arg gcs_url)" />
+		<arg name="tgt_system" value="$(arg tgt_system)" />
+		<arg name="tgt_component" value="$(arg tgt_component)" />
+		<arg name="log_output" value="$(arg log_output)" />
+	</include>
+
+	<!-- run xbee -->
+	<node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" />
+	<param name="Xbee_In_From_Buzz" type="str" value="outMavlink" />
+	<param name="Xbee_Out_To_Buzz" type="str" value="inMavlink" />
+	<param name="Xbee_In_From_Controller" type="str" value="xbee_cmd" />
+	<param name="No_of_dev" type="int" value="3" />
+
+	<!-- run rosbuzz -->
+	<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >	
+		<rosparam file="/home/ivan/catkin_ws/src/rosbuzz/launch/launch_config/solo.yaml"/>
+		<param name="bzzfile_name" value="/home/ivan/catkin_ws/src/rosbuzz/src/testalone.bzz" />
+		<param name="rcclient" value="true" /> 
+		<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="No_of_Robots" value="3"/>
+		<param name="stand_by" value="/home/ivan/catkin_ws/src/rosbuzz/src/stand_by.bo"/>
+	</node>
+		
+	<node pkg="rosloader" type="rosloader" name="rosloader" output="screen"/> 
+		
+	<!-- set streaming rate 
+	/mavros/cmd/arming
+	<node pkg="rosservice" type="rosservice" name="stream_rate" args="call /mavros/set_stream_rate 0 10 1" />
+	-->
+	
+
+</launch>
diff --git a/launch/rosbuzz.launch b/launch/rosbuzz.launch
index 47f5a0f..a187ab4 100644
--- a/launch/rosbuzz.launch
+++ b/launch/rosbuzz.launch
@@ -2,15 +2,14 @@
 
 <launch>
 		<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >	
-		<rosparam file="$(env ROS_WS)/src/ROSBuzz/launch/launch_config/$(env ROBOT).yaml"/>
-		<param name="bzzfile_name" value="$(env ROS_WS)/src/ROSBuzz/src/testalone.bzz" />
+		<rosparam file="/home/ivan/catkin_ws/src/rosbuzz/launch/launch_config/solo.yaml"/>
+		<param name="bzzfile_name" value="/home/ivan/catkin_ws/src/rosbuzz/src/testalone.bzz" />
         <param name="rcclient" value="true" /> 
 		<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="No_of_Robots" value="3"/>
-		<param name="stand_by" value="$(env ROS_WS)/src/ROSBuzz/src/stand_by.bo"/>
-		</node>
-		
+		<param name="stand_by" value="/home/ivan/catkin_ws/src/rosbuzz/src/stand_by.bo"/>
+		</node>	
 </launch>