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, "hover");
  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 set_mode_pub = nh.advertise<mavros_msgs::SetMode>("/mavros/set_mode", 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::SetMode set_mode_cmd;
  set_mode_cmd.request.custom_mode = "ALT_HOLD";
  if(!set_mode_pub.call(set_mode_cmd) || !set_mode_cmd.response.success)
  {
    ROS_ERROR("Failed to set the vehicle mode to ALT_HOLD");
    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] = 1500; // 升降舵通道，1500表示无动作
  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消息类型发布了一个悬停的控制指令。这个指令将无人机的所有通道都设定为1500，表示无动作。程序等待5秒钟后，发送一个解锁指令将无人机关闭。