马弗罗斯 机外 控制示例（C++）

本教程介绍了 机外 使用 MAVROS 控制在 Gazebo Classic/SITL 中模拟的 Iris 四旋翼飞行器。教程结束时，您将看到与下面视频中相同的行为，即缓慢起飞至 2 米高度。

警告

机外 控制是很危险的。如果您要在真实载具上进行操作，请务必想办法重新获得手动控制权，以防出现意外。

TIP

本例使用 C++。Python 中也有一个非常类似的例子。 ROS/MAVROS 船外示例（Python） (也可参见 integrationtests/python_src/px4_it/mavros).

代码

创建 offb_node.cpp 文件（将其添加到您的 CMakeList.txt 以便编译），并在其中粘贴以下内容：

/**
 * @ 文件 offb_node.cpp
 * @brief 使用 MAVROS 0.19.x 版、PX4 Pro Flight 编写的机外控制示例节点
 * 在 Gazebo Classic SITL 中堆叠和测试
 */

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

mavros_msgs当前状态；
空白 状态(缢 mavros_msgs::国家::ConstPtr及样品； 信息){
    current_state = *msg；
}

int 主要(int 参数, 烧焦 **参数)
{
    玫瑰::启动（argc、argv、 "offb_node"；);
    玫瑰::NodeHandle nh；

    玫瑰订阅者状态 = 订阅<；mavros_msgs国家>；
            ("mavros/state"；, 10，state_cb）；
    玫瑰::Publisher local_pos_pub = nh.advertise<；几何参数::PoseStamped>；
            ("mavros/setpoint_position/local"；, 10);
    玫瑰::ServiceClient arming_client = nh.serviceClient<；mavros_msgs::CommandBool>；
            ("mavros/cmd/arming"；);
    玫瑰::ServiceClient set_mode_client = nh.serviceClient<；mavros_msgs::SetMode>；
            ("mavros/set_mode"；);

    //设定点发布速率必须快于 2Hz
    玫瑰率 费率(20.0);

    // 等待 FCU 连接
    虽然(玫瑰::好的() &&； !current_state.connected){
        玫瑰::自旋一次();
        率。睡眠();
    }

    几何参数::PoseStamped pose；
    姿势.pose.position.x = 0;
    pose.pose.position.y = 0;
    pose.pose.position.z = 2;

    //启动前发送几个设定点
    对于(int i = 100; 玫瑰::好的() &&； i >； 0; --i){
        local_pos_pub.发布(摆姿势）；
        玫瑰::自旋一次();
        率。睡眠();
    }

    mavros_msgs::SetMode offb_set_mode；
    offb_set_mode.request.custom_mode = "OFFBOARD"；;

    mavros_msgs::CommandBool arm_cmd；
    arm_cmd.request.value = 真;

    玫瑰::Time last_request = 玫瑰::时间::现在();

    虽然(玫瑰::好的()){
        如果( current_state.mode != "OFFBOARD"； &&；
            (玫瑰::时间::现在() - 最后请求 >； 玫瑰::持续时间(5.0))){
            如果( set_mode_client.致电(offb_set_mode) &&；
                offb_set_mode.response.mode_sent){
                ROS_INFO("Offboard enabled"；);
            }
            最后请求 = 玫瑰::时间::现在();
        } 不然 {
            如果( !current_state.armed &&；
                (玫瑰::时间::现在() - 最后请求 >； 玫瑰::持续时间(5.0))){
                如果( arming_client。致电(arm_cmd) &&；
                    arm_cmd.response.success){
                    ROS_INFO(载具上膛"；);
                }
                最后请求 = 玫瑰::时间::现在();
            }
        }

        local_pos_pub.发布(摆姿势）；

        玫瑰::自旋一次();
        率。睡眠();
    }

    返回 0;
}

代码解释

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

mavros_msgs 包包含了操作 MAVROS 包提供的服务和主题所需的所有自定义消息。所有服务和主题及其相应的信息类型都记录在 马夫罗斯维基.

mavros_msgs当前状态；
空白 状态(缢 mavros_msgs::国家::ConstPtr及样品； 信息){
    current_state = *msg；
}

我们创建了一个简单的回调，用于保存自动驾驶仪的当前状态。这样我们就可以检查连接、上膛和 机外 国旗。

玫瑰订阅者状态 = 订阅<；mavros_msgs国家>；("mavros/state"；, 10，state_cb）；
玫瑰::Publisher local_pos_pub = nh.advertise<；几何参数::PoseStamped>；("mavros/setpoint_position/local"；, 10);
玫瑰::ServiceClient arming_client = nh.serviceClient<；mavros_msgs::CommandBool>；("mavros/cmd/arming"；);
玫瑰::ServiceClient set_mode_client = nh.serviceClient<；mavros_msgs::SetMode>；("mavros/set_mode"；);

我们将实例化一个发布器来发布指令本地位置以及相应的客户端来请求上膛和模式更改。请注意，对于您自己的系统，"mavros"前缀可能会有所不同，因为这取决于节点在启动文件中的名称。

//设定点发布速率必须快于 2Hz
玫瑰::费率 费率(20.0);

PX4 在两次运行之间的超时为 500 毫秒。 机外 命令。如果超过该超时时间，指挥官将退回到载具进入前的最后一个模式。 机外 模式。这就是为什么输出频率 必须 快于 2 Hz，以考虑到可能的延迟。这也是建议输入 机外 模式从 定位（POSITION） 模式，这样如果载具从 机外 模式下，它会停在原地盘旋。

// 等待 FCU 连接
虽然(玫瑰::好的() &&； !current_state.connected){
    玫瑰::自旋一次();
    率。睡眠();
}

在发布任何信息之前，我们需要等待 MAVROS 与自动驾驶仪之间建立连接。一旦收到心跳信息，这个循环就应立即退出。

几何参数::PoseStamped pose；
姿势.pose.position.x = 0;
pose.pose.position.y = 0;
pose.pose.position.z = 2;

尽管 PX4 Pro Flight Stack 在航空航天 NED 坐标框架内运行，但 MAVROS 会将这些坐标转换为标准 ENU 框架，反之亦然。这就是我们设置 z 为正 2。

//启动前发送几个设定点
对于(int i = 100; 玫瑰::好的() &&； i >； 0; --i){
  local_pos_pub.发布(摆姿势）；
  玫瑰::自旋一次();
  率。睡眠();
}

在进入 机外 模式时，您必须已经开始设置点流。否则，模式切换将被拒绝。这里、 100 是任意选择的。

mavros_msgs::SetMode offb_set_mode；
offb_set_mode.request.custom_mode = "OFFBOARD"；;

我们将自定义模式设置为 离岸.清单 支持的模式 可供参考。

mavros_msgs::CommandBool arm_cmd；
arm_cmd.request.value = 真;

玫瑰::Time last_request = 玫瑰::时间::现在();

虽然(玫瑰::好的()){
  如果( current_state.mode != "OFFBOARD"； &&；
    (玫瑰::时间::现在() - 最后请求 >； 玫瑰::持续时间(5.0))){
    如果( set_mode_client.致电(offb_set_mode) &&； offb_set_mode.response.mode_sent) {
      ROS_INFO("Offboard enabled"；);
    }
    最后请求 = 玫瑰::时间::现在();
  } 不然 {
    如果( !current_state.armed &&； (玫瑰::时间::现在() - 最后请求 >； 玫瑰::持续时间(5.0))){
      如果( arming_client。致电(arm_cmd) &&； arm_cmd.response.success) {
        ROS_INFO(载具上膛"；);
      }
      最后请求 = 玫瑰::时间::现在();
    }
  }

  local_pos_pub.发布(摆姿势）；

  玫瑰::自旋一次();
  率。睡眠();
}

代码的其余部分不言自明。我们尝试切换到 机外 模式，然后上膛四轴飞行器，允许其飞行。我们将服务调用间隔为 5 秒，以免自动驾驶仪被大量请求淹没。在同一个循环中，我们继续以适当的速率发送所请求的姿势。

TIP

为便于说明，本代码已简化到最低限度。在较大的系统中，通常需要创建一个新的线程，负责定期发布设定点。