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] 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