ROS 2 Offboard Control Приклад

Наступний приклад на C++ показує, як виконати керування положенням у режимі offboard mode з вузла ROS 2.

Приклад починає відправляти установочні точки, входить у режим offboard, озброюється, піднімається на 5 метрів і чекає. Незважаючи на простоту, він демонструє основні принципи використання offboard control і способи надсилання команд транспортному засобу.

Він був протестований на Ubuntu 20.04 з ROS 2 Foxy та PX4 main після PX4 v1.13.

WARNING

Offboard керування небезпечне. Якщо ви керуєте реальним транспортним засобом, то обов'язково майте можливість отримати назад ручне керування на випадок, якщо щось піде не так.

ROS та PX4 роблять кілька різних припущень, зокрема щодо конвенцій кадрів. Не існує неявного перетворення між типами кадрів при публікації або підписці на теми!

У цьому прикладі публікуються позиції у фреймі NED, як і очікує PX4. Щоб підписатися на дані, що надходять з вузлів, які публікують у іншій системі координат (наприклад, ENU, яка є стандартною системою координат у ROS/ROS 2), скористайтеся допоміжними функціями у бібліотеці frame_transforms.

Випробування

Follow the instructions in ROS 2 User Guide to install PX and run the simulator, install ROS 2, and start the XRCE-DDS Agent.

After that we can follow a similar set of steps to those in ROS 2 User Guide > Build ROS 2 Workspace to run the example.

Створити та запустити приклад:

1. Відкрийте новий термінал.

2. Створіть новий каталог робочого простору colcon і перейдіть до нього за допомогою:

   mkdir -p ~/ws_offboard_control/src/
   cd ~/ws_offboard_control/src/

3. Клонуйте репозиторій px4_msgs до каталогу /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 до каталогу /src:

   git clone https://github.com/PX4/px4_ros_com.git

5. Створіть середовище розробки ROS 2 у поточному терміналі і скомпілюйте робочу область за допомогою colcon:

   cd ..
   source /opt/ros/humble/setup.bash
   colcon build

:::

::::

1. Джерело local_setup.bash:

   source install/local_setup.bash

2. Запустіть приклад.

   ros2 run px4_ros_com offboard_control

Транспортний засіб повинен озброїтися, піднятися на 5 метрів і потім зачекати (вічно).

Імплементація

Вихідний код прикладу керування позашляховою платформою можна знайти за посиланням PX4/px4_ros_com у каталозі /src/examples/offboard/offboard_control.cpp.

PX4 публікує всі повідомлення, використані в цьому прикладі, як ROS теми за замовчуванням (див. dds_topics.yaml).

PX4 вимагає, щоб транспортний засіб вже отримував повідомлення OffboardControlMode перед тим, як він збере в режимі автономного керування, або перед тим, як він перейде в режим автономного керування під час польоту. Крім того, PX4 вийде з offboard mode, якщо частота потоку повідомлень OffboardControlMode впаде нижче приблизно 2 Гц. Необхідна поведінка реалізується за допомогою головного циклу, що виконується у вузлі 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(), щоб відправити OffboardControlMode та TrajectorySetpoint повідомлення до PX4. Повідомлення OffboardControlMode транслюються таким чином, що PX4 дозволить озброєння, як тільки перейде в режим offboard, тоді як повідомлення TrajectorySetpoint ігноруються (доки транспортний засіб перебуває в режимі offboard).

Після 10 циклів викликається publish_vehicle_command() для зміни на режим безпілотного керування, а також викликається arm() для озброєння транспортного засобу. Після того, як транспортний засіб озброюється та змінює режим, він починає відстежувати встановлені точки. Задані точки все ще відправляються в кожному циклі, щоб транспортний засіб не виходив з режиму поза платформою.

Реалізації методів publish_offboard_control_mode() та publish_trajectory_setpoint() показані нижче. Ці публікують повідомлення OffboardControlMode та TrajectorySetpoint до PX4 (відповідно).

Параметр OffboardControlMode потрібен для того, щоб повідомити PX4 про тип використовуваного режиму керування offboard. Тут ми використовуємо лише position control, тому поле 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.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, z і yaw жорстко закодовані на певні значення, але вони можуть оновлюватися динамічно за алгоритмом або навіть за допомогою зворотного виклику підписки на повідомлення, що надходять з іншого вузла.

/**
 * @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, ви також можете підтвердити, що задавання певної команди було успішним. Поля param і command відображають команди MAVLink та їхні значення параметрів.