Hello,
I trying to get global position from simulated drone, so I can use that for taking off, but when I’m trying to do that with attached code, it gives me zeros…
#include <ros/ros.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/CommandTOL.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <sensor_msgs/NavSatFix.h>
using namespace mavros_msgs;
using namespace sensor_msgs;
using namespace ros;
NavSatFix current_position;
void position_cb(const NavSatFix::ConstPtr& msg){
current_position = *msg;
}
static void testStuff(NodeHandle nh, State current_state){
Rate rate(25.0);
Subscriber position_sub = nh.subscribe<NavSatFix>
("mavros/global_position/global",10,position_cb);
ServiceClient arming_client = nh.serviceClient<CommandBool>
("mavros/cmd/arming");
ServiceClient set_mode_client = nh.serviceClient<SetMode>
("mavros/set_mode");
ServiceClient takeoff_client = nh.serviceClient<CommandTOL>
("mavros/cmd/takeoff");
NavSatFix home;
for(int i = 100; ok() && i>0;--i){
home = current_position; //it should get current position from GPS, but it not happens
spinOnce();
rate.sleep();
}
SetMode set_takeoff_mode;
set_takeoff_mode.request.custom_mode = "AUTO.TAKEOFF";
CommandBool arm_cmd;
arm_cmd.request.value = true;
CommandTOL takeoff_cmd;
takeoff_cmd.request.altitude = home.altitude + 4.5;
takeoff_cmd.request.latitude = home.latitude;
takeoff_cmd.request.longitude = home.longitude;
takeoff_cmd.request.min_pitch = 0;
takeoff_cmd.request.yaw = 0;
Time last_request = Time::now();
while (ok())
{
if(current_state.mode != "AUTO.TAKEOFF"&& // to fix later
Time::now()-last_request>Duration(5.0)){
if(set_mode_client.call(set_takeoff_mode)&&
set_takeoff_mode.response.mode_sent){
ROS_INFO("Takeoff mode enabled");
}
last_request = Time::now();
} else {
if(!current_state.armed&&
Time::now()-last_request > Duration(5.0)){
if(arming_client.call(arm_cmd)&&
arm_cmd.response.success){
ROS_INFO("Vehicle armed");
}
last_request = Time::now();
} else {
if(Time::now()-last_request>Duration(1.0)){
if(takeoff_client.call(takeoff_cmd)&&
takeoff_cmd.response.success){
ROS_INFO_STREAM("Taking off to alt. "<<takeoff_cmd.request.altitude);
}
last_request = Time::now();
}
}
}
spinOnce();
rate.sleep();
}
}
I checked if the topic is published with rostopic and I can get info from this tool;
I’m fresh in MAVROS, so I’m sorry for this bad code and sending best regards 
EDIT: I figured it out by getting code of takeoff command into main while loop (still, I need to figure out how to properly send it though; currently I end with “takeoff denied, please disarm and retry error”):
while (ok())
{
if(current_state.mode != "AUTO.TAKEOFF"&& // to fix later
Time::now()-last_request>Duration(5.0)){
if(set_mode_client.call(set_takeoff_mode)&&
set_takeoff_mode.response.mode_sent){
ROS_INFO("Takeoff mode enabled");
}
last_request = Time::now();
} else {
if(!current_state.armed&&
Time::now()-last_request > Duration(5.0)){
if(arming_client.call(arm_cmd)&&
arm_cmd.response.success){
ROS_INFO("Vehicle armed");
}
last_request = Time::now();
}
}
takeoff_cmd.request.altitude = current_position.altitude + 4.5;
takeoff_cmd.request.latitude = current_position.latitude;
takeoff_cmd.request.longitude = current_position.longitude;
takeoff_cmd.request.min_pitch = 0;
takeoff_cmd.request.yaw = 0;
if(Time::now()-last_request>Duration(1.0)){
if(takeoff_client.call(takeoff_cmd)&&
takeoff_cmd.response.success){
ROS_INFO_STREAM("Taking off to alt. "<<takeoff_cmd.request.altitude);
}
last_request = Time::now();
}
spinOnce();
rate.sleep();
}