Mavros not able to arm

asked 2017-12-02 10:34:56 -0500

MaximeRector gravatar image

updated 2017-12-02 10:35:16 -0500

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;
}
edit retag flag offensive close merge delete