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] 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!");
+		}
+	}
+
+
+
 }