C++程序：通过mavros向PX4飞控发布指令让四旋翼无人机向上飞行

以下是一个简单的通过mavros向PX4飞控发布指令让四旋翼无人机向上飞行的C++程序：

#include <ros/ros.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/CommandTOL.h>
#include <mavros_msgs/CommandLong.h>
#include <mavros_msgs/SetMode.h>

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

    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>("/mavros/cmd/arming");
    ros::ServiceClient takeoff_client = nh.serviceClient<mavros_msgs::CommandTOL>("/mavros/cmd/takeoff");
    ros::ServiceClient cmd_client = nh.serviceClient<mavros_msgs::CommandLong>("/mavros/cmd/command");
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("/mavros/set_mode");

    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;

    mavros_msgs::CommandTOL takeoff_cmd;
    takeoff_cmd.request.altitude = 2; // 设置起飞高度为2米

    mavros_msgs::CommandLong cmd;
    cmd.request.command = 400; // 设置向上飞行的指令
    cmd.request.param7 = 1; // 设置持续时间为1秒

    mavros_msgs::SetMode set_mode;
    set_mode.request.custom_mode = "GUIDED"; // 设置飞行模式为GUIDED

    ros::Rate rate(20.0);

    while (ros::ok())
    {
        // 等待连接到飞控
        while (ros::ok() && !arming_client.exists())
        {
            ROS_INFO("Waiting for arming service...");
            ros::Duration(1.0).sleep();
        }
        while (ros::ok() && !takeoff_client.exists())
        {
            ROS_INFO("Waiting for takeoff service...");
            ros::Duration(1.0).sleep();
        }
        while (ros::ok() && !cmd_client.exists())
        {
            ROS_INFO("Waiting for command service...");
            ros::Duration(1.0).sleep();
        }
        while (ros::ok() && !set_mode_client.exists())
        {
            ROS_INFO("Waiting for set_mode service...");
            ros::Duration(1.0).sleep();
        }

        // 解锁飞控
        if (!arming_client.call(arm_cmd) || !arm_cmd.response.success)
        {
            ROS_ERROR("Failed to arm the vehicle");
            return -1;
        }

        // 设置飞行模式为GUIDED
        if (!set_mode_client.call(set_mode) || !set_mode.response.success)
        {
            ROS_ERROR("Failed to set mode to GUIDED");
            return -1;
        }

        // 起飞
        if (!takeoff_client.call(takeoff_cmd) || !takeoff_cmd.response.success)
        {
            ROS_ERROR("Failed to takeoff");
            return -1;
        }

        // 发布向上飞行指令
        if (!cmd_client.call(cmd) || !cmd.response.success)
        {
            ROS_ERROR("Failed to send command");
            return -1;
        }

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

    return 0;
}

注意：此程序仅供参考，具体实现需要根据实际情况进行修改。同时，飞行时需要遵守相关规定和安全操作。