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

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

警告

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

TIP

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

# 代码

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

/** * @file offb_node.cpp * @brief 船外控制示例节点，使用 MAVROS 0.19.x 版、PX4 Pro Flight * Stack 编写，并在 Gazebo Classic SITL 中进行了测试 */

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

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

int 主要(int 参数, 烧焦 **参数)
{
    玫瑰::启动(参数, 参数, "offb_node"；);
    玫瑰::节点句柄 nh;

    玫瑰::订阅者状态_sub = nh.订阅<；mavros_msgs::国家>；
            ("mavros/state"；, 10, 状态);
    玫瑰::发布者 local_pos_pub = nh.登广告<；几何参数::姿势盖章>；
            ("mavros/setpoint_position/local"；, 10);
    玫瑰::服务客户端 = nh.服务客户端<；mavros_msgs::命令布尔>；
            ("mavros/cmd/arming"；);
    玫瑰::服务客户端 set_mode_client = nh.服务客户端<；mavros_msgs::设置模式>；
            ("mavros/set_mode"；);

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

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

    几何参数::盖章姿势;
    姿势.姿势.位置.x = 0;
    姿势.姿势.位置.y = 0;
    姿势.姿势.位置.z = 2;

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

    mavros_msgs::设置模式 offb_set_mode;
    offb_set_mode.要求.自定义模式 = "OFFBOARD"；;

    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.要求.价值 = 真;

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

    虽然(玫瑰::好的()){
        如果( current_state.模式 != "OFFBOARD"； &&；
            (玫瑰::时间::现在() - 最后请求 >； 玫瑰::持续时间(5.0))){
            如果( 设置客户端模式.致电(offb_set_mode) &&；
                offb_set_mode.回应.发送模式){
                ROS_INFO("Offboard enabled"；);
            }
            最后请求 = 玫瑰::时间::现在();
        } 不然 {
            如果( !current_state.上膛 &&；
                (玫瑰::时间::现在() - 最后请求 >； 玫瑰::持续时间(5.0))){
                如果( 上膛客户端.致电(arm_cmd) &&；
                    arm_cmd.回应.成功){
                    ROS_INFO(载具上膛"；);
                }
                最后请求 = 玫瑰::时间::现在();
            }
        }

        本地发布.发布(姿势);

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

    返回 0;
}

# 代码解释

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

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

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

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

玫瑰::订阅者状态_sub = nh.订阅<；mavros_msgs::国家>；("mavros/state"；, 10, 状态);
玫瑰::发布者 local_pos_pub = nh.登广告<；几何参数::姿势盖章>；("mavros/setpoint_position/local"；, 10);
玫瑰::服务客户端 = nh.服务客户端<；mavros_msgs::命令布尔>；("mavros/cmd/arming"；);
玫瑰::服务客户端 set_mode_client = nh.服务客户端<；mavros_msgs::设置模式>；("mavros/set_mode"；);

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

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

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

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

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

几何参数::盖章姿势;
姿势.姿势.位置.x = 0;
姿势.姿势.位置.y = 0;
姿势.姿势.位置.z = 2;

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

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

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

mavros_msgs::设置模式 offb_set_mode;
offb_set_mode.要求.自定义模式 = "OFFBOARD"；;

我们将自定义模式设置为 离岸.清单支持的模式 (打开新窗口）可供参考。

mavros_msgs::CommandBool arm_cmd;
arm_cmd.要求.价值 = 真;

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

虽然(玫瑰::好的()){
  如果( current_state.模式 != "OFFBOARD"； &&；
    (玫瑰::时间::现在() - 最后请求 >； 玫瑰::持续时间(5.0))){
    如果( 设置客户端模式.致电(offb_set_mode) &&； offb_set_mode.回应.发送模式) {
      ROS_INFO("Offboard enabled"；);
    }
    最后请求 = 玫瑰::时间::现在();
  } 不然 {
    如果( !current_state.上膛 &&； (玫瑰::时间::现在() - 最后请求 >； 玫瑰::持续时间(5.0))) {
      如果( 上膛客户端.致电(arm_cmd) &&； arm_cmd.回应.成功) {
        ROS_INFO(载具上膛"；);
      }
      最后请求 = 玫瑰::时间::现在();
    }
  }

  本地发布.发布(姿势);

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

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

TIP

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

← ROS/MAVROS 安装指南 ROS/MAVROS 船外示例（Python） →

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

# 代码

# 代码解释

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