/mavros/setpoint_attitude/attitude seems not work

[liang-tang]

hello every, i wrote a demo of mavros to test SET_ATTITUDE_TARGET, when i send /mavros/setpoint_attitude/cmd_vel & /mavros/setpoint_attitude/att_throttle,the vehicle behaves normally. but when i send /mavros/setpoint_attitude/attitude & /mavros/setpoint_attitude/att_throttle, the vehicle just Rise and fall(depends on throttle), can somebody tell me why ? thanks

int main(int argc, char **argv)
{
    ros::init(argc, argv, "attitude_control_demo");
    ros::NodeHandle nh("~");

    double thro, roll_rate, pitch_rate, yaw_rate;
    nh.getParam("throttle", thro);
    nh.getParam("roll_rate", roll_rate);
    nh.getParam("pitch_rate", pitch_rate);
    nh.getParam("yaw_rate", yaw_rate);
    ROS_INFO(" throttle %.2f roll_rate %.2f pitch_rate %.2f yaw_rate %.2f",
                thro, roll_rate, pitch_rate, yaw_rate);

    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
            ("/mavros/state", 10, state_cb);
    ros::Publisher throttle_pub = nh.advertise<std_msgs::Float64>
            ("/mavros/setpoint_attitude/att_throttle", 10);
    ros::Publisher attitude_pub = nh.advertise<geometry_msgs::PoseStamped>
            ("/mavros/setpoint_attitude/attitude", 10);
    ros::Publisher attitude_rate_pub = nh.advertise<geometry_msgs::TwistStamped>
            ("/mavros/setpoint_attitude/cmd_vel", 10);

    //the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(5.0);

    // wait for FCU connection
    while(ros::ok() && current_state.connected){
        ros::spinOnce();
        rate.sleep();
    }

    ros::Time last_change = ros::Time::now();

    geometry_msgs::TwistStamped attitude_rate;
    attitude_rate.twist.angular.x = roll_rate;
    attitude_rate.twist.angular.y = pitch_rate;;
    attitude_rate.twist.angular.z = yaw_rate;

    while(ros::ok()){
        std_msgs::Float64 throttle;
        throttle.data = thro; // 0~1
        throttle_pub.publish(throttle);

        geometry_msgs::PoseStamped attitude;
        attitude.pose.position.x = roll_rate;
        attitude.pose.position.y = pitch_rate;
        attitude.pose.position.z = yaw_rate;
        attitude_pub.publish(attitude);

        if (ros::Time::now() - last_change > ros::Duration(3.0)) {
                last_change = ros::Time::now();
        }

        //attitude_rate_pub.publish(attitude_rate);

        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}

[vooon]

What firmware used? Versions of ROS, MAVROS, Firmware?

[liang-tang]

jade, ros-jade-mavros, Firmware(commit: 9fedda43c6a501575773d83d715c36c46ac05f6a)

[Jaeyoung-Lim]

I would like to know what you are trying to do with the code It seems like mavros is functioning as it is supposed to but the code is misleading from your intentions.

a. It seems like you are trying to send the rate values in to the attitude.pose.position values. If you are using topic "/mavros/setpoint_attitude/attitude", then you should be sending the attitude values(on the quaternion), not the position values as shown in the code. such as

attitude.pose.orientation.x = q1;

b. You are publishing attitude topics and the attitude rate topic at the same time.

ros::Publisher attitude_pub = nh.advertise<geometry_msgs::PoseStamped>
            ("/mavros/setpoint_attitude/attitude", 10);
    ros::Publisher attitude_rate_pub = nh.advertise<geometry_msgs::TwistStamped>
            ("/mavros/setpoint_attitude/cmd_vel", 10);

I am not sure what happens if you publish both at the same time, but try publishing just one of the topics which you want to use.

c. If you are controlling attitude rate, 10Hz might be a little slow to have successful flight performance.

I hope this will be useful in solving your issues,

[liang-tang]

@Jaeyoung-Lim thank you very much:) ,you're absolutely right. it just was for testing,because i want to control vehicle by mavros. i don't know whether it works well or badly.