Mavros not able to arm
Hi,
I'm using mavros combined with a pixhawk, running PX4 firmware. For starting I'm using this example. The default example runs perfect in gazebo, but when I try to publish other messages, the drone wont arm. For example I'm trying to change the throttle to 0.9 and only publish this message. The drone just stays at the ground and I'm not getting an arming message. If I put the default (from the example) message alongside the throttle message, the drone will arm fine but he wont raise which I try to produce with the throttle to 0.9 and even change automatically the mode back to AUTO.RTL.
This is the information on the mavros wiki about changing the throttle:
~setpoint_attitude/att_throttle (std_msgs/Float64)
Send throttle value(0~1).
So what I did:
ros::Publisher mav_thr_pub = nh.advertise<std_msgs::Float64>( "mavros/setpoint_attitude/att_throttle", 10);
std_msgs::Float64 cmd_thr;
cmd_thr.data = 0.5;
mav_thr_pub.publish(cmd_thr);
My full code:
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <std_msgs/Float64.h>
using namespace std;
mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
current_state = *msg;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "offb_node");
ros::NodeHandle nh;
ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
("mavros/state", 10, state_cb);
ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
("mavros/setpoint_position/local", 10);
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
("mavros/cmd/arming");
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
("mavros/set_mode");
ros::Publisher mav_thr_pub = nh.advertise<std_msgs::Float64>
( "mavros/setpoint_attitude/att_throttle", 10);
//the setpoint publishing rate MUST be faster than 2Hz
ros::Rate rate(20.0);
// wait for FCU connection
while(ros::ok() && current_state.connected){
ros::spinOnce();
rate.sleep();
}
geometry_msgs::PoseStamped pose;
pose.pose.position.x = 0;
pose.pose.position.y = 0;
pose.pose.position.z = 2;
//send a few setpoints before starting
for(int i = 100; ros::ok() && i > 0; --i){
local_pos_pub.publish(pose);
ros::spinOnce();
rate.sleep();
}
mavros_msgs::SetMode offb_set_mode;
offb_set_mode.request.custom_mode = "OFFBOARD";
mavros_msgs::CommandBool arm_cmd;
arm_cmd.request.value = true;
ros::Time last_request = ros::Time::now();
std_msgs::Float64 cmd_thr;
cout << cmd_thr << endl;
cmd_thr.data = 0.9;
int maxCount = 200;
int count = 0;
while(ros::ok()){
if( current_state.mode != "OFFBOARD" &&
(ros::Time::now() - last_request > ros::Duration(5.0))){
if( set_mode_client.call(offb_set_mode) &&
offb_set_mode.response.mode_sent){
ROS_INFO("Offboard enabled");
}
last_request = ros::Time::now();
} else {
if( !current_state.armed &&
(ros::Time::now() - last_request > ros::Duration(5.0))){
if( arming_client.call(arm_cmd) &&
arm_cmd.response.success){
ROS_INFO("Vehicle armed");
}
last_request = ros::Time::now();
}
}
if(count < maxCount){
local_pos_pub.publish(pose);
cout << count << endl;
count++;
}
mav_thr_pub.publish(cmd_thr);
ros::spinOnce();
rate.sleep();
}
return 0;
}