使用mavros的c++代码，通过话题/mavros/setpoint_velocity/cmd_vel控制，使它以0.3m/s的速度起飞到1m高度，然后悬停5s，最后再降落

以下是一个使用mavros控制飞行器起飞，悬停和降落的示例代码：

#include <ros/ros.h>
#include <geometry_msgs/TwistStamped.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/CommandBool.h>

mavros_msgs::State current_state;
ros::Publisher velocity_pub;
ros::Subscriber state_sub;
ros::ServiceClient arming_client;
ros::ServiceClient set_mode_client;

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;

    velocity_pub = nh.advertise<geometry_msgs::TwistStamped>("/mavros/setpoint_velocity/cmd_vel", 10);
    state_sub = nh.subscribe<mavros_msgs::State>("mavros/state", 10, state_cb);
    arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
    set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");

    // Wait for FCU connection
    while(ros::ok() && !current_state.connected){
        ros::spinOnce();
        ros::Duration(0.01).sleep();
    }

    // Set the takeoff altitude
    double takeoff_alt = 1.0;

    // Send velocity commands to takeoff
    geometry_msgs::TwistStamped vel_cmd;
    vel_cmd.header.stamp = ros::Time::now();
    vel_cmd.twist.linear.z = 0.3;

    // Keep sending velocity commands until takeoff altitude is reached
    while(ros::ok() && (current_state.mode != "OFFBOARD" || !current_state.armed)){
        velocity_pub.publish(vel_cmd);
        ros::spinOnce();
        ros::Duration(0.01).sleep();

        if (current_state.mode != "OFFBOARD") {
            mavros_msgs::SetMode offb_set_mode;
            offb_set_mode.request.custom_mode = "OFFBOARD";
            set_mode_client.call(offb_set_mode);
        } else {
            if (!current_state.armed) {
                mavros_msgs::CommandBool arm_cmd;
                arm_cmd.request.value = true;
                arming_client.call(arm_cmd);
            }
        }
    }

    // Stabilize the drone for 5 seconds
    ros::Duration(5.0).sleep();

    // Send velocity commands to land
    vel_cmd.header.stamp = ros::Time::now();
    vel_cmd.twist.linear.z = -0.3;

    // Keep sending velocity commands until the drone lands
    while(ros::ok() && current_state.armed){
        velocity_pub.publish(vel_cmd);
        ros::spinOnce();
        ros::Duration(0.01).sleep();
    }

    return 0;
}

这个示例代码中，我们将飞行器的起飞高度设置为1米。在起飞前，我们将飞行器的速度设置为0.3m/s，直到飞行器进入OFFBOARD模式并且武器系统启动。在飞行器起飞后，我们将飞行器保持悬停状态5秒，然后将飞行器的速度设置为-0.3m/s，直到飞行器降落。