Skip to content

Інтерфейс навігації PX4 ROS 2

PX4 v1.15 Experimental

WARNING

Експериментальні налаштування На момент написання цієї статті, деякі частини бібліотеки інтерфейсу PX4 ROS 2 є експериментальними і, отже, можуть бути змінені.

[PX4 ROS 2 Interface бібліотека](../ros2/px4_ros2_interface_lib. d) інтерфейс навігації дозволяє розробникам надсилати дані щодо позиції PX4 безпосередньо з ROS 2 додатків, така як система VIO або система відповідності мап. Цей інтерфейс надає шар абстракції від PX4 та каркасу обміну повідомленнями uORB і вводить деякі перевірки на розумність стану оцінки оновлень, надісланих через інтерфейс. Ці вимірювання потім об'єднуються в EKF так само, як внутрішні вимірювання PX4.

Бібліотека надає два класи: LocalPositionMeasurementInterface та GlobalPositionMeasurementInterface, які обидва використовують схожий метод оновлення для надання оновлення локальної позиції або глобальної позиції до PX4 відповідно. Метод update очікує від позиції вимірювання struct (LocalPositionMeasurement або GlobalPositionMeasurement), які розробники можуть народитися власними вимірюваннями.

Установка та перший тест

Для початку роботи необхідно виконати наступні кроки:

  1. Make sure you have a working ROS 2 setup, with px4_msgs in the ROS 2 workspace.

  2. Клонуйте репозиторій в робочий простір:

    sh
    cd $ros_workspace/src
    git clone --recursive https://github.com/Auterion/px4-ros2-interface-lib

    INFO

    Для забезпечення сумісності, використовуйте останні main гілки для PX4, px4_msgs та бібліотеки. Дивіться також тут.

  3. Побудуйте робочий простір:

    sh
    cd ..
    colcon build
  4. У іншій оболонці запустіть PX4 SITL:

    sh
    cd $px4-autopilot
    make px4_sitl gazebo-classic

    (тут ми використовуємо Gazebo-Classic, але ви можете використовувати будь-яку модель або симулятор)

  5. У іншій оболонці запустіть агента micro XRCE (ви можете залишити його запущеним після цього):

    sh
  6. Поверніться до терміналу ROS 2, створіть робочу область, яку ви щойно створили (у кроці 3), і запустіть приклад global_navigation, який періодично надсилає фіктивні оновлення глобальної позиції:

    sh
    source install/setup.bash
    ros2 run example_global_navigation_cpp example_global_navigation

    Ви повинні отримати вивід, подібний до цього, що показує, що глобальний інтерфейс успішно надсилає оновлення позиції:

    sh
    [INFO] [1702030701.836897756] [example_global_navigation_node]: example_global_navigation_node running!
    [DEBUG] [1702030702.837279784] [example_global_navigation_node]: Successfully sent position update to navigation interface.
    [DEBUG] [1702030703.837223884] [example_global_navigation_node]: Successfully sent position update to navigation interface.
  7. У PX4 оболонці можна перевірити, що PX4 отримує глобальні оновлення позиції:

    sh
    listener aux_global_position

    Вихід має містити:

    sh
    TOPIC: aux_global_position
    aux_global_position
       timestamp: 46916000 (0.528000 seconds ago)
       timestamp_sample: 46916000 (0 us before timestamp)
       lat: 12.343210
       lon: 23.454320
       alt: 12.40000
       alt_ellipsoid: 0.00000
       delta_alt: 0.00000
       eph: 0.31623
       epv: 0.44721
       terrain_alt: 0.00000
       lat_lon_reset_counter: 0
       alt_reset_counter: 0
       terrain_alt_valid: False
       dead_reckoning: False
  8. Тепер ви готові використовувати навігаційний інтерфейс для надсилання своїх оновлень.

Як користуватися бібліотекою

Для надсилання вимірювання позиції ви заповнюєте структуру позиції з виміряними значеннями. Потім викликаєте функцію оновлення інтерфейсу з цією структурою як аргументом.

Для базового прикладу, як користуватися цим інтерфейсом, ознайомтеся з examples в Auterion/px4-rosface-lib репозиторію, наприклад examples/cpp/navigation/local_navigation або examples/cpps/cppation/globation](https://github.com/Auter/intertere-face-face-face-facb/mainb/mppation/mppation/example/example/navigation/navigation/navigation/navig/navigation/navigation/navig/navig/navig/navighblob/navig

Оновлення локальної позиції

Спочатку переконайтеся, що параметр PX4 EKF2_EV_CTRL налаштований належним чином для ефективного використання зовнішніх локальних вимірів, встановивши відповідні біти в true:

  • 0: Дані горизонтальної позиції
  • 1: Дані вертикальної позиції
  • 2: Дані швидкості
  • 3: Дані кута yaw
  1. Створіть LocalPositionMeasurementInterface, надаючи йому речі: ID ROS вузол, а також посилання на швидкість вашого вимірювання.
  2. Заповніть LocalPositionMeasurement struct своїми вимірюваннями.
  3. Передайте struct на LocalPositionMeasurementInterface update() метод.

Доступні рамки посилання на положення та швидкість для ваших вимірювань визначаються наступним переліком:

cpp
enum class PoseFrame
{
  Unknown,
  LocalNED,
  LocalFRD
};

enum class VelocityFrame
{
  Unknown,
  LocalNED,
  LocalFRD,
  BodyFRD
};

Структура LocalPositionMeasment визначено таким чином:

cpp
struct LocalPositionMeasurement
{
   rclcpp::Time timestamp_sample {};

   std::optional<Eigen::Vector2f> position_xy {std::nullopt};
   std::optional<Eigen::Vector2f> position_xy_variance {std::nullopt};
   std::optional<float> position_z {std::nullopt};
   std::optional<float> position_z_variance {std::nullopt};

   std::optional<Eigen::Vector2f> velocity_xy {std::nullopt};
   std::optional<Eigen::Vector2f> velocity_xy_variance {std::nullopt};
   std::optional<float> velocity_z {std::nullopt};
   std::optional<float> velocity_z_variance {std::nullopt};

   std::optional<Eigen::Quaternionf> attitude_quaternion {std::nullopt};
   std::optional<Eigen::Vector3f> attitude_variance {std::nullopt};
};

Метод update() глобального інтерфейсу очікує дотримання наступних умов для GlobalPositionMeasment:

  • Час відбору вибіркових вимірювань визначений.
  • Значення не мають NAN``.
  • Якщо надано значення вимірювання, його відповідне значення розбіжності добре визначено (наприклад, якщо lat_lon визначено, то необхідно вказати horizontal_variance).
  • Якщо надано значення вимірювання, його пов'язана рамка посилання не є невідомою (наприклад, якщо визначено position_xy, то інтерфейс було ініціалізовано з подовжувачем кадру, відмінного від PoseFrame:Unknown).

Наступний фрагмент коду є прикладом вузла ROS 2, який використовує локальний інтерфейс навігації для надсилання оновлень 3D-позиції у рамці посилання Північ-Схід-Дон (NED) до PX4:

cpp
class MyLocalMeasurementUpdateNode : public rclcpp::Node
{
public:
   MyLocalMeasurementUpdateNode()
   : Node("my_node_name")
   {
      // Set pose measurement reference frame to north-east-down
      const px4_ros2::PoseFrame pose_frame = px4_ros2::PoseFrame::LocalNED;
      // We will only send pose measurements in this example
      // Set velocity measurement reference frame to unknown
      const px4_ros2::VelocityFrame velocity_frame = px4_ros2::VelocityFrame::Unknown;
      // Initialize local interface [1]
      _local_position_measurement_interface =
         std::make_shared<px4_ros2::LocalPositionMeasurementInterface>(*this, pose_frame, velocity_frame);
   }

   void sendUpdate()
   {
      while (running) { // Potentially make method run as a callback or on a timer
         // Generate local position measurement
         rclcpp::Time timestamp_sample  = ...
         Eigen::Vector2f position_xy = ...
         Eigen::Vector2f position_xy_variance = ...
         float position_z = ...
         float position_z_variance = ...

         // Populate the local position measurement struct [2]
         px4_ros2::LocalPositionMeasurement local_position_measurement{};
         local_position_measurement.timestamp_sample = timestamp_sample;
         local_position_measurement.position_xy = position_xy;
         local_position_measurement.position_xy_variance = position_xy_variance;
         local_position_measurement.position_z = position_z;
         local_position_measurement.position_z_variance = position_z_variance;

         // Send measurement to PX4 using the interface [3]
         try {
            _local_position_measurement_interface->update(local_position_measurement);
         } catch (const px4_ros2::NavigationInterfaceInvalidArgument & e) {
            // Handle exceptions caused by invalid local_position_measurement definition
            RCLCPP_ERROR(get_logger(), "Exception caught: %s", e.what());
         }
      }
   }

private:
   std::shared_ptr<px4_ros2::LocalPositionMeasurementInterface> _local_position_measurement_interface;
};

Спочатку переконайтеся, що параметр PX4 EKF2_EV_CTRL налаштований належним чином для ефективного використання зовнішніх локальних вимірів, встановивши відповідні біти в true:

  • 0: Дані вертикальної позиції
  • 1: Дані вертикальної позиції

Щоб надіслати глобальне вимірювання на PX4:

  1. Створіть LocalPositionMeasurementInterface, надаючи йому речі: ID ROS вузол, а також посилання на швидкість вашого вимірювання.
  2. Заповніть LocalPositionMeasurement struct своїми вимірюваннями.
  3. Передайте struct на LocalPositionMeasurementInterface update() метод.

Структуру LocalPositionMeasment визначено таким чином:

cpp
struct GlobalPositionMeasurement
{
   rclcpp::Time timestamp_sample {};

   std::optional<Eigen::Vector2d> lat_lon {std::nullopt};
   std::optional<float> horizontal_variance {std::nullopt};

   std::optional<float> altitude_msl {std::nullopt};
   std::optional<float> vertical_variance {std::nullopt};
};

Метод update() глобального інтерфейсу очікує дотримання наступних умов для GlobalPositionMeasment:

  • Значення не мають NAN.
  • Якщо надано значення вимірювання, його відповідне значення розбіжності добре визначено (наприклад, якщо lat_lon визначено, то необхідно вказати horizontal_variance).

Наступний фрагмент коду є прикладом вузла ROS 2, який використовує локальний інтерфейс навігації для надсилання оновлень 3D-позиції у рамці посилання Північ-Схід-Дон (NED) до PX4:

cpp
class MyGlobalMeasurementUpdateNode : public rclcpp::Node
{
public:
   MyGlobalMeasurementUpdateNode()
   : Node("my_node_name")
   {
      // Initialize global interface [1]
      _global_position_measurement_interface =
         std::make_shared<px4_ros2::GlobalPositionMeasurementInterface>(*this);
   }

   void sendUpdate()
   {
      while (running) { // Potentially make method run as a callback or on a timer
         // Generate global position measurement
         rclcpp::Time timestamp_sample  = ...
         Eigen::Vector2d lat_lon = ...
         float horizontal_variance = ...
         float altitude_msl = ...
         float vertical_variance = ...

         // Populate the global position measurement struct [2]
         px4_ros2::GlobalPositionMeasurement global_position_measurement{};
         global_position_measurement.timestamp_sample = timestamp_sample;
         global_position_measurement.lat_lon = lat_lon;
         global_position_measurement.horizontal_variance = horizontal_variance;
         global_position_measurement.altitude_msl = altitude_msl;
         global_position_measurement.vertical_variance = vertical_variance;

         // Send measurement to PX4 using the interface [3]
         try {
            _global_position_measurement_interface->update(global_position_measurement);
         } catch (const px4_ros2::NavigationInterfaceInvalidArgument & e) {
            // Handle exceptions caused by invalid global_position_measurement definition
            RCLCPP_ERROR(get_logger(), "Exception caught: %s", e.what());
         }
      }
   }

private:
   std::shared_ptr<px4_ros2::GlobalPositionMeasurementInterface> _global_position_measurement_interface;
};

Кілька екземплярів інтерфейсу

Використання кількох екземплярів одного інтерфейсу (напр. локально та локально) для надсилання повідомлень про розрахунок буде передавати всі оновлення до однієї теми і що призведе до перехресної розмови. Це не повинно впливати на об'єднання вимірювань в EKF, але різні джерела вимірювань стануть нерозрізненними.