GUIDED模式控制无人船画圆

在仿真上用的apm.launch文件，所以里面的模式都是GUIDED，如果用px4.launch,就把所有GUIDED改为OFFBOARD

仿真上试了一下，发现用mavros_msgs::PositionTarget消息控制无人船画圆，虽然运动路线是圆，但是QGC上轨迹并不是圆。查了一下，发现这个消息类型的偏航角控制有点问题。要控制偏航角还得用mavros_msgs::AttitudeTarget消息。

#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 <mavros_msgs/PositionTarget.h>
#include <mavros_msgs/AttitudeTarget.h>

mavros_msgs::State current_state;

uint8_t flag=0;

void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

int main(int argc, char **argv)
{   
    setlocale(LC_ALL,"");
    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 position_raw_pub = nh.advertise<mavros_msgs::PositionTarget>        //控制位置速度角速度加速度
            ("mavros/setpoint_raw/local", 10);   
    ros::Publisher setpoint_att_pub = nh.advertise<mavros_msgs::AttitudeTarget>
            ("mavros/setpoint_raw/attitude", 10);

    //发布频率必须大于2hz
    ros::Rate rate(20.0);          

    while(ros::ok() && !current_state.connected)
    {
        ros::spinOnce();
        rate.sleep();
    }

    mavros_msgs::PositionTarget setpoint_pos_raw;
    mavros_msgs::AttitudeTarget setpoint_att_raw;

    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "GUIDED";     
    mavros_msgs::CommandBool arm_cmd;                   
    arm_cmd.request.value = true;                      

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

    while(ros::ok()){
        if( current_state.mode != "GUIDED" &&
            (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("GUIDED enabled");
            }     
            else
            {
                ROS_INFO("GUIDED failed");
            }
                                                                       
            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");                                 
                }    
                else
                {
                    ROS_INFO("arm failed");
                }
                                                                   
                last_request = ros::Time::now();
            }
        }

        if(ros::Time::now() - last_request > ros::Duration(10.0))
        {
            flag++;
            last_request = ros::Time::now();
        }

        if(flag==0)
        {   
            setpoint_pos_raw.type_mask = /*1+ 2 + 4 ＋8+ 16 + 32 +*/ 64 + 128 + 256 + 512 + 1024 + 2048;
            setpoint_pos_raw.coordinate_frame = 1;
            setpoint_pos_raw.position.x = 10;
            setpoint_pos_raw.velocity.x = 1;
            position_raw_pub.publish(setpoint_pos_raw);
            ROS_INFO("本地坐标系，位置控制");  
        }
        else if(flag>=1)
        {   
            setpoint_att_raw.type_mask =128;
            setpoint_att_raw.body_rate.z= 0.5;
            setpoint_att_raw.thrust = 1.0;          
            setpoint_att_pub.publish(setpoint_att_raw);
            ROS_INFO("姿态俯仰角速度，速度控制");  
        }
        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}