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

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

#include <ros/ros.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/CommandTOL.h>
#include <mavros_msgs/OverrideRCIn.h>

int main(int argc, char **argv)
{
  // 初始化ROS节点
  ros::init(argc, argv, "takeoff");
  ros::NodeHandle nh;

  // 发布mavros命令
  ros::Publisher rc_pub = nh.advertise<mavros_msgs::OverrideRCIn>("/mavros/rc/override", 10);
  ros::Publisher arming_pub = nh.advertise<mavros_msgs::CommandBool>("/mavros/cmd/arming", 10);
  ros::Publisher takeoff_pub = nh.advertise<mavros_msgs::CommandTOL>("/mavros/cmd/takeoff", 10);

  // 等待连接到飞控
  while(ros::ok() && !ros::service::exists("/mavros/set_mode", true))
  {
    ROS_INFO("Waiting for service /mavros/set_mode to become available");
    ros::Duration(1).sleep();
  }

  // 解锁飞控
  mavros_msgs::CommandBool arming_cmd;
  arming_cmd.request.value = true;
  if(!arming_pub.call(arming_cmd) || !arming_cmd.response.success)
  {
    ROS_ERROR("Failed to arm the vehicle");
    return -1;
  }

  // 起飞
  mavros_msgs::CommandTOL takeoff_cmd;
  takeoff_cmd.request.altitude = 2.0; // 指定起飞高度为2米
  if(!takeoff_pub.call(takeoff_cmd) || !takeoff_cmd.response.success)
  {
    ROS_ERROR("Failed to takeoff");
    return -1;
  }

  // 向上飞行
  mavros_msgs::OverrideRCIn rc_cmd;
  rc_cmd.channels[0] = 1500; // 油门通道，1500表示无动作
  rc_cmd.channels[1] = 1500; // 副翼通道，1500表示无动作
  rc_cmd.channels[2] = 1500; // 方向舵通道，1500表示无动作
  rc_cmd.channels[3] = 2000; // 升降舵通道，2000表示向上飞行
  rc_cmd.channels[4] = 1500; // 油门自动控制通道，1500表示关闭自动控制
  rc_cmd.channels[5] = 1500; // 油门自动控制通道，1500表示关闭自动控制
  rc_cmd.channels[6] = 1500; // 油门自动控制通道，1500表示关闭自动控制
  rc_cmd.channels[7] = 1500; // 油门自动控制通道，1500表示关闭自动控制
  rc_pub.publish(rc_cmd);

  // 等待5秒钟
  ros::Duration(5).sleep();

  // 关闭无人机
  arming_cmd.request.value = false;
  if(!arming_pub.call(arming_cmd) || !arming_cmd.response.success)
  {
    ROS_ERROR("Failed to disarm the vehicle");
    return -1;
  }

  return 0;
}

这个程序首先通过mavros库初始化ROS节点，并创建了三个发布器，用于发布解锁飞控、起飞和控制指令。接下来，程序进入一个while循环，等待连接到飞控。一旦连接成功，程序发送解锁指令并等待响应。如果解锁成功，则发送起飞指令并等待响应。最后，程序使用mavros库的OverrideRCIn消息类型发布了一个向上飞行的控制指令。这个指令将无人机的升降舵通道设定为2000，表示向上飞行。程序等待5秒钟后，发送一个解锁指令将无人机关闭。