Skip to content

ROS 2 Offboard 控制示例

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

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

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

WARNING

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

ROS and PX4 make a number of different assumptions, in particular with respect to frame conventions. 当主题发布或订阅时,坐标系类型之间没有隐含转换!

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

尝试一下

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

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

构建并运行示例:

  1. 打开一个新的终端。

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

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

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

    sh
    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:

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

    ros2 run px4_ros_com offboard_control

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

实现

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

PX4 publishes all the messages used in this example as ROS topics by default (see dds_topics.yaml).

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

cpp
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

cpp
/**
 * @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.thrust_and_torque = false;
    msg.direct_actuator = false;
    msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
    offboard_control_mode_publisher_->publish(msg);
}

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

cpp
/**
 * @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() ,但它也用于执行该函数。

cpp
/**
 * @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 is one of the simplest and most powerful ways to command PX4, and by subscribing to VehicleCommandAck you can also confirm that setting a particular command was successful. 参数和命令字段与 MAVLink 命令 及其参数值一致。