# ROS 2 Offboard 控制示例

下面的 C++ 示例显示如何在ROS 2 节点中进行 offboard模式 下的位置控制。

示例将首先发送设置点、进入offboard模式、解锁、起飞至5米,并悬停等待。 虽然简单,但它显示了如何使用offboard控制以及如何向无人机发送指令。

它已在 Ubuntu 20.04 ROS 2 Foxy环境及不低于 PX4 v1.13 的 main 分支进行了测试。

注意

Offboard 控制模式是危险的。 如果你是在一个真正的无人机平台上进行试验,请保证你已经设置了切换回手动的开关来防止紧急情况的发生。

ROS

和 PX4 有一些不同的定义,特别是在 坐标系定义 中。 当主题发布或订阅时,坐标系类型之间没有隐含转换!

这个例子按照PX4的定义在NED坐标系下发布位置。 订阅来自在不同坐标系发布的节点数据(例如ENU, ROS/ROS 2中的标准坐标系),使用 frame_transforms (opens new window) 库中的辅助函数来进行转换。

# 尝试一下

按照 ROS 2 用户向导 中的说明来安装PX 并运行模拟器,安装ROS 2, 并启动XRCE-DDS代理。

然后,我们可以采用 ROS 2 用户向导 > 构建 ROS 2 工作空间 中相似的步骤来运行这个例子。

构建并运行示例:

  1. 打开一个新的终端。

  2. 使用以下方法创建并切换至新的 colcon工作目录:

    mkdir -p ~/ws_offboard_control/src/
    cd ~/ws_offboard_control/src/
    
  3. 克隆 px4_msgs (opens new window) 仓库到 /src 目录(每一个 ROS 2 PX4 工作区都需要这个仓库!):

    git clone https://github.com/PX4/px4_msgs.git
    # checkout the matching release branch if not using PX4 main.
    
  4. 克隆示例仓库 px4_ros_com (opens new window)/src 目录:

    git clone https://github.com/PX4/px4_ros_com.git
    
  5. Source the ROS 2 development environment into the current terminal and compile the workspace using colcon:

    :::

    ::::

    1. 执行工作空间环境配置脚本 local_setup.bash:

      source install/local_setup.bash
      
    2. 启动例程。

      ros2 run px4_ros_com offboard_control
      

    飞行器将解锁、起飞至5米并悬停等待(永久)。

    # 实现

    Offboard控制示例的源代码见 PX4/px4_ros_com (opens new window) 目录的 /src/examples/offboard/offboard_control.cpp (opens new window)

    PX4 默认情况下将此示例中使用的所有消息作为ROS主题发布(见 [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/release/1.14/src/modules/uxrce_dds_client/dds_topics.yaml))。

    PX4 在offboard模式下解锁或者在飞行过程中切换至offboard模式都需要飞行器已经收到 OffboardControlMode 消息。 此外,如果 OffboardControlMode 消息的频率低于2Hz,PX4 将会切换出offboard模式。 该行为在ROS 2 节点的主循环中实现的,如下所示:

    auto timer_callback = [this]() -> void {
    
        if (offboard_setpoint_counter_ == 10) {
            // Change to Offboard mode after 10 setpoints
            this->publish_vehicle_command(VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6);
    
            // Arm the vehicle
            this->arm();
        }
    
        // OffboardControlMode needs to be paired with TrajectorySetpoint
        publish_offboard_control_mode();
        publish_trajectory_setpoint();
    
        // stop the counter after reaching 11
        if (offboard_setpoint_counter_ < 11) {
            offboard_setpoint_counter_++;
        }
    };
    timer_ = this->create_wall_timer(100ms, timer_callback);
    

    循环运行在一个100毫秒计时器。 在前10个周期中,它调用 publish_offboard_control_mode()publish_trajectory_setpoint() 发送 OffboardControlModeTracjectorySetpoint 消息到 PX4。 OffboardControlMode 消息已经被接到,所以PX4将允许被解锁一旦切换至offboard模式后, TrajectorySetpoint 将被忽略(直到飞行器处于offboard模式)。

    在 10 个周期后调用 publish_vehicle_command() 切换至offboard模式,并调用 arm() 来解锁飞行器。 在飞行器解锁并和切换模式后,它将开始跟踪位置设定值。 在每个周期内仍然发送设定值,确保飞行器不会切换出offboard模式。

    publish_offboard_control_mode()publish_trajectory_setpoint() 方法的实现如下。 发布 OffboardControlModeTrajectorySetpoint 消息到 PX4 (依次)。

    OffboardControlMode 是必须的以通知PX4 offboard模式所需使用的控制 类型 。 这里我们只使用 位置控制, 所以 position 字段设置为 true 所有其他字段设置为 false

    /**
     * @brief Publish the offboard control mode.
     *        For this example, only position and altitude controls are active.
     */
    void OffboardControl::publish_offboard_control_mode()
    {
        OffboardControlMode msg{};
        msg.position = true;
        msg.velocity = false;
        msg.acceleration = false;
        msg.attitude = false;
        msg.body_rate = false;
        msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
        offboard_control_mode_publisher_->publish(msg);
    }
    

    TrajectorySetpoint 提供了位置设置值。 在这种情况下, x, y, zyaw 字段被赋值为某些值,但它们可以根据算法动态更新,甚至也可以通过另一个订阅的回调函数来生成。

    /**
     * @brief Publish a trajectory setpoint
     *        For this example, it sends a trajectory setpoint to make the
     *        vehicle hover at 5 meters with a yaw angle of 180 degrees.
     */
    void OffboardControl::publish_trajectory_setpoint()
    {
        TrajectorySetpoint msg{};
        msg.position = {0.0, 0.0, -5.0};
        msg.yaw = -3.14; // [-PI:PI]
        msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
        trajectory_setpoint_publisher_->publish(msg);
    }
    

    publish_vehicle_command() 方法发送 VehicleCommand 带命令的消息到飞行控制器。 我们使用上面的方式来切换至offboard模式,也使用 arm() 来解锁飞行器。 此示例中我们没有 disarm() ,但它也用于执行该函数。

    /**
     * @brief Publish vehicle commands
     * @param command   Command code (matches VehicleCommand and MAVLink MAV_CMD codes)
     * @param param1    Command parameter 1
     * @param param2    Command parameter 2
     */
    void OffboardControl::publish_vehicle_command(uint16_t command, float param1, float param2)
    {
        VehicleCommand msg{};
        msg.param1 = param1;
        msg.param2 = param2;
        msg.command = command;
        msg.target_system = 1;
        msg.target_component = 1;
        msg.source_system = 1;
        msg.source_component = 1;
        msg.from_external = true;
        msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
        vehicle_command_publisher_->publish(msg);
    }
    

    注解

    VehicleCommand 是可用来指令PX4的最简单和最强大的方式之一。 可以通过订阅 VehicleCommandAck 消息来确认特定命令是否成功。 参数和命令字段与 MAVLink 命令 (opens new window) 及其参数值一致。