Глава 5. ROS2 API
ROS2 (Robot Operating System 2 – Роботизированная Операционная Система 2) — это набор программных инструментов, библиотек и соглашений, предназначенных для упрощения создания сложных роботизированных систем. Это не операционная система в привычном смысле, как Windows или Linux, а фреймворк, который работает поверх обычной ОС.
Система, построенная на ROS2, состоит из множества небольших, независимых программ, называемых нодами или узлами, которые обмениваются данными между собой. Например: Одна нода получает данные с камеры, другая обрабатывает изображение и распознает объекты, третья получает и обрабатывает информацию с камеры. Все они работают параллельно и общаются.
ROS2 предоставляет несколько способов общения между нодами:
Топики (Topics): для непрерывной потоковой передачи данных (например, показания с датчиков).
Сервисы (Services): для запрос-ответного взаимодействия (например, "запрос на выполнение конкретного действия").
Действия (Actions): для длительных операций с возможностью отслеживания прогресса и отмены (например, «проехать до точки А»).
Также в ROS2 есть такое понятие, как пакеты (Packages). Это основная единица организации кода в ROS2. Каждый пакет содержит логически связанный набор файлов: код, конфигурации, данные, зависимости. Например, пакет для работы с камерой, для навигации, пакет с общими утилитами и так далее.
Существуют системы сборки пакетов – это инструменты, которые преобразуют исходный код в исполняемые программы и управляют зависимостями.
Начало работы с ROS2 API на TrackimgCam v3 не требует дополнительных настроек ‒ все пакеты устанавливаются в рабочее пространство одноплатного компьютера при обновлении камеры. При подключении питания пакеты запустятся.
5.1 Подключение к дрону и среда взаимодействия с ним
Для подключения, создания пакетов, запуска кода, программирования и работы с дроном требуется доступ к среде модуля TrackingCam v3. Доступ к устройству может быть организован через различные рабочие пространства, каждое из которых предоставляет свой интерфейс взаимодействия с операционной системой модуля. Рассмотрим три варианта подключения и взаимодействия с TrackingCam v3:
Управляющий интерфейс (GUI) камеры TrackingCam v3 (подраздел 5.1.1);
Терминальный доступ по SSH (подраздел 5.1.2);
SSH-клиент Termius (подраздел 5.1.3).
Данные способы можно смешивать. В терминале компьютера, например, запускать программы; в GUI камеры – производить настройки; через Termius перекидывать файлы с компьютера на камеру и так далее. Рекомендуется попробовать несколько способов.
5.1.1 Управляющий интерфейс камеры
Для подключения через GUI камеры выполняем следующее:
Подключаемся к Wi-Fi к сети, к которой подключена камера, либо к сети, которую она раздает;
Открываем браузер и заходим в управляющий интерфейс камеры http://trackingcam3.local:9999, либо переходим по IP-адресу, указанному во вкладке «Сеть» в интерфейсе wlan0 (пример: http://10.42.0.1:9999);

Определение IP-адреса при подключении по Wi-Fi
Алгоритм получения IP-адреса камеры указан в подразделе 1.3.3 данного руководства.
- Можно начинать работу и взаимодействие с TrackingCam v3 для запуска кода.
5.1.2 Терминальный доступ по SSH
Также все действия в Терминале управляющего интерфейса камеры можно выполнять со своего компьютера, подключившись к TrackingCam v3 по SSH.
SSH — это защищенный сетевой протокол для удаленного управления компьютерами и серверами, который шифрует весь трафик, включая пароли и команды.
Чтобы установить SSH-соединение, необходимы два компонента: SSH-сервер и SSH-клиент. Сервер прослушивает определенный порт и при успешной аутентификации дает доступ пользователю. Все команды, которые используются на SSH-клиенте, отправляются через защищенный канал связи на SSH-сервер, на котором они выполняются и откуда отправляют результат работы обратно клиенту.

Схема работы SSH
Для подключения необходимо знать IP-адрес Wi-Fi сети TrackingCam v3. Его можно узнать, следуя алгоритму, указанному в подразделе 1.3.3 данного руководства.
Алгоритм подключения к TrackingCam v3 по SSH:
Подключаемся к Wi-Fi сети, которую раздает или к которой подключена TrackingCam v3.
Открываем Терминал компьютера.
Для Linux:
Для подключения к одноплатному компьютеру открываем Терминал.

Терминал Linux
Для Windows:
Для чтения сообщений с одноплатного компьютера достаточно предустановленного ПО операционной системы PowerShell или Командной строки. В поиске приложений вводим «Терминал» и запускаем командную строку или PowerShell.

Запуск Терминала в меню поиска Windows
- Вводим команду:
sh
ssh root@trackingcam3.localЛибо:
sh
ssh root@<ip*адрес>При первом запуске система уточнит, действительно ли необходимо подключение, вводим «yes».
Также при первом подключении к хосту выходит предупреждение (Warning) о том, что подключение к нему происходит впервые, а также система выводит уникальный идентификатор сервера для проверки.
После этого система запросит пароль ‒ 12345. Обратим внимание, что во время ввода пароля, символы не будут видны.

Ввод пароля в Командной строке Терминале Linux

Ввод пароля в Командной строке Windows
Если подключение прошло успешно, префикс поменяется на trackingcam3:~#

Успешное подключение к TrackingCam v3 в Терминале Linux

Успешное подключение к TrackingCam v3 в Командной строке Windows
Если вышла ошибка при подключении через ssh, то необходимо удалить файл с известными хостами («known_hosts») с вашего ПК, который хранится в каталоге пользователя в папке «.ssh».
Для Linux:
Вводим команду:
sh
ssh-keygen -f "/home/<имя*пользователя>/.ssh/known_hosts" -R "<ip-адрес>"Например:
sh
ssh-keygen -f "/home/user/.ssh/known_hosts" -R "10.42.0.1"А затем снова выполняем подключение.

Удаление файла known_hosts перед подключением в случае ошибки в терминале Linux
Для Windows:
Переходим в корневую папку Windows, далее в каталог пользователя. Открываем папку «.ssh» и удаляем файл «known_hosts».


Удаление файла known_hosts перед подключением в случае ошибки
После чего снова выполняем подключение через Командную строку.
Для выхода из подключения по SSH нажимаем Ctrl+D, либо вводим exit.


Выход из подключения в Командной строке Windows
5.1.3 SSH-клиент Termius
Удобно пользоваться утилитами SSH, например, Termius. Данный клиент позволяет подключаться к устройству с компьютера или мобильного устройства без необходимости повторного ввода IP-адресов, портов и паролей.
Переходим по ссылке: https://termius.com/
- Для подключения необходимо иметь аккаунт. Проходим регистрацию, нажав кнопку «Sign Up».

Главная страница сайта termius.com
- Скачиваем приложение под свою операционную систему во вкладке «Account» («Аккаунт»).

Скачивание приложения
- Создаем новый хост (Hosts -> NEW HOST).

Создание хоста
- Вводим следующие параметры хоста и нажимаем кнопку «Connect». В графу «Address» вводим IP-адрес камеры при беспроводном подключении (чтобы узнать IP-адрес, см. подраздел 1.3.3).

Параметры хоста
- Нажимаем «Add and continue».

Сохранение настроек
Настройки подключения сохранены, теперь для подключения достаточно нажать на область IP-адреса.

Кнопка подключения к хосту
- В открывшемся терминале отображается рабочее пространство TrackingCam v3.

Терминал утилиты Termius
Для отключения нажимаем Ctrl+D, либо вводим exit. Произойдет возврат во вкладку «Hosts».

Выход из подключения к хосту
5.2 Создание пакета для хранения ROS2 узлов
Для создания рабочего пространства с ROS2 необходимо подключиться к модулю TrackingCam v3 одним из способов, описанных в подразделе 5.1. Все команды, перечисленные ниже, одинаковы для любого из рабочих пространств. После подключения к модулю следуем алгоритму:
- Создаем папку example_ws (можно свое название), а в ней папку srs, выполнив в терминале команду:
mkdir -p example_ws/src
- Переходим в папку srs, выполнив команду
sh
cd example_ws/src
Создание директории и переход в нее
В ROS 2 пакеты собираются с помощью системы сборки ament. Она поддерживает разные типы пакетов, в зависимости от того, на каком языке они написаны и как должны устанавливаться. ament_cmake
Это надстройка над CMake, позволяющая удобно собирать ROS2 пакеты на C++ (и не только).
Используется, когда код пакета написан на C++ или нужен полноценный CMake-процесс.
В пакете с ament_cmake обычно есть файл:
CMakeLists.txt — основной сценарий сборки.
package.xml — метаданные пакета.
Применяется для драйверов, нод на C++, библиотек, систем реального времени.
Пример создания такого пакета:
sh
ros2 pkg create my_cpp_pkg --build-type ament_cmake --dependencies rclcppament_ python
Это система для Python-пакетов ROS2.
Основана на setuptools (стандартный способ установки Python-библиотек).
Используется, когда пакет содержит Python-ноды или утилиты.
В пакете с ament_python обычно есть:
setup.py — файл установки Python-пакета.
package.xml — описание пакета.
Пример создания пакета:
sh
ros2 pkg create my_py_pkg --build-type ament_python --dependencies rclpy- Создаем python файл и называем его cases_pkg. Для этого введем команду:
sh
ros2 pkg create cases_pkg --build-type ament_python --dependencies rclpy std_msgs
Создание Python файла
Флаг dependencies означает, какие зависимости понадобятся для сборки проекта, соответственно, при подаче аргументов rclpy (клиентская библиотека ROS2 для написания узлов на python) и std_msgs (стандартные сообщения) в файл package.xml, находящийся в директории пакета, добавляются следующие строки:
<depend>rclpy</depend>
<depend>std_msgs</depend>
5.3 Добавление примеров в пакет
Скачиваем примеры по ссылке:
После выполнения действий, описанных в подразделе 5.2, в папке example_ws/src появятся папки cases_pkg и example_ws:

Создание Python файла
Процесс добавления примеров в пакет отличается для каждого из рабочих пространств для взаимодействия с TracingCam v3, описанных в подразделе 5.1. Ниже рассмотрены действия для каждого из них.
GUI модуля TracingCam v3:
- Необходимо добавить установленные примеры в папку cases_pkg. Открываем вкладку «Navigator», и переходим по пути: /home/root/example_ws/src/cases_pkg/cases_pkg/



Переход к директории cases_pkg
Переносим python скрипт, либо загружаем его с помощью кнопки «upload», которая находится в верхнем правом углу.

Кнопка «upload»
После переноса скриптов директория выглядит следующим образом:

Директория после загрузки примеров
В папке cases_pkg будут храниться все проекты.
- Переходим во вкладку «Terminal». Делаем файлы исполняемыми, с помощью команд:
В общем случае:
sh
chmod +x путь*до*директории*с*файлами/имя*файла.pyНапример:
sh
chmod +x /home/root/example*ws/src/cases_pkg/cases_pkg/имя*файла.py
Ввод команд
Если вы находитесь в папке с примерами, команды могут выглядеть следующим образом:
sh
chmod +x имя*файла.py
Пример команд
- Возвращаемся во вкладку «Navigator» и переходим в директорию /home/root/example_ws/src/cases_pkg:
Нажимаем на стрелочку вверх в левом углу экрана.

Кнопка «Up»
Или в строке поиска вводим:
/home/root/example_ws/src/cases_pkg

Содержимое папки cases_pkg
- Открываем файл setup.py, два раза кликнув на него.

Открытый файл «setup.py» в управляющем интерфейсе модуля камеры
- В секцию entry_points добавляем узлы по типу:
entry_points={ 'console_scripts': [ 'my_node = cases_pkg.my_node:main', ],},
Здесь:
my_node — имя, под которым будет запущена нода через ros2 run.
casespkg.mynode:main — путь к Python-модулю (cases_pkg/my_node.py) и имя функции (main()), которая запускается.
В нашем случае:
entry_points={ 'console_scripts': [ " ArUco_boarding = cases_pkg.ArUco_boarding:main", " ArUco_follower = cases_pkg.ArUco_follower:main", " Blob_follower = cases_pkg.Blob_follower:main", " Takeoff_and_landing = cases_pkg.Takeoff_and_landing:main", " Takeoff_and_rotate = cases_pkg.Takeoff_and_rotate:main", " Wall_Follower = cases_pkg.Wall_Follower:main", ], },
После изменений файл выглядит следующим образом:

Файл «setup.py» после внесенных изменений
Сохраняем файл, нажав на значок в нижнем правом углу.
- Переходим во кладку «Terminal» и возвращаемся в директорию example_ws, выполнив команду:
sh
cd /home/root/example_ws
Переход в директорию example_ws
- Собираем пакет, с помощью инструмента colcon, введя команду:
sh
colcon build
Сборка пакета
При удачной сборке пакета отобразится сообщение:

Сообщение о удачной сборке пакета
Добавление примеров в пакет завершено.
Терминал компьютера:
Действия и выполнение команд для компьютеров на Windows и Linux не имеют существенных отличий. В большинстве случаев вводимые и выполняемые команды идентичны. Дальнейшая демонстрация выполнения команд приведена на примере компьютера под управлением Linux. Отдельная демонстрация будет приведена только при наличии значимых различий.
- Необходимо добавить установленные примеры в папку cases_pkg.
Переходим по пути /home/root/example_ws/src/cases_pkg/cases_pkg/ командой:
sh
cd /home/root/example_ws/src/cases_pkg/cases_pkg/![]()
Открываем папку с примерами на компьютере. Нажимаем правой кнопкой мыши и выбираем «Открыть в Терминале» («Open in Terminal»).

Запуск терминала в папке с примерами в Linux

Запуск терминала в папке с примерами в Windows
Откроется новое окно Терминала, вводим команду:
scp -r _ root@<ip-адрес или домен>:<полный путь до папки с примерами cases_pkg в системе модуля камеры>
Данная команда копирует все файлы и папки из текущей директории на компьютере на устройство по SSH.
Например:
scp -r _ root@10.42.0.1:/home/root/example_ws/src/cases_pkg/cases_pkg/
Затем система запросит пароль – 12345.

Добавление примеров в файловую систему модуля TrackingCam v3
После данной команды в окне терминала, где подключена камера можно ввести команду ls. Отобразится следующий вывод:

Содержание директории с примерами в файловой системе TrackingCam v3
В папке cases_pkg будут храниться все проекты.
- Делаем файлы исполняемыми, с помощью команд в терминале, где подключен модуль:
В общем случае:
sh
chmod +x путь*до*директории*с*файлами/имя*файла.pyНапример:
sh
chmod +x /home/root/example*ws/src/cases_pkg/cases_pkg/имя*файла.py
Ввод команд
Если вы находитесь в папке с примерами, команды могут выглядеть следующим образом:
sh
chmod +x имя*файла.py
Пример команд
- Переходим в директорию /home/root/example_ws/src/cases_pkg и заходим в файл setup.py. Вводим следующие команды:
sh
cd /home/root/example_ws/src/cases_pkglsnano setup.py
Выполнение команд в терминале
Первой командой переходим к необходимой директории, команда ls выведет ее содержимое, а nano открывает редактирование файла.
После ввода последней команды откроется содержимое файла «setup.py». Для управления курсором необходимо пользоваться стрелочками клавиатуры компьютера.

Открытый файл «setup.py» в терминале
- В секцию entry_points добавляем узлы по типу:
entry_points={ 'console_scripts': [ 'my_node = cases_pkg.my_node:main', ],},
Здесь:
my_node — имя, под которым будет запущена нода через ros2 run.
casespkg.mynode:main — путь к Python-модулю (cases_pkg/my_node.py) и имя функции (main()), которая запускается.
В нашем случае:
entry_points={ 'console_scripts': [ " ArUco_boarding = cases_pkg.ArUco_boarding:main", " ArUco_follower = cases_pkg.ArUco_follower:main", " Blob_follower = cases_pkg.Blob_follower:main", " Takeoff_and_landing = cases_pkg.Takeoff_and_landing:main", " Takeoff_and_rotate = cases_pkg.Takeoff_and_rotate:main", " Wall_Follower = cases_pkg.Wall_Follower:main", ], },
При помощи стрелочек клавиатуры перемещаемся к секции entry_points и добавляем недостающие блоки. Если вы скопировали их, вставляем блоки сочетанием клавиш Ctrl+Shift+V. После изменений файл выглядит следующим образом:

Файл «setup.py» после внесенных изменений в терминале компьютера
Для сохранения нажимаем сочетание клавиш Ctrl+O, выйдет интерфейс редактирования названия файла. Название файла не нужно редактировать, поэтому нажимаем клавишу Enter. После сохранения нажимаем Ctrl+X, изменения сохранятся.


Сохранение изменений в «setup.py» в терминале компьютера сочетанием клавиш Ctrl+O, затем Enter
Если внесены нежелательные изменения, которые не были сохранены, нажимаем Ctrl+X, затем N. Внесенные изменения не сохранятся.

Выход без сохранений
- В терминале возвращаемся в директорию example_ws, выполнив команду:
sh
cd /home/root/example_ws
Переход в директорию example_ws
- Собираем пакет, с помощью инструмента colcon:
sh
colcon build
Сборка пакета
При удачной сборке пакета отобразится сообщение:

Сообщение о удачной сборке пакета
Добавление примеров в пакет завершено.
Termius:
- Необходимо добавить установленные примеры в папку cases_pkg. Нажимаем SFTP. Откроется окно файловой системы компьютера и подключения к хосту. Нажимаем «Select host».

иллюстрации Переход во встроенный инструмент безопасного обмена файлами в Termius
Затем два раза кликаем на область хоста. Откроется файловая система TrackingCam v3.


Выбор необходимого хоста и окно файловой системы модуля
В файловой системе компьютера переходим в папку, где находятся примеры. В файловой системе модуля переходим к папке cases_pkg (example_ws→src→cases_pkg→cases_pkg).

Переход в файловые системы в интерфейсе Termius
Переносим примеры из файловой системы компьютера в папку cases_pkg файловой системы модуля.

Добавление примеров в файловую систему модуля TrackingCam v3 через Termius
В папке cases_pkg будут храниться все проекты.
- Возвращаемся в терминал и делаем файлы исполняемыми, с помощью команд:
В общем случае:
sh
chmod +x путь*до*директории*с*файлами/имя*файла.pyНапример:
sh
chmod +x /home/root/example*ws/src/cases_pkg/cases_pkg/имя*файла.py
Ввод команд
Если вы находитесь в папке с примерами, команды могут выглядеть следующим образом:
sh
chmod +x имя*файла.py
Пример команд
- Переходим в директорию /home/root/example_ws/src/cases_pkg и заходим в файл setup.py.
Снова нажимаем «SFTP» в верхней части экрана. В правом окне, где отображена файловая система модуля TrackingCam v3, нажимаем на стрелочку влево в верхнем левом углу, либо на папку cases_pkg.

Способы перехода в директорию /home/root/example_ws/src/cases_pkg в Termius
В открывшейся папке два раза кликаем на файл «setup.py».

Файл «setup.py»
Откроется редактор кода. В Windows и Linux он может открыться в разных редакторах.


Открытый файл «setup.py» в разных редакторах кода
- В секцию entry_points добавляем узлы по типу:
entry_points={ 'console_scripts': [ 'my_node = cases_pkg.my_node:main', ],},
Здесь:
my_node — имя, под которым будет запущена нода через ros2 run.
casespkg.mynode:main — путь к Python-модулю (cases_pkg/my_node.py) и имя функции (main()), которая запускается.
В нашем случае:
entry_points={ 'console_scripts': [ " ArUco_boarding = cases_pkg.ArUco_boarding:main", " ArUco_follower = cases_pkg.ArUco_follower:main", " Blob_follower = cases_pkg.Blob_follower:main", " Takeoff_and_landing = cases_pkg.Takeoff_and_landing:main", " Takeoff_and_rotate = cases_pkg.Takeoff_and_rotate:main", " Wall_Follower = cases_pkg.Wall_Follower:main", ], },
Сохраняем изменения в открывшемся редакторе кодов. Часто это можно сделать сочетанием клавиш Ctrl+S в редакторе. После изменений файл выглядит следующим образом:

Файл «setup.py» после внесенных изменений
После сохранения в интерфейсе Termius внизу появится строка, где необходимо нажать «Upload».

Сохранение изменений в Termius
- В терминале возвращаемся в директорию example_ws, выполнив команду:
sh
cd /home/root/example_ws
Переход в директорию example_ws
- Собираем пакет, с помощью инструмента colcon:
sh
colcon build
Сборка пакета
При удачной сборке пакета отобразится сообщение:

Сообщение о удачной сборке пакета
Добавление примеров в пакет завершено.
5.4 Используемые топики и сервисы
5.4.1 Просмотр топиков и сервисов
Для просмотра всех содержащихся топиков вводим команду:
sh
ros2 topic listВывод:
| /altitude /aruco_image/compressed /arucos /attitude /barometer /battery /beeper_control /blob_mask/compressed /blobs /camera_poses_from_markers /cmd_vel /device_control /diod_control /flags /imu /light /mag /magnet /odom /opticflow /parameter_events /rc_in /rosout /scan /target_height /tf /tf_static /ultrasonic |
|---|
Для просмотра всех доступных сервисов, вводим команду:
sh
ros2 service listВывод:
| /camera_driver/describe_parameters /camera_driver/get_parameter_types /camera_driver/get_parameters /camera_driver/list_parameters /camera_driver/set_parameters /camera_driver/set_parameters_atomically /device_control/describe_parameters /device_control/get_parameter_types /device_control/get_parameters /device_control/list_parameters /device_control/set_parameters /device_control/set_parameters_atomically /go_to_xy /height_control/describe_parameters /height_control/get_parameter_types /height_control/get_parameters /height_control/list_parameters /height_control/set_parameters /height_control/set_parameters_atomically /image_api/describe_parameters /image_api/get_parameter_types /image_api/get_parameters /image_api/list_parameters /image_api/set_parameters /image_api/set_parameters_atomically /msp_node_ros2/describe_parameters /msp_node_ros2/get_parameter_types /msp_node_ros2/get_parameters /msp_node_ros2/list_parameters /msp_node_ros2/set_parameters /msp_node_ros2/set_parameters_atomically /odom_calculation/describe_parameters /odom_calculation/get_parameter_types /odom_calculation/get_parameters /odom_calculation/list_parameters /odom_calculation/set_parameters /odom_calculation/set_parameters_atomically /odom_control/describe_parameters /odom_control/get_parameter_types /odom_control/get_parameters /odom_control/list_parameters /odom_control/set_parameters /odom_control/set_parameters_atomically /set_arm /set_height /set_odometry /set_yaw /take_off_board /websocket_api/describe_parameters /websocket_api/get_parameter_types /websocket_api/get_parameters /websocket_api/list_parameters /websocket_api/set_parameters /websocket_api/set_parameters_atomically /yaw_control/describe_parameters /yaw_control/get_parameter_types /yaw_control/get_parameters /yaw_control/list_parameters /yaw_control/set_parameters /yaw_control/set_parameters_atomically |
|---|
5.4.2 Описание топиков
Ниже приведено описание основных топиков и сервисов, через которые можно осуществлять взаимодействие с дроном.
Чтобы запустить чтение топика, вводим команду:
| ros2 topic echo /<имя топика>ros2 topic echo /<имя топика> Пример: ros2 topic echo /altituderos2 topic echo /altitude |
|---|
Чтобы запустить чтение топика через сообщение вводим команду:
| ros2 topic pub /<имя топика> <тип сообщения>ros2 topic pub /<имя топика> <тип сообщения> Пример: ros2 topic pub /imu sensor_msgs/msg/Imuros2 topic pub /imu sensor_msgs/msg/Imu |
|---|
Все типы сообщений и сервисов можно вызвать командой:
sh
ros2 interface listСообщения и сервисы установлены на TrackingCam v3 вместе с образом.
Топик imu:
Используемый тип сообщения: sensor_msgs/msg/Imu
Поля:
| geometry_msgs/Quaternion orientation - ориентация в формате кватерниона float64 x float64 y float64 z float64 w float64[9] orientation_covariance geometry_msgs/Vector3 angular_velocity - угловая скорость рад/c float64 x float64 y float64 z float64[9] angular_velocity_covariance geometry_msgs/Vector3 linear_acceleration - линейные ускорения float64 x float64 y float64 z float64[9] linear_acceleration_covariance |
|---|
Описание: данный топик содержит в себе показания инерциального датчика.
Топик opticalflow:
Используемый тип сообщения: eagle_eye_msgs/msg/OpticFlow
Поля:
| float32 flow_rate_x # показания датчика по оси X float32 flow_rate_y # показания датчика по оси Y float32 body_rate_x # приведенные показания IMU по оси X float32 body_rate_y # приведенные показания IMU по оси Y |
|---|
Описание: данный топик содержит в себе показания датчика оптического потока.
Топик battery:
Используемый тип сообщения: eagle_eye_msgs/msg/Battery
Поля:
| float32 voltage #текущий вольтаж float32 amperage #текущий ампераж float32 mah #сколько мАч потрачено int8 percent_of_voltage #оставшийся процент заряда батареи string count_of_cells #количество ячеек в батарее |
|---|
Описание: данный топик содержит в себе текущее состояние батареи.
Топик /altitude:
Используемый тип сообщения: eagle_eye_msgs/msg/Altitude
Поля:
| std_msgs/Header header float32 monotonic # усредненная высота float32 amsl # вертикальная скорость по барометру float32 local # не используется float32 relative # высота полученная с датчика opticalflow float32 terrain # высота по барометру float32 bottom_clearance # не используется |
|---|
Описание: данный топик содержит в себе текущие данные с различных датчиков высоты.
Топик /attitude:
Используемый тип сообщения: eagle_eye_msgs/msg/Attitude
Поля:
| std_msgs/Header header - #хэдер сообщения uint16 channels # не используется geometry_msgs/Quaternion orientation #фильтрованная ориентация в формате кватерниона float64 x float64 y float64 z float64 z geometry_msgs/Vector3 body_rate # Фильтрованная ориентация в формате углов Эйлера float64 x float64 y float64 z |
|---|
Описание: отфильтрованная ориентация БПЛА в различных форматах.
Топик rc_in:
Используемый тип сообщения: eagle_eye_msgs/msg/Channels
Поля:
| std_msgs/Header header uint8 rssi # не используется uint16[] channels # массив содержащий текущие показания каналов управления |
|---|
Описание: текущие показания каналов управления, получаемых с пульта.
Топик /flags:
Используемый тип сообщения: eagle_eye_msgs/msg/Flags
Поля:
| std_msgs/Header header int32 cycle_time #не используется int32 cpuload # не используется int32 arming_disable_count # число флагов запрещающих arm string[] arming_disable_flags # флаги запрещающие arm string[] active_sensors # активные датчики string[] mode # текущий режим управления |
|---|
Описание: топик содержащий текущие флаги полетного контроллера.
Топик cmd_vel:
Используемый тип сообщения: geometry_msgs/msg/Twist
Поля:
| Vector3 linear float64 x # целевая линейная скорость по оси x float64 y # целевая линейная скорость по оси y float64 z # не используется Vector3 angular float64 x # не используется float64 y # не используется float64 z # целевая угловая скорость вокруг оси Z |
|---|
Описание: через данный топик задается целевая скорость дрона при автономном полете с включенным 8-м каналом на пульте управления.
Топик odom:
Используемый тип сообщения: nav_msgs/msg/Odometry
Поля:
| std_msgs/Header header #хэдер сообщения string child_frame_id #tf фрейм сообщения geometry_msgs/PoseWithCovariance pose #текущая позиция geometry_msgs/TwistWithCovariance twist #текущая скорость |
|---|
Описание: данный топик позволяет получить текущие показания визуальной одометрии.
Топик /arucos:
Используемый тип сообщения: eagle_eye_msgs/msg/MarkerArray
Поля:
| std_msgs/Header header #хэдер eagle_eye_msgs/Marker[] markers # массив маркеров |
|---|
где eagle_eye_msgs/Marker:
std_msgs/Header header
uint32 id #id маркера
geometry_msgs/PoseWithCovariance pose #позиция маркера
float64 confidence
Описание: данный топик позволяет получить текущие детектируемые ArUco-маркеры.
Топик /camera_poses_from_markers:
Используемый тип сообщения: eagle_eye_msgs/msg/MarkerArray
Поля:
| std_msgs/Header header #хэдер eagle_eye_msgs/Marker[] markers # массив маркеров |
|---|
где eagle_eye_msgs/Marker:
std_msgs/Header header
uint32 id #id маркера
geometry_msgs/PoseWithCovariance pose #позиция маркера
float64 confidence
Описание: данный топик позволяет получить массив положений камеры относительно детектируемых маркеров.
5.4.3 Описание сервисов
Ниже приведено описание основных сервисов, через которые можно осуществлять взаимодействие с дроном.
Сервис set_yaw:
Используемый тип сервиса: eagle_eye_msgs/srv/SetYaw
Поля:
| float32 yaw #целевой угол поворота --- bool result #ответ сервиса: True/False |
|---|
Описание: данный сервис принимает на вход целевой угол поворота для дрона, на который он должен повернуться. Угол задается в радианах, угол в 0 радиан является стартовым при инициализации полетного контроллера. При задании угла > π (pi) или < - π, дрон повернется на угол, нормализованный до интервала [-π, π]. Если дрон не достиг целевого угла за 20 секунд, или вследствие какой-нибудь ошибки – сервис выдаст ответ False.
Сервис set_height:
Используемый тип сервиса: eagle_eye_msgs/srv/SetHeight
Поля:
| float32 height #целевая высота, отправляемая на полетный контроллер --- bool result #ответ сервиса: True/False string comment #комментарий к ответу сервиса |
|---|
Описание: данный сервис принимает на вход целевую высоту для дрона в метрах, отправляя ее в полетный контроллер. В случае, если задаваемая в сервисе высота выше текущей, целевая высота будет выставлена сразу, иначе будет плавное снижение высоты с фиксированным шагом. Максимальная высота взлета задается в настройках полетного контроллера, и она должна совпадать с параметрами сервиса в коде файла ноды.
Сервис takeoff_board:
Используемый тип сервиса: eagle_eye_msgs/srv/TakeOffBoard
Поля:
| bool takeoff #флаг взлета bool board #флаг посадки --- bool result #ответ сервиса: True/False |
|---|
Описание: данный сервис принимает на вход один из двух флагов для осуществления взлета/посадки. Для правильной работы сервиса необходимо задание одного из флагов равным True, другого False, иначе вызов сервиса будет пропущен и выдан отрицательный ответ. Взлет дрона осуществляется на высоту 1 метр.
Сервис go_to_xy:
Используемый тип сервиса: eagle_eye_msgs/srv/GoToXY
Поля:
| float32 x # координата по оси x float32 y # координата по оси y std_msgs/String frame # название системы координат, относительно которой осуществляется полет: odom/base_link --- bool result # ответ сервиса: True/False |
|---|
Описание: данный сервис принимает на вход название системы координат [frame]: odom (одометрия) или base_link (система координат дрона) и целевую точку в этой системе координат. После получения целевых координат дрон начинает движение в заданную точку в заданной системе координат. По достижении точки сервис возвращает результат True; False, если произошла ошибка в получении одометрии или дрон не достиг точки за заданное время (по умолчанию 20 секунд).
Сервис set_odometry:
Используемый тип сервиса: eagle_eye_msgs/srv/SetOdometry
Поля:
| float32 x # значение по x float32 y # значение по y --- std_msgs/Bool result #результат |
|---|
Описание: позволяет принудительно выставить текущие показания одометрии, может использоваться для обнуления.
5.5. Алгоритм автономного запуска БПЛА при использовании ROS2
Устанавливаем дрон в защитном кубе;
Аккуратно подключаем АКБ так, чтобы дрон не сместился, так как в момент включения происходит инициализация систем координат;
Подключаемся к Wi-Fi сети, которую раздает (или подключается) TrackingCam v3.
Подключаемся к модулю TrackingCam v3 одним из описанных вариантов в подразделе 5.1 (через управляющий интерфейс камеры, по SSH через терминал компьютера или Termius). Запуск программ для всех случаев идентичен.
В терминале вводим команды:
lscd example_ws (или название вашей папки)

Выполнение команд в Терминале
- Вводим команду:
. install/setup.bash. install/setup.bash

Ввод команды
Включаем пульт управления;
Переводим квадрокоптер в режим ARM. Моторы активируются;
Включаем режимы AltHold и PosHold;
Переключаем тумблер, закрепленный за 8 каналом в нижнее крайнее положение, это переключит квадрокоптер на управление с TrackingCam v3 и введет его в режим автономного полета, ручное управление с пульта будет недоступно;
Для запуска программы вводим команду:
sh
ros2 run cases_pkg Takeoff_and_landing (или другое название примера)
Пример запуска и выполнения программы
- После выполнения скрипта:
| Если квадрокоптер выполнил посадку в автономном режиме | – опускаем стик Throttle вниз; |
|---|---|
| – переводим тумблер 8 канала в верхнее крайнее положение, вернув дрон в ручное управление; | |
| – переходим к пункту 13 | |
| Если после завершения скрипта квадрокоптер находится в воздухе | – удерживаем на пульте стик Throttle посередине, чтобы при переходе в ручной режим квадрокоптер не упал. |
| – переводим тумблер 8 канала в верхнее крайнее положение, управление с пульта снова доступно. | |
| – выполняем посадку в ручном режиме, осторожно опуская стик Throttle. |
После завершения посадки переводим дрон в режим DISARM.
Выключаем режимы AltHold и PosHold, переведя тумблер, отвечающий за их включение в верхнее крайнее положение.
Если это требуется, в терминале нажимаем сочетание клавиш Ctrl+C для завершения работы программы.

Завершение работы программы
Отключаем АКБ.
Выключаем пульт.
5.6 Примеры и описание программ с использованием ROS2
Ниже приведен список доступных примеров, которые ранее были добавлены в пакет (см. подразделы 5.2 и 5.3). Системы координат, используемые в примерах такие же, как описаны в подразделах 3.4.1, 3.5.2, 3.6.2, а также в Приложении 1.
| Название примера | Описание |
|---|---|
| Простые примеры | |
| Takeoff_and_landing.py | Пример взлета и посадки |
| Takeoff_and_rotate.py | Пример взлета, поворота и посадки |
| Wall_Follower.py | Пример полета вдоль стены при помощи ультразвукового дальномера с ПИД-регулятором. |
| Примеры с ArUco-маркером | |
| Aruco_follower.py | Пример слежения за ArUco-маркером с помощью ПИД-регулятора. |
| Aruco_boarding.py | Пример автономной посадки на ArUco-маркер при помощи ПИД регулирования. |
| Примеры с цветовыми блобами | |
| Blob_follower.py | Пример слежения за цветным объектом с помощью ПИД-регулятора. |
5.6.1. Простые примеры программ
Взлет и посадка (Takeoff_and_landing.py)
Данный код реализует узел ROS2 для автономного взлета и посадки дрона. Для завершения работы программы в терминале нажимаем Ctrl+C после автономной посадки дрона.
python
import rclpy
from rclpy.node import Node
from eagle_eye_msgs.srv import TakeOffBoard # Сервис для взлета/посадки
import time # Для задержек
class SimpleTakeOffLand(Node):
def __init__(self):
super().__init__("simple_takeoff_land")
self.get_logger().info("Simple TakeOff and Land node started")
# Создаем клиент для сервиса взлета/посадки
self.takeoff_client = self.create_client(TakeOffBoard, "/take_off_board")
self.execute_sequence()
def execute_sequence(self):
# Ждем ответа сервиса
self.get_logger().info("Waiting for service...")
while not self.takeoff_client.wait_for_service(timeout_sec=1.0):
pass
# Взлет
self.get_logger().info("Taking off...")
request = TakeOffBoard.Request()
request.takeoff = True
request.board = False
future = self.takeoff_client.call_async(request)
# Ожидание завершения взлета
while not future.done():
rclpy.spin_once(self, timeout_sec=0.1)
time.sleep(10)
self.get_logger().info("Landing...")
request = TakeOffBoard.Request()
request.takeoff = False
request.board = True
future = self.takeoff_client.call_async(request)
# Ожидание завершения посадки
while not future.done():
rclpy.spin_once(self, timeout_sec=0.1)
def main(args=None):
rclpy.init(args=args)
node = SimpleTakeOffLand()
try:
rclpy.spin(node)
except:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()import rclpy – команда для импорта библиотеки rclpy (ROS Client library for Python). Node – базовый класс, от которого создаются все узлы ROS2 на Python. TakeoffBoard – описание формата сервиса (тип сообщения запроса/ответа). time – стандартный модуль Python для работы с задержками.
При инициализации узла создается экземпляр класса SimpleTakeOffLand, наследующегося от базового класса Node ROS2. Это означает, что объект этого класса является полноценным узлом ROS2 и получает все возможности базового класса: имя узла, систему логирования, подключение к ROS2 и тд.
super().init("simple_takeoff_land") Здесь super() обращается к родительскому классу Node и вызывает его конструктор. В качестве аргумента передается имя узла – «simple_takeoff_land». Без этого шага объект не был бы корректно зарегистрирован как ROS2-узел.
Метод self.get_logger().info("Simple TakeOff and Land node started") использует встроенный логгер ROS2. Это аналог print, но сообщения попадают в стандартную систему логов ROS2.
Далее создается клиент сервиса self.takeoff_client = self.create_client(TakeOffBoard, "/take_off_board")
тип сервиса: TakeoffBoard
имя сервиса: “/take_off_board”
Клиент позволяет отправлять запросы (например TakeoffBoard.Request) на этот сервис и получать ответы от другой ноды, которая реализует сам сервис.
После создания клиента вызывается основной метод self.execute_sequence(), то есть вся логика взлета и посадки запускается сразу при создании узла.
Основная логика программы сосредоточена в методе execute_sequence, который выполняет строго заданную последовательность операций.
Реализация ожидания доступности сервиса:
python
self.get_logger().info("Waiting for service...")
while not self.takeoff_client.wait_for_service(timeout_sec=1.0):
passУзел не может отправить запрос, пока сервис /take_off_board не будет запущен другим ROS2-узлом. Реализовано активное ожидание:
wait_for_service(timeout_sec=1.0) проверяет доступен ли сервис с задержкой в 1 секунду
пока сервис недоступен, цикл while продолжает проверку
Это гарантирует, что запросы будут отправлены только тогда, когда сервис готов их обработать.
Далее формируется запрос на взлет:
python
self.get_logger().info("Landing...")
request = TakeOffBoard.Request()
request.takeoff = True
request.board = False
future = self.takeoff_client.call_async(request)Здесь создается объект запроса к сервису:
request = TakeOffBoerd.Request() создает пустой запрос нужного типа;
request.takeoff = True включает команду взлета;
request.board = False посадка в этом запросе не запрашивается
Вызов future = self.takeoff_client.call_async(request) отправляет запрос асинхронно. Функция не ждет готового ответа сразу, а возвращает объект future, который будет содержать результат, когда сервис его обработает. Это позволяет узлу продолжать обрабатывать без блокировки и другие события.
Здесь реализован цикл ожидания завершения асинхронного вызова:
python
while not future.done():
rclpy.spin_once(self, timeout_sec=0.1)
time.sleep(10)future.done() возвращает True, когда ответ от сервиса получен. Пока ответа нет, функция spin_once позволяет ROS2 обрабатывать входящие сообщения, ответы сервисов и другие события для данного узла в течение указанного времени. После успешного взлета программа выдерживает фиксированную паузу продолжительностью 10 секунд, в течение которой дрон находится в полете на заданной высоте.
По истечении этого времени автоматически запускается процедура посадки через отправку второго запроса к тому же сервису с измененными параметрами: takeoff=False и board=True. Программа ожидает завершения процедуры посадки аналогичным способом – через циклическую проверку статуса асинхронного вызова.
После подтверждения успешной посадки вызывается rclpy.shutdown(), который инициирует корректное завершение работы всей инфраструктуры ROS2 для текущего процесса: закрываются соединения, останавливаются таймеры, корректно завершаются узлы.
Главная функция main обеспечивает базовую инициализацию ROS2, создание узла и запуск цикла обработки событий.
python
def main(args=None):
rclpy.init(args=args)
node = SimpleTakeOffLand()
try:
rclpy.spin(node)
except:
pass
finally:
node.destroy_node()
rclpy.shutdown()rclpy.init(args=args) – инициализация библиотеки ROS2. Обязательный вызов перед созданием любого узла.
node = SimpleTakeOffLand() – создание экземпляра класса-узла. В этот момент запускается конструктор, создается клиент сервиса и вызывается execute_sequence().
rclpy.spin(node) – цикл обработки событий. Пока узел активен, spin получает и обрабатывает входящие сообщения, ответы сервисов и тп.
В блоке finally гарантируется корректное завершение программы: node.destroy_node() закрывает работу узла, rclpy.shutdown() – это глобальное завершение работы библиотеки ROS2 для текущего процесса.
Взлет и поворот (Takeoff_and_rotate.py)
Данный код реализует узел ROS2 для автономного взлета и поворота дрона. Для завершения работы программы в терминале нажимаем Ctrl+C после автономной посадки дрона.
python
import rclpy
from rclpy.node import Node
from eagle_eye_msgs.srv import TakeOffBoard
from geometry_msgs.msg import Twist
import time
import math
class SimpleDroneControl(Node):
def __init__(self):
super().__init__("simple_drone_control")
self.get_logger().info("Simple Drone Control node started")
# Создаем клиент для сервиса взлета/посадки
self.takeoff_client = self.create_client(TakeOffBoard, "/take_off_board")
# Создаем издатель для управления скоростью
self.cmd_publisher = self.create_publisher(Twist, "cmd_vel", 10)
# Запускаем последовательность
self.execute_sequence()
def execute_sequence(self):
"""Основная последовательность управления дроном"""
# Установка соединения и взлет
self.call_takeoff_service(takeoff=True, board=False)
time.sleep(8) # Ждем завершения взлета
# Задержка 10 секунд
time.sleep(10)
# Поворот на 1.57 радиан (90 градусов) по часовой стрелке
self.rotate_drone(1.57) # 1.57 радиан = 90 градусов
# Разрыв соединения (посадка)
self.call_takeoff_service(takeoff=False, board=True)
def call_takeoff_service(self, takeoff: bool, board: bool):
"""Вызов сервиса взлета/посадки"""
# Ждем доступности сервиса
while not self.takeoff_client.wait_for_service(timeout_sec=1.0):
self.get_logger().info("Waiting for take_off_board service...")
# Создаем запрос
request = TakeOffBoard.Request()
request.takeoff = takeoff
request.board = board
# Отправляем запрос и ждем ответ
future = self.takeoff_client.call_async(request)
while not future.done():
rclpy.spin_once(self, timeout_sec=0.1)
try:
response = future.result()
if response.result:
if takeoff:
self.get_logger().info("Takeoff successful")
else:
self.get_logger().info("Landing successful")
else:
self.get_logger().error("Service call failed")
except Exception as e:
self.get_logger().error(f'Service call failed: {e}')
def rotate_drone(self, angle_rad):
"""Поворот дрона на заданный угол в радианах"""
# Создаем сообщение для поворота
twist_msg = Twist()
# Устанавливаем угловую скорость по Z (рыскание)
# Отрицательное значение - поворот по часовой стрелке
angular_speed = -0.5 # рад/сек
twist_msg.angular.z = angular_speed
# Рассчитываем время для поворота на нужный угол
rotation_time = abs(angle_rad / angular_speed)
# Публикуем команду поворота
self.get_logger().info(f"Rotating for {rotation_time:.2f} seconds")
start_time = time.time()
while time.time() - start_time < rotation_time:
self.cmd_publisher.publish(twist_msg)
time.sleep(0.1)
# Останавливаем вращение
stop_msg = Twist()
stop_msg.angular.z = 0.0
self.cmd_publisher.publish(stop_msg)
self.get_logger().info("Rotation completed")
def main(args=None):
rclpy.init(args=args)
node = SimpleDroneControl()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()В начале подключаются основные модули:
rclpy и Node – основа для работы узлов ROS2 на Python;
TakeOffBoard – сервис, отвечающий за взлет/посадку;
Twist – тип сообщения в ROS2, который описывает параметры линейных и угловых скоростей;
time, math – модули Python для работы с задержками и вычислениями.
При инициализации узла создается экземпляр класса SimpleDroneControl, наследующегося от базового класса Node ROS2, поэтому является полноценным узлом ROS2:
python
class SimpleDroneControl(Node):
def __init__(self):
super().__init__("simple_drone_control")
self.get_logger().info("Simple Drone Control node started")Вызов super().init("simple_drone_control") регистрирует узел в системе ROS2 с именем «simple_drone_control». Через get_logger().info() выводится служебное сообщение в лог ROS2. После создается клиент сервиса взлета/посадки self.takeoff_client = self.create_client(TakeOffBoard, "/take_off_board").
Создается издатель (publisher) команд скорости на топик cmd_vel self.cmd_publisher = self.create_publisher(Twist, "cmd_vel", 10):
Тип сообщения: Twist;
Размер очереди: 10 — буфер для исходящих команд.
Топик cmd_vel - стандартный канал в ROS2 для управления движением. В данной программе он используется для задания угловой скорости по рысканию ( вокруг оси Z), чтобы развернуть дрон.
Основная логика программы реализована в методе execute_sequence, который выполняет строго заданную последовательность операций:
Взлет. Вызов метода call_takeoff_service(takeoff=True, board=False) формирует запрос взлета/посадки с параметрами для взлета. После идет задержка в 8 секунд для ожидания завершения взлета;
Дополнительная пауза после взлета в 10 секунд;
Поворот. Вызов метода rotate_drone(1.57) поворачивает дрон на 1.57 радиан;
Посадка. Повторный вызов call_takeoff_service(takeoff=False, board=True) с параметрами для посадки.
Сначала вызывается метод call_takeoff_service, который используется для выполнения взлета или посадки:
python
def call_takeoff_service(self, takeoff: bool, board: bool):
while not self.takeoff_client.wait_for_service(timeout_sec=1.0):
self.get_logger().info("Waiting for take_off_board service...")
# Создаем запрос
request = TakeOffBoard.Request()
request.takeoff = takeoff
request.board = boardВнутри метода узел начинает с проверки доступности сервиса /take_off_board с помощью wait_for_service(timeout_sec=1.0). Эта проверка выполняется в цикле до тех пор, пока сервис, реализованный в другом узле, не станет доступен.
После того как сервис становится доступен, создается объект запроса TakeOffBoard.Request(), в который записываются флаги takeoff и board, определяющие требуемое действие (взлет или посадка).
Затем запрос отправляется асинхронно с помощью метода call_async, после чего узел ожидает завершения выполнения сервиса, отслеживая статус объекта future.
python
future = self.takeoff_client.call_async(request)
while not future.done():
rclpy.spin_once(self, timeout_sec=0.1)Как только сервис возвращает результат (future.done() = True), в строке response = future.result() происходит извлечение объекта ответа из future. Если response.result=True выводится сообщение об успешном взлете или посадки, в ином случае выводится сообщение об ошибке. Такой подход делает работу с сервисом более надежной – программа проверяет результат и логирует итог.
python
try:
response = future.result()
if response.result:
if takeoff:
self.get_logger().info("Takeoff successful")
else:
self.get_logger().info("Landing successful")
else:
self.get_logger().error("Service call failed")
except Exception as e:
self.get_logger().error(f'Service call failed: {e}')После получения логов управление возвращается в основной метод execute_sequence() для выполнения следующих шагов миссии – ожидания 10-и секунд и выполнения поворота на 90 градусов (1.57 радиан) по часовой стрелке, реализованного в методе rotate_drone().
python
def rotate_drone(self, angle_rad):
twist_msg = Twist()
angular_speed = -0.5 # рад/сек
twist_msg.angular.z = angular_speed
rotation_time = abs(angle_rad / angular_speed)Алгоритм поворота использует публикацию сообщений Twist с отрицательной угловой скоростью по оси Z (-0.5 рад/сек) для вращения по часовой стрелке. Эта скорость записывается в поле angular.z сообщения Twist. Время вращения рассчитывается исходя из требуемого угла, передаваемого с вызовом метода, и заданной угловой скорости: rotation_time = angle_rad / angular_speed. Используется abc() чтобы время получитось положительным независимо от знака угла и скорости.
Программа непрерывно публикует команды вращения в течение расчетного времени, обеспечивая точный поворот на заданный угол:
python
self.get_logger().info(f"Rotating for {rotation_time:.2f} seconds")
start_time = time.time()
while time.time() - start_time < rotation_time:
self.cmd_publisher.publish(twist_msg)
time.sleep(0.1)Выводится расчетное время поворота;
Запоминается текущее время;
В цикле, пока не истечет rotation_time, публикуется команда twist_msg на топик cmd_vel;
Пауза time.sleep(0.1) означает, что команда поворота публикуется каждые 0.1 секунды.
После завершения цикла поворота отправляется сообщение Twist с нулевой угловой скоростью, чтобы остановить вращение дрона и выводится соответствующее сообщение:
python
stop_msg = Twist()
stop_msg.angular.z = 0.0
self.cmd_publisher.publish(stop_msg)
self.get_logger().info("Rotation completed")Главная функция main обеспечивает стандартную инициализацию ROS2, создание узла и запуск цикла обработки событий с обработкой прерывания KeyboardInterrupt и гарантированным освобождением ресурсов. KeyboardInterrupt — это исключение, которое Python генерирует, когда пользователь прерывает команду с клавиатуры (Ctrl+C), что помогает корректно остановить ROS2 в finally.
Полет вдоль стены с использованием ПИД-регулятора (Wall_Follower.py)
Данный код реализует узел ROS2 для автономного движения дрона вдоль стены с поддержанием заданного расстояния. Система использует ультразвуковой датчик для измерения расстояния и PID-регулирование для точного позиционирования.
Перед запуском примера выполняем инициализацию DXL-устройств по инструкции, описанной в подразделе 1.4. Если данный шаг был проделан, можно приступать к запуску примера.
Для завершения работы программы необходимо нажать Ctrl+C в терминале и выполнить посадку дрона в ручном режиме.
python
# Импорт необходимых библиотек ROS 2
import rclpy
from rclpy.node import Node
import time # Для задержек
from eagle_eye_msgs.srv import TakeOffBoard # Сервис для взлета/посадки
from geometry_msgs.msg import Twist # Сообщение для управления скоростью
from std_msgs.msg import Float32MultiArray # Сообщение для данных с ультразвуковых датчиков
class WallFollower(Node):
def __init__(self):
super().__init__("wall_follower")
self.get_logger().info("Wall Follower node initialized")
# Издатель для отправки команд скорости
self.cmd_publisher = self.create_publisher(Twist, "cmd_vel", 10)
# Клиент сервиса для взлета/посадки
self.takeoff_board_client = self.create_client(TakeOffBoard, "take_off_board")
# Подписчик на данные с ультразвуковых датчиков
self.ultrasonic_subscriber = self.create_subscription(Float32MultiArray, "ultrasonic", self.distance_callback, 10)
# Желаемое расстояние до стены
self.desired_distance = 0.5
# Переменная для хранения ошибки расстояния
self.distance_error = 0.0
# Инициализация PID-регулятора для удержания на нужном расстоянии
self.pid_pitch = PID(2.5, 0.0005, 0.5)
# Скорость движения вдоль стены (используется как константа для бокового движения)
self.roll_speed = 0.8
# Вызов сервиса взлета в начале работы узла
self.call_takeoff_board_service()
# Флаг, указывающий на завершение взлета
self.takeoff = False
# Сообщение Twist с нулевыми скоростями для остановки дрона
self.zero_twist = Twist()
def distance_callback(self, msg: Float32MultiArray):
# Обработка данных с ультразвукового датчика только после взлета
if self.takeoff:
# Получаем расстояние до стены (первый элемент массива, конвертируем из см в метры)
distance_to_wall = msg.data[0]/100
# Вычисляем ошибку расстояния (текущее - желаемое)
self.distance_error = distance_to_wall - self.desired_distance
# Вызов функции регулирования движения вдоль стены
self.WallRegulation(self.distance_error)
else:
self.get_logger().info("Takeoff not completed")
# Если взлет не завершен, отправляем нулевые скорости
self.cmd_publisher.publish(self.zero_twist)
def constrain(self, value, threshold):
# Функция для ограничения значения в заданных пределах
if value > threshold:
value = threshold
if value < -threshold:
value = -threshold
return value
def WallRegulation(self, pitch_error):
# Точность регулирования (порог, в пределах которого считается, что дрон находится на нужном расстоянии)
accuracy = 0.1
# Максимальное значение управляющего воздействия PID
max_pid = 1.0
# Если ошибка расстояния превышает порог точности
if abs(pitch_error) > accuracy:
self.pid_pitch.update_control(pitch_error) # Обновляем PID-регулятор
PID_PITCH = self.pid_pitch.get_control() # Получаем управляющее воздействие
PID_PITCH = self.constrain(PID_PITCH, max_pid) # Ограничиваем управляющее воздействие
ROLL = 0.0
else:
# Если дрон находится в пределах точности, останавливаем движение вперед/назад и начинаем боковое движение
PID_PITCH = 0.0 # Отсутствие движения вперед/назад
ROLL = self.roll_speed # Боковое движение (влево/вправо) для следования стене
print(f"PID_PITCH: {PID_PITCH}, ROLL: {ROLL}")
# Создаем и публикуем сообщение Twist для управления дроном
cmd_msg = Twist()
cmd_msg.linear.x = PID_PITCH # Линейная скорость по оси X (вперед/назад)
cmd_msg.linear.y = ROLL # Линейная скорость по оси Y (влево/вправо)
cmd_msg.angular.z = 0.0 # Угловая скорость по оси Z (вращение)
self.cmd_publisher.publish(cmd_msg)
def call_takeoff_board_service(self):
# Ожидание доступности сервиса взлета/посадки
while not self.takeoff_board_client.wait_for_service(timeout_sec=1.0):
self.get_logger().info("Waiting for take_off_board service...")
# Создаем запрос на взлет
request = TakeOffBoard.Request()
request.takeoff = True
request.board = False
# Отправляем запрос асинхронно
future = self.takeoff_board_client.call_async(request)
# Устанавливаем колбэк для обработки ответа сервиса
future.add_done_callback(self.takeoff_board_response_callback)
def takeoff_board_response_callback(self, future):
# Обработка ответа от сервиса взлета/посадки
try:
response = future.result()
if response.result:
self.get_logger().info('TakeOffBoard service call successful')
time.sleep(10) # Задержка для стабилизации дрона после взлета
self.takeoff = True # Установка флага взлета в True
else:
self.get_logger().warn('TakeOffBoard service call failed')
except Exception as e:
self.get_logger().error(f'Service call failed: {e}')
class PID:
"""
Класс PID-регулятора для управления системой с обратной связью
Реализует пропорционально-интегрально-дифференциальное управление
"""
def __init__(self, Kp, Ti, Td):
"""
Инициализация PID-регулятора
Args:
Kp (float): Пропорциональный коэффициент
Ti (float): Интегральный коэффициент
Td (float): Дифференциальный коэффициент
"""
# Коэффициенты PID-регулятора
self.Kp = Kp # Пропорциональный коэффициент
self.Td = Td # Дифференциальный коэффициент
self.Ti = Ti # Интегральный коэффициент
# Переменные для хранения ошибок
self.curr_error = 0 # Текущая ошибка
self.prev_error = 0 # Предыдущая ошибка
self.sum_error = 0 # Накопленная сумма ошибок (для интегральной составляющей)
self.prev_error_deriv = 0 # Предыдущая производная ошибки
self.curr_error_deriv = 0 # Текущая производная ошибки
self.control = 0 # Выходной сигнал регулятора
def update_control(self, current_error, reset_prev=False):
"""
Обновляет выходной сигнал PID-регулятора на основе текущей ошибки
Args:
current_error (float): Текущее значение ошибки
reset_prev (bool): Флаг сброса предыдущих значений (не используется)
"""
# Сохраняем предыдущую ошибку и обновляем текущую
self.prev_error = self.curr_error
self.curr_error = current_error
# Вычисляем интегральную составляющую (накопление ошибки)
self.sum_error = self.sum_error + self.curr_error
# Вычисляем дифференциальную составляющую (скорость изменения ошибки)
self.curr_error_deriv = self.curr_error - self.prev_error
# Вычисляем итоговый управляющий сигнал PID
# U(t) = Kp*e(t) + Ki*∫e(t)dt + Kd*de(t)/dt
self.control = (self.Kp * self.curr_error +
self.Ti * self.sum_error +
self.Td * self.curr_error_deriv)
def get_control(self):
"""
Возвращает текущее значение управляющего сигнала
Returns:
float: Значение управляющего сигнала
"""
return self.control
def main(args=None):
# Инициализация rclpy
rclpy.init(args=args)
# Создание экземпляра узла WallFollower
wall_follower = WallFollower()
try:
# Запуск узла ROS 2
rclpy.spin(wall_follower)
except KeyboardInterrupt:
pass
finally:
# Корректное завершение работы узла и rclpy
wall_follower.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()В начале подключаются основные модули. Класс WallFollower наследуется от базового класса Node из ROS2. При инициализации создается узел с именем "wall_follower", предназначенный для автономного движения вдоль стен с использованием ультразвуковых датчиков. После выводится сообщение о запуске узла.
Создаются основные интерфейсы ROS2:
python
self.cmd_publisher = self.create_publisher(Twist, "cmd_vel", 10)
self.takeoff_board_client = self.create_client(TakeOffBoard, "take_off_board")
self.ultrasonic_subscriber = self.create_subscription(Float32MultiArray, "ultrasonic", self.distance_callback, 10)cmd_publisher – издатель в топик cmd_vel (тип Twist) для команд линейной и угловой скорости дрона;
takeoff_board_client – клиент сервиса take_off_board типа TakeOffBoard для взлета/посадки;
ultrasonic_subscriber – подписчик на массив чисел Float32MultiArray из топика ultrasonic. В distance_callback приходят данные ультразвуковых датчиков.
Задаются желаемое расстояние дрона до стены и его постоянная боковая скорость вдоль стены. Создается PID-регулятор с коэффициентами для реагирования на изменение расстояния, а также готовое сообщение с нулевыми скоростями для остановки дрона.
Обработка данных ультразвукового датчика реализована в функции distance_callback:
python
def distance_callback(self, msg: Float32MultiArray):
if self.takeoff:
distance_to_wall = msg.data[0]/100
self.distance_error = distance_to_wall - self.desired_distance
self.WallRegulation(self.distance_error)
else:
self.get_logger().info("Takeoff not completed")
self.cmd_publisher.publish(self.zero_twist)Функция вызывается при каждом новом сообщении в ultrasonic. Берется первый элемент массива msg.data[0] как расстояние до стены в сантиметрах и переводится в метры. Вычисляется ошибка (разница между текущим расстоянием до стены и желаемым), которая затем передается в функцию WallRegulation. Если же взлет еще не завершен, то публикуется нулевая команда скорости.
Логика следования по стене реализована в функции WallRegulation:
python
def WallRegulation(self, pitch_error):
accuracy = 0.1
max_pid = 1.0
if abs(pitch_error) > accuracy:
self.pid_pitch.update_control(pitch_error)
PID_PITCH = self.pid_pitch.get_control()
PID_PITCH = self.constrain(PID_PITCH, max_pid)
ROLL = 0.0
else:
PID_PITCH = 0.0
ROLL = self.roll_speed
print(f"PID_PITCH: {PID_PITCH}, ROLL: {ROLL}")
cmd_msg = Twist()
cmd_msg.linear.x = PID_PITCH
cmd_msg.linear.y = ROLL
cmd_msg.angular.z = 0.0
self.cmd_publisher.publish(cmd_msg)accuracy – допустимое отклонение по расстоянию. Если ошибка меньше этого порога, то, считаем, что дрон находится на желаемом расстоянии.
max_pid – максимально допустимое по модулю управляющее воздействие PID-регулятора.
Если дрон заметно отклонен от нужного расстояния, то обновляется PID-регулятор по ошибке расстояния, отдавая на выходе управляющий сигнал по оси pitch. При этом боковая скорость roll обнуляется, дрон сначала выравнивается на нужное расстояние, не двигаясь вдоль стены.
Функция constrain ограничивает значение управляющего сигнала, полученного от PID-регулятора, удерживая его в заданных пределах и предотвращая выход за допустимые значения.
Если дрон находится в пределах заданной точности, то останавливается движение вперед/назад, и включается постоянная боковая скорость вдоль стены, задаваемая в атрибуте roll_speed.
Далее формируется и публикуется команда через объект сообщения cmd_msg типа Twist, через который узел отправляет команды движения дрона в топик cmd_vel. Узел self.cmd_publisher.publish(cmd_msg) публикует cmd_msg в топик cmd_vel, контроллер подписан на этот узел и читает сообщения, по значениям которых задаются управляющие сигналы моторам.
Взлет реализован через сервис call_takeoff_board_service по такой же логике, как и в прошлых программах.
Класс PID реализует классический пропорционально-интегрально-дифференциальный регулятор, накапливает ошибку для интегральной составляющей и вычисляет производную как разность между текущей и предыдущей ошибкой. Управляющий сигнал рассчитывается как сумма трех компонент: пропорциональной (Kperror), интегральной (Tisum_error) и дифференциальной (Td*error_deriv).
Программа работает в непрерывном цикле обработки событий ROS2 (rclpy.spin – запускает цикл обработки событий, где работают подписчики, сервисы), обеспечивая постоянную реакцию на данные датчиков и корректировку траектории движения. При завершении работы все ресурсы узла корректно освобождаются через destroy_node() и rclpy.shutdown().
5.6.2. Примеры программ с ArUco-маркерами
Перед запуском рассматриваемых ниже примеров выполняем настройку распознавания ArUco-маркеров в графическом интерфейсе в разделе «Настройки». Включаем «Обнаружение маркеров ArUco», выдираем словарь и размер маркера, соответствующие вашему маркеру. О том, как войти в графический интерфейс, можно узнать в подразделе 2.1.

Проверка настроек ArUco-маркера в графическом интерфейсе
Слежение за ArUco-маркерами с ПИД-регулятором (Aruco_follower.py)
Данный код представляет собой узел ROS2 для автономного слежения дрона за ArUco-маркерами с использованием PID-регулятора. Для завершения работы программы нажимаем Ctrl+C в терминале и выполняем посадку дрона в ручном режиме.
python
# Импорт необходимых библиотек ROS 2
import rclpy
from rclpy.node import Node
from eagle_eye_msgs.msg import MarkerArray # Сообщение для массива маркеров ArUco
from eagle_eye_msgs.srv import TakeOffBoard # Сервис для взлета/посадки
from geometry_msgs.msg import Twist # Сообщение для управления скоростью
import time # Для задержек
class ArucoFollower(Node):
def __init__(self):
super().__init__("aruco_follower")
self.get_logger().info("Aruco Follower node initialized")
# Подписка на топик с маркерами ArUco
self.marker_sub = self.create_subscription(MarkerArray, "arucos", self.marker_callback, 10)
# Издатель для отправки команд скорости
self.cmd_publisher = self.create_publisher(Twist, "cmd_vel", 10)
# Создаем клиент сервиса для взлета/посадки
self.takeoff_board_client = self.create_client(TakeOffBoard, "/take_off_board")
# Создаем переменную для хранения желаемого расстояния до маркера
self.desired_distance = 1.1
# Инициализируем переменные для ошибок регулирования по осям X (крен), Z (расстояние) и вращению (рыскание)
self.roll_error = 0.0
self.pitch_error = 0.0
self.yaw_error = 0.0
# Инициализация PID-регуляторов для крена, тангажа (движение вперед/назад) и рыскания
self.pid_roll = PID(1.0, 0.002, 0.2)
self.pid_pitch = PID(1.0, 0.002, 0.2)
self.pid_yaw = PID(0.6, 0.00, 0.5)
# Флаг состояния взлета
self.takeoff = False
# Вызов сервиса для взлета
self.call_takeoff_board_service()
# Инициализируем и публикуем нулевые скорости для остановки дрона
test_msg = Twist()
test_msg.linear.x = 0.0
test_msg.linear.y = 0.0
test_msg.angular.z = 0.0
self.cmd_publisher.publish(test_msg)
def marker_callback(self, msg):
# Обработка маркеров только после успешного взлета
if self.takeoff:
aruco_markers = msg.markers
# Если обнаружены маркеры
if len(aruco_markers) > 0:
# Получаем ошибку по оси X (крен) из первого маркера
self.roll_error = aruco_markers[0].pose.pose.position.x
# Получаем расстояние до маркера по оси Z
distance_to_marker = aruco_markers[0].pose.pose.position.z
# Вычисляем ошибку по тангажу (разница между текущим и желаемым расстоянием)
self.pitch_error = distance_to_marker - self.desired_distance
# Получаем ошибку по рысканию (ориентация)
self.yaw_error = aruco_markers[0].pose.pose.orientation.z
self.get_logger().info(f"Roll error: {self.roll_error}, Pitch error: {self.pitch_error}")
# Вызов функции регулирования на основе ошибок
self.ArucoRegulation(self.pitch_error, self.roll_error, self.yaw_error)
else:
self.get_logger().info("No aruco markers found")
# Если маркеры не найдены, продолжаем регулирование с последними известными ошибками
self.ArucoRegulation(self.pitch_error, self.roll_error, self.yaw_error)
def constrain(self, value, threshold):
# Функция для ограничения значения в заданных пределах
if value > threshold:
value = threshold
if value < -threshold:
value = -threshold
return value
def ArucoRegulation(self, pitch_error, roll_error, yaw_error):
# Порог точности для ошибок регулирования
accuracy = 0.1
# Максимальное значение управляющего воздействия PID
max_pid = 1.0
# Регулирование по тангажу (движение вперед/назад)
if abs(pitch_error) > accuracy:
self.pid_pitch.update_control(pitch_error)
PID_PITCH = self.pid_pitch.get_control()
PID_PITCH = self.constrain(PID_PITCH, max_pid)
else:
PID_PITCH = 0.0
# Регулирование по крену (движение влево/вправо)
if abs(roll_error) > accuracy:
self.pid_roll.update_control(roll_error)
PID_ROLL = self.pid_roll.get_control()
PID_ROLL = self.constrain(PID_ROLL, max_pid)
else:
PID_ROLL = 0.0
# Регулирование по рысканию (вращение)
if abs(yaw_error) > 0.1:
self.pid_yaw.update_control(yaw_error)
PID_YAW = - self.pid_yaw.get_control()
PID_YAW = self.constrain(PID_YAW, 0.3)
else:
PID_YAW = 0.0
print(f"PID Control: PITCH={PID_PITCH}, ROLL={PID_ROLL}, YAW = {PID_YAW}")
# Создаем и публикуем сообщение Twist для управления дроном
cmd_msg = Twist()
cmd_msg.linear.x = PID_PITCH # Линейная скорость по оси X (вперед/назад)
cmd_msg.linear.y = PID_ROLL # Линейная скорость по оси Y (влево/вправо)
cmd_msg.angular.z = PID_YAW # Угловая скорость по оси Z (вращение)
self.cmd_publisher.publish(cmd_msg)
def call_takeoff_board_service(self):
# Ожидание доступности сервиса взлета/посадки
while not self.takeoff_board_client.wait_for_service(timeout_sec=1.0):
self.get_logger().info("Waiting for take_off_board service...")
# Создаем запрос на взлет
request = TakeOffBoard.Request()
request.takeoff = True
request.board = False
# Отправляем запрос асинхронно
future = self.takeoff_board_client.call_async(request)
# Устанавливаем колбэк для обработки ответа сервиса
future.add_done_callback(self.takeoff_board_response_callback)
def takeoff_board_response_callback(self, future):
# Обработка ответа от сервиса взлета/посадки
try:
response = future.result()
if response.result:
self.get_logger().info('TakeOffBoard service call successful')
time.sleep(10) # Задержка для стабилизации дрона после взлета
self.takeoff = True # Установка флага взлета в True
else:
self.get_logger().warn('TakeOffBoard service call failed')
except Exception as e:
self.get_logger().error(f'Service call failed: {e}')
class PID:
# Инициализация PID-регулятора
def __init__(self, Kp, Ti, Td):
self.Kp = Kp # Пропорциональный коэффициент
self.Td = Td # Дифференциальный коэффициент
self.Ti = Ti # Интегральный коэффициент
self.curr_error = 0 # Текущая ошибка
self.prev_error = 0 # Предыдущая ошибка
self.sum_error = 0 # Сумма ошибок (для интегральной составляющей)
self.prev_error_deriv = 0 # Предыдущая производная ошибки
self.curr_error_deriv = 0 # Текущая производная ошибки
self.control = 0 # Выходное управляющее воздействие
# Обновление управляющего воздействия PID-регулятора
def update_control(self, current_error,
reset_prev=False):
self.prev_error = self.curr_error
self.curr_error = current_error
# Расчет интегральной ошибки
self.sum_error = self.sum_error + self.curr_error
# Расчет производной ошибки
self.curr_error_deriv = self.curr_error - self.prev_error
# Расчет управляющего воздействия PID
self.control = self.Kp * self.curr_error + self.Ti * self.sum_error + self.Td * self.curr_error_deriv
# Получение текущего управляющего воздействия
def get_control(self):
return self.control
def main(args=None):
# Инициализация rclpy
rclpy.init(args=args)
# Создание экземпляра узла ArucoFollower
node = ArucoFollower()
# Запуск узла ROS 2
rclpy.spin(node)
# Корректное завершение работы узла и rclpy
rclpy.shutdown()
if __name__ == '__main__':
main()При инициализации узла создается экземпляр класса ArucoFollower, наследующегося от базового класса Node и регистрируется в ROS2 под именем aruco_follower, после выводится сообщение о запуске узла.
Далее в процессе инициализации создаются основные ROS-интерфейсы: подписчик на топик с маркерами ArUco, издатель команд скорости и клиент сервиса взлета/посадки:
python
self.marker_sub = self.create_subscription(MarkerArray, "arucos", self.marker_callback, 10)
self.cmd_publisher = self.create_publisher(Twist, "cmd_vel", 10)
self.takeoff_board_client = self.create_client(TakeOffBoard, "/take_off_board")marker_sub – подписчик на MarkerArray из топика arucos (положение и ориентация ArUco-маркеров);
cmd_publisher – издатель в cmd_vel (тип Twist) для команд движения;
takeoff_board_client – клиент сервиса взлета/посадки.
После создания всех интерфейсов вызывается метод call_takeoff_board_service (строка 35), который инициирует процедуру взлета. Узел ожидает доступности сервиса /take_off_board и отправляет асинхронный запрос с параметром takeoff = True. После успешного взлета выдерживается 10-секундная пауза для стабилизации дрона, после чего устанавливается флаг self.takeoff в True, разрешающий работу основного алгоритма слежения.
Создается переменная desired_distance требуемого расстояния до маркера. Переменные ошибок используются для хранения последних значений по трем осям (roll_error, pitch_error, yaw_error). Инициализируется PID-регулятор с коэффициентами для крена (pid_roll), тангажа (pid_pitch) и рыскания (pid_yaw).
Обработка маркеров активируется в callback-функции marker_callback, которая вызывается при поступлении новых данных о ArUco-маркерах. Функция обрабатывает первый обнаруженный маркер из массива, извлекая три ключевых параметра позиционирования (см. Приложение 1 «Системы координат ArUco-маркеров»):
смещение по оси X (roll_error) – горизонтальное отклонение от центра маркера;
расстояние по оси Z (pitch_error) – разница между текущим и желаемым расстоянием 1.1 метра;
ориентацию по оси Z (yaw_error) – угловое отклонение относительно маркера.
Все три ошибки передаются в функцию регулирования ArucoRegulation. Если же маркер не обнаружен или потерян, выводится сообщение "No aruco markers found" и выполняется регулирование с последними зафиксированными ошибками.
Однако выполнение алгоритма обработки маркеров и управления дроном внутри callback ограничено проверкой флага self.takeoff. До завершения сервиса взлета и установки self.takeoff = True обработка маркеров фактически не выполняется, и управляющие команды не формируются.
В функции ArucoRegulation реализовано регулирование относительно маркера. Для этого задается порог чувствительности accuracy = 0.1, в пределах которого ошибка считается незначительной, и управляющее воздействие по соответствующей оси обнуляется. Также ограничивается максимальное значение выходного сигнала PID-регулятора max_pid, чтобы предотвратить слишком большие управляющие команды.
Если дрон слишком близко или далеко от маркера, PID-регулятор формирует управляющее воздействие, обеспечивающее движение вперед или назад. Если ошибка pitch_error меньше заданного порога accuracy, движение по данной оси прекращается (PID_PITCH = 0.0).
Аналогично реализовано управление по крену (roll) и рысканию (yaw): при боковом смещении или отклонении ориентации относительно маркера PID-регуляторы формируют соответствующие команды линейной и угловой скорости, обеспечивая выравнивание дрона относительно центра маркера и корректное позиционирование.
Формирование команд движения:
python
cmd_msg = Twist()
cmd_msg.linear.x = PID_PITCH # Линейная скорость по оси X (вперед/назад)
cmd_msg.linear.y = PID_ROLL # Линейная скорость по оси Y (влево/вправо)
cmd_msg.angular.z = PID_YAW # Угловая скорость по оси Z (вращение)
self.cmd_publisher.publish(cmd_msg)Команды движения публикуются в cmd_vel. Дрон одновременно двигается к маркеру или от него, центрируется по боковому смещению и поворачивается к маркеру.
Класс PID реализует классический алгоритм ПИД-регулирования.
Функция rclpy.spin(node) запускает основной цикл обработки событий ROS2, включая получение сообщений, выполнение callback'ов и обработку сервисов. Это обеспечивает непрерывную работу узла до получения сигнала завершения. При завершении работы все ресурсы узла корректно освобождаются через rclpy.shutdown().
Посадка на ArUco маркер (Aruco_boarding.py)
Данный код реализует узел ROS2 для автономной посадки дрона на платформу с ArUco-маркерами. Система использует постепенное снижение высоты с PID-регулированием для точного позиционирования.
Перед запуском данного примера необходимо перекрутить TrackingCam v3 так, чтобы камера смотрела вниз. Для завершения работы программы в терминале нажимаем сочетание клавиш Ctrl+C после автономной посадки дрона.
python
# Импорт необходимых библиотек и сообщений ROS 2
import rclpy
from rclpy.impl.rcutils_logger import Once
from rclpy.node import Node
from eagle_eye_msgs.msg import MarkerArray, Altitude # Пользовательские сообщения для маркеров и высоты
from eagle_eye_msgs.srv import TakeOffBoard, SetHeight # Пользовательские сервисы для взлета/посадки и установки высоты
from geometry_msgs.msg import Twist # Сообщение для управления скоростью
import time # Для работы со временем
class ArucoBoarding(Node):
def __init__(self):
super().__init__("aruco_boarding")
self.get_logger().info("Aruco Boarding node initialized")
# Подписка на топик с маркерами ArUco
self.marker_sub = self.create_subscription(MarkerArray, "arucos", self.marker_callback, 10)
# Издатель для отправки команд скорости
self.cmd_publisher = self.create_publisher(Twist, "cmd_vel", 10)
# Создаем клиент сервиса для взлета/посадки
self.takeoff_board_client = self.create_client(TakeOffBoard, "/take_off_board")
# Создаем клиент сервиса для установки высоты
self.set_height_client = self.create_client(SetHeight, "/set_height")
# Инициализируем переменные для ошибок регулирования по осям X (крен) и Y (тангаж)
self.roll_error = 0.0
self.pitch_error = 0.0
# Начальная целевая высота для дрона
self.target_height = 1.5
self.initial_target_height = self.target_height
# Инициализация PID-регуляторов для крена и тангажа
self.pid_roll = PID(0.7, 0.000, 0.1)
self.pid_pitch = PID(0.7, 0.000, 0.1)
# Флаг состояния взлета
self.takeoff = False
# Флаг, указывающий на процесс снижения
self.descending_flag = False
# Время последнего обнаружения маркера, используется для таймаута
self.last_marker_seen = 0
# Флаг состояния посадки
self.boarding = False
# Вызов сервиса для взлета (takeoff=True) и запрета посадки(board=False)
self.call_takeoff_board_service(takeoff=True, board=False)
# Инициализация и публикация нулевых скоростей для остановки дрона
test_msg = Twist()
test_msg.linear.x = 0.0
test_msg.linear.y = 0.0
test_msg.angular.z = 0.0
self.cmd_publisher.publish(test_msg)
def marker_callback(self, msg):
# Обработка маркеров только после успешного взлета
if self.takeoff:
aruco_markers = msg.markers
current_time = time.time()
# Если обнаружены маркеры
if len(aruco_markers) > 0:
# Получаем ошибки по осям X (крен) и Y (тангаж) из первого маркера
self.roll_error = aruco_markers[0].pose.pose.position.x
self.pitch_error = aruco_markers[0].pose.pose.position.y
self.get_logger().info(f"Roll error: {self.roll_error}, Pitch error: {self.pitch_error}")
# Вызов функции регулирования на основе ошибок
self.ArucoRegulation(self.pitch_error, self.roll_error)
# Обновляем время последнего обнаружения маркера
self.last_marker_seen = current_time
# Проверяем, можно ли начать спуск (если ошибки малы и не идет процесс снижения)
if (abs(self.pitch_error) < 0.1 and abs(self.roll_error) < 0.1) and not self.descending_flag:
self.get_logger().info("Descending")
# Уменьшаем целевую высоту на 50%
self.target_height *= 0.5
self.get_logger().info(f"Target height: {self.target_height}")
# Вызов сервиса для установки новой целевой высоты
self.call_set_height_service(self.target_height)
else:
self.get_logger().info("No aruco markers found")
# Продолжаем регулирование даже без маркеров (используя предыдущие ошибки)
self.ArucoRegulation(self.pitch_error, self.roll_error)
# Если маркеры не видны более 10 секунд и дрон не на начальной высоте, поднимаемся выше
if current_time - self.last_marker_seen > 10.0 and self.target_height * 1.2 < self.initial_target_height:
self.get_logger().info("No marker seen for 10 seconds, setting new target height")
# Увеличиваем целевую высоту на 20%
self.target_height *= 1.2
self.call_set_height_service(self.target_height)
self.last_marker_seen = current_time
# Если целевая высота слишком низка (ниже 0.4м) и дрон все еще в полете, начинаем посадку на платформу
if self.target_height < 0.4 and self.takeoff:
self.boarding = True
# Отправляем нулевые скорости для остановки дрона
twist = Twist()
self.cmd_publisher.publish(twist)
self.get_logger().info("Target height is too low, start boarding")
# Вызов сервиса для посадки на платформу (board=True)
self.call_takeoff_board_service(takeoff=False, board=True)
self.takeoff = False
def ArucoRegulation(self, pitch_error, roll_error):
# Максимальное значение для PID-регуляторов
max_pid = 1.0
# Обновление и ограничение значения PID для тангажа
self.pid_pitch.update_control(pitch_error)
PID_PITCH = -self.pid_pitch.get_control()
PID_PITCH = self.constrain(PID_PITCH, max_pid)
# Обновление и ограничение значения PID для крена
self.pid_roll.update_control(roll_error)
PID_ROLL = self.pid_roll.get_control()
PID_ROLL = self.constrain(PID_ROLL, max_pid)
print(f"PID Control: PITCH={PID_PITCH}, ROLL={PID_ROLL}")
# Создание и публикация сообщения Twist для управления дроном
cmd_msg = Twist()
cmd_msg.linear.x = PID_PITCH # Управление вперед/назад через тангаж
cmd_msg.linear.y = PID_ROLL # Управление влево/вправо через крен
cmd_msg.angular.z = 0.0 # Отсутствие вращения
self.cmd_publisher.publish(cmd_msg)
def constrain(self, value, threshold):
# Функция для ограничения значения в заданных пределах
if value > threshold:
value = threshold
if value < -threshold:
value = -threshold
return value
def call_set_height_service(self, target_height):
# Ожидание доступности сервиса установки высоты
while not self.takeoff_board_client.wait_for_service(timeout_sec=1.0):
self.get_logger().info("Waiting for set_height service...")
# Создание запроса на установку высоты
request = SetHeight.Request()
request.height = target_height
# Установка флага снижения
self.descending_flag = True
# Асинхронный вызов сервиса и установка колбэка для обработки ответа
future = self.set_height_client.call_async(request)
future.add_done_callback(self.set_height_response_callback)
def set_height_response_callback(self, future):
# Обработка ответа от сервиса установки высоты
try:
response = future.result()
if response.result:
self.get_logger().info("Set height service call successful")
# Сброс флага снижения после успешной установки высоты
self.descending_flag = False
except Exception as e:
self.get_logger().error(f'Service call failed: {e}')
def call_takeoff_board_service(self, takeoff: bool, board: bool):
# Ждем, пока сервис станет доступным
while not self.takeoff_board_client.wait_for_service(timeout_sec=1.0):
self.get_logger().info("Waiting for take_off_board service...")
# Создаем запрос на взлет или посадку
request = TakeOffBoard.Request()
request.takeoff = takeoff
request.board = board
# Отправляем запрос асинхронно и устанавливаем соответствующий колбэк
future = self.takeoff_board_client.call_async(request)
if takeoff:
future.add_done_callback(self.takeoff_response_callback)
else:
future.add_done_callback(self.board_response_callback)
def board_response_callback(self, future):
# Обработка ответа от сервиса посадки на платформу
try:
response = future.result()
if response.result:
self.get_logger().info("Start Aruco Boarding")
self.takeoff = False # Флаг взлета сбрасывается после начала посадки
except Exception as e:
self.get_logger().error(f'Service call failed: {e}')
def takeoff_response_callback(self, future):
# Обработка ответа от сервиса взлета
try:
response = future.result()
if response.result:
self.get_logger().info('TakeOffBoard service call successful')
time.sleep(8) # Ожидание стабилизации после взлета
self.get_logger().info("Setting target height")
self.call_set_height_service(self.initial_target_height) # Установка начальной целевой высоты
time.sleep(4) # Дополнительное ожидание
self.takeoff = True # Установка флага взлета в True
else:
self.get_logger().warn('TakeOffBoard service call failed')
except Exception as e:
self.get_logger().error(f'Service call fasiled: {e}')
class PID:
# Инициализация PID-регулятора
def __init__(self, Kp, Ti, Td):
self.Kp = Kp # Пропорциональный коэффициент
self.Td = Td # Дифференциальный коэффициент
self.Ti = Ti # Интегральный коэффициент
self.curr_error = 0 # Текущая ошибка
self.prev_error = 0 # Предыдущая ошибка
self.sum_error = 0 # Сумма ошибок (для интегральной составляющей)
self.prev_error_deriv = 0 # Предыдущая производная ошибки
self.curr_error_deriv = 0 # Текущая производная ошибки
self.control = 0 # Выходное управляющее воздействие
# Обновление управляющего воздействия PID-регулятора
def update_control(self, current_error,
reset_prev=False):
self.prev_error = self.curr_error
self.curr_error = current_error
# Расчет интегральной ошибки
self.sum_error = self.sum_error + self.curr_error
# Расчет производной ошибки
self.curr_error_deriv = self.curr_error - self.prev_error
# Расчет управляющего воздействия PID
self.control = self.Kp * self.curr_error + self.Ti * self.sum_error + self.Td * self.curr_error_deriv
# Получение текущего управляющего воздействия
def get_control(self):
return self.control
def main(args=None):
# Инициализация rclpy
rclpy.init(args=args)
# Создание экземпляра узла ArucoBoarding
aruco_boarding = ArucoBoarding()
# Запуск узла ROS 2
rclpy.spin(aruco_boarding)
# Остановка узла и завершение работы rclpy
aruco_boarding.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()При инициализации узла создается экземпляр класса ArucoBoarding, который наследуется от базового класса Node, имя в графе ROS2 - aruco_boarding. В конструкторе инициализируются все необходимые компоненты: подписчик на топик "arucos" для получения данных о обнаруженных маркерах, издатель в топик "cmd_vel" для отправки команд управления, клиент сервиса взлета/посадки "/take_off_board" и клиент сервиса изменения целевой высоты "/set_height".
Далее инициализируются переменные состояния ошибок регулирования, задается текущая целевая высота при взлете, равная 1.5 метрам. Алгоритм позиционирования использует два независимых PID-регулятора с одинаковыми коэффициентами Kp=0.7, Ti=0.0, Td=0.1.
Создается переменная descending_flag для определения выполнения команды на изменение высоты, чтобы не слать новую команду, пока не отработала старая. И создается переменная last_marker_seen – время последнего обнаружения маркера.
После создания узла инициируется процедура взлета через вызов сервиса TakeOffBoard с параметрами takeoff = True и board = False. После успешного взлета программа выдерживает 8-секундную паузу для стабилизации дрона, затем устанавливает начальную целевую высоту 1.5 метра через сервис SetHeight и активирует флаг self.takeoff, разрешающий работу основного алгоритма выравнивания и посадки на маркер.
Обработка данных о маркерах активируется в callback-функции marker_callback:
python
aruco_markers = msg.markers
if len(aruco_markers) > 0:
self.roll_error = aruco_markers[0].pose.pose.position.x
self.pitch_error = aruco_markers[0].pose.pose.position.y
self.get_logger().info(f"Roll error: {self.roll_error}, Pitch error: {self.pitch_error}")
self.ArucoRegulation(self.pitch_error, self.roll_error)Программа извлекает из первого обнаруженного маркера два ключевых параметра:
смещение по оси X (ошибка по крену) – горизонтальное отклонение от центра маркера;
смещение по оси Y (ошибка по тангажу) – вертикальное отклонение.
Эти ошибки передаются в метод ArucoRegulation для коррекции положения дрона:
python
max_pid = 1.0
self.pid_pitch.update_control(pitch_error)
PID_PITCH = -self.pid_pitch.get_control()
PID_PITCH = self.constrain(PID_PITCH, max_pid)
self.pid_roll.update_control(roll_error)
PID_ROLL = self.pid_roll.get_control()
PID_ROLL = self.constrain(PID_ROLL, max_pid)для тангажа и крена считаются управляющие воздействия PID-регулятора;
для управляющего воздействия по тангажу есть минус, чтобы при положительной ошибке (когда ArUco-маркер позади квадрокоптера и находится в нижней части кадра) дрон двигался в нужном направлении (см. Приложение 1 «Скоростная система координат» и «Системы координат ArUco-маркеров»);
оба сигнала ограничиваются по модулю max_pid=1.
Далее формируются и публикуются сообщения для управления дроном.
Когда дрон достигает точного позиционирования (ошибки по обеим осям менее 0.1), система автоматически уменьшает целевую высоту на 50% через вызов сервиса /set_height:
python
if (abs(self.pitch_error) < 0.1 and abs(self.roll_error) < 0.1) and not self.descending_flag:
self.get_logger().info("Descending")
self.target_height *= 0.5
self.get_logger().info(f"Target height: {self.target_height}")
self.call_set_height_service(self.target_height)При этом устанавливается флаг descending_flag = True внутри call_set_height_service (строка 137), предотвращающий многократное снижение в одном цикле, чтобы не запускать параллельных команд высоты.
В функции set_height_response_callback логируется ответ и сбрасывается descending_flag, чтобы вновь приступить к расчету и установке новой целевой высоты.
Если же ArUco-маркер не обнаружен, то управление дроном продолжается на основе последних ошибок. Если маркер не обнаруживается более 10 секунд и текущая высота значительно ниже начальной, программа увеличивает целевую высоту на 20%, чтобы повторно найти маркер. Текущая целевая высота target_height умножается на 1.2 и передается в сервис /set_height.
python
else:
self.get_logger().info("No aruco markers found")
self.ArucoRegulation(self.pitch_error, self.roll_error)
if current_time - self.last_marker_seen > 10.0 and self.target_height * 1.2 < self.initial_target_height:
self.get_logger().info("No marker seen for 10 seconds, setting new target height")
self.target_height *= 1.2
self.call_set_height_service(self.target_height)
self.last_marker_seen = current_timeФинальная стадия алгоритма – посадка – активируется, когда целевая высота опускается ниже 0.4 метра. Флаг boarding переводится в True, то есть начинается посадка; публикуется нулевая скорость; вызывается сервиса TakeOffBoard с параметрами takeoff = False и board = True. После успешной посадки флаг self.takeoff сбрасывается, завершая работу узла.
python
if self.target_height < 0.4 and self.takeoff:
self.boarding = True
twist = Twist()
self.cmd_publisher.publish(twist)
self.get_logger().info("Target height is too low, start boarding")
self.call_takeoff_board_service(takeoff=False, board=True)
self.takeoff = FalseКласс PID реализует стандартный алгоритм пропорционально-интегрально-дифференциального регулирования. Программа работает в непрерывном цикле обработки событий ROS2, обеспечивая плавное и безопасное снижение дрона на платформу с коррекцией положения на каждом этапе посадки.
После завершения посадки останавливается узел и завершается работа всей инфраструктуры ROS2 для текущего процесса.
5.6.3. Примеры программ с цветовыми блобами
Перед запуском примеров выполняем настройку цветового блоба, который будет распознавать дрон. Проверяем раздел «Настройки» Графического интерфейса. Включаем «Обнаружение Блобов», переходим во вкладку «Блобы» и выполняем настройку цветового маркера: настраиваем компоненты HSV так, чтобы желаемый блоб распознавался полностью.
Настройка компонентов HSV рассмотрена в подразделе 3.6.1, а также в Приложении 3.

Проверка настроек цветового блоба
После настройки следует выполнить полет в ручном режиме и оценить корректность распознавания: убедиться, что блоб стабильно распознается, в его границы не попадают посторонние объекты. Примеры некорректно настроенного цветового блоба приведены на иллюстрации


Примеры распознавания блоба в полете при плохой настройке
Некорректная настройка блоба может привести к непредсказуемому поведению дрона при запуске примера, приведенного ниже. Следует выполнить донастройку HSV, чтобы в полете границы блоба совпадали с границами объекта.
На иллюстрации приведен пример распознавания блоба при хорошей настройке HSV: в кадре отсутствуют или незначительны шумы от объектов вокруг, блоб распознается с ближнего и дальнего расстояния и при обзоре с разных сторон.


Пример распознавания блоба в полете при корректной настройке HSV
Слежение за цветным объектом (Blob_follower.py)
Данный код реализует узел ROS2 для автономного слежения дрона за цветными объектами (блобами) с использованием компьютерного зрения и PID-регулирования. Система анализирует видеопоток и управляет дроном для поддержания заданного расстояния до цветного объекта.
Для завершения работы программы нажимаем сочетание клавиш Ctrl+C в терминале и выполняем посадку дрона в ручном режиме.
python
import rclpy
from rclpy.node import Node
from eagle_eye_msgs.msg import Target, BoundingBox
from eagle_eye_msgs.srv import TakeOffBoard
from geometry_msgs.msg import Twist
import time
# Команда для настройки автоэкспозиции камеры
# v4l2-ctl -c ae_low_limit=30,ae_high_limit=40
class BlobFollower(Node):
"""
Класс для управления дроном с целью следования за цветными объектами (блобами)
Использует PID-регуляторы для стабилизации движения и компьютерное зрение для обнаружения целей
"""
def __init__(self):
"""
Инициализация узла BlobFollower
Настраивает издателей, подписчиков, клиентов сервисов и PID-регуляторы
"""
super().__init__("blob_follower")
self.get_logger().info("Fixed Blob Follower node initialized")
# Создаем издателя для команд управления
self.cmd_publisher = self.create_publisher(Twist, "cmd_vel", 10)
# Создаем клиента для сервиса взлета/посадки
self.takeoff_board_client = self.create_client(TakeOffBoard, "/take_off_board")
# Подписываемся на топик с обнаруженными объектами
self.target_subscriber = self.create_subscription(Target, "/blobs", self.target_callback, 10)
# PID-регуляторы
self.pid_pitch = PID(0.003, 0.00001, 0.001)
self.pid_roll = PID(0.005, 0.0001, 0.005)
# Желаемая площадь объекта (в пикселях) для поддержания расстояния
self.target_area = 1000
# Мертвая зона
self.dead_zone_area = 50 # ±50 пикселей
self.dead_zone_center = 20 # ±20 пикселей по центру
# Центр изображения камеры
self.image_center_x = 160
self.image_center_y = 120
# Флаг готовности к полету
self.takeoff = False
# Переменная для хранения текущей цели
self.target = Target()
self.last_area = 0
self.last_center_x = 0
# Запускаем процедуру взлета
self.call_takeoff_board_service()
def target_callback(self, msg: Target):
"""
Callback-функция для обработки сообщений с обнаруженными объектами
Args:
msg (Target): Сообщение содержащее информацию об обнаруженных объектах
"""
if self.takeoff:
# Дрон готов к полету, обрабатываем обнаруженные объекты
self.target = msg
boxes = self.target.boxes
if len(boxes) > 0:
box = boxes[0]
self.get_logger().info(f"Box area: {box.area}, Center: ({box.center.x}, {box.center.y})")
# Вычисляем ошибки для PID-регуляторов
# Ошибка по горизонтали: разность между центром объекта и центром изображения
self.roll_error = box.center.x - self.image_center_x
# Ошибка по расстоянию: разность между желаемой и текущей площадью объекта
self.pitch_error = self.target_area - box.area
# Если площадь больше целевой - объект близко - нужно отлететь назад
# Если площадь меньще целевой - объект далеко - нужно подлететь вперед
self.last_area = box.area
self.last_center_x = box.center.x
# Применяем регулирование для следования за объектом
self.BlobRegulation(self.pitch_error, self.roll_error)
else:
self.get_logger().warn("No objects detected")
# Если объектов нет - останавливаемся
self.stop_movement()
else:
self.get_logger().info("Waiting for takeoff completion...")
def stop_movement(self):
cmd_msg = Twist()
self.cmd_publisher.publish(cmd_msg)
def constrain(self, value, threshold):
if value > threshold:
return threshold
if value < -threshold:
return -threshold
return value
def apply_dead_zone(self, error, dead_zone):
if abs(error) < dead_zone:
return 0.0
return error
def BlobRegulation(self, pitch_error, roll_error):
"""
Основная функция регулирования движения дрона для следования за объектом
Использует PID-регуляторы для вычисления команд управления
Args:
pitch_error (float): Ошибка по расстоянию (разность площадей)
roll_error (float): Ошибка по горизонтальному положению
"""
# Применяем мертвые зоны
pitch_error_clean = self.apply_dead_zone(pitch_error, self.dead_zone_area)
roll_error_clean = self.apply_dead_zone(roll_error, self.dead_zone_center)
# Максимальные значения PID
max_pid_linear = 0.5
max_pid_angular = 0.3
# Регулирование по расстоянию
if abs(pitch_error_clean) > 0:
self.pid_pitch.update_control(pitch_error_clean)
PID_PITCH = self.pid_pitch.get_control()
PID_PITCH = self.constrain(PID_PITCH, max_pid_linear)
else:
PID_PITCH = 0.0
# Регулирование по горизонтали
if abs(roll_error_clean) > 0:
self.pid_roll.update_control(roll_error_clean)
PID_ROLL = self.pid_roll.get_control()
PID_ROLL = self.constrain(PID_ROLL, max_pid_linear)
else:
PID_ROLL = 0.0
# Отладочная информация
self.get_logger().info(
f"Errors - Area: {self.last_area}, Pitch_err: {pitch_error:.1f}, "
f"Roll_err: {roll_error:.1f} | Controls - Pitch: {PID_PITCH:.3f}, Roll: {PID_ROLL:.3f}"
)
# Формируем команду для управления движением
cmd_msg = Twist()
cmd_msg.linear.x = PID_PITCH
cmd_msg.linear.y = PID_ROLL
cmd_msg.angular.z = 0.0
# Отправляем команду управления
self.cmd_publisher.publish(cmd_msg)
def call_takeoff_board_service(self):
"""
Инициирует процедуру взлета дрона через сервис
Отправляет асинхронный запрос на взлет и настраивает callback для обработки ответа
"""
# Ждем, пока сервис взлета/посадки станет доступным
while not self.takeoff_board_client.wait_for_service(timeout_sec=1.0):
self.get_logger().info("Waiting for take_off_board service...")
# Создаем запрос на взлет
request = TakeOffBoard.Request()
request.takeoff = True # Запрашиваем взлет
request.board = False # Не запрашиваем посадку
# Отправляем запрос асинхронно (не блокируем выполнение)
future = self.takeoff_board_client.call_async(request)
# Устанавливаем callback-функцию для обработки ответа сервиса
future.add_done_callback(self.takeoff_board_response_callback)
def takeoff_board_response_callback(self, future):
"""
Callback-функция для обработки ответа сервиса взлета/посадки
Args:
future: Объект Future содержащий результат вызова сервиса
"""
try:
response = future.result()
if response.result:
self.get_logger().info('Takeoff successful')
# Ждем 10 секунд для стабилизации дрона после взлета
time.sleep(10)
# Устанавливаем флаг готовности к полету
self.takeoff = True
self.stop_movement()
else:
self.get_logger().error('Takeoff failed')
except Exception as e:
self.get_logger().error(f'Service call failed: {e}')
class PID:
"""
Класс PID-регулятора для управления системой с обратной связью
Реализует пропорционально-интегрально-дифференциальное управление
"""
def __init__(self, Kp, Ti, Td):
"""
Инициализация PID-регулятора
Args:
Kp (float): Пропорциональный коэффициент
Ti (float): Интегральный коэффициент
Td (float): Дифференциальный коэффициент
"""
# Коэффициенты PID-регулятора
self.Kp = Kp
self.Td = Td
self.Ti = Ti
# Переменные для хранения ошибок
self.curr_error = 0 # Текущая ошибка
self.prev_error = 0 # Предыдущая ошибка
self.sum_error = 0 # Накопленная сумма ошибок (для интегральной составляющей)
self.prev_error_deriv = 0 # Предыдущая производная ошибки
self.curr_error_deriv = 0 # Текущая производная ошибки
self.control = 0 # Выходной сигнал регулятора
# Ограничение интегральной составляющей
self.max_integral = 1000
def update_control(self, current_error, reset_prev=False):
"""
Обновляет выходной сигнал PID-регулятора на основе текущей ошибки
Args:
current_error (float): Текущее значение ошибки
reset_prev (bool): Флаг сброса предыдущих значений (не используется)
"""
# Сохраняем предыдущую ошибку и обновляем текущую
self.prev_error = self.curr_error
self.curr_error = current_error
# Вычисляем интегральную составляющую (накопление ошибки) с ограничением
self.sum_error += self.curr_error
if abs(self.sum_error) > self.max_integral:
self.sum_error = self.max_integral if self.sum_error > 0 else -self.max_integral
# Вычисляем дифференциальную составляющую (скорость изменения ошибки)
self.curr_error_deriv = self.curr_error - self.prev_error
self.control = (self.Kp * self.curr_error +
self.Ti * self.sum_error +
self.Td * self.curr_error_deriv)
def get_control(self):
"""
Возвращает текущее значение управляющего сигнала
Returns:
float: Значение управляющего сигнала
"""
return self.control
def main(args=None):
"""
Главная функция для запуска узла BlobFollower
Args:
args: Аргументы командной строки (по умолчанию None)
"""
# Инициализация ROS2
rclpy.init(args=args)
# Создание экземпляра узла
node = BlobFollower()
try:
# Запуск основного цикла обработки сообщений
rclpy.spin(node)
except KeyboardInterrupt:
node.get_logger().info("Node stopped by user")
finally:
node.stop_movement()
# Корректное завершение работы
node.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()При инициализации узла создается экземпляр класса BlobFollower, который наследуется от базового класса Node в ROS2 под именем blob_follower. В конструкторе инициализируются все необходимые компоненты: издатель для отправки команд управления в топик «cmd_vel», клиент сервиса взлета/посадки «/take_off_board», а также подписчик на топик «/blobs» для получения данных об обнаруженных объектах от системы компьютерного зрения.
Далее инициализируются переменные:
pid_pitch и pid_roll – PID-регулятор по расстоянию до объекта (вперед/назад) и по горизонтальному смещения (влево/вправо) соответственно;
задаются dead_zone_area, dead_zone_center диапазоны ошибки, в которых управляющее воздействие обнуляется;
задаются image_center_x, image_center_y – координаты центра изображения камеры, относительно которых будет считаться ошибка;
переменные target, last_area, last_center_x для хранения последних данных об объекте (blob).
Обработка обнаруженных объектов происходит в callback-функции target_callback, которая обрабатывает сообщения типа Target, содержащие информацию об обнаруженных цветных объектах. При поступлении данных программа извлекает первый обнаруженный объект:
python
box = boxes[0]
self.get_logger().info(f"Box area: {box.area}, Center: ({box.center.x}, {box.center.y})")box_area – площадь из массива bounding boxes;
box.center.x, box.center.y – координаты центра объекта в пикселях.
Далее вычисляются два ключевых параметра для управления: горизонтальное смещение объекта относительно центра кадра (roll_error) и разницу между текущей площадью объекта и целевой площадью self.target_area = 1000 пикселей (pitch_error). Таким образом реализуется логика: если объект больше target_area, он ближе, чем нужно, следовательно необходимо отлететь назад, но, если объект меньше, то необходимо подлететь вперед.
Вспомогательная функция stop_movement публикует нулевые скорости для прекращения движения дрона при отсутствии обнаруженных цветовых объектов, что предотвращает неконтролируемое движение.
python
def stop_movement(self):
cmd_msg = Twist()
self.cmd_publisher.publish(cmd_msg)Рассчитанные ошибки отправляются в функцию выравнивания относительно объекта BlobRegulation. В ней реализован расчет управляющих скоростей с учетом ошибок, которые сначала проходят через метод apply_dead_zone.
python
# Применяем мертвые зоны
pitch_error_clean = self.apply_dead_zone(pitch_error, self.dead_zone_area)
roll_error_clean = self.apply_dead_zone(roll_error, self.dead_zone_center)
# Реализация зон нечувствительности
def apply_dead_zone(self, error, dead_zone):
if abs(error) < dead_zone:
return 0.0
return errorДля ошибки площади установлена мертвая зона ±50 пикселей, а для центрирования – ±20 пикселей. Если ошибки меньше этих значений, метод apply_dead_zone обнулит их, что позволит свести к минимуму управляющее воздействие. Это предотвращает постоянные колебания дрона при незначительных отклонениях и снижает износ двигателей.
Далее рассчитанное при помощи класса PID управляющее воздействие ограничивается по модулю (max_pid_linear = 0.5) при помощи функции constrain. После расчетов публикуются команды на движение:
python
cmd_msg = Twist()
cmd_msg.linear.x = PID_PITCH
cmd_msg.linear.y = PID_ROLL
cmd_msg.angular.z = 0.0
self.cmd_publisher.publish(cmd_msg)Одновременно задаются скорости по тангажу и крену, при этом вращение по оси z обнулено. Контроллер дрона, подписанный на cmd_vel, использует эти команды для движения так, чтобы удерживать объект в центре кадра и на нужной дистанции.
Класс PID реализует классический алгоритм ПИД-регулирования с дополнением – ограничением интегральной составляющей (max_integral=1000).
Система работает в непрерывном цикле обработки событий ROS2, постоянно корректируя положение дрона относительно цветного объекта. Реализована корректная обработка прерывания работы через KeyboardInterrupt, обеспечивающая остановку дрона перед завершением программы.