From 6a1c51fbd30ad2af64faadc0e1d19907eb5f1994 Mon Sep 17 00:00:00 2001
From: isvogor <isvogor@gmail.com>
Date: Tue, 9 May 2017 21:38:14 -0400
Subject: [PATCH] open loop, xbee wait

---
 launch/rosbuzz-solo.launch |  2 +-
 src/roscontroller.cpp      | 15 ++++++++-------
 2 files changed, 9 insertions(+), 8 deletions(-)

diff --git a/launch/rosbuzz-solo.launch b/launch/rosbuzz-solo.launch
index 97cfb46..3564ce1 100644
--- a/launch/rosbuzz-solo.launch
+++ b/launch/rosbuzz-solo.launch
@@ -30,7 +30,7 @@
 	<!-- 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/script/testflockfev.bzz" />
+		<param name="bzzfile_name" value="/home/pi/ros_catkinKin_ws/src/ROSBuzz/script/testsolo.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 2a6a018..8a45656 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -36,11 +36,12 @@ namespace rosbzz_node{
 			robot_id= strtol(robot_name.c_str() + 5, NULL, 10);
 		setpoint_counter = 0;
 		fcu_timeout = TIMEOUT;
-        
-
-		//ros::Duration(0.1).sleep();
-
 
+		while(cur_pos[2] == 0.0f){
+			ROS_INFO("Waiting for GPS. ");
+			ros::Duration(0.5).sleep();
+			ros::spinOnce();
+		}
 	}
 
 	/*---------------------
@@ -58,7 +59,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)){
@@ -67,8 +68,8 @@ namespace rosbzz_node{
 		}
 
 		robot_id=robot_id_srv_response.value.integer;
-		*/
-		robot_id = 8;
+
+		//robot_id = 8;
 	}
 
 	/*-------------------------------------------------