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/38] 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/38] 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/38] 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/38] 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/38] :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/38] ...

---
 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/38] 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/38] 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/38] 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/38] 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/38] :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/38] ...

---
 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 5f4aaeb1e01a414eef4e9bea43db76c446f985e6 Mon Sep 17 00:00:00 2001
From: isvogor <isvogor@gmail.com>
Date: Thu, 23 Feb 2017 21:10:49 -0500
Subject: [PATCH 13/38] Ready for final test :feelsgood: -- SOLO

---
 include/roscontroller.h       |  13 +-
 launch/rosbuzz-testing.launch |   3 +-
 src/buzz_utility.cpp          | 997 ++++++++++++++++------------------
 src/roscontroller.cpp         | 214 +++++---
 4 files changed, 605 insertions(+), 622 deletions(-)

diff --git a/include/roscontroller.h b/include/roscontroller.h
index e0f76c5..c6c2ec7 100644
--- a/include/roscontroller.h
+++ b/include/roscontroller.h
@@ -11,9 +11,9 @@
 #include "mavros_msgs/State.h"
 #include "mavros_msgs/BatteryStatus.h"
 #include "mavros_msgs/Mavlink.h"
+#include "sensor_msgs/NavSatStatus.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>
 #include <sstream>
@@ -86,7 +86,7 @@ private:
 
   	ros::ServiceClient mission_client;
 
-	void Initialize_pub_sub(ros::NodeHandle n_c);
+    void Initialize_pub_sub(ros::NodeHandle n_c);
 
 	/*Obtain data from ros parameter server*/	
 	void Rosparameters_get(ros::NodeHandle n_c);
@@ -153,16 +153,15 @@ 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);
 
-	void WaypointMissionSetup(float lat, float lng, float alg);
+	void WaypointMissionSetup(float lat, float lng, float alt);
 
+	void fc_command_setup();
 };
 
 }
diff --git a/launch/rosbuzz-testing.launch b/launch/rosbuzz-testing.launch
index d674c14..f76f47d 100644
--- a/launch/rosbuzz-testing.launch
+++ b/launch/rosbuzz-testing.launch
@@ -26,12 +26,11 @@
 	<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" />
 
 	<!-- 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="bzzfile_name" value="/home/pi/ros_catkin_ws/src/ROSBuzz/src/testflockfev.bzz" />
 		<param name="rcclient" value="true" /> 
 		<param name="rcservice_name" value="/buzzcmd" />
 		<param name="in_payload" value="/inMavlink"/>
diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp
index d7cae2a..31bd30b 100644
--- a/src/buzz_utility.cpp
+++ b/src/buzz_utility.cpp
@@ -8,164 +8,220 @@
 
 #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
+	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);
+	/****************************************/
+	/*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;
+   		/* 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*/
-/***************************************************/
+			/*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*/
+	/***************************************************/
 
-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;
+   	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;
 		}
 
-<<<<<<< HEAD
+	/****************************************/
+	/*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*/
 	/****************************************/
@@ -229,379 +285,269 @@ uint64_t* out_msg_process() {
    		buzzvm_gstore(VM);
    	return BUZZVM_STATE_READY;
 	}
-=======
-		/* 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);
->>>>>>> 4ac9d89f7c5e82fe99a48f616c587efd01d50ddd
 
-		/* Get rid of message */
-		buzzoutmsg_queue_next(VM);
-		buzzmsg_payload_destroy(&m);
-	} while (1);
+	/****************************************/
+	/*Sets the .bzz and .bdbg file*/
+	/****************************************/
 
-	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;
+	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);
 	}
-	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);*/
-}
+	/****************************************/
+	/*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);*/
+	}
 
 /****************************************/
 /*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");
 }
 
+
 /****************************************/
 /****************************************/
 
@@ -610,39 +556,40 @@ fprintf(stdout, "Script execution stopped.\n");
 /****************************************/
 
 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 22d6106..144909f 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -1,6 +1,7 @@
 #include "roscontroller.h"
 #include <thread>
 
+
 namespace rosbzz_node{
 
 	/***Constructor***/
@@ -76,7 +77,6 @@ namespace rosbzz_node{
  				/*sleep for the mentioned loop rate*/
     				timer_step+=1;
    				maintain_pos(timer_step);
-				
 			}
 			/* Destroy updater and Cleanup */
     			//update_routine(bcfname.c_str(), dbgfname.c_str(),1);
@@ -162,11 +162,13 @@ 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);
+		mission_client = n_c.serviceClient<mavros_msgs::WaypointPush>("/mavros/mission/push");
+
 
 		multi_msg=true;
+		armstate = 0;
 	}
 	
 	void roscontroller::Subscribe(ros::NodeHandle n_c){
@@ -226,6 +228,18 @@ namespace rosbzz_node{
 		}
 	}
 
+	void roscontroller::SetMode(){
+		mavros_msgs::SetMode set_mode_message;
+		set_mode_message.request.base_mode = 0;
+		set_mode_message.request.custom_mode = "GUIDED"; // shit!
+		if(mode_client.call(set_mode_message)) {
+			ROS_INFO("Set Mode Service call successful!");
+		} else {
+			ROS_INFO("Set Mode Service call failed!");
+		}
+	}
+
+
 	void roscontroller::WaypointMissionSetup(float lat, float lng, float alt){
 		mavros_msgs::WaypointPush srv;
 		mavros_msgs::Waypoint waypoint;
@@ -241,36 +255,46 @@ namespace rosbzz_node{
 
 		srv.request.waypoints.push_back(waypoint);
 		if(mission_client.call(srv)){
-			ROS_INFO("Mission service called!");
+			ROS_INFO("Waypoint setup service called!");
 		} else {
-			ROS_INFO("Mission service failed!");
+			ROS_INFO("Waypoint setup service failed!");
 		}
 
 	}
 
-	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 = mode;
-		if(mode_client.call(set_mode_message)) {
-			ROS_INFO("Set Mode Service call successful!");
-		} else {
-			ROS_INFO("Set Mode Service call failed!");
+	void roscontroller::fc_command_setup(){
+ 		/* flight controller client call if requested from Buzz*/
+		/*FC call for takeoff,land and gohome*/
+		int tmp = buzzuav_closures::bzz_cmd();
+    	double* goto_pos = buzzuav_closures::getgoto();
+
+    	if (tmp == 1){
+			cmd_srv.request.param7 = goto_pos[2];
+			//cmd_srv.request.z = goto_pos[2];
+			cmd_srv.request.command =  buzzuav_closures::getcmd();
+			if (mav_client.call(cmd_srv)){
+				ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
+			}
+	    	else
+	    		ROS_ERROR("Failed to call service from flight controller");
+
+		} else if (tmp == 2) { /*FC call for goto*/
+			/*prepare the goto publish message */
+			cmd_srv.request.param5 = goto_pos[0];
+    			cmd_srv.request.param6 = goto_pos[1];
+    			cmd_srv.request.param7 = goto_pos[2];
+    			cmd_srv.request.command =  buzzuav_closures::getcmd();
+			if (mav_client.call(cmd_srv)){ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); }
+	    		else ROS_ERROR("Failed to call service from flight controller");
+    			cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START;
+			if (mav_client.call(cmd_srv)){ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); }
+	    		else ROS_ERROR("Failed to call service from flight controller");
+		} else if (tmp == 3) { /*FC call for arm*/
+			Arm();
 		}
 	}
 
-	//todo: this
-	void roscontroller::SetModeAsync(std::string mode, int delay_miliseconds){
-		std::thread([&](){SetMode(mode, delay_miliseconds);}).detach();
-		cout << "Async call called... " <<endl;
-	}
-
-
+	// TODO: split this
 	/*Prepare messages for each step and publish*/
 	/*******************************************************************************************************/
 	/* Message format of payload (Each slot is uint64_t)						       */
@@ -278,34 +302,51 @@ namespace rosbzz_node{
 	/*|	|     |	    |						     |			            |  */
 	/*|Pos x|Pos y|Pos z|Size in Uint64_t|robot_id|Buzz_msg_size|Buzz_msg|Buzz_msgs with size.........  |  */
 	/*|_____|_____|_____|________________________________________________|______________________________|  */
-	/*******************************************************************************************************/	
+	/*******************************************************************************************************/
 	void roscontroller::prepare_msg_and_publish(){
-		
- 		/* flight controller client call if requested from Buzz*/
-		/*FC call for takeoff,land and gohome*/
-		int tmp = buzzuav_closures::bzz_cmd();
-    		double* goto_pos = buzzuav_closures::getgoto();
-		if (tmp == 1){
-			cmd_srv.request.param7 = goto_pos[2];
-			//cmd_srv.request.z = goto_pos[2];
-			cmd_srv.request.command =  buzzuav_closures::getcmd();  		
-			if (mav_client.call(cmd_srv)){ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); }
-	    		else ROS_ERROR("Failed to call service from flight controller"); 
-		} else if (tmp == 2) { /*FC call for goto*/ 
-			/*prepare the goto publish message */
-			cmd_srv.request.param5 = goto_pos[0];
-    			cmd_srv.request.param6 = goto_pos[1];
-    			cmd_srv.request.param7 = goto_pos[2];
-    			cmd_srv.request.command =  buzzuav_closures::getcmd();
-			if (mav_client.call(cmd_srv)){ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); }
-	    		else ROS_ERROR("Failed to call service from flight controller"); 
-    			cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START;
-			if (mav_client.call(cmd_srv)){ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); }
-	    		else ROS_ERROR("Failed to call service from flight controller"); 
-		} else if (tmp == 3) { /*FC call for arm*/
-			Arm(); 
+
+		if(fcclient_name == "/mavros/cmd/command"){
+			int tmp = buzzuav_closures::bzz_cmd();
+	    	double* goto_pos = buzzuav_closures::getgoto();
+
+	    	switch(tmp){
+	    	// TAKEOFF -- LAND
+	    	case 1:
+	    		if(!armstate){
+					SetMode("LOITER", 0);
+	    			Arm();
+	    			armstate = 1;
+					SetMode("GUIDED", 2000);
+	    		}
+				cmd_srv.request.param7 = goto_pos[2];
+				cmd_srv.request.command =  buzzuav_closures::getcmd();
+
+	    		// just to be safe -- while landing!
+	    		if(armstate == 1 && buzzuav_closures::getcmd() == 22){
+					SetMode("GUIDED", 0);
+	    		}
+
+				if (mav_client.call(cmd_srv)){
+					ROS_INFO("Reply: %ld", (long int) cmd_srv.response.success);
+				}
+		    	else
+		    		ROS_ERROR("Failed to call service from flight controller");
+				break;
+			// GOTO
+	    	case 2:
+				WaypointMissionSetup(goto_pos[0], goto_pos[1], goto_pos[2]);
+				break;
+			// ARM
+	    	case 3:
+	    		Arm();
+	    		break;
+	    	}
+
+		} else {
+			fc_command_setup();
 		}
-    		/*obtain Pay load to be sent*/  
+
+    		/*obtain Pay load to be sent*/
 		//fprintf(stdout, "before getting msg from utility\n");
    		uint64_t* payload_out_ptr= buzz_utility::out_msg_process();
     		uint64_t  position[3];
@@ -316,10 +357,10 @@ namespace rosbzz_node{
     		payload_out.payload64.push_back(position[1]);
     		payload_out.payload64.push_back(position[2]);
     		/*Append Buzz message*/
-    		uint16_t* out = buzz_utility::u64_cvt_u16(payload_out_ptr[0]);  
+    		uint16_t* out = buzz_utility::u64_cvt_u16(payload_out_ptr[0]);
     		for(int i=0;i<out[0];i++){
     			payload_out.payload64.push_back(payload_out_ptr[i]);
-    		}	
+    		}
     		/*int i=0;
 		uint64_t message_obt[payload_out.payload64.size()];
     		for(std::vector<long unsigned int>::const_iterator it = payload_out.payload64.begin();
@@ -340,13 +381,13 @@ namespace rosbzz_node{
 		else message_number++;
 		payload_out.sysid=(uint8_t)robot_id;
 		payload_out.msgid=(uint32_t)message_number;
-		
+
      		/*publish prepared messages in respective topic*/
     		payload_pub.publish(payload_out);
     		delete[] out;
     		delete[] payload_out_ptr;
 
-		
+
 		if((int)get_update_mode()!=CODE_RUNNING && is_msg_present()==1 && multi_msg){
 			uint8_t* buff_send = 0;
 	   		uint16_t updater_msgSize=*(uint16_t*) (getupdate_out_msg_size());;
@@ -390,10 +431,10 @@ namespace rosbzz_node{
 		//	mavros_msgs::Mavlink stop_req_packet;
 		//	stop_req_packet.payload64.push_back((uint64_t) XBEE_STOP_TRANSMISSION);
 		//	payload_pub.publish(stop_req_packet);
-		//	std::cout << "request xbee to stop multi-packet transmission" << std::endl;	
-		
+		//	std::cout << "request xbee to stop multi-packet transmission" << std::endl;
+
 		//}
-		
+
 		}
 
 
@@ -586,7 +627,7 @@ namespace rosbzz_node{
    		}
 	}
 
-	/*battery status callback*/ 
+	/*battery status callback*/
 	void roscontroller::battery(const mavros_msgs::BatteryStatus::ConstPtr& msg){
 		buzzuav_closures::set_battery(msg->voltage,msg->current,msg->remaining);
 		//ROS_INFO("voltage : %d  current : %d  remaining : %d",msg->voltage, msg->current, msg ->remaining);
@@ -601,7 +642,7 @@ namespace rosbzz_node{
 			buzzuav_closures::flight_status_update(1);
 		else if (msg->mode == "LAND")
 			buzzuav_closures::flight_status_update(4);
-		else
+		else // ground standby = LOITER?
 			buzzuav_closures::flight_status_update(1);//?
 	}
 
@@ -614,7 +655,7 @@ namespace rosbzz_node{
 		set_cur_pos(msg->latitude,msg->longitude,msg->altitude);
 		buzzuav_closures::set_currentpos(msg->latitude,msg->longitude,msg->altitude);
 	}
-	/*Obstacle distance call back*/	
+	/*Obstacle distance call back*/
 	void roscontroller::obstacle_dist(const sensor_msgs::LaserScan::ConstPtr& msg){
 		float obst[5];
 		for(int i=0;i<5;i++)
@@ -629,8 +670,8 @@ namespace rosbzz_node{
 	/*|	|     |	    |						     |			            |  */
 	/*|Pos x|Pos y|Pos z|Size in Uint64_t|robot_id|Buzz_msg_size|Buzz_msg|Buzz_msgs with size.........  |  */
 	/*|_____|_____|_____|________________________________________________|______________________________|  */
-	/*******************************************************************************************************/	
-	
+	/*******************************************************************************************************/
+
 	void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg){
 
 		/*Ack from xbee about its transfer complete of multi packet*/
@@ -705,9 +746,25 @@ namespace rosbzz_node{
 			delete[] out;
 			buzz_utility::in_msg_process((message_obt+3));
 		}
-   
+
 	}
- 	
+
+	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 = mode;
+		if(mode_client.call(set_mode_message)) {
+			ROS_INFO("Set Mode Service call successful!");
+		} else {
+			ROS_INFO("Set Mode Service call failed!");
+		}
+	}
+
 	/* RC command service */
 	bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request  &req,
 		         mavros_msgs::CommandLong::Response &res){
@@ -720,19 +777,14 @@ namespace rosbzz_node{
    				ROS_INFO("RC_call: TAKE OFF!!!!");
 				rc_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF;
 				/* arming */
-<<<<<<< HEAD
 				SetMode();
+				cout << "ARM: " << armstate <<endl;
 				if(!armstate) {
-					armstate =1;
+					armstate = 1;
+					SetMode("LOITER", 0);
 					Arm();
+					if(fcclient_name == "/mavros/cmd/command") { SetMode("LOITER", 2000); }
 				}
-=======
-				SetMode("LOITER", 0);
-				//SetMode("GUIDED", 0);
-				cout << "this..." << endl;
-				SetModeAsync("GUIDED", 2000);
-				Arm();
->>>>>>> 4ac9d89f7c5e82fe99a48f616c587efd01d50ddd
 				buzzuav_closures::rc_call(rc_cmd);
 				res.success = true;
 				break;
@@ -759,25 +811,12 @@ namespace rosbzz_node{
 				res.success = true;
 				break;
 			case mavros_msgs::CommandCode::NAV_WAYPOINT:
-   				ROS_INFO("RC_Call: GO TO!!!! ");
-
-   				//WaypointMissionSetup();
-   				double rc_goto[3];
+				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;
@@ -797,4 +836,3 @@ namespace rosbzz_node{
 	
 }
 
-

From 133f6dbd0235219f1fa0a53c18162c6bca54ba1c Mon Sep 17 00:00:00 2001
From: isvogor <isvogor@gmail.com>
Date: Thu, 23 Feb 2017 22:13:44 -0500
Subject: [PATCH 14/38] fixes

---
 src/roscontroller.cpp | 13 ++++++++++---
 1 file changed, 10 insertions(+), 3 deletions(-)

diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index 144909f..193cb23 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -308,6 +308,7 @@ namespace rosbzz_node{
 		if(fcclient_name == "/mavros/cmd/command"){
 			int tmp = buzzuav_closures::bzz_cmd();
 	    	double* goto_pos = buzzuav_closures::getgoto();
+	    	cout << "Flying Solo today? " << endl;
 
 	    	switch(tmp){
 	    	// TAKEOFF -- LAND
@@ -322,8 +323,8 @@ namespace rosbzz_node{
 				cmd_srv.request.command =  buzzuav_closures::getcmd();
 
 	    		// just to be safe -- while landing!
-	    		if(armstate == 1 && buzzuav_closures::getcmd() == 22){
-					SetMode("GUIDED", 0);
+	    		if(armstate == 1 && buzzuav_closures::getcmd() == 21){
+					SetMode("LAND", 0);
 	    		}
 
 				if (mav_client.call(cmd_srv)){
@@ -643,7 +644,10 @@ namespace rosbzz_node{
 		else if (msg->mode == "LAND")
 			buzzuav_closures::flight_status_update(4);
 		else // ground standby = LOITER?
+		{
+			cout << "Warning -- something else, going to GUIDED!" << endl;
 			buzzuav_closures::flight_status_update(1);//?
+		}
 	}
 
 	/*flight extended status update*/
@@ -783,7 +787,7 @@ namespace rosbzz_node{
 					armstate = 1;
 					SetMode("LOITER", 0);
 					Arm();
-					if(fcclient_name == "/mavros/cmd/command") { SetMode("LOITER", 2000); }
+					if(fcclient_name == "/mavros/cmd/command") { SetMode("GUIDED", 2000); }
 				}
 				buzzuav_closures::rc_call(rc_cmd);
 				res.success = true;
@@ -791,6 +795,9 @@ namespace rosbzz_node{
 			case mavros_msgs::CommandCode::NAV_LAND:
    				ROS_INFO("RC_Call: LAND!!!!");
 				rc_cmd=mavros_msgs::CommandCode::NAV_LAND;
+				// again -- to be safe:
+				SetMode("LAND", 0);
+
 				buzzuav_closures::rc_call(rc_cmd);
 				res.success = true;
 				break;

From f5899ecac0f06e1e4fa395d004155ed714eaf23f Mon Sep 17 00:00:00 2001
From: isvogor <isvogor@gmail.com>
Date: Thu, 23 Feb 2017 22:34:19 -0500
Subject: [PATCH 15/38] fixes

---
 src/roscontroller.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index 193cb23..92a3eaf 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -323,7 +323,7 @@ namespace rosbzz_node{
 				cmd_srv.request.command =  buzzuav_closures::getcmd();
 
 	    		// just to be safe -- while landing!
-	    		if(armstate == 1 && buzzuav_closures::getcmd() == 21){
+	    		if(buzzuav_closures::getcmd() == 21){
 					SetMode("LAND", 0);
 	    		}
 

From 8be25947d920eca73d64d118d338a63cd210c3de Mon Sep 17 00:00:00 2001
From: isvogor <isvogor@gmail.com>
Date: Wed, 1 Mar 2017 15:48:16 -0500
Subject: [PATCH 16/38] added solo specifics

---
 include/roscontroller.h            |   4 +-
 launch/rosbuzz-testing-sitl.launch |  16 +--
 src/roscontroller.cpp              | 163 ++++++++++++-----------------
 src/testflockfev.bzz               |   3 +-
 4 files changed, 80 insertions(+), 106 deletions(-)

diff --git a/include/roscontroller.h b/include/roscontroller.h
index f009be4..29a201a 100644
--- a/include/roscontroller.h
+++ b/include/roscontroller.h
@@ -86,6 +86,8 @@ private:
 
   	ros::ServiceClient mission_client;
 
+  	std::string current_mode; // SOLO SPECIFIC: just so you don't call the switch to same mode
+
     void Initialize_pub_sub(ros::NodeHandle n_c);
 
 	/*Obtain data from ros parameter server*/	
@@ -141,8 +143,6 @@ private:
 
 	void Arm();
 
-	void SetMode();
-
 	void SetMode(std::string mode, int delay_miliseconds);
 
 	void Subscribe(ros::NodeHandle n_c);
diff --git a/launch/rosbuzz-testing-sitl.launch b/launch/rosbuzz-testing-sitl.launch
index d832e25..7dd98ff 100644
--- a/launch/rosbuzz-testing-sitl.launch
+++ b/launch/rosbuzz-testing-sitl.launch
@@ -1,5 +1,5 @@
 <launch>
-	<!-- RUN mavros -->
+	<!-- RUN mavros 
 	<arg name="fcu_url" default="tcp://127.0.0.1:5760" />
 	<arg name="gcs_url" default="" />
 	<arg name="tgt_system" default="1" />
@@ -15,18 +15,18 @@
 		<arg name="tgt_component" value="$(arg tgt_component)" />
 		<arg name="log_output" value="$(arg log_output)" />
 	</include>
-
-	<!-- run xbee -->
+-->
+	<!-- 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="bzzfile_name" value="/home/ivan/catkin_ws/src/rosbuzz/src/testflockfev.bzz" />
 		<param name="rcclient" value="true" /> 
 		<param name="rcservice_name" value="/buzzcmd" />
 		<param name="in_payload" value="/inMavlink"/>
@@ -35,9 +35,11 @@
 		<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" />
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index 001b4fc..ed514fe 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -1,7 +1,5 @@
 #include "roscontroller.h"
 #include <thread>
-
-
 namespace rosbzz_node{
 
 	/***Constructor***/
@@ -16,6 +14,8 @@ namespace rosbzz_node{
  		Compile_bzz();
 		set_bzz_file(bzzfile_name.c_str());
 		/*Initialize variables*/
+		//
+		SetMode("LOITER", 0);
 		armstate = 0;
 		multi_msg = true;
 	}
@@ -80,6 +80,7 @@ namespace rosbzz_node{
  				/*sleep for the mentioned loop rate*/
     				timer_step+=1;
    				maintain_pos(timer_step);
+
 			}
 			/* Destroy updater and Cleanup */
     			//update_routine(bcfname.c_str(), dbgfname.c_str(),1);
@@ -173,11 +174,8 @@ namespace rosbzz_node{
 		arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient);
 		mode_client =  n_c.serviceClient<mavros_msgs::SetMode>(modeclient);
 		mav_client = n_c.serviceClient<mavros_msgs::CommandLong>(fcclient_name);
-		mission_client = n_c.serviceClient<mavros_msgs::WaypointPush>("/mavros/mission/push");
-
 
 		multi_msg=true;
-		armstate = 0;
 	}
 	
 	void roscontroller::Subscribe(ros::NodeHandle n_c){
@@ -243,18 +241,6 @@ namespace rosbzz_node{
 		}
 	}
 
-	void roscontroller::SetMode(){
-		mavros_msgs::SetMode set_mode_message;
-		set_mode_message.request.base_mode = 0;
-		set_mode_message.request.custom_mode = "GUIDED"; // shit!
-		if(mode_client.call(set_mode_message)) {
-			ROS_INFO("Set Mode Service call successful!");
-		} else {
-			ROS_INFO("Set Mode Service call failed!");
-		}
-	}
-
-
 	/*-----------------------------------------------------------------
 	/Prepare Buzz messages payload for each step and publish
 	/
@@ -272,91 +258,62 @@ namespace rosbzz_node{
 		/* TODO: this should go in a separate function and be called by the main Buzz step */
 
 		int tmp = buzzuav_closures::bzz_cmd();
+		std::cout<< "Going: " << tmp << std::endl;
     	double* goto_pos = buzzuav_closures::getgoto();
-
-    	if (tmp == 1){
+    	// TODO: Make sure that land and takeoff use the right statuses in bzz
+		if (tmp == 1){
 			cmd_srv.request.param7 = goto_pos[2];
-			//cmd_srv.request.z = goto_pos[2];
 			cmd_srv.request.command =  buzzuav_closures::getcmd();
+			std::cout << " CMD: " << buzzuav_closures::getcmd() << std::endl;
+			// SOLO SPECIFIC: SITL DOES NOT USE 21 TO LAND -- workaround: set to LAND mode
+
+			switch(buzzuav_closures::getcmd()){
+			case mavros_msgs::CommandCode::NAV_LAND:
+				if(current_mode != "LAND")
+					SetMode("LAND", 0);
+				break;
+			case mavros_msgs::CommandCode::NAV_TAKEOFF:
+				if(current_mode != "GUIDED")
+					SetMode("GUIDED", 0); // for real solo, just add 2000ms delay (it should always be in loiter after arm/disarm)
+				break;
+			}
+
+			if (mav_client.call(cmd_srv))
+				{ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); }
+			else
+				ROS_ERROR("Failed to call service from flight controller");
+
+		} else if (tmp == 2) { /*FC call for goto*/
+			/*prepare the goto publish message */
+			cmd_srv.request.param5 = goto_pos[0];
+			cmd_srv.request.param6 = goto_pos[1];
+			cmd_srv.request.param7 = goto_pos[2];
+			cmd_srv.request.command =  buzzuav_closures::getcmd();
+
+			if (mav_client.call(cmd_srv)) {
+				ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
+			}
+	    	else
+	    		ROS_ERROR("Failed to call service from flight controller");
+
+			cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START;
+
 			if (mav_client.call(cmd_srv)){
 				ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
 			}
 	    	else
 	    		ROS_ERROR("Failed to call service from flight controller");
 
-		} else if (tmp == 2) { /*FC call for goto*/
-			/*prepare the goto publish message */
-			cmd_srv.request.param5 = goto_pos[0];
-    			cmd_srv.request.param6 = goto_pos[1];
-    			cmd_srv.request.param7 = goto_pos[2];
-    			cmd_srv.request.command =  buzzuav_closures::getcmd();
-			if (mav_client.call(cmd_srv)){ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); }
-	    		else ROS_ERROR("Failed to call service from flight controller");
-    			cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START;
-			if (mav_client.call(cmd_srv)){ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); }
-	    		else ROS_ERROR("Failed to call service from flight controller");
 		} else if (tmp == 3) { /*FC call for arm*/
+			// SOLO SPECIFIC: when you land, mode is LANDED, which is not armable, therefore change it to LOITER
+			SetMode("LOITER", 0);
 			armstate=1;
 			Arm(); 
 		} else if (tmp == 4){
+			SetMode("LOITER", 0);
 			armstate=0;
 			Arm();
 		}
-	}
-
-	// TODO: split this
-	/*Prepare messages for each step and publish*/
-	/*******************************************************************************************************/
-	/* Message format of payload (Each slot is uint64_t)						       */
-	/* _________________________________________________________________________________________________   */
-	/*|	|     |	    |						     |			            |  */
-	/*|Pos x|Pos y|Pos z|Size in Uint64_t|robot_id|Buzz_msg_size|Buzz_msg|Buzz_msgs with size.........  |  */
-	/*|_____|_____|_____|________________________________________________|______________________________|  */
-	/*******************************************************************************************************/
-	void roscontroller::prepare_msg_and_publish(){
-
-		if(fcclient_name == "/mavros/cmd/command"){
-			int tmp = buzzuav_closures::bzz_cmd();
-	    	double* goto_pos = buzzuav_closures::getgoto();
-	    	cout << "Flying Solo today? " << endl;
-
-	    	switch(tmp){
-	    	// TAKEOFF -- LAND
-	    	case 1:
-	    		if(!armstate){
-					SetMode("LOITER", 0);
-	    			Arm();
-	    			armstate = 1;
-					SetMode("GUIDED", 2000);
-	    		}
-				cmd_srv.request.param7 = goto_pos[2];
-				cmd_srv.request.command =  buzzuav_closures::getcmd();
-
-	    		// just to be safe -- while landing!
-	    		if(buzzuav_closures::getcmd() == 21){
-					SetMode("LAND", 0);
-	    		}
-
-				if (mav_client.call(cmd_srv)){
-					ROS_INFO("Reply: %ld", (long int) cmd_srv.response.success);
-				}
-		    	else
-		    		ROS_ERROR("Failed to call service from flight controller");
-				break;
-			// GOTO
-	    	case 2:
-				WaypointMissionSetup(goto_pos[0], goto_pos[1], goto_pos[2]);
-				break;
-			// ARM
-	    	case 3:
-	    		Arm();
-	    		break;
-	    	}
-
-		} else {
-			fc_command_setup();
-		}
-
     		/*obtain Pay load to be sent*/
 		//fprintf(stdout, "before getting msg from utility\n");
    		uint64_t* payload_out_ptr= buzz_utility::out_msg_process();
@@ -517,10 +474,7 @@ namespace rosbzz_node{
 		else if (msg->mode == "LAND")
 			buzzuav_closures::flight_status_update(4);
 		else // ground standby = LOITER?
-		{
-			cout << "Warning -- something else, going to GUIDED!" << endl;
 			buzzuav_closures::flight_status_update(1);//?
-		}
 	}
 
 	/*flight extended status update*/
@@ -624,15 +578,13 @@ namespace rosbzz_node{
    		switch(req.command){
 			case mavros_msgs::CommandCode::NAV_TAKEOFF:
    				ROS_INFO("RC_call: TAKE OFF!!!!");
+				rc_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF;
 				buzzuav_closures::rc_call(rc_cmd);
 				res.success = true;
 				break;
 			case mavros_msgs::CommandCode::NAV_LAND:
    				ROS_INFO("RC_Call: LAND!!!!");
 				rc_cmd=mavros_msgs::CommandCode::NAV_LAND;
-				// again -- to be safe:
-				SetMode("LAND", 0);
-
 				buzzuav_closures::rc_call(rc_cmd);
 				res.success = true;
 				break;
@@ -663,10 +615,8 @@ namespace rosbzz_node{
 				rc_goto[1] = req.param6;
 				rc_goto[2] = req.param7;
 
-				WaypointMissionSetup(req.param5, req.param6, req.param7);
-
 				buzzuav_closures::rc_set_goto(rc_goto);
-				rc_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
+				rc_cmd= mavros_msgs::CommandCode::NAV_WAYPOINT;
 				buzzuav_closures::rc_call(rc_cmd);
 				res.success = true;
 				break;
@@ -681,5 +631,26 @@ namespace rosbzz_node{
 	
 	}
 	
-}
+	/*
+	 * SOLO SPECIFIC FUNCTIONS
+	 */
 
+
+	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 = mode;
+		current_mode = mode;
+		if(mode_client.call(set_mode_message)) {
+			ROS_INFO("Set Mode Service call successful!");
+		} else {
+			ROS_INFO("Set Mode Service call failed!");
+		}
+	}
+
+}
diff --git a/src/testflockfev.bzz b/src/testflockfev.bzz
index 98abcd6..5d1990d 100644
--- a/src/testflockfev.bzz
+++ b/src/testflockfev.bzz
@@ -125,7 +125,8 @@ function takeoff() {
 }
 function land() {
 	log("Land: ", flight.status)
-	if(flight.status == 2 or flight.status == 3){
+	# SOLO: land = status 4, guided = 1
+	if(flight.status == 1){
 		neighbors.broadcast("cmd", 21)
 		uav_land()
 	}

From 002baf636d1206251ac7a3cbf790297cdc6d4b3b Mon Sep 17 00:00:00 2001
From: isvogor <isvogor@gmail.com>
Date: Tue, 14 Mar 2017 11:09:01 -0400
Subject: [PATCH 17/38] PositionTarget.msg testing

---
 include/roscontroller.h |  5 ++++
 src/roscontroller.cpp   | 61 +++++++++++++++++++++++++++++++++++------
 2 files changed, 58 insertions(+), 8 deletions(-)

diff --git a/include/roscontroller.h b/include/roscontroller.h
index 29a201a..a3062c1 100644
--- a/include/roscontroller.h
+++ b/include/roscontroller.h
@@ -14,6 +14,7 @@
 #include "sensor_msgs/NavSatStatus.h"
 #include "mavros_msgs/WaypointPush.h"
 #include "mavros_msgs/Waypoint.h"
+#include "mavros_msgs/PositionTarget.h"
 #include <sensor_msgs/LaserScan.h>
 #include <rosbuzz/neigh_pos.h>
 #include <sstream>
@@ -71,6 +72,8 @@ private:
 	ros::Subscriber flight_status_sub;
 	ros::Subscriber obstacle_sub;
 	ros::Subscriber Robot_id_sub;
+	ros::Publisher local_position_pub;
+
 	/*Commands for flight controller*/
   	//mavros_msgs::CommandInt cmd_srv;
   	mavros_msgs::CommandLong cmd_srv;
@@ -150,6 +153,8 @@ private:
 	void WaypointMissionSetup(float lat, float lng, float alt);
 
 	void fc_command_setup();
+
+	void LocalPosition(float x, float y, float z, float yaw);
 };
 
 }
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index ed514fe..2ef62df 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -58,12 +58,13 @@ namespace rosbzz_node{
 					//std::cout<<"long obt"<<neigh_tmp.longitude<<endl;  					
 					}
 				neigh_pos_pub.publish(neigh_pos_array); 
+
 			
 				/*Check updater state and step code*/
   				update_routine(bcfname.c_str(), dbgfname.c_str());
       				
 				/*Step buzz script */
-      				buzz_utility::buzz_script_step();
+      			buzz_utility::buzz_script_step();
 				/*Prepare messages and publish them in respective topic*/
 	
 		  		prepare_msg_and_publish();
@@ -81,6 +82,8 @@ namespace rosbzz_node{
     				timer_step+=1;
    				maintain_pos(timer_step);
 
+
+
 			}
 			/* Destroy updater and Cleanup */
     			//update_routine(bcfname.c_str(), dbgfname.c_str(),1);
@@ -168,6 +171,11 @@ namespace rosbzz_node{
 		Robot_id_sub = n_c.subscribe("/device_id_xbee_", 1000, &roscontroller::set_robot_id,this);
   		obstacle_sub= n_c.subscribe("/guidance/obstacle_distance",100, &roscontroller::obstacle_dist,this);
   		/*publishers*/
+
+  		//mavros_msgs/PositionTarget
+  		local_position_pub = n_c.advertise<mavros_msgs::PositionTarget>("/mavros/setpoint_raw/local", 10);
+  		//mavros_msgs::PositionTarget message;
+
 		payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 1000);
 		neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("/neighbours_pos",1000);	
 		/* Service Clients*/
@@ -241,13 +249,11 @@ namespace rosbzz_node{
 		}
 	}
 
-	/*-----------------------------------------------------------------
-	/Prepare Buzz messages payload for each step and publish
-	/
+	/* Prepare Buzz messages payload for each step and publish */
 	/*******************************************************************************************************/
-	/* Message format of payload (Each slot is uint64_t)						       */
+	/* Message format of payload (Each slot is uint64_t)						      					   */
 	/* _________________________________________________________________________________________________   */
-	/*|	|     |	    |						     |			            |  */
+	/*|	|     |	    |						     |			            | 								   */
 	/*|Pos x|Pos y|Pos z|Size in Uint64_t|robot_id|Buzz_msg_size|Buzz_msg|Buzz_msgs with size.........  |  */
 	/*|_____|_____|_____|________________________________________________|______________________________|  */
 	/*******************************************************************************************************/	
@@ -575,6 +581,7 @@ namespace rosbzz_node{
 	bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request  &req,
 		         mavros_msgs::CommandLong::Response &res){
 		int rc_cmd;
+
    		switch(req.command){
 			case mavros_msgs::CommandCode::NAV_TAKEOFF:
    				ROS_INFO("RC_call: TAKE OFF!!!!");
@@ -609,11 +616,14 @@ namespace rosbzz_node{
 				res.success = true;
 				break;
 			case mavros_msgs::CommandCode::NAV_WAYPOINT:
-   				ROS_INFO("RC_Call: GO TO!!!! ");
+   				ROS_INFO("RC_Call: GO TO!!!! --- Doing this! ");
 				double rc_goto[3];
+				// testing PositionTarget
 				rc_goto[0] = req.param5;
 				rc_goto[1] = req.param6;
 				rc_goto[2] = req.param7;
+				// param 4 for yaw
+				LocalPosition(rc_goto[0], rc_goto[1], rc_goto[2], req.param4);
 
 				buzzuav_closures::rc_set_goto(rc_goto);
 				rc_cmd= mavros_msgs::CommandCode::NAV_WAYPOINT;
@@ -628,13 +638,48 @@ namespace rosbzz_node{
 	}
 	void roscontroller::set_robot_id(const std_msgs::UInt8::ConstPtr& msg){
 	robot_id=(int)msg->data;
-	
+
 	}
 	
 	/*
 	 * SOLO SPECIFIC FUNCTIONS
 	 */
 
+	void roscontroller::LocalPosition(float x, float y, float z, float yaw){
+		// http://docs.ros.org/kinetic/api/mavros_msgs/html/msg/PositionTarget.html
+		// http://ardupilot.org/dev/docs/copter-commands-in-guided-mode.html#copter-commands-in-guided-mode-set-position-target-local-ned
+
+		mavros_msgs::PositionTarget moveMsg;
+		/*
+		#define C_EARTH (double) 6378137.0
+
+		double d_lon = destination_longitude - global_position.longitude;
+		double d_lat = destination_latitude - global_position.latitude;
+
+		flight_ctrl_data.x = ((d_lat) * M_PI/180.0) * C_EARTH;
+		flight_ctrl_data.y = ((d_lon) * M_PI/180.0) * C_EARTH * cos((dst_latitude)*M_PI/180.0);
+		*/
+
+
+		moveMsg.coordinate_frame = mavros_msgs::PositionTarget::FRAME_BODY_OFFSET_NED;
+		moveMsg.type_mask = mavros_msgs::PositionTarget::IGNORE_AFX |
+		                                     mavros_msgs::PositionTarget::IGNORE_AFY |
+		                                     mavros_msgs::PositionTarget::IGNORE_AFZ |
+		                                     mavros_msgs::PositionTarget::IGNORE_VX  |
+		                                     mavros_msgs::PositionTarget::IGNORE_VY  |
+		                                     mavros_msgs::PositionTarget::IGNORE_VZ;
+
+		moveMsg.header.stamp = ros::Time::now();
+		moveMsg.position.x = x;
+		moveMsg.position.y = y;
+		moveMsg.position.z = z;
+		moveMsg.yaw = yaw * 0.0174532925; //DEG to RAD
+		//moveMsg.yaw_rate = 1;
+
+
+		local_position_pub.publish(moveMsg);
+		ROS_INFO("Sent message!");
+	}
 
 	void roscontroller::SetMode(std::string mode, int delay_miliseconds){
 		// wait if necessary

From 91a398c02bf5be9221c8677e8a653c11a7405712 Mon Sep 17 00:00:00 2001
From: isvogor <isvogor@gmail.com>
Date: Wed, 15 Mar 2017 11:54:42 -0400
Subject: [PATCH 18/38] merged with dev, tested, adapted for solo

---
 include/roscontroller.h |   9 +--
 src/buzz_utility.cpp    | 149 ++++++++++++++--------------------------
 src/roscontroller.cpp   | 129 +++++++++++++++-------------------
 src/testflockfev.bzz    |   5 +-
 4 files changed, 111 insertions(+), 181 deletions(-)

diff --git a/include/roscontroller.h b/include/roscontroller.h
index 882af74..ab8e836 100644
--- a/include/roscontroller.h
+++ b/include/roscontroller.h
@@ -74,7 +74,6 @@ private:
 	ros::Subscriber flight_status_sub;
 	ros::Subscriber obstacle_sub;
 	ros::Subscriber Robot_id_sub;
-	ros::Publisher local_position_pub;
 
 	/*Commands for flight controller*/
   	//mavros_msgs::CommandInt cmd_srv;
@@ -94,8 +93,6 @@ private:
 
   	std::string current_mode; // SOLO SPECIFIC: just so you don't call the switch to same mode
 
-    void Initialize_pub_sub(ros::NodeHandle n_c);
-
 	/*Obtain data from ros parameter server*/	
 	void Rosparameters_get(ros::NodeHandle n_c);
 
@@ -159,16 +156,16 @@ private:
 	void Arm();
 
 	/*set mode like guided for solo*/
-	void SetMode();
+	void SetMode(std::string mode, int delay_miliseconds);
 
 	/*Robot independent subscribers*/
 	void Subscribe(ros::NodeHandle n_c);
 
-	void WaypointMissionSetup(float lat, float lng, float alt);
+	//void WaypointMissionSetup(float lat, float lng, float alt);
 
 	void fc_command_setup();
 
-	void LocalPosition(float x, float y, float z, float yaw);
+	void SetLocalPosition(float x, float y, float z, float yaw);
 };
 
 }
diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp
index bf52e51..c2c6ffb 100644
--- a/src/buzz_utility.cpp
+++ b/src/buzz_utility.cpp
@@ -78,27 +78,7 @@ namespace buzz_utility{
    		size_t tot = sizeof(uint32_t);
 		/* Go through the messages until there's nothing else to read */
       		uint16_t unMsgSize=0;
-<<<<<<< HEAD
-		//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;
-
-=======
 		
->>>>>>> dev
 			/*Obtain Buzz messages only when they are present*/
 	      			do {
 		 			/* Get payload size */
@@ -128,33 +108,6 @@ namespace buzz_utility{
    		/* Send robot id */
    		*(uint16_t*)(buff_send+tot) = (uint16_t) VM->robot;
    		tot += sizeof(uint16_t);
-<<<<<<< HEAD
-		//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);
-		//}
-=======
->>>>>>> dev
  			/* Send messages from FIFO */
 	   		do {
 				/* Are there more messages? */
@@ -532,67 +485,67 @@ namespace buzz_utility{
 	   buzzvm_gstore(VM);*/
 	}
 
-/****************************************/
-/*Destroy the bvm and other resorces*/
-/****************************************/
+	/****************************************/
+	/*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");
-}
+	void buzz_script_destroy() {
+	   if(VM) {
+		  if(VM->state != BUZZVM_STATE_READY) {
+			 fprintf(stderr, "%s: execution terminated abnormally: %s\n\n",
+					 BO_FNAME,
+					 buzz_error_info());
+			 buzzvm_dump(VM);
+		  }
+		  buzzvm_function_call(VM, "destroy", 0);
+		  buzzvm_destroy(&VM);
+		  free(BO_FNAME);
+		  buzzdebug_destroy(&DBG_INFO);
+	   }
+	   fprintf(stdout, "Script execution stopped.\n");
+	}
 
 
-/****************************************/
-/****************************************/
+	/****************************************/
+	/****************************************/
 
-/****************************************/
-/*Execution completed*/
-/****************************************/
+	/****************************************/
+	/*Execution completed*/
+	/****************************************/
 
-int buzz_script_done() {
-   return VM->state != BUZZVM_STATE_READY;
-}
+	int buzz_script_done() {
+		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_battery(VM);
+		buzzuav_closures::buzzuav_update_prox(VM);
+		buzzuav_closures::buzzuav_update_currentpos(VM);
+		buzzuav_closures::buzzuav_update_flight_status(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(){
-/* 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);
-return (uint16_t)id;
-}
+	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);
+		return (uint16_t)id;
+	}
 
-buzzvm_t get_vm(){
-return VM;
-}
+	buzzvm_t get_vm() {
+		return VM;
+	}
 
 }
 
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index c860dcf..15a06b6 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -161,14 +161,9 @@ namespace rosbzz_node{
 		Robot_id_sub = n_c.subscribe("/device_id_xbee_", 1000, &roscontroller::set_robot_id,this);
   		obstacle_sub= n_c.subscribe("/guidance/obstacle_distance",100, &roscontroller::obstacle_dist,this);
   		/*publishers*/
-
-  		//mavros_msgs/PositionTarget
-  		local_position_pub = n_c.advertise<mavros_msgs::PositionTarget>("/mavros/setpoint_raw/local", 10);
-  		//mavros_msgs::PositionTarget message;
-
 		payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 1000);
 		neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("/neighbours_pos",1000);
-		localsetpoint_pub = n_c.advertise<mavros_msgs::PositionTarget>("/setpoint_raw/local",1000);	
+		localsetpoint_pub = n_c.advertise<mavros_msgs::PositionTarget>("/mavros/setpoint_raw/local",1000);
 		/* Service Clients*/
 		arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient);
 		mode_client =  n_c.serviceClient<mavros_msgs::SetMode>(modeclient);
@@ -255,6 +250,7 @@ namespace rosbzz_node{
 	void roscontroller::Arm(){
 		mavros_msgs::CommandBool arming_message;
 		arming_message.request.value = armstate;
+		ROS_INFO("FC Arm Service called!------------------------------------------------------");
 		if(arm_client.call(arming_message)) {
 			if(arming_message.response.success==1)
 				ROS_INFO("FC Arm Service called!");
@@ -264,20 +260,6 @@ namespace rosbzz_node{
 			ROS_INFO("FC Arm Service call failed!");
 		}
 	}
-	/*---------------------------------------------------------
-	/ Set mode for the solos
-	/---------------------------------------------------------*/
-	void roscontroller::SetMode(){
-		mavros_msgs::SetMode set_mode_message;
-		set_mode_message.request.base_mode = 0;
-		set_mode_message.request.custom_mode = "GUIDED"; // shit!
-		if(mode_client.call(set_mode_message)) {
-			ROS_INFO("Set Mode Service call successful!");
-		} else {
-			ROS_INFO("Set Mode Service call failed!");
-		}
-	}
-
 
 	/*-----------------------------------------------------------------------------------------------------
 	/Prepare Buzz messages payload for each step and publish
@@ -357,45 +339,59 @@ namespace rosbzz_node{
 	/*---------------------------------------------------------------------------------
 	/Flight controller service call every step if there is a command set from bzz script
 	/-------------------------------------------------------------------------------- */
-	void roscontroller::flight_controler_service_call(){
+	void roscontroller::flight_controler_service_call() {
 		/* flight controller client call if requested from Buzz*/
 		/*FC call for takeoff,land, gohome and arm/disarm*/
 		int tmp = buzzuav_closures::bzz_cmd();
-    		double* goto_pos = buzzuav_closures::getgoto();
-		if (tmp == 1){
+		double* goto_pos = buzzuav_closures::getgoto();
+		if (tmp == 1) {
 			cmd_srv.request.param7 = goto_pos[2];
-			//cmd_srv.request.z = goto_pos[2];
-			cmd_srv.request.command =  buzzuav_closures::getcmd();  		
-			if (mav_client.call(cmd_srv)){ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); }
-	    		else ROS_ERROR("Failed to call service from flight controller"); 
-		} else if (tmp == 2) { /*FC call for goto*/ 
+			cmd_srv.request.command =  buzzuav_closures::getcmd();
+			//std::cout << " CMD: " << buzzuav_closures::getcmd() << std::endl;
+			// SOLO SPECIFIC: SITL DOES NOT USE 21 TO LAND -- workaround: set to LAND mode
+			switch(buzzuav_closures::getcmd()){
+			case mavros_msgs::CommandCode::NAV_LAND:
+				if(current_mode != "LAND")
+					SetMode("LAND", 0);
+				break;
+			case mavros_msgs::CommandCode::NAV_TAKEOFF:
+				if(current_mode != "GUIDED")
+					SetMode("GUIDED", 0); // for real solo, just add 2000ms delay (it should always be in loiter after arm/disarm)
+				break;
+			}
+
+			if (mav_client.call(cmd_srv))
+				{ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); }
+			else
+				ROS_ERROR("Failed to call service from flight controller");
+
+		} else if (tmp == 2) { /*FC call for goto*/
 			/*prepare the goto publish message */
 			cmd_srv.request.param5 = goto_pos[0];
-    			cmd_srv.request.param6 = goto_pos[1];
-    			cmd_srv.request.param7 = goto_pos[2];
-    			cmd_srv.request.command =  buzzuav_closures::getcmd();
-			if (mav_client.call(cmd_srv)){ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); }
-	    		else ROS_ERROR("Failed to call service from flight controller"); 
-    			cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START;
-			if (mav_client.call(cmd_srv)){ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); }
-	    		else ROS_ERROR("Failed to call service from flight controller"); 
+			cmd_srv.request.param6 = goto_pos[1];
+			cmd_srv.request.param7 = goto_pos[2];
+			cmd_srv.request.command = buzzuav_closures::getcmd();
+			if (mav_client.call(cmd_srv)) {
+				ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
+			} else
+				ROS_ERROR("Failed to call service from flight controller");
+			cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START;
+			if (mav_client.call(cmd_srv)) {
+				ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
+			} else
+				ROS_ERROR("Failed to call service from flight controller");
 		} else if (tmp == 3) { /*FC call for arm*/
-			armstate=1;
-			Arm(); 
-		} else if (tmp == 4){
-			armstate=0;
+			SetMode("LOITER", 0);
+			armstate = 1;
 			Arm();
-		} else if (tmp == 5) { /*Buzz call for moveto*/ 
+		} else if (tmp == 4) {
+			armstate = 0;
+			SetMode("LOITER", 0);
+			Arm();
+		} else if (tmp == 5) { /*Buzz call for moveto*/
 			/*prepare the goto publish message */
-			mavros_msgs::PositionTarget pt;
-			pt.type_mask = mavros_msgs::PositionTarget::IGNORE_VX && 				mavros_msgs::PositionTarget::IGNORE_VY && mavros_msgs::PositionTarget::IGNORE_VZ && mavros_msgs::PositionTarget::IGNORE_AFX && mavros_msgs::PositionTarget::IGNORE_AFY && mavros_msgs::PositionTarget::IGNORE_AFZ && mavros_msgs::PositionTarget::IGNORE_YAW_RATE;
-			pt.coordinate_frame =  mavros_msgs::PositionTarget::FRAME_LOCAL_NED;
-			pt.position.x = goto_pos[0];
-    			pt.position.y = goto_pos[1];
-    			pt.position.z = goto_pos[2];
-    			pt.yaw = 0;//goto_pos[3];
-			localsetpoint_pub.publish(pt);
- 		}
+			roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], 0);
+		}
 	}
 	/*----------------------------------------------
 	/Refresh neighbours Position for every ten step
@@ -575,7 +571,6 @@ namespace rosbzz_node{
 	bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request  &req,
 		         mavros_msgs::CommandLong::Response &res){
 		int rc_cmd;
-
    		switch(req.command){
 			case mavros_msgs::CommandCode::NAV_TAKEOFF:
    				ROS_INFO("RC_call: TAKE OFF!!!!");
@@ -584,7 +579,7 @@ namespace rosbzz_node{
 				res.success = true;
 				break;
 			case mavros_msgs::CommandCode::NAV_LAND:
-   				ROS_INFO("RC_Call: LAND!!!!");
+   				ROS_INFO("RC_Call: LAND!!!! sending land");
 				rc_cmd=mavros_msgs::CommandCode::NAV_LAND;
 				buzzuav_closures::rc_call(rc_cmd);
 				res.success = true;
@@ -616,8 +611,8 @@ namespace rosbzz_node{
 				rc_goto[0] = req.param5;
 				rc_goto[1] = req.param6;
 				rc_goto[2] = req.param7;
-				// param 4 for yaw
-				LocalPosition(rc_goto[0], rc_goto[1], rc_goto[2], req.param4);
+				// for test
+				//SetLocalPosition(rc_goto[0], rc_goto[1], rc_goto[2], 0);
 
 				buzzuav_closures::rc_set_goto(rc_goto);
 				rc_cmd= mavros_msgs::CommandCode::NAV_WAYPOINT;
@@ -635,30 +630,18 @@ namespace rosbzz_node{
 	/ TODO: check for integrity of this subscriber call back
 	/----------------------------------------------------*/
 	void roscontroller::set_robot_id(const std_msgs::UInt8::ConstPtr& msg){
-	robot_id=(int)msg->data;
-
+		robot_id=(int)msg->data;
 	}
 	
 	/*
 	 * SOLO SPECIFIC FUNCTIONS
 	 */
 
-	void roscontroller::LocalPosition(float x, float y, float z, float yaw){
+	void roscontroller::SetLocalPosition(float x, float y, float z, float yaw){
 		// http://docs.ros.org/kinetic/api/mavros_msgs/html/msg/PositionTarget.html
 		// http://ardupilot.org/dev/docs/copter-commands-in-guided-mode.html#copter-commands-in-guided-mode-set-position-target-local-ned
 
 		mavros_msgs::PositionTarget moveMsg;
-		/*
-		#define C_EARTH (double) 6378137.0
-
-		double d_lon = destination_longitude - global_position.longitude;
-		double d_lat = destination_latitude - global_position.latitude;
-
-		flight_ctrl_data.x = ((d_lat) * M_PI/180.0) * C_EARTH;
-		flight_ctrl_data.y = ((d_lon) * M_PI/180.0) * C_EARTH * cos((dst_latitude)*M_PI/180.0);
-		*/
-
-
 		moveMsg.coordinate_frame = mavros_msgs::PositionTarget::FRAME_BODY_OFFSET_NED;
 		moveMsg.type_mask = mavros_msgs::PositionTarget::IGNORE_AFX |
 		                                     mavros_msgs::PositionTarget::IGNORE_AFY |
@@ -666,22 +649,20 @@ namespace rosbzz_node{
 		                                     mavros_msgs::PositionTarget::IGNORE_VX  |
 		                                     mavros_msgs::PositionTarget::IGNORE_VY  |
 		                                     mavros_msgs::PositionTarget::IGNORE_VZ;
-
 		moveMsg.header.stamp = ros::Time::now();
 		moveMsg.position.x = x;
 		moveMsg.position.y = y;
 		moveMsg.position.z = z;
-		moveMsg.yaw = yaw * 0.0174532925; //DEG to RAD
-		//moveMsg.yaw_rate = 1;
+		moveMsg.yaw = 0;
 
+		localsetpoint_pub.publish(moveMsg);
 
-		local_position_pub.publish(moveMsg);
-		ROS_INFO("Sent message!");
+		ROS_INFO("Sent local position message!");
 	}
 
 	void roscontroller::SetMode(std::string mode, int delay_miliseconds){
 		// wait if necessary
-		if (delay_miliseconds > 0){
+		if (delay_miliseconds != 0){
 			std::this_thread::sleep_for( std::chrono::milliseconds ( delay_miliseconds ) );
 		}
 		// set mode
diff --git a/src/testflockfev.bzz b/src/testflockfev.bzz
index fd4a3e7..5d42601 100644
--- a/src/testflockfev.bzz
+++ b/src/testflockfev.bzz
@@ -132,8 +132,7 @@ function takeoff() {
 }
 function land() {
 	log("Land: ", flight.status)
-	# SOLO: land = status 4, guided = 1
-	if(flight.status == 1){
+	if(flight.status == 2 or flight.status == 1){
 		neighbors.broadcast("cmd", 21)
 		uav_land()
 	}
@@ -186,4 +185,4 @@ function reset() {
  
 # Executed once at the end of experiment.
 function destroy() {
-}
+}
\ No newline at end of file

From e0ac252f9d17b4e9150a72685db1f42df4d71d0d Mon Sep 17 00:00:00 2001
From: isvogor <isvogor@gmail.com>
Date: Wed, 29 Mar 2017 14:57:31 -0400
Subject: [PATCH 19/38] set stream rate

---
 include/roscontroller.h |  3 +++
 src/roscontroller.cpp   | 22 +++++++++++++++++++---
 2 files changed, 22 insertions(+), 3 deletions(-)

diff --git a/include/roscontroller.h b/include/roscontroller.h
index ab8e836..b18aa1b 100644
--- a/include/roscontroller.h
+++ b/include/roscontroller.h
@@ -16,6 +16,7 @@
 #include "mavros_msgs/WaypointPush.h"
 #include "mavros_msgs/Waypoint.h"
 #include "mavros_msgs/PositionTarget.h"
+#include "mavros_msgs/StreamRate.h"
 #include <sensor_msgs/LaserScan.h>
 #include <rosbuzz/neigh_pos.h>
 #include <sstream>
@@ -166,6 +167,8 @@ private:
 	void fc_command_setup();
 
 	void SetLocalPosition(float x, float y, float z, float yaw);
+
+	void SetStreamRate(int id, int rate, int on_off);
 };
 
 }
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index 15a06b6..87cfde7 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -16,10 +16,12 @@ namespace rosbzz_node{
  		Compile_bzz();
 		set_bzz_file(bzzfile_name.c_str());
 		/*Initialize variables*/
-		//
+		// Solo things
 		SetMode("LOITER", 0);
 		armstate = 0;
 		multi_msg = true;
+		// set stream rate
+		SetStreamRate(0, 10, 1);
 	}
 
 	/*---------------------
@@ -356,7 +358,7 @@ namespace rosbzz_node{
 				break;
 			case mavros_msgs::CommandCode::NAV_TAKEOFF:
 				if(current_mode != "GUIDED")
-					SetMode("GUIDED", 0); // for real solo, just add 2000ms delay (it should always be in loiter after arm/disarm)
+					SetMode("GUIDED", 2000); // for real solo, just add 2000ms delay (it should always be in loiter after arm/disarm)
 				break;
 			}
 
@@ -386,7 +388,7 @@ namespace rosbzz_node{
 			Arm();
 		} else if (tmp == 4) {
 			armstate = 0;
-			SetMode("LOITER", 0);
+		SetMode("LOITER", 0);
 			Arm();
 		} else if (tmp == 5) { /*Buzz call for moveto*/
 			/*prepare the goto publish message */
@@ -677,4 +679,18 @@ namespace rosbzz_node{
 		}
 	}
 
+	void roscontroller::SetStreamRate(int id, int rate, int on_off){
+		mavros_msgs::StreamRate message;
+		message.request.stream_id = id;
+		message.request.message_rate = rate;
+		message.request.on_off = on_off;
+		if(mav_client.call(message)){
+			ROS_INFO("Set Mode Service call successful!");
+		} else {
+			ROS_INFO("Set Mode Service call failed!");
+		}
+	}
+
+
+
 }

From a9335d973a7ffbcc0f56516e642bbbb8774248d1 Mon Sep 17 00:00:00 2001
From: isvogor <isvogor@gmail.com>
Date: Wed, 29 Mar 2017 17:32:56 -0400
Subject: [PATCH 20/38] set stream rate fix

---
 include/roscontroller.h        | 3 ++-
 launch/launch_config/solo.yaml | 1 +
 src/roscontroller.cpp          | 6 ++++--
 3 files changed, 7 insertions(+), 3 deletions(-)

diff --git a/include/roscontroller.h b/include/roscontroller.h
index b18aa1b..fddc23f 100644
--- a/include/roscontroller.h
+++ b/include/roscontroller.h
@@ -61,7 +61,7 @@ private:
 	int armstate;
 	int barrier;
 	int message_number=0;
-	std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by;
+	std::string bzzfile_name, fcclient_name, armclient, stream_client_name, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by;
 	bool rcclient;
 	bool multi_msg;
 	ros::ServiceClient mav_client;
@@ -75,6 +75,7 @@ private:
 	ros::Subscriber flight_status_sub;
 	ros::Subscriber obstacle_sub;
 	ros::Subscriber Robot_id_sub;
+	ros::ServiceClient stream_client;
 
 	/*Commands for flight controller*/
   	//mavros_msgs::CommandInt cmd_srv;
diff --git a/launch/launch_config/solo.yaml b/launch/launch_config/solo.yaml
index 7d3ceb4..7b33e06 100644
--- a/launch/launch_config/solo.yaml
+++ b/launch/launch_config/solo.yaml
@@ -5,6 +5,7 @@ topics:
   fcclient:   /mavros/cmd/command
   armclient:  /mavros/cmd/arming
   modeclient: /mavros/set_mode
+  stream:  /mavros/set_stream_rate
 type:
   gps  :  sensor_msgs/NavSatFix
   # for SITL Solo 
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index 87cfde7..0081b91 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -133,7 +133,6 @@ namespace rosbzz_node{
 		node_handle.getParam("type/battery", battery_type);
 		m_smTopic_infos.insert(pair <std::string, std::string>(battery_topic, battery_type));
 
-
 		std::string status_topic, status_type;
 		node_handle.getParam("topics/status", status_topic);
 		node_handle.getParam("type/status", status_type);
@@ -146,6 +145,8 @@ namespace rosbzz_node{
   		else {ROS_ERROR("Provide an arm client name in Launch file"); system("rosnode kill rosbuzz_node");}  
   		if(node_handle.getParam("topics/modeclient", modeclient));
   		else {ROS_ERROR("Provide a mode client name in Launch file"); system("rosnode kill rosbuzz_node");}  
+  		if(node_handle.getParam("topics/stream", stream_client_name));
+  		else {ROS_ERROR("Provide a mode client name in Launch file"); system("rosnode kill rosbuzz_node");}
 	}
 
 	/*--------------------------------------------------------
@@ -170,6 +171,7 @@ namespace rosbzz_node{
 		arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient);
 		mode_client =  n_c.serviceClient<mavros_msgs::SetMode>(modeclient);
 		mav_client = n_c.serviceClient<mavros_msgs::CommandLong>(fcclient_name);
+		stream_client = n_c.serviceClient<mavros_msgs::StreamRate>(stream_client_name);
 
 		multi_msg=true;
 	}
@@ -684,7 +686,7 @@ namespace rosbzz_node{
 		message.request.stream_id = id;
 		message.request.message_rate = rate;
 		message.request.on_off = on_off;
-		if(mav_client.call(message)){
+		if(stream_client.call(message)){
 			ROS_INFO("Set Mode Service call successful!");
 		} else {
 			ROS_INFO("Set Mode Service call failed!");

From dfa7519bd5d5f869c5f98fa9dde7f034bc8ff3b8 Mon Sep 17 00:00:00 2001
From: isvogor <isvogor@gmail.com>
Date: Fri, 31 Mar 2017 15:13:25 -0400
Subject: [PATCH 21/38] stream rate wait

---
 launch/rosbuzz-testing.launch | 2 +-
 src/roscontroller.cpp         | 9 +++++----
 2 files changed, 6 insertions(+), 5 deletions(-)

diff --git a/launch/rosbuzz-testing.launch b/launch/rosbuzz-testing.launch
index f76f47d..d955758 100644
--- a/launch/rosbuzz-testing.launch
+++ b/launch/rosbuzz-testing.launch
@@ -18,7 +18,7 @@
 	</include>
 
 	<!-- set streaming rate -->
-	<node pkg="rosservice" type="rosservice" name="freq" args="call /mavros/set_stream_rate 0 10 1" output="screen" />
+	<!-- 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" />
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index 0081b91..adb80a9 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -686,11 +686,12 @@ namespace rosbzz_node{
 		message.request.stream_id = id;
 		message.request.message_rate = rate;
 		message.request.on_off = on_off;
-		if(stream_client.call(message)){
-			ROS_INFO("Set Mode Service call successful!");
-		} else {
-			ROS_INFO("Set Mode Service call failed!");
+
+		while(!stream_client.call(message)){
+			ROS_INFO("Set stream rate call failed!, trying again...");
+			std::this_thread::sleep_for( std::chrono::milliseconds ( 2000 ) );
 		}
+		ROS_INFO("Set stream rate call successful");
 	}
 
 

From 96ebb3f4703aba55e6f97b57265fc94dc116bb7a Mon Sep 17 00:00:00 2001
From: isvogor <isvogor@gmail.com>
Date: Mon, 3 Apr 2017 15:41:03 -0400
Subject: [PATCH 22/38] merge

---
 include/roscontroller.h            |  1 +
 launch/rosbuzz-testing-sitl.launch |  3 +--
 src/roscontroller.cpp              | 38 +++++++++++++++++-------------
 src/testflockfev.bzz               |  7 +++---
 4 files changed, 27 insertions(+), 22 deletions(-)

diff --git a/include/roscontroller.h b/include/roscontroller.h
index 4da30d8..4524129 100644
--- a/include/roscontroller.h
+++ b/include/roscontroller.h
@@ -17,6 +17,7 @@
 #include "mavros_msgs/Waypoint.h"
 #include "mavros_msgs/PositionTarget.h"
 #include "mavros_msgs/StreamRate.h"
+#include "mavros_msgs/ParamGet.h"
 #include <sensor_msgs/LaserScan.h>
 #include <rosbuzz/neigh_pos.h>
 #include <sstream>
diff --git a/launch/rosbuzz-testing-sitl.launch b/launch/rosbuzz-testing-sitl.launch
index 7dd98ff..a6e6405 100644
--- a/launch/rosbuzz-testing-sitl.launch
+++ b/launch/rosbuzz-testing-sitl.launch
@@ -31,8 +31,7 @@
 		<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="xbee_status_srv" value="/xbee_status"/>
 		<param name="stand_by" value="/home/ivan/catkin_ws/src/rosbuzz/src/stand_by.bo"/>
 	</node>
 	
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index f7c2163..8ee3530 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -41,18 +41,19 @@ namespace rosbzz_node{
 	/--------------------------------------------------*/
 	void roscontroller::RosControllerRun(){
 		mavros_msgs::ParamGet::Request robot_id_srv_request; robot_id_srv_request.param_id="id";
-        	mavros_msgs::ParamGet::Response robot_id_srv_response;
+        mavros_msgs::ParamGet::Response robot_id_srv_response;
+
+
 		while(!xbeestatus_srv.call(robot_id_srv_request,robot_id_srv_response)){
-			/*run once*/
     			ros::spinOnce();
-			/*loop rate of ros*/
 			ros::Rate loop_rate(10);
 			loop_rate.sleep();
- 			/*sleep for the mentioned loop rate*/
 			ROS_ERROR("Waiting for Xbee to respond to get device ID");
 		}
+
 		robot_id=robot_id_srv_response.value.integer;
-			
+
+        //robot_id = 84;
 		/* Set the Buzz bytecode */
 		if(buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(),robot_id)) {
 			fprintf(stdout, "Bytecode file found and set\n");
@@ -369,7 +370,7 @@ namespace rosbzz_node{
 		/*FC call for takeoff,land, gohome and arm/disarm*/
 		int tmp = buzzuav_closures::bzz_cmd();
 		double* goto_pos = buzzuav_closures::getgoto();
-		if (tmp == 1) {
+		if (tmp == buzzuav_closures::COMMAND_TAKEOFF || tmp== buzzuav_closures::COMMAND_LAND || tmp==buzzuav_closures::COMMAND_GOHOME) {
 			cmd_srv.request.param7 = goto_pos[2];
 			cmd_srv.request.command =  buzzuav_closures::getcmd();
 			//std::cout << " CMD: " << buzzuav_closures::getcmd() << std::endl;
@@ -390,7 +391,7 @@ namespace rosbzz_node{
 			else
 				ROS_ERROR("Failed to call service from flight controller");
 
-		} else if (tmp == 2) { /*FC call for goto*/
+		} else if (tmp == buzzuav_closures::COMMAND_GOTO) { /*FC call for goto*/
 			/*prepare the goto publish message */
 			cmd_srv.request.param5 = goto_pos[0];
 			cmd_srv.request.param6 = goto_pos[1];
@@ -405,15 +406,15 @@ namespace rosbzz_node{
 				ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
 			} else
 				ROS_ERROR("Failed to call service from flight controller");
-		} else if (tmp == 3) { /*FC call for arm*/
+		} else if (tmp == buzzuav_closures::COMMAND_ARM) { /*FC call for arm*/
 			SetMode("LOITER", 0);
 			armstate = 1;
 			Arm();
-		} else if (tmp == 4) {
+		} else if (tmp == buzzuav_closures::COMMAND_DISARM) {
 			armstate = 0;
 		SetMode("LOITER", 0);
 			Arm();
-		} else if (tmp == 5) { /*Buzz call for moveto*/
+		} else if (tmp == buzzuav_closures::COMMAND_MOVETO) { /*Buzz call for moveto*/
 			/*prepare the goto publish message */
 			roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], 0);
 		}
@@ -505,7 +506,7 @@ namespace rosbzz_node{
 		else if (msg->mode == "LAND")
 			buzzuav_closures::flight_status_update(4);
 		else // ground standby = LOITER?
-			buzzuav_closures::flight_status_update(1);//?
+			buzzuav_closures::flight_status_update(7);//?
 	}
 
 	/*------------------------------------------------------------
@@ -518,7 +519,7 @@ namespace rosbzz_node{
 	/ Update current position into BVM from subscriber
 	/-------------------------------------------------------------*/
 	void roscontroller::current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg){
-		set_cur_pos(msg->latitude,msg->longitude,msg->altitude);
+		set_cur_pos(msg->latitude,msg->longitude, msg->altitude);
 		buzzuav_closures::set_currentpos(msg->latitude,msg->longitude,msg->altitude);
 	}
 	/*-------------------------------------------------------------
@@ -689,8 +690,9 @@ namespace rosbzz_node{
 				no_cnt=0;
 			}
 		}
-		//if(count_robots.current !=0){
-			/*std::map< int,  int> count_count;
+		/*
+		if(count_robots.current !=0){
+			std::map< int,  int> count_count;
 			uint8_t index=0;
 			count_robots.history[count_robots.index]=neighbours_pos_map.size()+1;
 			//count_robots.current=neighbours_pos_map.size()+1;
@@ -710,8 +712,8 @@ namespace rosbzz_node{
 			if(odd_count>current_count){
 				count_robots.current=odd_val;
 			}	
-		//}
-		/*else{
+		}
+		else{
 			if(neighbours_pos_map.size()!=0){
 				count_robots.history[count_robots.index]=neighbours_pos_map.size()+1;
 				//count_robots.current=neighbours_pos_map.size()+1;
@@ -721,7 +723,9 @@ namespace rosbzz_node{
 					count_robots.current=neighbours_pos_map.size()+1;
 				} 
 			}
-		}*/
+		}
+		*/
+	}
 	/*
 	 * SOLO SPECIFIC FUNCTIONS
 	 */
diff --git a/src/testflockfev.bzz b/src/testflockfev.bzz
index 628269f..f68b347 100644
--- a/src/testflockfev.bzz
+++ b/src/testflockfev.bzz
@@ -106,15 +106,16 @@ function takeoff() {
 	CURSTATE = "TAKEOFF"
 	statef=takeoff
 	log("TakeOff: ", flight.status)
-	if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
+	uav_takeoff(TARGET_ALTITUDE)
+	
+	if( flight.status == 1) {
 		barrier_set(ROBOTS,hexagon)
 		barrier_ready()
 		#statef=hexagon
 	}
-	else if( flight.status !=3){
+	else if( flight.status == 7 ){
 		log("Altitude: ", TARGET_ALTITUDE)
 		neighbors.broadcast("cmd", 22)
-		uav_takeoff(TARGET_ALTITUDE)
 	}
 }
 function land() {

From 9f3a02e8ec5fc5b61e5bba25d038e24a19eca7f1 Mon Sep 17 00:00:00 2001
From: isvogor <isvogor@gmail.com>
Date: Mon, 3 Apr 2017 19:50:09 -0400
Subject: [PATCH 23/38] changes to NED and REL_ALT

---
 include/roscontroller.h        |  7 ++++
 launch/launch_config/solo.yaml |  2 +
 launch/rosbuzz.launch          |  5 +--
 src/roscontroller.cpp          | 71 +++++++++++++++++++++++++---------
 src/testflockfev.bzz           | 18 ++++++---
 5 files changed, 76 insertions(+), 27 deletions(-)

diff --git a/include/roscontroller.h b/include/roscontroller.h
index 4524129..4a0c02c 100644
--- a/include/roscontroller.h
+++ b/include/roscontroller.h
@@ -18,6 +18,7 @@
 #include "mavros_msgs/PositionTarget.h"
 #include "mavros_msgs/StreamRate.h"
 #include "mavros_msgs/ParamGet.h"
+#include "std_msgs/Float64.h"
 #include <sensor_msgs/LaserScan.h>
 #include <rosbuzz/neigh_pos.h>
 #include <sstream>
@@ -59,6 +60,7 @@ private:
 	}; typedef struct num_robot_count Num_robot_count ;
 
  	double cur_pos[3];
+ 	double cur_rel_altitude;
 	uint64_t payload;
 	std::map< int,  buzz_utility::Pos_struct> neighbours_pos_map;
 	std::map< int,  buzz_utility::Pos_struct> raw_neighbours_pos_map;
@@ -76,6 +78,7 @@ private:
 	int old_val=0;	
 	std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by,xbeesrv_name;
 	std::string stream_client_name;
+	std::string relative_altitude_sub_name;
 	bool rcclient;
 	bool multi_msg;
 	Num_robot_count count_robots;
@@ -91,6 +94,7 @@ private:
 	ros::Subscriber flight_status_sub;
 	ros::Subscriber obstacle_sub;
 	ros::Subscriber Robot_id_sub;
+	ros::Subscriber relative_altitude_sub;
 	ros::ServiceClient stream_client;
 
 	/*Commands for flight controller*/
@@ -154,6 +158,9 @@ private:
 	/*current position callback*/
 	void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg);
 
+	/*current relative altitude callback*/
+	void current_rel_alt(const std_msgs::Float64::ConstPtr& msg);
+
 	/*payload callback callback*/
 	void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg);
 
diff --git a/launch/launch_config/solo.yaml b/launch/launch_config/solo.yaml
index 7b33e06..d9f7961 100644
--- a/launch/launch_config/solo.yaml
+++ b/launch/launch_config/solo.yaml
@@ -6,6 +6,7 @@ topics:
   armclient:  /mavros/cmd/arming
   modeclient: /mavros/set_mode
   stream:  /mavros/set_stream_rate
+  altitude: /mavros/global_position/rel_alt
 type:
   gps  :  sensor_msgs/NavSatFix
   # for SITL Solo 
@@ -13,5 +14,6 @@ type:
   # for solo
   #battery :   mavros_msgs/BatteryStatus
   status  :   mavros_msgs/State
+  altitude: std_msgs/Float64
 environment : 
   environment : solo-simulator
diff --git a/launch/rosbuzz.launch b/launch/rosbuzz.launch
index 0bc0db4..d79e016 100644
--- a/launch/rosbuzz.launch
+++ b/launch/rosbuzz.launch
@@ -3,13 +3,12 @@
 <launch>
 		<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="bzzfile_name" value="/home/ivan/catkin_ws/src/rosbuzz/src/testmoveto.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="xbee_status_srv" value="/xbee_status"/>
 		<param name="stand_by" value="$(env ROS_WS)/src/ROSBuzz/src/stand_by.bo"/>
-		</node>
-		
+		</node>		
 </launch>
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index 8ee3530..38bbce2 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -43,7 +43,7 @@ namespace rosbzz_node{
 		mavros_msgs::ParamGet::Request robot_id_srv_request; robot_id_srv_request.param_id="id";
         mavros_msgs::ParamGet::Response robot_id_srv_response;
 
-
+        /*
 		while(!xbeestatus_srv.call(robot_id_srv_request,robot_id_srv_response)){
     			ros::spinOnce();
 			ros::Rate loop_rate(10);
@@ -52,6 +52,8 @@ namespace rosbzz_node{
 		}
 
 		robot_id=robot_id_srv_response.value.integer;
+		*/
+        robot_id = 32;
 
         //robot_id = 84;
 		/* Set the Buzz bytecode */
@@ -159,6 +161,11 @@ namespace rosbzz_node{
 		node_handle.getParam("type/status", status_type);
 		m_smTopic_infos.insert(pair <std::string, std::string>(status_topic, status_type));
 
+		std::string altitude_topic, altitude_type;
+		node_handle.getParam("topics/altitude", altitude_topic);
+		node_handle.getParam("type/altitude", altitude_type);
+		m_smTopic_infos.insert(pair <std::string, std::string>(altitude_topic, altitude_type));
+
   		/*Obtain fc client name from parameter server*/
   		if(node_handle.getParam("topics/fcclient", fcclient_name));
   		else {ROS_ERROR("Provide a fc client name in Launch file"); system("rosnode kill rosbuzz_node");}  
@@ -215,6 +222,9 @@ namespace rosbzz_node{
   			else if(it->second == "sensor_msgs/NavSatFix"){
   		  		current_position_sub = n_c.subscribe(it->first, 1000, &roscontroller::current_pos, this);
  			}
+  			else if(it->second == "std_msgs/Float64"){
+  				relative_altitude_sub = n_c.subscribe(it->first, 1000, &roscontroller::current_rel_alt, this);
+  			}
 
 	  		std::cout << "Subscribed to: " << it->first << endl;
   		}
@@ -370,6 +380,7 @@ namespace rosbzz_node{
 		/*FC call for takeoff,land, gohome and arm/disarm*/
 		int tmp = buzzuav_closures::bzz_cmd();
 		double* goto_pos = buzzuav_closures::getgoto();
+		ros::Rate loop_rate(0.25);
 		if (tmp == buzzuav_closures::COMMAND_TAKEOFF || tmp== buzzuav_closures::COMMAND_LAND || tmp==buzzuav_closures::COMMAND_GOHOME) {
 			cmd_srv.request.param7 = goto_pos[2];
 			cmd_srv.request.command =  buzzuav_closures::getcmd();
@@ -378,19 +389,29 @@ namespace rosbzz_node{
 			switch(buzzuav_closures::getcmd()){
 			case mavros_msgs::CommandCode::NAV_LAND:
 				if(current_mode != "LAND")
-					SetMode("LAND", 0);
+					{SetMode("LAND", 0);}
+				if (mav_client.call(cmd_srv)){
+					ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
+				} else
+					{ROS_ERROR("Failed to call service from flight controller");}
+				armstate = 0;
 				break;
 			case mavros_msgs::CommandCode::NAV_TAKEOFF:
+				if(!armstate){
+					SetMode("LOITER", 0);
+					armstate = 1;
+					Arm();
+					loop_rate.sleep();
+				}
 				if(current_mode != "GUIDED")
 					SetMode("GUIDED", 2000); // for real solo, just add 2000ms delay (it should always be in loiter after arm/disarm)
+				if (mav_client.call(cmd_srv))
+					{ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);}
+				else
+					ROS_ERROR("Failed to call service from flight controller");
 				break;
 			}
 
-			if (mav_client.call(cmd_srv))
-				{ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); }
-			else
-				ROS_ERROR("Failed to call service from flight controller");
-
 		} else if (tmp == buzzuav_closures::COMMAND_GOTO) { /*FC call for goto*/
 			/*prepare the goto publish message */
 			cmd_srv.request.param5 = goto_pos[0];
@@ -407,16 +428,20 @@ namespace rosbzz_node{
 			} else
 				ROS_ERROR("Failed to call service from flight controller");
 		} else if (tmp == buzzuav_closures::COMMAND_ARM) { /*FC call for arm*/
-			SetMode("LOITER", 0);
-			armstate = 1;
-			Arm();
+			if(!armstate){
+				SetMode("LOITER", 0);
+				armstate = 1;
+				Arm();
+			}
 		} else if (tmp == buzzuav_closures::COMMAND_DISARM) {
-			armstate = 0;
-		SetMode("LOITER", 0);
-			Arm();
+			if(armstate){
+				armstate = 0;
+				SetMode("LOITER", 0);
+				Arm();
+			}
 		} else if (tmp == buzzuav_closures::COMMAND_MOVETO) { /*Buzz call for moveto*/
-			/*prepare the goto publish message */
-			roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], 0);
+			/*prepare the goto publish message ATTENTION: ENU FRAME FOR MAVROS STANDARD (then converted to NED)*/
+			roscontroller::SetLocalPosition(goto_pos[1], goto_pos[0], goto_pos[2], 0);
 		}
 	}
 	/*----------------------------------------------
@@ -460,7 +485,6 @@ namespace rosbzz_node{
 		cur_pos [2] =altitude;
 	}
 
-
 	/*-----------------------------------------------------------
 	/ Compute Range and Bearing of a neighbor in a local reference frame
 	/ from GPS coordinates
@@ -519,8 +543,17 @@ namespace rosbzz_node{
 	/ Update current position into BVM from subscriber
 	/-------------------------------------------------------------*/
 	void roscontroller::current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg){
-		set_cur_pos(msg->latitude,msg->longitude, msg->altitude);
-		buzzuav_closures::set_currentpos(msg->latitude,msg->longitude,msg->altitude);
+		ROS_INFO("Altitude out: %f", cur_rel_altitude);
+		set_cur_pos(msg->latitude,msg->longitude, cur_rel_altitude);//msg->altitude);
+		buzzuav_closures::set_currentpos(msg->latitude,msg->longitude, cur_rel_altitude);//msg->altitude);
+	}
+	/*-------------------------------------------------------------
+	/ Update altitude into BVM from subscriber
+	/-------------------------------------------------------------*/
+	void roscontroller::current_rel_alt(const std_msgs::Float64::ConstPtr& msg){
+		ROS_INFO("Altitude in: %f", msg->data);
+		cur_rel_altitude = (double)msg->data;
+
 	}
 	/*-------------------------------------------------------------
 	/Set obstacle Obstacle distance table into BVM from subscriber
@@ -735,7 +768,7 @@ namespace rosbzz_node{
 		// http://ardupilot.org/dev/docs/copter-commands-in-guided-mode.html#copter-commands-in-guided-mode-set-position-target-local-ned
 
 		mavros_msgs::PositionTarget moveMsg;
-		moveMsg.coordinate_frame = mavros_msgs::PositionTarget::FRAME_BODY_OFFSET_NED;
+		moveMsg.coordinate_frame = mavros_msgs::PositionTarget::FRAME_LOCAL_OFFSET_NED;
 		moveMsg.type_mask = mavros_msgs::PositionTarget::IGNORE_AFX |
 		                                     mavros_msgs::PositionTarget::IGNORE_AFY |
 		                                     mavros_msgs::PositionTarget::IGNORE_AFZ |
diff --git a/src/testflockfev.bzz b/src/testflockfev.bzz
index f68b347..cd00626 100644
--- a/src/testflockfev.bzz
+++ b/src/testflockfev.bzz
@@ -11,7 +11,7 @@ function updated_neigh(){
 neighbors.broadcast(updated, update_no)
 }
 
-TARGET_ALTITUDE = 5.0
+TARGET_ALTITUDE = 3.0
 CURSTATE = "TURNEDOFF"
  
 # Lennard-Jones parameters
@@ -43,7 +43,14 @@ function hexagon() {
     math.vec2.scale(accum, 1.0 / neighbors.count())
   # Move according to vector
   #print("Robot ", id, "must push ",accum.length, "; ", accum.angle)
-  uav_moveto(accum.x, accum.y)
+
+  if(timeW>=WAIT_TIMEOUT) {	#FOR MOVETO TESTS
+  	timeW =0
+  	statef=land
+  	} else {
+  	timeW = timeW+1
+  	uav_moveto(0.5,0.0)
+  	}
 }
  
 ########################################
@@ -106,16 +113,17 @@ function takeoff() {
 	CURSTATE = "TAKEOFF"
 	statef=takeoff
 	log("TakeOff: ", flight.status)
-	uav_takeoff(TARGET_ALTITUDE)
+	log("Relative position: ", position.altitude)
 	
-	if( flight.status == 1) {
+	if( flight.status == 1 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
 		barrier_set(ROBOTS,hexagon)
 		barrier_ready()
 		#statef=hexagon
 	}
-	else if( flight.status == 7 ){
+	else {
 		log("Altitude: ", TARGET_ALTITUDE)
 		neighbors.broadcast("cmd", 22)
+		uav_takeoff(TARGET_ALTITUDE)
 	}
 }
 function land() {

From 0dbccdde44ffd1484f7e7e93453a1d9fc96d0994 Mon Sep 17 00:00:00 2001
From: David St-Onge <david.st-onge@polymtl.ca>
Date: Wed, 5 Apr 2017 17:02:35 -0400
Subject: [PATCH 24/38] ensure compatibility with M100

---
 include/roscontroller.h        |  1 +
 launch/launch_config/m100.yaml |  4 ++++
 src/roscontroller.cpp          | 44 +++++++++++++++++-----------------
 src/testflockfev.bzz           | 21 ++++++++--------
 4 files changed, 38 insertions(+), 32 deletions(-)

diff --git a/include/roscontroller.h b/include/roscontroller.h
index 4a0c02c..1e5ae9e 100644
--- a/include/roscontroller.h
+++ b/include/roscontroller.h
@@ -194,6 +194,7 @@ private:
 	void SetStreamRate(int id, int rate, int on_off);
 	
 	void get_number_of_robots();
+	void GetRobotId();
 };
 
 }
diff --git a/launch/launch_config/m100.yaml b/launch/launch_config/m100.yaml
index b7f1eb8..664b9b2 100644
--- a/launch/launch_config/m100.yaml
+++ b/launch/launch_config/m100.yaml
@@ -5,7 +5,11 @@ topics:
   fcclient :   /dji_mavcmd
   armclient:   /dji_mavarm
   modeclient:  /dji_mavmode
+  altitude: /rel_alt
+  stream:  /set_stream_rate
+
 type:
   gps  :  sensor_msgs/NavSatFix
   battery :   mavros_msgs/BatteryStatus
   status  :   mavros_msgs/ExtendedState
+  altitude: std_msgs/Float64
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index 38bbce2..d7e2803 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -20,8 +20,10 @@ namespace rosbzz_node{
 		SetMode("LOITER", 0);
 		armstate = 0;
 		multi_msg = true;
-		// set stream rate
+		// set stream rate - wait for the FC to be started
 		SetStreamRate(0, 10, 1);
+		/// Get Robot Id - wait for Xbee to be started
+		GetRobotId();
 	}
 
 	/*---------------------
@@ -36,26 +38,25 @@ namespace rosbzz_node{
    		uav_done();
 	}
 
-	/*-------------------------------------------------
-	/rosbuzz_node loop method executed once every step
-	/--------------------------------------------------*/
-	void roscontroller::RosControllerRun(){
+	void roscontroller::GetRobotId()
+	{
+        
 		mavros_msgs::ParamGet::Request robot_id_srv_request; robot_id_srv_request.param_id="id";
-        mavros_msgs::ParamGet::Response robot_id_srv_response;
-
-        /*
+        	mavros_msgs::ParamGet::Response robot_id_srv_response;
 		while(!xbeestatus_srv.call(robot_id_srv_request,robot_id_srv_response)){
-    			ros::spinOnce();
-			ros::Rate loop_rate(10);
-			loop_rate.sleep();
+			ros::Duration(0.1).sleep();
 			ROS_ERROR("Waiting for Xbee to respond to get device ID");
 		}
 
 		robot_id=robot_id_srv_response.value.integer;
-		*/
-        robot_id = 32;
 
-        //robot_id = 84;
+	}
+
+	/*-------------------------------------------------
+	/rosbuzz_node loop method executed once every step
+	/--------------------------------------------------*/
+	void roscontroller::RosControllerRun(){
+
 		/* Set the Buzz bytecode */
 		if(buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(),robot_id)) {
 			fprintf(stdout, "Bytecode file found and set\n");
@@ -69,7 +70,7 @@ namespace rosbzz_node{
 				/*Check updater state and step code*/
   				update_routine(bcfname.c_str(), dbgfname.c_str());
 				/*Step buzz script */
-      			buzz_utility::buzz_script_step();
+      				buzz_utility::buzz_script_step();
 				/*Prepare messages and publish them in respective topic*/
 		  		prepare_msg_and_publish();
 				/*call flight controler service to set command long*/
@@ -380,7 +381,6 @@ namespace rosbzz_node{
 		/*FC call for takeoff,land, gohome and arm/disarm*/
 		int tmp = buzzuav_closures::bzz_cmd();
 		double* goto_pos = buzzuav_closures::getgoto();
-		ros::Rate loop_rate(0.25);
 		if (tmp == buzzuav_closures::COMMAND_TAKEOFF || tmp== buzzuav_closures::COMMAND_LAND || tmp==buzzuav_closures::COMMAND_GOHOME) {
 			cmd_srv.request.param7 = goto_pos[2];
 			cmd_srv.request.command =  buzzuav_closures::getcmd();
@@ -401,7 +401,7 @@ namespace rosbzz_node{
 					SetMode("LOITER", 0);
 					armstate = 1;
 					Arm();
-					loop_rate.sleep();
+					ros::Duration(0.1).sleep();
 				}
 				if(current_mode != "GUIDED")
 					SetMode("GUIDED", 2000); // for real solo, just add 2000ms delay (it should always be in loiter after arm/disarm)
@@ -526,9 +526,9 @@ namespace rosbzz_node{
 		// TODO: Handle landing
 		std::cout << "Message: " << msg->mode << std::endl;
 		if(msg->mode == "GUIDED")
-			buzzuav_closures::flight_status_update(1);
+			buzzuav_closures::flight_status_update(2);
 		else if (msg->mode == "LAND")
-			buzzuav_closures::flight_status_update(4);
+			buzzuav_closures::flight_status_update(1);
 		else // ground standby = LOITER?
 			buzzuav_closures::flight_status_update(7);//?
 	}
@@ -543,7 +543,7 @@ namespace rosbzz_node{
 	/ Update current position into BVM from subscriber
 	/-------------------------------------------------------------*/
 	void roscontroller::current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg){
-		ROS_INFO("Altitude out: %f", cur_rel_altitude);
+		//ROS_INFO("Altitude out: %f", cur_rel_altitude);
 		set_cur_pos(msg->latitude,msg->longitude, cur_rel_altitude);//msg->altitude);
 		buzzuav_closures::set_currentpos(msg->latitude,msg->longitude, cur_rel_altitude);//msg->altitude);
 	}
@@ -551,7 +551,7 @@ namespace rosbzz_node{
 	/ Update altitude into BVM from subscriber
 	/-------------------------------------------------------------*/
 	void roscontroller::current_rel_alt(const std_msgs::Float64::ConstPtr& msg){
-		ROS_INFO("Altitude in: %f", msg->data);
+		//ROS_INFO("Altitude in: %f", msg->data);
 		cur_rel_altitude = (double)msg->data;
 
 	}
@@ -811,7 +811,7 @@ namespace rosbzz_node{
 
 		while(!stream_client.call(message)){
 			ROS_INFO("Set stream rate call failed!, trying again...");
-			std::this_thread::sleep_for( std::chrono::milliseconds ( 2000 ) );
+			ros::Duration(0.1).sleep();
 		}
 		ROS_INFO("Set stream rate call successful");
 	}
diff --git a/src/testflockfev.bzz b/src/testflockfev.bzz
index cd00626..7fe2891 100644
--- a/src/testflockfev.bzz
+++ b/src/testflockfev.bzz
@@ -43,14 +43,15 @@ function hexagon() {
     math.vec2.scale(accum, 1.0 / neighbors.count())
   # Move according to vector
   #print("Robot ", id, "must push ",accum.length, "; ", accum.angle)
+  uav_moveto(accum.x,accum.y)
 
-  if(timeW>=WAIT_TIMEOUT) {	#FOR MOVETO TESTS
-  	timeW =0
-  	statef=land
-  	} else {
-  	timeW = timeW+1
-  	uav_moveto(0.5,0.0)
-  	}
+#  if(timeW>=WAIT_TIMEOUT) {	#FOR MOVETO TESTS
+#  	timeW =0
+#  	statef=land
+#  	} else {
+#  	timeW = timeW+1
+#  	uav_moveto(0.5,0.0)
+#  	}
 }
  
 ########################################
@@ -115,7 +116,7 @@ function takeoff() {
 	log("TakeOff: ", flight.status)
 	log("Relative position: ", position.altitude)
 	
-	if( flight.status == 1 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
+	if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
 		barrier_set(ROBOTS,hexagon)
 		barrier_ready()
 		#statef=hexagon
@@ -130,7 +131,7 @@ function land() {
 	CURSTATE = "LAND"
 	statef=land
 	log("Land: ", flight.status)
-	if(flight.status == 2 or flight.status == 1){
+	if(flight.status == 2 or flight.status == 3){
 		neighbors.broadcast("cmd", 21)
 		uav_land()
 	}
@@ -200,4 +201,4 @@ function reset() {
  
 # Executed once at the end of experiment.
 function destroy() {
-}
\ No newline at end of file
+}

From 7c5340cdd6df036abfc8fbcaf8402cd97fa0acb1 Mon Sep 17 00:00:00 2001
From: David St-Onge <david.st-onge@polymtl.ca>
Date: Wed, 5 Apr 2017 17:58:03 -0400
Subject: [PATCH 25/38] timeout change

---
 include/roscontroller.h | 6 +++---
 src/roscontroller.cpp   | 4 ++--
 src/testflockfev.bzz    | 3 ++-
 3 files changed, 7 insertions(+), 6 deletions(-)

diff --git a/include/roscontroller.h b/include/roscontroller.h
index 1e5ae9e..fa8b58f 100644
--- a/include/roscontroller.h
+++ b/include/roscontroller.h
@@ -72,10 +72,10 @@ private:
 	int armstate;
 	int barrier;
 	int message_number=0;
-	int no_of_robots=0;
+	uint8_t no_of_robots=0;
 	/*tmp to be corrected*/
-	int no_cnt=0;
-	int old_val=0;	
+	uint8_t no_cnt=0;
+	uint8_t old_val=0;	
 	std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by,xbeesrv_name;
 	std::string stream_client_name;
 	std::string relative_altitude_sub_name;
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index d7e2803..9ebc7f8 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -712,9 +712,9 @@ namespace rosbzz_node{
 				old_val=neighbours_pos_map.size()+1;
 			
 			}			
-			else if(old_val==neighbours_pos_map.size()+1){
+			else if(no_cnt!=0 && old_val==neighbours_pos_map.size()+1){
 				no_cnt++;
-				if(no_cnt==3){
+				if(no_cnt>=4){
 					no_of_robots=neighbours_pos_map.size()+1;
 					no_cnt=0;
 				}
diff --git a/src/testflockfev.bzz b/src/testflockfev.bzz
index 7fe2891..9546df2 100644
--- a/src/testflockfev.bzz
+++ b/src/testflockfev.bzz
@@ -86,7 +86,7 @@ function barrier_ready() {
 #
 # Executes the barrier
 #
-WAIT_TIMEOUT = 100
+WAIT_TIMEOUT = 200
 timeW=0
 function barrier_wait(threshold, transf) {
   barrier.get(id)
@@ -136,6 +136,7 @@ function land() {
 		uav_land()
 	}
 	else {
+		timeW=0
 		barrier = nil
 		statef=idle
 	}

From 6fb86e7dd01c448a4c300e7c994635fddbe63ddb Mon Sep 17 00:00:00 2001
From: David St-Onge <david.st-onge@polymtl.ca>
Date: Wed, 5 Apr 2017 18:11:27 -0400
Subject: [PATCH 26/38] setpoint launch

---
 include/roscontroller.h        | 2 +-
 launch/launch_config/m100.yaml | 1 +
 launch/launch_config/solo.yaml | 1 +
 src/roscontroller.cpp          | 4 +++-
 4 files changed, 6 insertions(+), 2 deletions(-)

diff --git a/include/roscontroller.h b/include/roscontroller.h
index fa8b58f..8f85965 100644
--- a/include/roscontroller.h
+++ b/include/roscontroller.h
@@ -76,7 +76,7 @@ private:
 	/*tmp to be corrected*/
 	uint8_t no_cnt=0;
 	uint8_t old_val=0;	
-	std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by,xbeesrv_name;
+	std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by,xbeesrv_name, setpoint_name;
 	std::string stream_client_name;
 	std::string relative_altitude_sub_name;
 	bool rcclient;
diff --git a/launch/launch_config/m100.yaml b/launch/launch_config/m100.yaml
index 664b9b2..758aa8e 100644
--- a/launch/launch_config/m100.yaml
+++ b/launch/launch_config/m100.yaml
@@ -3,6 +3,7 @@ topics:
   battery  :   /power_status
   status   :   /flight_status
   fcclient :   /dji_mavcmd
+  setpoint :   /setpoint_raw/local
   armclient:   /dji_mavarm
   modeclient:  /dji_mavmode
   altitude: /rel_alt
diff --git a/launch/launch_config/solo.yaml b/launch/launch_config/solo.yaml
index d9f7961..a7e2e20 100644
--- a/launch/launch_config/solo.yaml
+++ b/launch/launch_config/solo.yaml
@@ -3,6 +3,7 @@ topics:
   battery :   /mavros/battery
   status  :   /mavros/state
   fcclient:   /mavros/cmd/command
+  setpoint:   /mavros/setpoint_raw/local
   armclient:  /mavros/cmd/arming
   modeclient: /mavros/set_mode
   stream:  /mavros/set_stream_rate
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index 9ebc7f8..39819d3 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -170,6 +170,8 @@ namespace rosbzz_node{
   		/*Obtain fc client name from parameter server*/
   		if(node_handle.getParam("topics/fcclient", fcclient_name));
   		else {ROS_ERROR("Provide a fc client name in Launch file"); system("rosnode kill rosbuzz_node");}  
+  		if(node_handle.getParam("topics/setpoint", setpoint_name));
+  		else {ROS_ERROR("Provide a Set Point name in Launch file"); system("rosnode kill rosbuzz_node");}  
   		if(node_handle.getParam("topics/armclient", armclient));
   		else {ROS_ERROR("Provide an arm client name in Launch file"); system("rosnode kill rosbuzz_node");}  
   		if(node_handle.getParam("topics/modeclient", modeclient));
@@ -195,7 +197,7 @@ namespace rosbzz_node{
   		/*publishers*/
 		payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 1000);
 		neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("/neighbours_pos",1000);
-		localsetpoint_pub = n_c.advertise<mavros_msgs::PositionTarget>("/mavros/setpoint_raw/local",1000);
+		localsetpoint_pub = n_c.advertise<mavros_msgs::PositionTarget>(setpoint_name,1000);
 		/* Service Clients*/
 		arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient);
 		mode_client =  n_c.serviceClient<mavros_msgs::SetMode>(modeclient);

From 1cc44d3d56ac5660ed202b9bdfe3adc92a21b086 Mon Sep 17 00:00:00 2001
From: David St-Onge <david.st-onge@polymtl.ca>
Date: Mon, 10 Apr 2017 14:49:01 -0400
Subject: [PATCH 27/38] swarm test

---
 src/buzz_update.cpp  |  2 +-
 src/buzz_utility.cpp | 17 +++++++++--------
 src/testflockfev.bzz |  3 +++
 3 files changed, 13 insertions(+), 9 deletions(-)

diff --git a/src/buzz_update.cpp b/src/buzz_update.cpp
index be9d43e..d622228 100644
--- a/src/buzz_update.cpp
+++ b/src/buzz_update.cpp
@@ -454,7 +454,7 @@ void collect_data(){
 double time_spent =   (t2.tv_sec - t1.tv_sec) * 1000.0; //(double)(end - begin) / CLOCKS_PER_SEC;
 time_spent += (t2.tv_usec - t1.tv_usec) / 1000.0;
 //int bytecodesize=(int);
-fprintf(stdout,"Data logger Info : %i , %i , %f , %ld , %i , %d \n",(int)buzz_utility::get_robotid(),neigh,time_spent,*(size_t*)updater->bcode_size,(int)no_of_robot,*(uint8_t*)updater->update_no);
+fprintf(stdout,"Data logger Info : %i , %i , %f , %ld , %i , %d \n",(int)no_of_robot,neigh,time_spent,*(size_t*)updater->bcode_size,(int)no_of_robot,*(uint8_t*)updater->update_no);
 //FILE *Fileptr;
 //Fileptr=fopen("/home/ubuntu/ROS_WS/update_logger.csv", "a");
 //fprintf(Fileptr,"%i,%i,%i,%i,%i,%u\n",(int)buzz_utility::get_robotid(),neigh,timer_steps,(int)*(size_t*)updater->bcode_size,(int)no_of_robot, *(uint8_t*)updater->update_no);
diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp
index e44957c..899ff05 100644
--- a/src/buzz_utility.cpp
+++ b/src/buzz_utility.cpp
@@ -473,17 +473,18 @@ namespace buzz_utility{
 	   /*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/
 	   /* look into this currently we don't have swarm feature tested */
 	   /*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/
+	   usleep(10000);
 	   /*Print swarm*/
-	   //buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
+	   buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
 	   /* Check swarm state */
-	   /*  int status = 1;
+	     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);*/
+	   buzzvm_gstore(VM);
 	}
 
 	/****************************************/
@@ -533,16 +534,16 @@ namespace buzz_utility{
 		return a == BUZZVM_STATE_READY;
 	}
 
-	uint16_t get_robotid() {
-		/* Get hostname */
+/*	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 */
+		// 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() {
 		return VM;
diff --git a/src/testflockfev.bzz b/src/testflockfev.bzz
index 9546df2..aed623a 100644
--- a/src/testflockfev.bzz
+++ b/src/testflockfev.bzz
@@ -144,6 +144,9 @@ function land() {
 
 # Executed once at init time.
 function init() {
+	s = swarm.create(1)
+	s.select(1)
+	s.join()
 	statef=idle
 	CURSTATE = "IDLE"
 }

From 8f0b4dd79b4f4601fd806ea9f69f4d928ca46fcf Mon Sep 17 00:00:00 2001
From: David St-Onge <david.st-onge@polymtl.ca>
Date: Mon, 10 Apr 2017 19:08:03 -0400
Subject: [PATCH 28/38] swarm table test

---
 src/buzz_utility.cpp | 6 +++---
 src/testflockfev.bzz | 2 +-
 2 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp
index 899ff05..278e28c 100644
--- a/src/buzz_utility.cpp
+++ b/src/buzz_utility.cpp
@@ -473,18 +473,18 @@ namespace buzz_utility{
 	   /*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/
 	   /* look into this currently we don't have swarm feature tested */
 	   /*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/
-	   usleep(10000);
+//	   usleep(10000);
 	   /*Print swarm*/
 	   buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
 	   /* Check swarm state */
-	     int status = 1;
+/*	     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);
+	   buzzvm_gstore(VM);*/
 	}
 
 	/****************************************/
diff --git a/src/testflockfev.bzz b/src/testflockfev.bzz
index aed623a..34e0d6c 100644
--- a/src/testflockfev.bzz
+++ b/src/testflockfev.bzz
@@ -145,7 +145,7 @@ function land() {
 # Executed once at init time.
 function init() {
 	s = swarm.create(1)
-	s.select(1)
+#	s.select(1)
 	s.join()
 	statef=idle
 	CURSTATE = "IDLE"

From c5df7e00a4b2d7343ab30a61cf52f753301b62a7 Mon Sep 17 00:00:00 2001
From: David St-Onge <david.st-onge@polymtl.ca>
Date: Mon, 10 Apr 2017 19:24:05 -0400
Subject: [PATCH 29/38] test swarm table

---
 src/buzz_utility.cpp | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp
index 278e28c..d1a8bb3 100644
--- a/src/buzz_utility.cpp
+++ b/src/buzz_utility.cpp
@@ -470,13 +470,13 @@ namespace buzz_utility{
 		      buzz_error_info());
 	      buzzvm_dump(VM);
 	   }
-	   /*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/
-	   /* look into this currently we don't have swarm feature tested */
-	   /*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/
 //	   usleep(10000);
 	   /*Print swarm*/
 	   buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
-	   /* Check swarm state */
+	   int SwarmSize = buzzdict_size(VM->swarmmembers)+1;
+	   fprintf(stderr, "Real Swarm Size: %i\n",SwarmSize);
+
+	   /* Check swarm state -- SOMETHING CRASHING HERE!! */
 /*	     int status = 1;
 	   buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status);
 	   if(status == 1 &&

From fbc2d4875dd91859b7ce820791fa4b15cf9f2f7c Mon Sep 17 00:00:00 2001
From: isvogor <isvogor@gmail.com>
Date: Mon, 10 Apr 2017 20:23:04 -0400
Subject: [PATCH 30/38] temp with setpoint_local - tested outside: working -
 TODO: handle local position home

---
 include/roscontroller.h            |  8 ++++
 launch/launch_config/solo.yaml     |  1 +
 launch/rosbuzz-testing-sitl.launch |  7 ++--
 src/roscontroller.cpp              | 59 ++++++++++++++++++++++++------
 src/testflockfev.bzz               | 16 ++++----
 5 files changed, 69 insertions(+), 22 deletions(-)

diff --git a/include/roscontroller.h b/include/roscontroller.h
index 8f85965..87bd038 100644
--- a/include/roscontroller.h
+++ b/include/roscontroller.h
@@ -18,6 +18,7 @@
 #include "mavros_msgs/PositionTarget.h"
 #include "mavros_msgs/StreamRate.h"
 #include "mavros_msgs/ParamGet.h"
+#include "geometry_msgs/PoseStamped.h"
 #include "std_msgs/Float64.h"
 #include <sensor_msgs/LaserScan.h>
 #include <rosbuzz/neigh_pos.h>
@@ -79,6 +80,7 @@ private:
 	std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by,xbeesrv_name, setpoint_name;
 	std::string stream_client_name;
 	std::string relative_altitude_sub_name;
+	std::string setpoint_nonraw;
 	bool rcclient;
 	bool multi_msg;
 	Num_robot_count count_robots;
@@ -87,6 +89,7 @@ private:
 	ros::Publisher payload_pub;
 	ros::Publisher neigh_pos_pub;
 	ros::Publisher localsetpoint_pub;
+	ros::Publisher localsetpoint_nonraw_pub;
 	ros::ServiceServer service;
 	ros::Subscriber current_position_sub;
 	ros::Subscriber battery_sub;
@@ -97,6 +100,9 @@ private:
 	ros::Subscriber relative_altitude_sub;
 	ros::ServiceClient stream_client;
 
+	int setpoint_counter;
+	double my_x = 0, my_y = 0;
+
 	/*Commands for flight controller*/
   	//mavros_msgs::CommandInt cmd_srv;
   	mavros_msgs::CommandLong cmd_srv;
@@ -191,6 +197,8 @@ private:
 
 	void SetLocalPosition(float x, float y, float z, float yaw);
 
+	void SetLocalPositionNonRaw(float x, float y, float z, float yaw);
+
 	void SetStreamRate(int id, int rate, int on_off);
 	
 	void get_number_of_robots();
diff --git a/launch/launch_config/solo.yaml b/launch/launch_config/solo.yaml
index a7e2e20..650ba45 100644
--- a/launch/launch_config/solo.yaml
+++ b/launch/launch_config/solo.yaml
@@ -4,6 +4,7 @@ topics:
   status  :   /mavros/state
   fcclient:   /mavros/cmd/command
   setpoint:   /mavros/setpoint_raw/local
+  setpoint_nonraw:   /mavros/setpoint_position/local
   armclient:  /mavros/cmd/arming
   modeclient: /mavros/set_mode
   stream:  /mavros/set_stream_rate
diff --git a/launch/rosbuzz-testing-sitl.launch b/launch/rosbuzz-testing-sitl.launch
index a6e6405..141f0a0 100644
--- a/launch/rosbuzz-testing-sitl.launch
+++ b/launch/rosbuzz-testing-sitl.launch
@@ -16,13 +16,14 @@
 		<arg name="log_output" value="$(arg log_output)" />
 	</include>
 -->
-	<!-- run xbee 
+	<!-- 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" />
--->
+	<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/ivan/catkin_ws/src/rosbuzz/launch/launch_config/solo.yaml"/>
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index 39819d3..d029c5e 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -24,6 +24,7 @@ namespace rosbzz_node{
 		SetStreamRate(0, 10, 1);
 		/// Get Robot Id - wait for Xbee to be started
 		GetRobotId();
+		setpoint_counter = 0;
 	}
 
 	/*---------------------
@@ -40,7 +41,7 @@ namespace rosbzz_node{
 
 	void roscontroller::GetRobotId()
 	{
-        
+        /*
 		mavros_msgs::ParamGet::Request robot_id_srv_request; robot_id_srv_request.param_id="id";
         	mavros_msgs::ParamGet::Response robot_id_srv_response;
 		while(!xbeestatus_srv.call(robot_id_srv_request,robot_id_srv_response)){
@@ -49,7 +50,8 @@ namespace rosbzz_node{
 		}
 
 		robot_id=robot_id_srv_response.value.integer;
-
+		*/
+		robot_id = 8;
 	}
 
 	/*-------------------------------------------------
@@ -178,6 +180,8 @@ namespace rosbzz_node{
   		else {ROS_ERROR("Provide a mode client name in Launch file"); system("rosnode kill rosbuzz_node");}  
   		if(node_handle.getParam("topics/stream", stream_client_name));
   		else {ROS_ERROR("Provide a mode client name in Launch file"); system("rosnode kill rosbuzz_node");}
+  		if(node_handle.getParam("topics/setpoint_nonraw", setpoint_nonraw));
+  		else {ROS_ERROR("Provide a mode setpoint non raw client in Launch file"); system("rosnode kill rosbuzz_node");}
 	}
 
 	/*--------------------------------------------------------
@@ -198,6 +202,7 @@ namespace rosbzz_node{
 		payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 1000);
 		neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("/neighbours_pos",1000);
 		localsetpoint_pub = n_c.advertise<mavros_msgs::PositionTarget>(setpoint_name,1000);
+		localsetpoint_nonraw_pub = n_c.advertise<geometry_msgs::PoseStamped>(setpoint_nonraw,1000);
 		/* Service Clients*/
 		arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient);
 		mode_client =  n_c.serviceClient<mavros_msgs::SetMode>(modeclient);
@@ -403,7 +408,7 @@ namespace rosbzz_node{
 					SetMode("LOITER", 0);
 					armstate = 1;
 					Arm();
-					ros::Duration(0.1).sleep();
+					ros::Duration(0.5).sleep();
 				}
 				if(current_mode != "GUIDED")
 					SetMode("GUIDED", 2000); // for real solo, just add 2000ms delay (it should always be in loiter after arm/disarm)
@@ -443,7 +448,9 @@ namespace rosbzz_node{
 			}
 		} else if (tmp == buzzuav_closures::COMMAND_MOVETO) { /*Buzz call for moveto*/
 			/*prepare the goto publish message ATTENTION: ENU FRAME FOR MAVROS STANDARD (then converted to NED)*/
-			roscontroller::SetLocalPosition(goto_pos[1], goto_pos[0], goto_pos[2], 0);
+			// roscontroller::SetLocalPosition(goto_pos[1], goto_pos[0], goto_pos[2], 0);
+
+			roscontroller::SetLocalPositionNonRaw(goto_pos[1], goto_pos[0], goto_pos[2], 0);
 		}
 	}
 	/*----------------------------------------------
@@ -773,14 +780,17 @@ namespace rosbzz_node{
 		moveMsg.coordinate_frame = mavros_msgs::PositionTarget::FRAME_LOCAL_OFFSET_NED;
 		moveMsg.type_mask = mavros_msgs::PositionTarget::IGNORE_AFX |
 		                                     mavros_msgs::PositionTarget::IGNORE_AFY |
-		                                     mavros_msgs::PositionTarget::IGNORE_AFZ |
-		                                     mavros_msgs::PositionTarget::IGNORE_VX  |
-		                                     mavros_msgs::PositionTarget::IGNORE_VY  |
-		                                     mavros_msgs::PositionTarget::IGNORE_VZ;
+		                                     mavros_msgs::PositionTarget::IGNORE_AFZ;
+		// alt: 0b0000111111111000
+		moveMsg.header.frame_id = 1;
+		moveMsg.header.seq = setpoint_counter++;
 		moveMsg.header.stamp = ros::Time::now();
-		moveMsg.position.x = x;
-		moveMsg.position.y = y;
-		moveMsg.position.z = z;
+		moveMsg.position.x = 0.5;
+		moveMsg.position.y = 0.0;
+		moveMsg.position.z = 2;
+		moveMsg.velocity.x = 0.5;
+		moveMsg.velocity.y = 0.0;
+		moveMsg.velocity.z = 0.5;
 		moveMsg.yaw = 0;
 
 		localsetpoint_pub.publish(moveMsg);
@@ -788,6 +798,33 @@ namespace rosbzz_node{
 		ROS_INFO("Sent local position message!");
 	}
 
+	void roscontroller::SetLocalPositionNonRaw(float x, float y, float z, float yaw){
+		// http://docs.ros.org/kinetic/api/mavros_msgs/html/msg/PositionTarget.html
+		// http://ardupilot.org/dev/docs/copter-commands-in-guided-mode.html#copter-commands-in-guided-mode-set-position-target-local-ned
+
+		geometry_msgs::PoseStamped moveMsg;
+		moveMsg.header.stamp = ros::Time::now();
+		moveMsg.header.seq = setpoint_counter++;
+		moveMsg.header.frame_id = 1;
+
+		my_x += x;
+		my_y += y;
+
+		moveMsg.pose.position.x = my_x;
+		moveMsg.pose.position.y = my_y;
+		moveMsg.pose.position.z = 3;
+
+		moveMsg.pose.orientation.x = 0;
+		moveMsg.pose.orientation.y = 0;
+		moveMsg.pose.orientation.z = 0;
+		moveMsg.pose.orientation.w = 1;
+
+
+		localsetpoint_nonraw_pub.publish(moveMsg);
+
+		ROS_INFO("Sent local NON RAW position message!");
+	}
+
 	void roscontroller::SetMode(std::string mode, int delay_miliseconds){
 		// wait if necessary
 		if (delay_miliseconds != 0){
diff --git a/src/testflockfev.bzz b/src/testflockfev.bzz
index 9546df2..8ef9a02 100644
--- a/src/testflockfev.bzz
+++ b/src/testflockfev.bzz
@@ -43,15 +43,15 @@ function hexagon() {
     math.vec2.scale(accum, 1.0 / neighbors.count())
   # Move according to vector
   #print("Robot ", id, "must push ",accum.length, "; ", accum.angle)
-  uav_moveto(accum.x,accum.y)
+#  uav_moveto(accum.x,accum.y)
 
-#  if(timeW>=WAIT_TIMEOUT) {	#FOR MOVETO TESTS
-#  	timeW =0
-#  	statef=land
-#  	} else {
-#  	timeW = timeW+1
-#  	uav_moveto(0.5,0.0)
-#  	}
+  if(timeW>=WAIT_TIMEOUT) {	#FOR MOVETO TESTS
+  	timeW =0
+  	statef=land
+  	} else {
+  	timeW = timeW+1
+  	uav_moveto(0.0,0.02)
+  	}
 }
  
 ########################################

From 8df538d056c11d703029cae4a1f02c84609ea5ac Mon Sep 17 00:00:00 2001
From: vivek-shankar <robovivek@gmail.com>
Date: Tue, 11 Apr 2017 20:43:57 -0400
Subject: [PATCH 31/38] delete correction to free

---
 src/buzz_utility.cpp  | 4 ++--
 src/roscontroller.cpp | 4 ++--
 2 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp
index d1a8bb3..ce2a40e 100644
--- a/src/buzz_utility.cpp
+++ b/src/buzz_utility.cpp
@@ -91,7 +91,7 @@ namespace buzz_utility{
 		    			tot += unMsgSize;
 		 			}
 	      			}while(size - tot > sizeof(uint16_t) && unMsgSize > 0);
-		delete[] pl;
+		free(pl);
    		/* Process messages */
 		buzzvm_process_inmsgs(VM);
 	}
@@ -141,7 +141,7 @@ namespace buzz_utility{
    		uint64_t* payload_64 = new uint64_t[total_size];
 
    		memcpy((void*)payload_64, (void*)buff_send, total_size*sizeof(uint64_t));
-		delete[] buff_send;
+		free(buff_send);
   		/*for(int i=0;i<total_size;i++){
    		cout<<" payload from out msg  "<<*(payload_64+i)<<endl;
    		}*/
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index d029c5e..0aabbb1 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -363,7 +363,7 @@ namespace rosbzz_node{
 		    	uint16_t total_size =(ceil((float)(float)tot/(float)sizeof(uint64_t))); 
 		    	uint64_t* payload_64 = new uint64_t[total_size];
 	  	    	memcpy((void*)payload_64, (void*)buff_send, total_size*sizeof(uint64_t));
-		    	delete[] buff_send;
+		    	free(buff_send);
 		    	/*Send a constant number to differenciate updater msgs*/
 		    	update_packets.payload64.push_back((uint64_t)UPDATER_MESSAGE_CONSTANT);
 		    	for(int i=0;i<total_size;i++){
@@ -608,7 +608,7 @@ namespace rosbzz_node{
 			      	code_message_inqueue_process();
 				//fprintf(stdout,"after in queue process : utils\n");
 			}
-   			delete[] pl;
+   			free(pl);
 		}
 		/*BVM FIFO message*/
 		else if(msg->payload64.size() > 3){

From 6a1f18d6567d275128da44ea41035f0bce20a77d Mon Sep 17 00:00:00 2001
From: isvogor <isvogor@gmail.com>
Date: Wed, 12 Apr 2017 15:29:27 -0400
Subject: [PATCH 32/38] test: suspecting offset is ignored

---
 src/roscontroller.cpp | 34 +++++++++++++++++++++-------------
 src/testflockfev.bzz  |  2 +-
 2 files changed, 22 insertions(+), 14 deletions(-)

diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index d029c5e..a4f3269 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -448,9 +448,9 @@ namespace rosbzz_node{
 			}
 		} else if (tmp == buzzuav_closures::COMMAND_MOVETO) { /*Buzz call for moveto*/
 			/*prepare the goto publish message ATTENTION: ENU FRAME FOR MAVROS STANDARD (then converted to NED)*/
-			// roscontroller::SetLocalPosition(goto_pos[1], goto_pos[0], goto_pos[2], 0);
+			roscontroller::SetLocalPosition(goto_pos[1], goto_pos[0], goto_pos[2], 0);
 
-			roscontroller::SetLocalPositionNonRaw(goto_pos[1], goto_pos[0], goto_pos[2], 0);
+			//roscontroller::SetLocalPositionNonRaw(goto_pos[1], goto_pos[0], goto_pos[2], 0);
 		}
 	}
 	/*----------------------------------------------
@@ -777,21 +777,29 @@ namespace rosbzz_node{
 		// http://ardupilot.org/dev/docs/copter-commands-in-guided-mode.html#copter-commands-in-guided-mode-set-position-target-local-ned
 
 		mavros_msgs::PositionTarget moveMsg;
+		const uint16_t ignore_all_except_xyz_y = (1 << 11) | (7 << 6) | (7 << 3);
+
 		moveMsg.coordinate_frame = mavros_msgs::PositionTarget::FRAME_LOCAL_OFFSET_NED;
-		moveMsg.type_mask = mavros_msgs::PositionTarget::IGNORE_AFX |
-		                                     mavros_msgs::PositionTarget::IGNORE_AFY |
-		                                     mavros_msgs::PositionTarget::IGNORE_AFZ;
-		// alt: 0b0000111111111000
+		moveMsg.type_mask = ignore_all_except_xyz_y;
+
 		moveMsg.header.frame_id = 1;
 		moveMsg.header.seq = setpoint_counter++;
 		moveMsg.header.stamp = ros::Time::now();
-		moveMsg.position.x = 0.5;
-		moveMsg.position.y = 0.0;
-		moveMsg.position.z = 2;
-		moveMsg.velocity.x = 0.5;
-		moveMsg.velocity.y = 0.0;
-		moveMsg.velocity.z = 0.5;
-		moveMsg.yaw = 0;
+
+		my_x += x;
+		my_y += y;
+
+		moveMsg.position.x = x;
+		moveMsg.position.y = y;
+		moveMsg.position.z = 3;
+		moveMsg.yaw_rate = 0.0;
+		moveMsg.yaw = 0.0;
+
+
+		geometry_msgs::Vector3 zeroVec;
+		zeroVec.x = 0.0; zeroVec.y = 0.0; zeroVec.z = 0.0;
+		moveMsg.acceleration_or_force = zeroVec;
+		moveMsg.velocity = zeroVec;
 
 		localsetpoint_pub.publish(moveMsg);
 
diff --git a/src/testflockfev.bzz b/src/testflockfev.bzz
index 3bda4c7..7b1df3f 100644
--- a/src/testflockfev.bzz
+++ b/src/testflockfev.bzz
@@ -50,7 +50,7 @@ function hexagon() {
   	statef=land
   	} else {
   	timeW = timeW+1
-  	uav_moveto(0.0,0.02)
+  	uav_moveto(0.0,0.35)
   	}
 }
  

From 3ee97bcf207d360d71f9e238bf0d198b0e631f7a Mon Sep 17 00:00:00 2001
From: FranckHumanitas <franck@humanitas.io>
Date: Wed, 12 Apr 2017 21:38:07 +0000
Subject: [PATCH 33/38] solo launch file

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

diff --git a/launch/rosbuzz-solo.launch b/launch/rosbuzz-solo.launch
new file mode 100644
index 0000000..26ba3e7
--- /dev/null
+++ b/launch/rosbuzz-solo.launch
@@ -0,0 +1,42 @@
+<launch>
+	<!-- 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" />
+
+	<include file="/home/pi/ros_catkinKin_ws/src/mavros/mavros/launch/node.launch">
+		<arg name="pluginlists_yaml" value="/home/pi/ros_catkinKin_ws/src/mavros/mavros/launch/apm_pluginlists.yaml" />
+		<arg name="config_yaml" value="/home/pi/ros_catkinKin_ws/src/mavros/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" />
+	<param name="Xbee_In_From_Controller" type="str" value="xbee_cmd" />
+
+
+	<!-- run rosbuzz -->
+	<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >	
+		<rosparam file="/home/pi/ros_catkinKin_ws/src/ROSBuzz/launch/launch_config/solo.yaml"/>
+		<param name="bzzfile_name" value="/home/pi/ros_catkinKin_ws/src/ROSBuzz/src/testflockfev.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="xbee_status_srv" value="/xbee_status"/>
+		<param name="stand_by" value="/home/pi/ros_catkinKin_ws/src/ROSBuzz/src/stand_by.bo"/>
+	</node>
+
+</launch>

From 8b971e410b1e66b375d6e456d2d2427b05def20d Mon Sep 17 00:00:00 2001
From: David St-Onge <david.st-onge@polymtl.ca>
Date: Wed, 12 Apr 2017 17:51:22 -0400
Subject: [PATCH 34/38] added nonraw topic

---
 launch/launch_config/m100.yaml | 1 +
 src/roscontroller.cpp          | 4 +---
 2 files changed, 2 insertions(+), 3 deletions(-)

diff --git a/launch/launch_config/m100.yaml b/launch/launch_config/m100.yaml
index 758aa8e..f9cb809 100644
--- a/launch/launch_config/m100.yaml
+++ b/launch/launch_config/m100.yaml
@@ -4,6 +4,7 @@ topics:
   status   :   /flight_status
   fcclient :   /dji_mavcmd
   setpoint :   /setpoint_raw/local
+  setpoint_nonraw :   /setpoint_position/local
   armclient:   /dji_mavarm
   modeclient:  /dji_mavmode
   altitude: /rel_alt
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index bdc3437..80dfb55 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -133,12 +133,10 @@ namespace rosbzz_node{
   		n_c.getParam("out_payload", out_payload);
 		/*Obtain in payload name*/
   		n_c.getParam("in_payload", in_payload);
-		/*Obtain Number of robots for barrier*/
-		n_c.getParam("No_of_Robots", barrier);
 		/*Obtain standby script to run during update*/
 		n_c.getParam("stand_by", stand_by);
 		n_c.getParam("xbee_status_srv", xbeesrv_name);
-		
+
 		GetSubscriptionParameters(n_c);
 		// initialize topics to null?
 

From 458099a8a6d6f01fa923ee143d6b7cfed5f75f84 Mon Sep 17 00:00:00 2001
From: David St-Onge <david.st-onge@polymtl.ca>
Date: Wed, 12 Apr 2017 19:23:53 -0400
Subject: [PATCH 35/38] added comm failsafe and setpoint non-offset

---
 include/roscontroller.h        |  6 ++-
 launch/launch_config/m100.yaml |  3 +-
 launch/launch_config/solo.yaml |  3 +-
 src/buzzuav_closures.cpp       |  2 +-
 src/roscontroller.cpp          | 83 +++++++++++-----------------------
 5 files changed, 35 insertions(+), 62 deletions(-)

diff --git a/include/roscontroller.h b/include/roscontroller.h
index 87bd038..d9aa948 100644
--- a/include/roscontroller.h
+++ b/include/roscontroller.h
@@ -37,6 +37,7 @@
 #define UPDATER_MESSAGE_CONSTANT 987654321
 #define XBEE_MESSAGE_CONSTANT 586782343
 #define XBEE_STOP_TRANSMISSION 4355356352
+#define TIMEOUT	60
 
 using namespace std;
 
@@ -61,6 +62,7 @@ private:
 	}; typedef struct num_robot_count Num_robot_count ;
 
  	double cur_pos[3];
+ 	double home[3];
  	double cur_rel_altitude;
 	uint64_t payload;
 	std::map< int,  buzz_utility::Pos_struct> neighbours_pos_map;
@@ -70,6 +72,7 @@ private:
 	int robot_id=0;
         //int oldcmdID=0;
 	int rc_cmd;
+	float fcu_timeout;
 	int armstate;
 	int barrier;
 	int message_number=0;
@@ -127,7 +130,7 @@ private:
 	void Compile_bzz();
 
 	/*Flight controller service call*/
-	void flight_controler_service_call();
+	void flight_controller_service_call();
 	
 	/*Neighbours pos publisher*/
 	void neighbours_pos_publisher();
@@ -151,6 +154,7 @@ private:
 			 double altitude);
 	/*convert from spherical to cartesian coordinate system callback */
 	void cvt_rangebearing_coordinates(double neighbours_pos_payload [], double out[], double pos[]);
+	void cvt_ned_coordinates(double neighbours_pos_payload [], double out[], double pos[]);
 
 	/*battery status callback*/ 
 	void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg);
diff --git a/launch/launch_config/m100.yaml b/launch/launch_config/m100.yaml
index f9cb809..1ffd380 100644
--- a/launch/launch_config/m100.yaml
+++ b/launch/launch_config/m100.yaml
@@ -3,8 +3,7 @@ topics:
   battery  :   /power_status
   status   :   /flight_status
   fcclient :   /dji_mavcmd
-  setpoint :   /setpoint_raw/local
-  setpoint_nonraw :   /setpoint_position/local
+  setpoint :   /setpoint_position/local
   armclient:   /dji_mavarm
   modeclient:  /dji_mavmode
   altitude: /rel_alt
diff --git a/launch/launch_config/solo.yaml b/launch/launch_config/solo.yaml
index 650ba45..8930701 100644
--- a/launch/launch_config/solo.yaml
+++ b/launch/launch_config/solo.yaml
@@ -3,8 +3,7 @@ topics:
   battery :   /mavros/battery
   status  :   /mavros/state
   fcclient:   /mavros/cmd/command
-  setpoint:   /mavros/setpoint_raw/local
-  setpoint_nonraw:   /mavros/setpoint_position/local
+  setpoint:   /mavros/setpoint_position/local
   armclient:  /mavros/cmd/arming
   modeclient: /mavros/set_mode
   stream:  /mavros/set_stream_rate
diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp
index a77c79d..6fa15b9 100644
--- a/src/buzzuav_closures.cpp
+++ b/src/buzzuav_closures.cpp
@@ -104,7 +104,7 @@ namespace buzzuav_closures{
 	   double d = sqrt(dx*dx+dy*dy);	//range
 	   goto_pos[0]=dx;
            goto_pos[1]=dy;
-           goto_pos[2]=0;
+           goto_pos[2]=height;
 	   /*double b = atan2(dy,dx);		//bearing
 	   printf(" Vector for Goto: %.7f,%.7f\n",dx,dy);
 	   gps_from_rb(d, b, goto_pos);
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index 80dfb55..386563c 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -25,6 +25,7 @@ namespace rosbzz_node{
 		/// Get Robot Id - wait for Xbee to be started
 		GetRobotId();
 		setpoint_counter = 0;
+		fcu_timeout = TIMEOUT;
 	}
 
 	/*---------------------
@@ -76,7 +77,7 @@ namespace rosbzz_node{
 				/*Prepare messages and publish them in respective topic*/
 		  		prepare_msg_and_publish();
 				/*call flight controler service to set command long*/
-				flight_controler_service_call();
+				flight_controller_service_call();
 				/*Set multi message available after update*/
 				if(get_update_status()){
 					set_read_update_status();
@@ -94,6 +95,10 @@ namespace rosbzz_node{
 				/*loop rate of ros*/
 				 ros::Rate loop_rate(10);
 				 loop_rate.sleep();
+				 if(fcu_timeout<=0)
+					buzzuav_closures::rc_call(mavros_msgs::CommandCode::NAV_LAND);
+				 else
+					fcu_timeout -= 1/10;
  				/*sleep for the mentioned loop rate*/
     				timer_step+=1;
    				maintain_pos(timer_step);
@@ -178,8 +183,6 @@ namespace rosbzz_node{
   		else {ROS_ERROR("Provide a mode client name in Launch file"); system("rosnode kill rosbuzz_node");}  
   		if(node_handle.getParam("topics/stream", stream_client_name));
   		else {ROS_ERROR("Provide a mode client name in Launch file"); system("rosnode kill rosbuzz_node");}
-  		if(node_handle.getParam("topics/setpoint_nonraw", setpoint_nonraw));
-  		else {ROS_ERROR("Provide a mode setpoint non raw client in Launch file"); system("rosnode kill rosbuzz_node");}
 	}
 
 	/*--------------------------------------------------------
@@ -199,8 +202,7 @@ namespace rosbzz_node{
   		/*publishers*/
 		payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 1000);
 		neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("/neighbours_pos",1000);
-		localsetpoint_pub = n_c.advertise<mavros_msgs::PositionTarget>(setpoint_name,1000);
-		localsetpoint_nonraw_pub = n_c.advertise<geometry_msgs::PoseStamped>(setpoint_nonraw,1000);
+		localsetpoint_pub = n_c.advertise<geometry_msgs::PoseStamped>(setpoint_name,1000);
 		/* Service Clients*/
 		arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient);
 		mode_client =  n_c.serviceClient<mavros_msgs::SetMode>(modeclient);
@@ -381,7 +383,7 @@ namespace rosbzz_node{
 	/*---------------------------------------------------------------------------------
 	/Flight controller service call every step if there is a command set from bzz script
 	/-------------------------------------------------------------------------------- */
-	void roscontroller::flight_controler_service_call() {
+	void roscontroller::flight_controller_service_call() {
 		/* flight controller client call if requested from Buzz*/
 		/*FC call for takeoff,land, gohome and arm/disarm*/
 		int tmp = buzzuav_closures::bzz_cmd();
@@ -506,15 +508,14 @@ namespace rosbzz_node{
 		out[0] = sqrt(ned_x*ned_x+ned_y*ned_y);
 		out[1] = atan2(ned_y,ned_x);
 		out[2] = 0.0;
-/*		double lat1 = cur[0]*M_PI/180.0;
-		double lon1 = cur[1]*M_PI/180.0;
-		double lat2 = nei[0]*M_PI/180.0;
-		double lon2 = nei[1]*M_PI/180.0;
-		out[0] = acos(sin(lat1) * sin(lat2) +cos(lat2) * cos(lat1)*cos(lon2-lon1))*EARTH_RADIUS;
-		double y = sin(lon1-lon2)*cos(lat1);
-		double x =  cos(lat2)*sin(lat1) - sin(lat2)*cos(lat1)*cos(lon1-lon2);
-		out[1] = atan2(y,x)+M_PI;
-		out[2] = 0.0;*/
+	}
+
+	void roscontroller::cvt_ned_coordinates(double nei[], double out[], double cur[]){
+ 		double d_lon = nei[1] - cur[1];
+        	double d_lat = nei[0] - cur[0];
+        	out[0] = DEG2RAD(d_lat) * EARTH_RADIUS;
+        	out[1] = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0]));
+		out[2] = cur[2];
 	}
 
 	/*------------------------------------------------------
@@ -551,6 +552,12 @@ namespace rosbzz_node{
 	/-------------------------------------------------------------*/
 	void roscontroller::current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg){
 		//ROS_INFO("Altitude out: %f", cur_rel_altitude);
+		fcu_timeout = TIMEOUT;
+		if(home[0]==0){
+			home[0]=msg->latitude;
+			home[1]=msg->longitude;
+			home[2]=cur_rel_altitude;
+		}
 		set_cur_pos(msg->latitude,msg->longitude, cur_rel_altitude);//msg->altitude);
 		buzzuav_closures::set_currentpos(msg->latitude,msg->longitude, cur_rel_altitude);//msg->altitude);
 	}
@@ -774,51 +781,15 @@ namespace rosbzz_node{
 		// http://docs.ros.org/kinetic/api/mavros_msgs/html/msg/PositionTarget.html
 		// http://ardupilot.org/dev/docs/copter-commands-in-guided-mode.html#copter-commands-in-guided-mode-set-position-target-local-ned
 
-		mavros_msgs::PositionTarget moveMsg;
-		const uint16_t ignore_all_except_xyz_y = (1 << 11) | (7 << 6) | (7 << 3);
-
-		moveMsg.coordinate_frame = mavros_msgs::PositionTarget::FRAME_LOCAL_OFFSET_NED;
-		moveMsg.type_mask = ignore_all_except_xyz_y;
-
-		moveMsg.header.frame_id = 1;
-		moveMsg.header.seq = setpoint_counter++;
-		moveMsg.header.stamp = ros::Time::now();
-
-		my_x += x;
-		my_y += y;
-
-		moveMsg.position.x = x;
-		moveMsg.position.y = y;
-		moveMsg.position.z = 3;
-		moveMsg.yaw_rate = 0.0;
-		moveMsg.yaw = 0.0;
-
-
-		geometry_msgs::Vector3 zeroVec;
-		zeroVec.x = 0.0; zeroVec.y = 0.0; zeroVec.z = 0.0;
-		moveMsg.acceleration_or_force = zeroVec;
-		moveMsg.velocity = zeroVec;
-
-		localsetpoint_pub.publish(moveMsg);
-
-		ROS_INFO("Sent local position message!");
-	}
-
-	void roscontroller::SetLocalPositionNonRaw(float x, float y, float z, float yaw){
-		// http://docs.ros.org/kinetic/api/mavros_msgs/html/msg/PositionTarget.html
-		// http://ardupilot.org/dev/docs/copter-commands-in-guided-mode.html#copter-commands-in-guided-mode-set-position-target-local-ned
-
 		geometry_msgs::PoseStamped moveMsg;
 		moveMsg.header.stamp = ros::Time::now();
 		moveMsg.header.seq = setpoint_counter++;
 		moveMsg.header.frame_id = 1;
-
-		my_x += x;
-		my_y += y;
-
-		moveMsg.pose.position.x = my_x;
-		moveMsg.pose.position.y = my_y;
-		moveMsg.pose.position.z = 3;
+		double local_pos[3];
+		cvt_ned_coordinates(cur_pos,local_pos,home);
+		moveMsg.pose.position.x = local_pos[0]+x;
+		moveMsg.pose.position.y = local_pos[1]+y;
+		moveMsg.pose.position.z = z;
 
 		moveMsg.pose.orientation.x = 0;
 		moveMsg.pose.orientation.y = 0;

From ea573d888e17ab95133eb6732f2714344a74475b Mon Sep 17 00:00:00 2001
From: David St-Onge <david.st-onge@polymtl.ca>
Date: Wed, 12 Apr 2017 19:26:56 -0400
Subject: [PATCH 36/38] swarm test

---
 src/buzz_utility.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp
index ce2a40e..1d0275f 100644
--- a/src/buzz_utility.cpp
+++ b/src/buzz_utility.cpp
@@ -477,9 +477,9 @@ namespace buzz_utility{
 	   fprintf(stderr, "Real Swarm Size: %i\n",SwarmSize);
 
 	   /* Check swarm state -- SOMETHING CRASHING HERE!! */
-/*	     int status = 1;
+	     int status = 1;
 	   buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status);
-	   if(status == 1 &&
+/*	   if(status == 1 &&
 	      buzzdict_size(VM->swarmmembers) < 9)
 	      status = 2;
 	   buzzvm_pushs(VM, buzzvm_string_register(VM, "swarm_status", 1));

From 02e15997d1b1576e5bc55f60ed3902a3a1b02eb0 Mon Sep 17 00:00:00 2001
From: David St-Onge <david.st-onge@polymtl.ca>
Date: Wed, 12 Apr 2017 19:42:28 -0400
Subject: [PATCH 37/38] swarm test

---
 src/buzz_utility.cpp  | 5 +++--
 src/roscontroller.cpp | 3 ---
 2 files changed, 3 insertions(+), 5 deletions(-)

diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp
index 1d0275f..f4bb43a 100644
--- a/src/buzz_utility.cpp
+++ b/src/buzz_utility.cpp
@@ -425,6 +425,7 @@ namespace buzz_utility{
 	void check_swarm_members(const void* key, void* data, void* params) {
 	   buzzswarm_elem_t e = *(buzzswarm_elem_t*)data;
 	   int* status = (int*)params;
+	fprintf(stderr, "CHECKING SWARM MEMBERS\n");
 	   if(*status == 3) return;
 	   if(buzzdarray_size(e->swarms) != 1) {
 	      fprintf(stderr, "Swarm list size is not 1\n");
@@ -481,10 +482,10 @@ namespace buzz_utility{
 	   buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status);
 /*	   if(status == 1 &&
 	      buzzdict_size(VM->swarmmembers) < 9)
-	      status = 2;
+	      status = 2;*/
 	   buzzvm_pushs(VM, buzzvm_string_register(VM, "swarm_status", 1));
 	   buzzvm_pushi(VM, status);
-	   buzzvm_gstore(VM);*/
+	   buzzvm_gstore(VM);
 	}
 
 	/****************************************/
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index 386563c..5a13086 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -42,7 +42,6 @@ namespace rosbzz_node{
 
 	void roscontroller::GetRobotId()
 	{
-        /*
 		mavros_msgs::ParamGet::Request robot_id_srv_request; robot_id_srv_request.param_id="id";
         	mavros_msgs::ParamGet::Response robot_id_srv_response;
 		while(!xbeestatus_srv.call(robot_id_srv_request,robot_id_srv_response)){
@@ -51,8 +50,6 @@ namespace rosbzz_node{
 		}
 
 		robot_id=robot_id_srv_response.value.integer;
-		*/
-		robot_id = 8;
 	}
 
 	/*-------------------------------------------------

From 3af2a03ffc8b08be7a34f094af1f7426e33a81b7 Mon Sep 17 00:00:00 2001
From: isvogor <isvogor@gmail.com>
Date: Thu, 13 Apr 2017 09:39:45 -0400
Subject: [PATCH 38/38] topic fix

---
 include/roscontroller.h | 1 -
 src/roscontroller.cpp   | 6 ++++--
 src/testflockfev.bzz    | 2 +-
 3 files changed, 5 insertions(+), 4 deletions(-)

diff --git a/include/roscontroller.h b/include/roscontroller.h
index d9aa948..738371a 100644
--- a/include/roscontroller.h
+++ b/include/roscontroller.h
@@ -92,7 +92,6 @@ private:
 	ros::Publisher payload_pub;
 	ros::Publisher neigh_pos_pub;
 	ros::Publisher localsetpoint_pub;
-	ros::Publisher localsetpoint_nonraw_pub;
 	ros::ServiceServer service;
 	ros::Subscriber current_position_sub;
 	ros::Subscriber battery_sub;
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index 5a13086..47473d1 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -42,6 +42,7 @@ namespace rosbzz_node{
 
 	void roscontroller::GetRobotId()
 	{
+
 		mavros_msgs::ParamGet::Request robot_id_srv_request; robot_id_srv_request.param_id="id";
         	mavros_msgs::ParamGet::Response robot_id_srv_response;
 		while(!xbeestatus_srv.call(robot_id_srv_request,robot_id_srv_response)){
@@ -50,6 +51,8 @@ namespace rosbzz_node{
 		}
 
 		robot_id=robot_id_srv_response.value.integer;
+
+		//robot_id = 8;
 	}
 
 	/*-------------------------------------------------
@@ -793,8 +796,7 @@ namespace rosbzz_node{
 		moveMsg.pose.orientation.z = 0;
 		moveMsg.pose.orientation.w = 1;
 
-
-		localsetpoint_nonraw_pub.publish(moveMsg);
+		localsetpoint_pub.publish(moveMsg);
 
 		ROS_INFO("Sent local NON RAW position message!");
 	}
diff --git a/src/testflockfev.bzz b/src/testflockfev.bzz
index 7b1df3f..30e5b80 100644
--- a/src/testflockfev.bzz
+++ b/src/testflockfev.bzz
@@ -50,7 +50,7 @@ function hexagon() {
   	statef=land
   	} else {
   	timeW = timeW+1
-  	uav_moveto(0.0,0.35)
+  	uav_moveto(0.0,0.0)
   	}
 }