mirror of
https://github.com/CopterExpress/clever-show.git
synced 2026-05-26 23:19:33 +00:00
Merge pull request #79 from CopterExpress/animation-update
Animation update
This commit is contained in:
Arthur Golubtsov
73
docs/ru/animation.md
Normal file
73
docs/ru/animation.md
Normal file
@@ -0,0 +1,73 @@
|
||||
# Модуль анимации
|
||||
|
||||
За обработку анимации отвечает отдельный модуль [animation](../../drone/modules/animation.py). При загрузке анимации на коптер модуль производит разделение последовательности кадров анимации на 5 ключевых этапов:
|
||||
|
||||
1. Коптер неподвижен в начале анимации - `static_begin`
|
||||
2. Коптер взлетает - `takeoff`
|
||||
3. Коптер следует по маршруту анимации - `route`
|
||||
4. Коптер выполняет посадку - `land`
|
||||
5. Коптер неподвижен до завершения файла анимации - `static_end`
|
||||
|
||||
Кадр анимации - это набор данных, необходимых для позиционирования коптера и определения цвета его подсветки. В текущей версии ПО кадр анимации представлен последовательностью чисел `x y z yaw r g b` в строке `.csv` файла с анимацией, где:
|
||||
|
||||
* `x`, `y`, `z` - координаты коптера в текущем кадре в метрах
|
||||
* `yaw` - рыскание коптера в радианах
|
||||
* `r`, `g`, `b` - компоненты цвета подсветки коптера, целые числа от 0 до 255
|
||||
|
||||
После разделения анимации на ключевые этапы модуль формирует выходную последовательность кадров, определяющих положение коптера и цвет его подсветки, а также последовательность действий при полёте к первой точке анимации.
|
||||
|
||||
Настройка модуля производится в разделе [ANIMATION](client.md#раздел-animation).
|
||||
|
||||
Первичный отбор кадров производится с помощью набора флагов [[OUTPUT]], устанавливающих, какие последовательности кадров из 5 ключевых этапов будут использованы в полёте, а какие - нет.
|
||||
|
||||
Ключевым параметром, определяющим логику воспроизведения анимации является параметр `start_action`, определяющий первое действие при запуске воспроизведения анимации. Доступные варианты его значений:
|
||||
|
||||
* `auto` - автоматический выбор действия между `takeoff` (взлёт) или `fly` (мгновенный полёт по точкам) на основе текущего уровня высоты коптера. Если (`z` в начальной точке анимации) > (уровень взлета `takeoff_level`), то значение устанавливается в `takeoff`, иначе значение устанавливается в `fly`.
|
||||
* `fly` - выполнение *логики полёта по точкам*
|
||||
* `takeoff` - выполнение *логики взлёта к первой точке*.
|
||||
|
||||
Если в файле анимации коптер взлетает с земли, при старте анимации будет применена **логика полёта по точкам (fly)**: коптер с выключенными моторами воспроизводит цвет из анимации, пока неподвижен, включает двигатели перед моментом взлёта, затем через время `arming_time` начинает следовать точкам, указанным в анимации.
|
||||
|
||||
Если в файле анимации коптер начинает полёт в воздухе, при старте анимации будет применена **логики взлёта к первой точке (takeoff)**: коптер с выключенными моторами воспроизводит цвет из анимации, пока неподвижен, включает двигатели перед моментом взлёта, затем взлетает на высоту `takeoff_height` за время `takeoff_time`, затем перемещается к первой точке за время `reach_first_point_time`, и затем начинает следовать точкам, указанным в анимации.
|
||||
|
||||
Если параметр `start_action` установлен в `takeoff`, а флаг `takeoff` в разделе [[OUTPUT]] установлен в значение `True`, то последовательность кадров со взлётом коптера по точкам заменяется на 2 последовательных действия:
|
||||
|
||||
* взлёт относительно текущей позиции на высоту `takeoff_height` за время `takeoff_time`
|
||||
* полёт по прямой до начальной точки последовательности кадров маршрута коптера (`route`) за время `reach_first_point_time`
|
||||
|
||||
## Подготовка и загрузка анимации
|
||||
|
||||
Создайте анимацию объектов в [Blender](https://www.blender.org) или воспользуйтесь [примерами](../../examples/animations).
|
||||
|
||||
Коптер можно изобразить любым трёхмерным объектом (например кубом или шаром), а цвет подсветки ленты будет извлекаться при экспорте из свойства цвета объекта. При создании анимации учитывайте следующие факты и рекомендации:
|
||||
|
||||
* Для удобной конвертации и загрузки анимации на коптеры, объекты, соответствующие коптерам, должны иметь имена, соответствующие именам этих коптеров.
|
||||
* Условная единица расстояния в Blender конвертируется в метры.
|
||||
* Задержка между кадрами по-умолчанию в [настройках коптера](../../drone/config/spec/configspec_client.ini) равна 0.1 секунды (параметр `frame_delay` в разделе ANIMATION), будьте внимательны при настройке частоты кадров в анимации Blender.
|
||||
* Следите за скоростями коптеров, чтобы они были не слишком большими (для помещения - не более 3 м/с, для улицы - не более 5 м/с): аддон выдаст предупреждение, но всё равно сконвертирует анимацию.
|
||||
|
||||
Сконвертируйте анимацию с помощью [аддона для Blender](blender-addon.md).
|
||||
|
||||
Если в анимации несколько объектов и их имена соответствуют именам коптеров, загрузите папку с анимацией на выделенные в таблице коптеры с помощью команды `Send -> Animations` на [сервере](server.md#раздел-selected-drones).
|
||||
|
||||
Также любой файл анимации можно загрузить отдельно на все выделенные в таблице коптеры с помощью команды `Send -> Animation`.
|
||||
|
||||
## Анализ анимации
|
||||
|
||||
Если вам нужна информация о том, по каким точкам полетит коптер в результате загрузки анимации по текущим параметрам клиента, возпользуйтесь утилитой [animation_info](../../tools/animation_info.py).
|
||||
|
||||
```cmd
|
||||
usage: python animation_info.py [-h] [--config] [animation]
|
||||
|
||||
Get animation info
|
||||
|
||||
positional arguments:
|
||||
animation Path to animation. Default is
|
||||
../examples/animations/basic/basic.csv.
|
||||
|
||||
optional arguments:
|
||||
-h, --help show this help message and exit
|
||||
--config Set this option to print config info.
|
||||
```
|
||||
|
||||
Данная утилита выводит полную информацию о настройках анимации и конфигурации клиента (опционально), а также выводит оба возможных варианта воспроизведения анимации, что позволяет проанализировать действия коптеров перед реальным полётом.
|
||||
@@ -116,16 +116,19 @@
|
||||
После разделения анимации на ключевые этапы модуль формирует выходную последовательность кадров, определяющих положение коптера и цвет его подсветки, а также последовательность действий при полёте к первой точке анимации. Настройка модуля производится с помощью следующих параметров:
|
||||
|
||||
* `start_action` - первое действие при запуске воспроизведения анимации. Доступные варианты:
|
||||
* `auto` - автоматический выбор действия между `takeoff` (взлёт) или `fly` (мгновенный полёт по точкам) на основе текущего уровня высоты коптера. Если (`z` в начальной точке анимации) - (текущая высота коптера) > (уровень взлета `takeoff_level`), то значение устанавливается в `takeoff`, иначе значение устанавливается в `fly`.
|
||||
* `takeoff` - выполнение *логики полёта к первой точке*.
|
||||
* `auto` - автоматический выбор действия между `takeoff` (взлёт) или `fly` (мгновенный полёт по точкам) на основе текущего уровня высоты коптера. Если (`z` в начальной точке анимации) > (уровень взлета `takeoff_level`), то значение устанавливается в `takeoff`, иначе значение устанавливается в `fly`.
|
||||
* `fly` - выполнение *логики немедленного полёта*
|
||||
* `takeoff` - выполнение *логики полёта к первой точке*.
|
||||
|
||||
Если в файле анимации коптер взлетает с земли, при старте анимации будет применена *логика немедленного воспроизведения*: коптер включает двигатели через время, которое требуется для выполнения начальных кадров анимации, которые не включены в выходную последовательность, затем через время `arming_time` начинает следовать точкам, указанным в анимации. Если в файле анимации коптер начинает полёт в воздухе, при старте анимации будет применена *логика полёта к первой точке*: через время, которое требуется для выполнения начальных кадров анимации, которые не включены в выходную последовательность, коптер в начале взлетает на высоту `takeoff_height` за время `takeoff_time`, затем перемещается к первой точке за время `reach_first_point_time`, и затем начинает следовать точкам, указанным в анимации.
|
||||
Если в файле анимации коптер взлетает с земли, при старте анимации будет применена **логика немедленного воспроизведения (fly)**: коптер с выключенными моторами воспроизводит цвет из анимации, пока неподвижен, включает двигатели перед моментом взлёта, затем через время `arming_time` начинает следовать точкам, указанным в анимации.
|
||||
|
||||
Если в файле анимации коптер начинает полёт в воздухе, при старте анимации будет применена **логика полёта к первой точке (takeoff)**: коптер с выключенными моторами воспроизводит цвет из анимации, пока неподвижен, включает двигатели перед моментом взлёта, затем взлетает на высоту `takeoff_height` за время `takeoff_time`, затем перемещается к первой точке за время `reach_first_point_time`, и затем начинает следовать точкам, указанным в анимации.
|
||||
|
||||
* `takeoff_level` - уровень взлёта для автоматического определения первого действия коптера при старте анимации
|
||||
* `ground_level` - уровень земли, используется для проверки, не попытается ли коптер улететь под землю при воспроизведении анимации. Доступные варианты настройки:
|
||||
* `current` - за уровень земли принимается текущий уровень высоты коптера перед стартом.
|
||||
* координата z в метрах
|
||||
* `check_ground` - логическое значение, определяет, нужно ли производить проверку уровня земли в анимации.
|
||||
* `frame_delay` - время воспроизведения одного кадра в секундах.
|
||||
* `yaw` - поворот коптера при полёте по точкам, в градусах. Если значение `nan` - коптер сохраняет изначальную ориентацию в полёте. Если значение `animation` - коптер берёт поворот по yaw из файла с анимацией.
|
||||
* `ratio` - масштаб анимации (ratio_x, ratio_y, ratio_z) по осям (x, y, z)
|
||||
|
||||
178
docs/ru/positioning.md
Normal file
178
docs/ru/positioning.md
Normal file
@@ -0,0 +1,178 @@
|
||||
# Настройка систем позиционирования
|
||||
|
||||
ПО `clover` официально поддерживает работу со следующими [системами позиционирования](https://clover.coex.tech/ru/programming.html#positioning):
|
||||
|
||||
* [optical flow](https://clover.coex.tech/ru/optical_flow.html)
|
||||
* [aruco](https://clover.coex.tech/ru/aruco.html)
|
||||
* [gps](https://clover.coex.tech/ru/gps.html)
|
||||
|
||||
`clever-show` поддерживает все системы позиционирования, что и `clover`.
|
||||
|
||||
**Приведённые ниже примеры настроек предназначены для коптера Клевер 4, собранного и настроенного согласно [документации](https://clover.coex.tech)**.
|
||||
|
||||
## Организация процесса настройки
|
||||
|
||||
Перед групповым запуском коптеров рекомендуется настроить и проверить один коптер из группы и размножить его настройки на остальные коптеры.
|
||||
|
||||
Настройте один коптер на работу с любой системой позиционирования из перечисленных. Настройка может включать в себя следующие этапы:
|
||||
|
||||
* Редактирование `.launch` файлов ROS пакета `clover`. Данные файлы находятся в директории `/home/pi/catkin_ws/src/clover/clover/launch` на коптере.
|
||||
* Настройка параметров полётного контроллера.
|
||||
* Редактирование файла конфигурации [клиента](client.md) `clever-show`.
|
||||
* Редактирование файла конфигурации [сервера](server.md) `clever-show`.
|
||||
* [Калибровка камеры](https://clover.coex.tech/ru/camera_calibration.html).
|
||||
|
||||
Проверьте автономный взлёт коптера: для этого выделите в таблице только один коптер, полёт которого вы ходите проверить, и нажмите кнопку `Takeoff` в правой панели приложения [сервера](server.md#тестовые-команды). Коптер должен взлететь на высоту, указанную в параметре `takeoff_height` раздела FLIGHT в конфигурации коптера (1 метр по умолчанию), и удерживать свою позицию.
|
||||
|
||||
Если взлёт прошёл успешно, размножьте конфигурацию клиента, настройки позиционирования и параметры полётного контроллера на остальные коптеры. Выделите в таблице успешно настроенный коптер и загрузите необходимые файлы настроек с выделенного коптера к себе на компьютер:
|
||||
|
||||
* `.launch` файлы для настройки ПО `clover` можно сохранить с помощью команды `Selected drones -> Retrieve file` в приложении сервера. В открывшемся диалоговом окне введите путь к файлу на коптере: `launch` файлы `clover` находятся в `/home/pi/catkin_ws/src/clover/clover/launch/`, к этому пути нужно дописать нужное имя `.launch` файла, например `/home/pi/catkin_ws/src/clover/clover/launch/clover.launch`. После нажатия кнопки `OK` в диалоговом окне откроется новое диалоговое окно с выбором пути сохранения указанного файла.
|
||||
* Файл конфигурации коптера (`.ini`) можно сохранить, кликнув правой кнопкой мыши на строку с настроенным коптером, выбрав из выпадающего меню `Edit config`, затем нажав на кнопку `Save as`. Также можно перетянуть ячейку из столбца `configuration` в файловый менеджер вашей системы - сервер автоматически скопирует файл настройки в открытую директорию файлового менеджера.
|
||||
* Файл конфигурации сервера (`.ini`) можно сохранить, выбрав пункт `Server -> Edit server config` из верхнего меню, затем нажав на кнопку `Save as`.
|
||||
* Файл настройки полётного контроллера можно сохранить, подключившись к полётному контроллеру через приложение [QGroundControl](http://qgroundcontrol.com). Можно подключиться напрямую к полётному контроллеру [через USB порт](https://clover.coex.tech/ru/connection.html), либо через [TCP или UDP мост](https://clover.coex.tech/ru/gcs_bridge.html) (по умолчанию в образе `clever-show` настроен TCP мост, в графе `Host Address` вместо ip адреса можно ввести имя коптера с добавлением .local в конце, например clover-1.local). После подключения нужно перейти в [раздел](https://docs.px4.io/master/en/advanced_config/parameters.html#tools) `Parameters -> Tools -> Save to file...` и выбрать путь для сохранения файла параметров.
|
||||
* Файл калибровки камеры полезен для уточнения визуального позиционирования. Название файла калибровки должно состоять из id коптера, для которого была сделана калибровка, с добавлением расширения `.yaml`, например `clover-1.yaml`. Для получения файла калибровки возпользуйтесь [инструкцией](https://clover.coex.tech/ru/camera_calibration.html).
|
||||
|
||||
После загрузки необходимых файлов с настроенного коптера, скопируйте эти файлы на остальные коптеры: выделите нужные коптеры в таблице и воспользуйтесь командами `Send -> Configuration`, `Send -> Launch files folder`, `Send -> FCU parameters file`, `Send -> Camera calibrations` из раздела `Selected drones` приложения [сервера](server.md#раздел-selected-drones).
|
||||
|
||||
## Настройки сервера и клиента clever-show
|
||||
|
||||
Набор ПО `clever-show` включает в себя множество проверок состояния коптеров, чтобы свести к минимуму количество неудачных запусков, а также набор параметров для настройки систем позиционирования. Все параметры хранятся в файлах конфигурации приложений клиента и сервера. Каждая система позиционирования обладает своими особенностями, которые необходимо учесть при настройке взаимодействия сервера и клиента. Ниже перечислены настройки, на которые необходимо обратить внимание при настройке клиента и сервера:
|
||||
|
||||
* Сервер:
|
||||
* раздел [CHECKS](server.md#раздел-checks) - проверки телеметрии коптеров на стороне сервера
|
||||
|
||||
* Клиент:
|
||||
* раздел [FLIGHT](client.md#раздел-flight) - имя опорной системы координат frame_id, параметры полёта
|
||||
* раздел [FLOOR FRAME](client.md#раздел-floor-frame) - позволяет создать новую систему координат с названием `floor` относительно любой существующей системы координат:
|
||||
* `map` - совпадает с начальным положением коптера при использовании optical flow или gps
|
||||
* `aruco_map` - совпадает с началом координат карты ArUco маркеров
|
||||
* `gps` - начало координат находится в заданной GPS координате с поворотом на заданный угол относительно начального положения коптера, настраивается в разделе [GPS FRAME](client.md#раздел-gps-frame) и позволяет задать систему координат с общим началом для всех коптеров
|
||||
* раздел [FAILSAFE](client.mf#раздел-failsafe) - отключен по умолчанию, но позволяет настроить условия экстренной посадки коптера:
|
||||
* при потере визуальной позиции - полезно при использовании системы позиционирования по ArUco маркерам
|
||||
* при большой разнице между текущей позицией и точкой, где должен находиться коптер - с помощью данной проверки можно избежать непредвиденного поведения коптеров при столкновениях или любых физических неполадках
|
||||
* раздел [EMERGENCY LAND](client.md#раздел-emergency-land) - задаёт настройку экстренной посадки коптера: параметр `thrust` задаёт уровень газа моторов для начала посадки, через время `decrease_thrust_after` коптер начинает в постепенно снижать уровень газа до 0. **Внимание!** Уровень газа экстренной посадки по умолчанию равен 45% - эта настройка работает для коптера Клевер 4 с 3S аккумулятором. Если ваша конфигурация отличается, нужно предварительно определить газ висения, а затем установить параметр `thrust` в значение на 5% меньше, чем газ висения. В случае, если газ экстренной посадки превышает газ висения коптера, коптер при экстренной посадке первые 3 секунды (значение `decrease_thrust_after` по умолчанию) будет лететь вверх и только после этого начнет плавно снижать мощность моторов до 0.
|
||||
|
||||
## Optical flow
|
||||
|
||||
Optical flow - способ позиционирования с помощью расчёта скоростей движения камеры по вычислению сдвига пикселей между соседними кадрами.
|
||||
|
||||
`Optical flow` подходит для демонстрации полёта одного коптера или же для синхронного полёта нескольких коптеров по одной и той же траектории внутри помещения. Однако следует учитывать, что данная система координат не задаёт общее начало координат для всех коптеров - началом координат является стартовое положение каждого конкретного коптера. Также стоит принять во внимание тот факт, что данный способ рассчитывает позицию коптера по скорости перемещения его камеры - а значит в позиции коптера постоянно накапливается ошибка и позиция становится менее точной со временем. Поэтому не рекомендуется использовать эту систему координат для сложных и продолжительных групповых полётов.
|
||||
|
||||
Образ `clever-show` для коптера настроен по умолчанию на полёт по данной системе позиционирования (как и образ `clover`) - на коптере должен быть установлер лазерный дальномер, а камера должна быть наклонена вниз шлейфом назад.
|
||||
|
||||
ROS пакет clover, настроенный по умолчанию предполагает автономный полёт с использованием `optical flow`. Параметры, загружаемые по умолчанию при загрузке прошивки, адаптированной для Клевер 4, предполагают позиционирование по `optical flow`. Файлы конфигураций клиента и сервера, настроенные по умолчанию, также не требуют измений для полёта по данной системе позиционирования.
|
||||
|
||||
Однако, если вам требуется перейти на систему позиционирования `optical flow` с другой предварительно настроенной системы или же изменить настройки данной системы, вам поможет информация, расположенная ниже.
|
||||
|
||||
Все файлы настроек для конфигурации `optical flow` находятся в папке [examples/positioning/optical flow](../../examples/positioning/optical%20flow/).
|
||||
|
||||
### Настройка ROS пакета clover
|
||||
|
||||
Настройка позиционирования по `optical flow` описана в [документации](https://clover.coex.tech/ru/optical_flow.html) `clover`.
|
||||
|
||||
Пример `.launch` файла для настройки: [clover.launch](../../examples/positioning/optical%20flow/launch/clover.launch).
|
||||
|
||||
### Настройка полётного контроллера
|
||||
|
||||
Параметры, настраивающие полётный контроллер на возможность полёта по `optical flow`: [optical_flow.params](../../examples/positioning/optical%20flow/optical_flow.params).
|
||||
|
||||
Для загрузки параметров на выделенные в таблице коптеры воспользуйтесь командой `Selected drones -> Send -> FCU parameters file` из верхнего меню и укажите путь к файлу с параметрами полётного контроллера.
|
||||
|
||||
### Настройка клиента
|
||||
|
||||
#### Вариант 1. Загрузка значимых параметров из примера конфигурации
|
||||
|
||||
Конфигурация клиента со значениями, применимыми для работы с `optical flow`: [client.ini](../../examples/positioning/optical%20flow/client.ini).
|
||||
|
||||
Для загрузки воспользуйтесь командой `Selected drones -> Send -> Configuration` из верхнего меню сервера. В открывшемся диалоговом окне выберите вариант `Modify` и укажите путь к файлу `client.ini`.
|
||||
|
||||
#### Вариант 2. Сброс всех настроек клиента
|
||||
|
||||
Для того, чтобы настроить конфигурацию клиента для позиционирования по optical flow, можно сбросить данные конфигурации в значения по умолчанию. Для этого нужно удалить сгенерированные файлы конфигураций на клиентах:
|
||||
|
||||
* Выделите в таблице коптер или коптеры, конфигурацию которых хотите сбросить.
|
||||
* Выполните команду удаления конфигурации на выделенных коптерах, выбрав пункт `Selected drones -> Send -> Command`. В открывшемся диалоговом окне введите команду `rm config/client.ini` и нажмите `OK`.
|
||||
|
||||
### Настройка сервера
|
||||
|
||||
#### Вариант 1. Загрузка значимых параметров из примера конфигурации
|
||||
|
||||
Конфигурация сервера со значениями, применимыми для работы с `optical flow`: [server.ini](../../examples/positioning/optical%20flow/server.ini).
|
||||
|
||||
Для установки параметров вручную воспользуйтесь командой `Server -> Edit config` из верхнего меню сервера.
|
||||
|
||||
#### Вариант 2. Сброс всех настроек сервера
|
||||
|
||||
Для того, чтобы настроить конфигурацию сервера для позиционирования по optical flow, можно сбросить данные конфигурации в значения по умолчанию. Для этого нужно удалить сгенерированный файл конфигурации на сервере:
|
||||
|
||||
* Выполните команду `rm config/server.ini` из директории с расположением приложения `server.py`.
|
||||
* Перезагрузите сервер, выбрав команду `Server -> Restart server` из верхнего пункта меню.
|
||||
|
||||
## Aruco
|
||||
|
||||
ArUco-маркеры — это популярная технология для позиционирования робототехнических систем с использованием компьютерного зрения. Позиционирование происходит с помощью получения информации о расположении специальных визуальных маркеров.
|
||||
|
||||
Данная система позиционирования вариативна: маркеры могут располагаться на полу, на потолке или на стенах. Главное условие для позиционирования - правильно внести координаты в специальную карту маркеров и обозначить её наклон относительно пола.
|
||||
|
||||
Если карта маркеров расположена на полу, появляются нюансы: сразу после включения коптер не знает своего положения до тех пор, пока камера не увидит карту меток. Соответственно некоторые проверки по позиции коптера и безопасности полёта становятся не применимы:
|
||||
|
||||
* Не имеет смысла проверка текущего положения коптера, т.к. сразу после загрузки оно не определено, но коптер при этом всё равно имеет механизм взлёта
|
||||
* Не имеет смысла проверять максимальное расстояние стартовой точки анимации от текущей позиции коптера, т.к. она никогда не будет совпадать при старте
|
||||
* Стартовое действие анимации `fly` в системе координат `aruco_map` будет иметь непредсказуемые последствия, т.к. на старте реальная позиция коптера будет сильно отличаться от позиции, рассчитанной полётным контроллером. Соответственно единственный вариант параметра `start_action` в разделе [ANIMATION] - это `takeoff`: подъём на высоту, определённую параметром `takeoff_height` из раздела [FLIGHT], относительно текущей позиции коптера.
|
||||
|
||||
### Настройка ROS пакета clover
|
||||
|
||||
Настройка системы позиционирования по ArUco маркерам описана в [документации](https://clover.coex.tech/ru/aruco_map.html) `clover`.
|
||||
|
||||
Пример `.launch` файлов для настройки карты меток на полу:
|
||||
|
||||
* [clover.launch](../../examples/positioning/aruco%20floor/launch/clover.launch)
|
||||
* [aruco.launch](../../examples/positioning/aruco%20floor/launch/aruco.launch)
|
||||
|
||||
### Настройка полётного контроллера
|
||||
|
||||
Параметры, настраивающие полётный контроллер на возможность полёта по `ArUco`: [aruco.params](../../examples/positioning/aruco%20floor/aruco.params).
|
||||
|
||||
Для загрузки параметров на выделенные в таблице коптеры воспользуйтесь командой `Selected drones -> Send -> FCU parameters file` из верхнего меню и укажите путь к файлу с параметрами полётного контроллера.
|
||||
|
||||
### Настройка клиента
|
||||
|
||||
Конфигурация клиента со значениями, применимыми для работы с `Aruco`: [client.ini](../../examples/positioning/aruco%20floor/client.ini).
|
||||
|
||||
Для загрузки воспользуйтесь командой `Selected drones -> Send -> Configuration` из верхнего меню сервера. В открывшемся диалоговом окне выберите вариант `Modify` и укажите путь к файлу `client.ini`.
|
||||
|
||||
### Настройка сервера
|
||||
|
||||
Конфигурация сервера со значениями, применимыми для работы с `ArUco`: [server.ini](../../examples/positioning/aruco%20floor/server.ini).
|
||||
|
||||
Для установки параметров вручную воспользуйтесь командой `Server -> Edit config` из верхнего меню сервера.
|
||||
|
||||
## GPS
|
||||
|
||||
Позиционирование по спутниковым координатам - предпочтительный способ позиционирования для уличных полётов. Для позиционирования по GPS необходим [один из официально поддерживаемых модулей для PX4](https://docs.px4.io/v1.9.0/en/gps_compass/). Точность позиционирования по GPS составляет порядка одного метра. Использование наземной станции поправок [GPS RTK](https://docs.px4.io/v1.9.0/en/gps_compass/rtk_gps.html) и специальных GPS RTK модулей позволяет добиться дециметровой точности позиционирования.
|
||||
|
||||
### Настройка ROS пакета clover
|
||||
|
||||
Настройка позиционирования по `GPS` описана в [документации](https://clover.coex.tech/ru/gps.html) `clover`.
|
||||
|
||||
Пример `.launch` файла для настройки: [clover.launch](../../examples/positioning/gps/launch/clover.launch).
|
||||
|
||||
### Настройка полётного контроллера
|
||||
|
||||
Параметры, настраивающие полётный контроллер на возможность полёта по `gps`: [gps.params](../../examples/positioning/gps/gps.params).
|
||||
|
||||
Для загрузки параметров на выделенные в таблице коптеры воспользуйтесь командой `Selected drones -> Send -> FCU parameters file` из верхнего меню и укажите путь к файлу с параметрами полётного контроллера.
|
||||
|
||||
### Настройка клиента
|
||||
|
||||
Внимание! Начальная точка позиционирования фрейма `gps` должна быть изменена перед загрузкой. Для примера выбрана точка тестовой полётной зоны на территории Технополиса Москва.
|
||||
|
||||
Конфигурация клиента со значениями, применимыми для работы с `GPS`: [client.ini](../../examples/positioning/gps/client.ini).
|
||||
|
||||
Для загрузки воспользуйтесь командой `Selected drones -> Send -> Configuration` из верхнего меню сервера. В открывшемся диалоговом окне выберите вариант `Modify` и укажите путь к файлу `client.ini`.
|
||||
|
||||
### Настройка сервера
|
||||
|
||||
Конфигурация сервера со значениями, применимыми для работы с `GPS`: [server.ini](../../examples/positioning/gps/server.ini).
|
||||
|
||||
Для установки параметров вручную воспользуйтесь командой `Server -> Edit config` из верхнего меню сервера.
|
||||
@@ -1,11 +1,11 @@
|
||||
# Быстрый старт на квадрокоптере Клевер
|
||||
# Быстрый старт на квадрокоптере Клевер 4
|
||||
|
||||
## Список оборудования
|
||||
|
||||
Данное ПО предназначено для управления несколькими квадрокоптерами с компьютера-сервера. Для полноценной работы необходимо следующее оборудование:
|
||||
`clever-show` - это набор ПО для управления несколькими квадрокоптерами по сети. Для полноценной работы необходимо следующее оборудование:
|
||||
|
||||
* Один или несколько квадрокоптеров, работающих на базе ПО [Клевер](https://github.com/CopterExpress/clover).
|
||||
* Компьютер с операционной системой Ubuntu 18.04.
|
||||
* Один или несколько квадрокоптеров Клевер 4, работающих на базе ПО [clover](https://github.com/CopterExpress/clover).
|
||||
* Компьютер с операционной системой Ubuntu 18.04 для управления квадрокоптерами с помощью серверного приложения.
|
||||
* Wifi роутер, работающий на частоте 2.4 ГГц, либо 5.8 ГГц, если эту частоту поддерживают wifi модули коптеров и компьютера.
|
||||
|
||||
## Подготовка ПО
|
||||
@@ -27,7 +27,7 @@
|
||||
## Установка и запуск клиента
|
||||
|
||||
* Запишите образ на microSD карту, используя [Etcher](https://www.balena.io/etcher/).
|
||||
* Вставьте флешку в Raspberry Pi, включите коптер. Дождитесь появления сети `clever-show-XXXX`.
|
||||
* Вставьте флешку в Raspberry Pi на коптере и включите его. Дождитесь появления сети `clever-show-XXXX`.
|
||||
* Подключитесь к сети коптера, используя пароль `cloverwifi`.
|
||||
* Подключитесь к Raspberry Pi на коптере с помощью ssh, используя статический ip `192.168.11.1`, имя пользователя `pi` и пароль `raspberry`.
|
||||
|
||||
@@ -41,9 +41,9 @@ ssh pi@192.168.11.1
|
||||
sudo client-setup <SSID> <password> <copter name>
|
||||
```
|
||||
|
||||
* Теперь при запуске серверного приложения настроенные коптеры будут отображаться в виде таблицы. Также можно подключаться к Raspberry Pi на коптере по его имени с добавкой .local через `ssh` в указанной при настройке wifi сети, например `ssh pi@clover-1.local`, пароль `raspberry`.
|
||||
* Теперь при запуске серверного приложения настроенные коптеры будут отображаться в виде строк в таблице. Также можно подключаться к Raspberry Pi на коптере по его имени с добавкой .local через `ssh` в указанной при настройке wifi сети, например `ssh pi@clover-1.local`, пароль `raspberry`.
|
||||
|
||||
> **Подробная документация по настройке клиентской части находится [здесь](client.md).**
|
||||
**Подробная документация по настройке клиентской части находится [здесь](client.md).**
|
||||
|
||||
## Установка и запуск сервера
|
||||
|
||||
@@ -74,18 +74,67 @@ cd clever-show/server
|
||||
python3 server.py
|
||||
```
|
||||
|
||||
> **Подробная документация по настройке серверной части находится [здесь](server.md).**
|
||||
* Через некоторое время коптеры с настроенным образом подключатся к серверу и отобразятся в виде строк в таблице.
|
||||
|
||||
## Подготовка дрона
|
||||
**Подробная документация по настройке серверной части находится [здесь](server.md).**
|
||||
|
||||
Для запуска анимации все коптеры должны иметь настроенную систему позиционирования. По-умолчанию в образе настроен полёт по optical flow - на коптере должен быть установлер лазерный дальномер, а камера должна быть наклонена вниз шлейфом назад.
|
||||
## Подготовка коптера
|
||||
|
||||
Дальнейшие инструкции написаны для коптеров, элементы которых расположены согласно [инструкции по сборке](https://clover.coex.tech/ru/assemble_4.html). Полётный контроллер коптера должен быть предварительно настроен и откалиброван согласно [статьям по настройке](https://clover.coex.tech/ru/setup.html):
|
||||
|
||||
* Первоначальная настройка
|
||||
* Калибровка датчиков
|
||||
* Настройка пульта
|
||||
* Полётные режимы
|
||||
|
||||
Перед тем как приступать к настройке программного обеспечения, проверьте, что коптер управляется с пульта в режиме `Stabilized`.
|
||||
|
||||
Включите коптер и запустите серверное приложение на компьютере. Дождитесь подключения коптера к серверу и отображения данных его телеметрии в таблице.
|
||||
|
||||
### Проверка позиционирования
|
||||
|
||||
Для автономного воспроизведения анимации все коптеры должны иметь настроенную систему позиционирования. Образ `clever-show` для коптера настроен по умолчанию на полёт с использованием `optical flow`: на коптере должен быть установлен лазерный дальномер, а камера должна быть наклонена вниз шлейфом назад. Данная система позиционирования подходит для демонстрации полёта одного коптера или же для синхронного полёта нескольких коптеров по одной и той же анимации внутри помещения.
|
||||
|
||||
Перед проверкой автономного взлёта проведите автоматическую проверку корректности настроек коптера согласно [статье](https://clover.coex.tech/ru/selfcheck.html).
|
||||
|
||||
Проверьте, что коптер удерживает позицию автономно: отметьте чекбокс около названия коптера и нажмите кнопку Takeoff в правой панели интерфейса сервера. Коптер должен взлететь на высоту, указанную в параметре `takeoff_height` раздела FLIGHT [конфигурации клиента](../../drone/config/spec/configspec_client.ini). По умолчанию эта высота равна 1 метр. Если коптер взлетел и удерживает позицию на высоте 1 метр, проверка пройдена. Посадите коптер на землю, нажав на кнопку `Land` или `Land All`. **Внимание!** Для вашей безопасности рекомендуется проводить проверку автономного взлёта с включенным пультом и возможностью перехвата коптера в режим ручного управления.
|
||||
|
||||
Вы можете настроить коптер на другую систему позиционирования. Официально поддерживаются следующие [системы позиционирования](https://clover.coex.tech/ru/programming.html#positioning):
|
||||
|
||||
* optical flow
|
||||
* aruco
|
||||
* gps
|
||||
|
||||
**Подробная информация про работу с системами позиционирования и их настройку находится [здесь](positioning.md).**
|
||||
|
||||
> После тестирования образа версии 0.3 выяснилась неработоспособность конфигурации системы позиционирования по умолчанию (optical flow). Для нормального функционирования требуется система позиционирования с постоянным потоком данных через топик визуальной позиции. Рекомендуемая конфигурация для образа 0.3 - [потолочные aruco маркеры](https://clever.coex.tech/ru/aruco_map.html#расположение-маркеров-на-потолке).
|
||||
|
||||
Вы можете настроить один коптер на любую систему позиционирования, которую поддерживает пакет [clever](https://clever.coex.tech/ru/programming.html#positioning), а затем размножить настройки на остальные коптеры с [сервера](server.md#раздел-server) с помощью команды `Send launch files`.
|
||||
### Проверка работы светодиодной ленты
|
||||
|
||||
## Подготовка анимации
|
||||
Светодиодная лента должна быть подключена к порту GPIO 21 Raspberry Pi и иметь не более 60 светодиодов для работы с настроенным по умолчанию образом `clever-show`. Проверьте работу ленты, выделив нужный коптер в таблице и нажав кнопку `Test leds` - лента на коптере должна 2 раза мигнуть белым цветом.
|
||||
|
||||
* Создайте анимацию объектов в Blender или воспользуйтесь [примерами](../../examples/animations). Условная единица расстояния в Blender конвертируется в метры. Задержка между кадрами по-умолчанию в [настройках коптера](../../drone/config/client.ini) равна 0.1 секунды (параметр frame_delay в разделе ANIMATION), будьте внимательны при настройке частоты кадров в анимации Blender. Следите за скоростями коптеров, чтобы они были не слишком большими: аддон выдаст предупреждение, но всё равно сконвертирует анимацию.
|
||||
* Сконвертируйте анимацию с помощью [аддона для Blender](blender-addon.md).
|
||||
* Загрузите анимацию с помощью команды `Send animations` на [сервере](server.md#раздел-server).
|
||||
Описание настройки и работы со светодиодной ленты находится в [документации](https://clover.coex.tech/ru/leds.html) `clover`.
|
||||
|
||||
### Синхронизация времени
|
||||
|
||||
Для корректного воспроизведения анимации очень важна синхронизация времени между всеми коптерами, участвующими в анимации, и сервером. Чем точнее будет синхронизировано время, тем более согласованным будет полёт группы коптеров. В качестве инструмента синхронизации времени рекомендуется успользовать сервис [chrony](https://chrony.tuxfamily.org). Процесс установки и настройки данного сервиса для сервера описан [выше](#установка-и-запуск-сервера), в образе `clever-show` данный сервис уже установлен.
|
||||
|
||||
После первого подключения коптера к серверу, сервис `chrony` в коптере автоматически настраивается на подключение по ip адресу сервера и перезагружается. Однако на сервере сервис `chrony` может перестать посылать пакеты синхронизации времени при смене wifi сети и время между коптерами и сервером перестанет синхронизироваться. Разница между временем, пришедшим с коптера, и временем сервера отображается в столбце `dt` в таблице сервера. Нормальный уровень разницы по времени должен быть **не больше 0.1 секунды** (порядка 0.01 секунды), однако может быть и больше вследствие сетевых задержек при передаче телеметрии с коптера. Если разница по времени больше 0.1 секунды, рекомендуется перезапустить сервис `chrony` с помощью команды из верхнего меню сервера `Selected drones -> Restart service -> chrony`. Данная команда перезагружает сервис синхронизации времени на сервере (потребуется ввести пароль пользователя) и на коптерах.
|
||||
|
||||
## Подготовка и запуск анимации
|
||||
|
||||
По умолчанию в клиент уже загружена анимация [basic](../../examples/animations/basic/basic.csv):
|
||||
|
||||
<img src="../../examples/animations/basic/basic.gif" width="400px" alt="basic animation">
|
||||
|
||||
Красная линия - ось x, зелёная - ось y. Куб в анимации двигается в положительном направлении по оси x. Модуль воспроизведения анимации проведёт коптер по точкам, указанным в файле анимации, относительно системы координат, заданной в настройке `frame_id` раздела FLIGHT [конфигурации клиента](../../drone/config/spec/configspec_client.ini) (по умолчанию `map`). При этом коптер запустит двигатели перед взлётом и выключит их после посадки. Момент взлёта и посадки коптера определяется автоматически.
|
||||
|
||||
Информация о текущем положении коптера указана в столбце `current x y z yaw frame_id` таблицы сервера. Информация о стартовой точке анимации и времени, через которое коптер включит моторы, указана в столбце `start x y z yaw action delay`. Для первой проверки анимации важно, чтобы координаты в этих столбцах совпадали. Если это не так, самый простой способ решить эту проблему - перезагрузить коптер и дождаться его загрузки.
|
||||
|
||||
Проверьте воспроизведение анимации, нажав кнопку `Start animation`: первые две секунды коптер будет изменять цвет ленты, затем запустит моторы, взлетит на 1 метр вверх, затем пролетит 1 метр вправо и начнёт опускаться вниз. После касания земли в анимации коптер перейдёт в режим посадки, заглушит двигатели, и продолжит менять цвет светодиодной ленты до окончания анимации.
|
||||
|
||||
Результат выполнения анимации должен выглядеть так (с точностью до настройки коптера):
|
||||
|
||||
<img src="../../examples/animations/basic/basic_real.gif" width="400px" alt="basic animation">
|
||||
|
||||
**Подробная информация по работе модуля анимации находится [здесь](animation.md).**
|
||||
|
||||
134
drone/client.py
134
drone/client.py
@@ -130,7 +130,9 @@ class CopterClient(client_core.Client):
|
||||
if self.config.clover_dir == 'auto':
|
||||
self.check_clover_dir()
|
||||
self.telemetry = None
|
||||
self.animation = animation.Animation(self.config, "animation.csv")
|
||||
self.animation = animation.Animation("animation.csv", self.config)
|
||||
self.gps_thread_run = False
|
||||
self.gps_thread_is_running = False
|
||||
|
||||
def load_config(self):
|
||||
super(CopterClient, self).load_config()
|
||||
@@ -166,6 +168,15 @@ class CopterClient(client_core.Client):
|
||||
client_thread.daemon = True
|
||||
client_thread.start()
|
||||
|
||||
def on_config_update(self):
|
||||
self.gps_thread_run = False
|
||||
self.load_config()
|
||||
self.animation.on_config_update(self.config)
|
||||
if self.config.flight_frame_id == "floor":
|
||||
self.start_floor_frame_broadcast()
|
||||
elif self.config.flight_frame_id == "gps":
|
||||
self.start_gps_frame_broadcast()
|
||||
|
||||
def start_floor_frame_broadcast(self):
|
||||
if self.config.floor_frame_parent == "gps":
|
||||
self.start_gps_frame_broadcast()
|
||||
@@ -194,8 +205,13 @@ class CopterClient(client_core.Client):
|
||||
gps_frame_thread.start()
|
||||
|
||||
def gps_frame_broadcast_loop(self):
|
||||
while self.gps_thread_is_running:
|
||||
rospy.sleep(1)
|
||||
logger.info("Wait until previous gps thread stop")
|
||||
self.gps_thread_run = True
|
||||
self.gps_thread_is_running = True
|
||||
rate = rospy.Rate(1)
|
||||
while not rospy.is_shutdown():
|
||||
while not rospy.is_shutdown() and self.gps_thread_run:
|
||||
telem = flight.get_telemetry_locked(frame_id = "map")
|
||||
if telem is not None:
|
||||
if math.isnan(telem.lat) or math.isnan(telem.lon) or math.isnan(telem.x) or math.isnan(telem.y):
|
||||
@@ -220,6 +236,7 @@ class CopterClient(client_core.Client):
|
||||
static_broadcaster.sendTransform(trans)
|
||||
|
||||
rate.sleep()
|
||||
self.gps_thread_is_running = False
|
||||
|
||||
|
||||
def restart_service(name):
|
||||
@@ -470,7 +487,8 @@ def _command_test(*args, **kwargs):
|
||||
|
||||
@messaging.message_callback("update_animation")
|
||||
def _command_update_animation(*args, **kwargs):
|
||||
copter.animation.update_frames(copter.config, "animation.csv")
|
||||
pass # should be updated via watchdog event
|
||||
#copter.animation.update_frames(copter.config, "animation.csv")
|
||||
|
||||
|
||||
@messaging.message_callback("move_start")
|
||||
@@ -478,7 +496,7 @@ def _command_move_start_to_current_position(*args, **kwargs):
|
||||
private_offset = copter.config.animation_private_offset
|
||||
offset = numpy.array(private_offset) + numpy.array(copter.config.animation_common_offset)
|
||||
try:
|
||||
xs, ys, zs = copter.animation.get_start_point(copter.config.animation_ratio, offset)
|
||||
xs, ys, zs = copter.animation.get_start_frame(copter.telemetry.start_action).get_pos()
|
||||
except ValueError:
|
||||
logger.error("Can't get start point. Check animation file!")
|
||||
else:
|
||||
@@ -670,84 +688,27 @@ def _play_animation(*args, **kwargs):
|
||||
return
|
||||
|
||||
# Get output frames
|
||||
offset = numpy.array(copter.config.animation_private_offset) + numpy.array(copter.config.animation_common_offset)
|
||||
frames = copter.animation.get_scaled_output(copter.config.animation_ratio, offset)
|
||||
frames = copter.animation.get_output_frames(copter.telemetry.start_action)
|
||||
if not frames:
|
||||
logger.error("start: No frames in animation!")
|
||||
return
|
||||
|
||||
# Get current telemetry
|
||||
telem = copter.telemetry.ros_telemetry
|
||||
if not valid([telem.x, telem.y, telem.z]):
|
||||
logger.error("start: Position is not valid!")
|
||||
return
|
||||
# telem = copter.telemetry.ros_telemetry
|
||||
# if not valid([telem.x, telem.y, telem.z]):
|
||||
# logger.error("start: Position is not valid!")
|
||||
# return
|
||||
|
||||
# Get start action and delay
|
||||
try:
|
||||
start_action, start_delay = copter.telemetry.start_position[-2:]
|
||||
except ValueError:
|
||||
logger.error("start: Can't get animation start position and delay")
|
||||
return
|
||||
# Reset task manager
|
||||
task_manager.reset(interrupt_next_task=False)
|
||||
|
||||
# Set animation logic
|
||||
if start_action == 'takeoff':
|
||||
# Takeoff first at start_time + start_delay_time
|
||||
takeoff_time = start_time + start_delay
|
||||
task_manager.add_task(takeoff_time, 0, animation.takeoff,
|
||||
task_kwargs={
|
||||
"z": copter.config.flight_takeoff_height,
|
||||
"timeout": copter.config.flight_takeoff_time,
|
||||
"safe_takeoff": False,
|
||||
"use_leds": copter.config.led_use & copter.config.led_takeoff_indication,
|
||||
})
|
||||
# Fly to first point
|
||||
rfp_time = takeoff_time + copter.config.flight_takeoff_time
|
||||
task_manager.add_task(rfp_time, 0, animation.execute_frame,
|
||||
task_kwargs={
|
||||
"frame": frames[0],
|
||||
"frame_id": copter.config.flight_frame_id,
|
||||
"use_leds": copter.config.led_use,
|
||||
"flight_func": flight.reach_point,
|
||||
})
|
||||
# Calculate first frame start time
|
||||
frame_time = rfp_time + copter.config.flight_reach_first_point_time
|
||||
|
||||
elif start_action == 'fly':
|
||||
# Calculate start time
|
||||
arm_time = start_time + start_delay # + 1.0
|
||||
task_manager.add_task(arm_time, 0, animation.execute_frame,
|
||||
task_kwargs={
|
||||
"frame": frames[0],
|
||||
"frame_id": copter.config.flight_frame_id,
|
||||
"use_leds": copter.config.led_use,
|
||||
"flight_func": flight.navto,
|
||||
"auto_arm": True,
|
||||
})
|
||||
# Calculate first frame start time
|
||||
frame_time = arm_time + copter.config.flight_arming_time
|
||||
logger.debug("Start queue {}".format(task_manager.task_queue))
|
||||
# Play animation file
|
||||
# Play animation!
|
||||
frame_time = start_time
|
||||
for frame in frames:
|
||||
task_manager.add_task(frame_time, 0, animation.execute_frame,
|
||||
task_kwargs={
|
||||
"frame": frame,
|
||||
"frame_id": copter.config.flight_frame_id,
|
||||
"use_leds": copter.config.led_use,
|
||||
"flight_func": flight.navto,
|
||||
"config": copter.config,
|
||||
})
|
||||
frame_time += frame.delay
|
||||
# Calculate land_time
|
||||
land_time = frame_time + copter.config.flight_land_delay
|
||||
# Land
|
||||
task_manager.add_task(land_time, 0, animation.land,
|
||||
task_kwargs={
|
||||
"timeout": copter.config.flight_land_timeout,
|
||||
"frame_id": copter.config.flight_frame_id,
|
||||
"use_leds": copter.config.led_use & copter.config.led_land_indication,
|
||||
})
|
||||
|
||||
task_manager.add_task(frame_time, 0, animation.turn_off_led)
|
||||
|
||||
# noinspection PyAttributeOutsideInit
|
||||
class Telemetry:
|
||||
@@ -774,6 +735,7 @@ class Telemetry:
|
||||
self._max_interruptions = 2
|
||||
self._tasks_cleared = False
|
||||
self.ros_telemetry = None
|
||||
self.start_action = None
|
||||
|
||||
for key, value in self.params_default_dict.items():
|
||||
setattr(self, key, value)
|
||||
@@ -801,22 +763,18 @@ class Telemetry:
|
||||
return "{} V{}".format(copter.config.config_name, copter.config.config_version)
|
||||
|
||||
def get_start_position(self):
|
||||
offset = numpy.array(copter.config.animation_private_offset) + numpy.array(copter.config.animation_common_offset)
|
||||
try:
|
||||
x, y, z = copter.animation.get_start_point(copter.config.animation_ratio, offset)
|
||||
except ValueError:
|
||||
return [float('nan'),float('nan'),float('nan'),float('nan'),'error: no start pos in animation',float('nan')]
|
||||
x, y, z = copter.animation.get_start_frame('fly').get_pos()
|
||||
except (ValueError, AttributeError):
|
||||
return [float('nan'),float('nan'),float('nan'),float('nan'),"error: can't get start pos in animation",float('nan')]
|
||||
else:
|
||||
start_delay = copter.animation.start_delay
|
||||
yaw = copter.animation.get_start_yaw()
|
||||
start_delay = copter.animation.start_time
|
||||
yaw = copter.animation.get_start_frame('fly').yaw
|
||||
if not self.ros_telemetry:
|
||||
start_action = 'error: no telemetry data'
|
||||
self.start_action = 'error: no telemetry data'
|
||||
else:
|
||||
start_action = copter.animation.get_start_action(
|
||||
copter.config.animation_start_action, self.ros_telemetry.z,
|
||||
copter.config.animation_takeoff_level, copter.config.animation_ground_level,
|
||||
copter.config.animation_ratio, offset, self.fcu_status)
|
||||
return [x,y,z,yaw,start_action,start_delay]
|
||||
self.start_action = copter.animation.get_start_action(self.ros_telemetry.z, self.fcu_status)
|
||||
return [x,y,z,yaw,self.start_action,start_delay]
|
||||
|
||||
@classmethod
|
||||
def get_battery(cls, ros_telemetry):
|
||||
@@ -923,7 +881,7 @@ class Telemetry:
|
||||
state = [self.mode, self.armed, task_manager.get_last_task_name()]
|
||||
mode, armed, last_task = state
|
||||
# check external interruption
|
||||
external_interruption = (mode != "OFFBOARD" and armed == True and last_task not in [None, 'land'])
|
||||
external_interruption = (mode != "OFFBOARD" and armed == True and last_task not in [None, 'land', 'stand'])
|
||||
log_msg = ''
|
||||
if emergency:
|
||||
log_msg += 'emergency and '
|
||||
@@ -1025,10 +983,14 @@ class AnimationEventHandler(FileSystemEventHandler):
|
||||
def on_any_event(self, event):
|
||||
logger.info('{} is {}'.format(event.src_path, event.event_type))
|
||||
# logger.info(os.path.splitext(event.src_path))
|
||||
if (os.path.splitext(event.src_path)[-1] == '.csv' and event.event_type != "deleted") or event.src_path.split('/')[-1] == 'client.ini':
|
||||
if event.src_path.split('/')[-1] == 'client.ini':
|
||||
if os.path.exists("animation.csv"):
|
||||
logger.info("Update frames from animation.csv")
|
||||
copter.animation.update_frames(copter.config, "animation.csv")
|
||||
copter.on_config_update()
|
||||
logger.info("Config updated!")
|
||||
elif (os.path.splitext(event.src_path)[-1] == '.csv' and event.event_type != "deleted"):
|
||||
if os.path.exists("animation.csv"):
|
||||
copter.animation.on_animation_update("animation.csv")
|
||||
logger.info("Animation updated!")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
||||
@@ -56,6 +56,7 @@ takeoff_level = float(default=0.5)
|
||||
# * 'current' - current height of copter
|
||||
# * z coordinate in meters
|
||||
ground_level = string(default=current)
|
||||
check_ground = boolean(default=True)
|
||||
# Frame delay in seconds
|
||||
frame_delay = float(default=0.1, min=0.01)
|
||||
# Available options:
|
||||
@@ -74,11 +75,11 @@ private_offset = float_list(default=list(0, 0, 0), min=3, max=3)
|
||||
common_offset = float_list(default=list(0, 0, 0), min=3, max=3)
|
||||
# Flags for output frames
|
||||
[[OUTPUT]]
|
||||
static_begin = boolean(default=False)
|
||||
static_begin = boolean(default=True)
|
||||
takeoff = boolean(default=True)
|
||||
route = boolean(default=True)
|
||||
land = boolean(default=False)
|
||||
static_end = boolean(default=False)
|
||||
land = boolean(default=True)
|
||||
static_end = boolean(default=True)
|
||||
|
||||
[LED]
|
||||
use = boolean(default=True)
|
||||
|
||||
@@ -30,14 +30,60 @@ def moving(f1, f2, delta, x=True, y=True, z=True):
|
||||
|
||||
def get_numbers(frames):
|
||||
numbers = []
|
||||
if frames:
|
||||
for frame in frames:
|
||||
numbers.append(frame.number)
|
||||
for frame in frames:
|
||||
numbers.append(frame.number)
|
||||
return numbers
|
||||
|
||||
def get_actions(frames):
|
||||
actions = []
|
||||
for frame in frames:
|
||||
actions.append(frame.action)
|
||||
return actions
|
||||
|
||||
def get_delays(frames):
|
||||
delays = []
|
||||
for frame in frames:
|
||||
delays.append(frame.delay)
|
||||
return delays
|
||||
|
||||
def get_stats(frames):
|
||||
stats = []
|
||||
for frame in frames:
|
||||
stats.append([frame.number, frame.action, frame.delay])
|
||||
return stats
|
||||
|
||||
def get_table(frames, header):
|
||||
table = []
|
||||
for frame in frames:
|
||||
array = []
|
||||
for name in header:
|
||||
array.append(getattr(frame, name))
|
||||
table.append(array)
|
||||
return table
|
||||
|
||||
def get_default_header():
|
||||
return["number", "action", "delay", "x", "y", "z", "yaw", "red", "green", "blue"]
|
||||
|
||||
def get_start_frame_index(frames):
|
||||
index = 0
|
||||
for frame in frames:
|
||||
if frame.action == 'stand':
|
||||
index += 1
|
||||
else:
|
||||
break
|
||||
return index
|
||||
|
||||
def get_duration(frames):
|
||||
duration = 0
|
||||
for frame in frames:
|
||||
duration += frame.delay
|
||||
return duration
|
||||
|
||||
|
||||
class Frame(object):
|
||||
params_dict = {
|
||||
"number": None,
|
||||
"action": 'fly', # fly, arm, land, stand
|
||||
"x": None,
|
||||
"y": None,
|
||||
"z": None,
|
||||
@@ -45,7 +91,7 @@ class Frame(object):
|
||||
"red": None,
|
||||
"green": None,
|
||||
"blue": None,
|
||||
"delay": None,
|
||||
"delay": 0,
|
||||
}
|
||||
def __init__(self, csv_row=None, delay=None):
|
||||
for key, value in self.params_dict.items():
|
||||
@@ -86,287 +132,402 @@ class Frame(object):
|
||||
return self.get_pos() and (self.yaw is not None)
|
||||
|
||||
class Animation(object):
|
||||
def __init__(self, config=None, filepath="animation.csv"):
|
||||
self.reset(filepath)
|
||||
# filepath - path to csv animation file, config - config 'ANIMATION' section (dictionary)
|
||||
def __init__(self, filepath="animation.csv", config=None):
|
||||
self.reset(filepath, config)
|
||||
if config is not None:
|
||||
self.update_frames(config, filepath)
|
||||
self.on_animation_update(self.filepath)
|
||||
|
||||
def reset(self, filepath):
|
||||
def reset(self, filepath, config):
|
||||
self.id = None
|
||||
self.start_delay = 0
|
||||
self.original_frames = None
|
||||
self.static_begin_frames = None
|
||||
self.static_begin_time = 0
|
||||
self.takeoff_frames = None
|
||||
self.takeoff_time = 0
|
||||
self.route_frames = None
|
||||
self.route_time = 0
|
||||
self.land_frames = None
|
||||
self.land_time = 0
|
||||
self.static_end_frames = None
|
||||
self.static_end_time = 0
|
||||
self.output_frames = None
|
||||
self.original_frames = []
|
||||
self.transformed_frames = []
|
||||
self.static_begin_index = 0
|
||||
self.takeoff_index = 0
|
||||
self.route_index = 0
|
||||
self.land_index = 0
|
||||
self.static_end_index = 0
|
||||
self.output_frames = []
|
||||
self.output_frames_min_z = None
|
||||
self.output_frames_takeoff = []
|
||||
self.output_frames_takeoff_min_z = None
|
||||
self.start_time = None
|
||||
self.filepath = filepath
|
||||
self.config = config
|
||||
self.state = None
|
||||
|
||||
def load(self, filepath="animation.csv", config = None):
|
||||
self.original_frames = []
|
||||
self.corrected_frames = []
|
||||
self.filepath = filepath
|
||||
def set_state(self, state, log_error=False):
|
||||
self.state = state
|
||||
if log_error:
|
||||
logger.error(state)
|
||||
|
||||
def load(self):
|
||||
self.state = "OK"
|
||||
delay = config.animation_frame_delay
|
||||
try:
|
||||
animation_file = open(filepath)
|
||||
delay = self.config.animation_frame_delay
|
||||
except (TypeError, KeyError):
|
||||
self.set_state("Bad animation delay from config 'ANIMATION' section", log_error=True)
|
||||
return
|
||||
try:
|
||||
animation_file = open(self.filepath)
|
||||
except IOError:
|
||||
logger.debug("File {} can't be opened".format(filepath))
|
||||
self.state = "No animation"
|
||||
self.set_state("File {} can't be opened".format(self.filepath), log_error=True)
|
||||
else:
|
||||
with animation_file:
|
||||
current_frame_delay = delay
|
||||
csv_reader = csv.reader(
|
||||
animation_file, delimiter=',', quotechar='|'
|
||||
)
|
||||
row_0 = csv_reader.next()
|
||||
try:
|
||||
row_0 = csv_reader.next()
|
||||
except StopIteration:
|
||||
self.set_state("Animation file is empty", log_error=True)
|
||||
return
|
||||
if len(row_0) == 1:
|
||||
self.id = row_0[0]
|
||||
logger.debug("Got animation_id: {}".format(self.id))
|
||||
elif len(row_0) == 2:
|
||||
current_frame_delay = float(row_0[1])
|
||||
logger.debug("Got new frame delay: {}".format(current_frame_delay))
|
||||
try:
|
||||
current_frame_delay = float(row_0[1])
|
||||
logger.debug("Got new frame delay: {}".format(current_frame_delay))
|
||||
except ValueError as e:
|
||||
self.set_state("Can't parse delay row in csv file. {}".format(e), log_error=True)
|
||||
return
|
||||
else:
|
||||
logger.debug("No animation id in file")
|
||||
self.id = "No animation id"
|
||||
try:
|
||||
frame = Frame(row_0, current_frame_delay)
|
||||
except ValueError as e:
|
||||
logger.error("Can't parse row in csv file. {}".format(e))
|
||||
self.state = "Bad animation file"
|
||||
return
|
||||
try:
|
||||
frame.set_yaw(config.animation_yaw)
|
||||
except ValueError as e:
|
||||
logger.error("Can't set yaw from configuration")
|
||||
self.state = "Bad yaw from config"
|
||||
self.set_state("Can't parse frame row in csv file. {}".format(e), log_error=True)
|
||||
return
|
||||
self.original_frames.append(frame)
|
||||
for row in csv_reader:
|
||||
if len(row) == 2:
|
||||
current_frame_delay = float(row[1])
|
||||
logger.debug("Got new frame delay: {}".format(current_frame_delay))
|
||||
try:
|
||||
current_frame_delay = float(row_0[1])
|
||||
logger.debug("Got new frame delay: {}".format(current_frame_delay))
|
||||
except ValueError as e:
|
||||
self.set_state("Can't parse delay row in csv file. {}".format(e), log_error=True)
|
||||
return
|
||||
else:
|
||||
try:
|
||||
frame = Frame(row, current_frame_delay)
|
||||
except ValueError as e:
|
||||
logger.error("Can't parse row in csv file. {}".format(e))
|
||||
self.state = "Bad animation file"
|
||||
return
|
||||
try:
|
||||
frame.set_yaw(config.animation_yaw)
|
||||
except ValueError as e:
|
||||
logger.error("Can't set yaw from configuration")
|
||||
self.state = "Bad yaw from config"
|
||||
self.set_state("Can't parse frame row in csv file. {}".format(e), log_error=True)
|
||||
return
|
||||
self.original_frames.append(frame)
|
||||
self.split_animation()
|
||||
self.set_yaw()
|
||||
if self.state == "OK":
|
||||
if self.original_frames:
|
||||
self.split()
|
||||
else:
|
||||
self.set_state("No frames loaded!", log_error=True)
|
||||
|
||||
'''
|
||||
Split animation into 5 parts: static_begin, takeoff, route, land, static_end
|
||||
* static_begin and static_end are chains of frames in the beginning and the end of animation,
|
||||
where the drone doesn't move
|
||||
* takeoff and land are chains of frames after and before static frames of animation,
|
||||
where the drone doesn't move in xy plane, and it's z coordinate only increases or decreases, respectively.
|
||||
* route is the rest of the animation
|
||||
Count static_begin_time and takeoff_time
|
||||
'''
|
||||
def split_animation(self, move_delta=0.01):
|
||||
self.static_begin_frames = []
|
||||
self.takeoff_frames = []
|
||||
self.route_frames = []
|
||||
self.land_frames = []
|
||||
self.static_end_frames = []
|
||||
self.static_begin_time = 0
|
||||
self.takeoff_time = 0
|
||||
self.route_time = 0
|
||||
self.land_time = 0
|
||||
def set_yaw(self):
|
||||
try:
|
||||
yaw = self.config.animation_yaw
|
||||
except (TypeError, KeyError) as e:
|
||||
self.set_state("Can't set yaw from config 'ANIMATION' section. {}".format(e), log_error=True)
|
||||
return
|
||||
for frame in self.original_frames:
|
||||
try:
|
||||
frame.set_yaw(yaw)
|
||||
except ValueError as e:
|
||||
self.set_state("Can't set yaw from config 'ANIMATION' section. {}".format(e), log_error=True)
|
||||
return
|
||||
|
||||
def split(self, move_delta=0.01):
|
||||
'''
|
||||
Split animation into 5 parts: static_begin, takeoff, route, land, static_end
|
||||
* static_begin and static_end are chains of frames in the beginning and the end of animation,
|
||||
where the drone doesn't move
|
||||
* takeoff and land are chains of frames after and before static frames of animation,
|
||||
where the drone doesn't move in xy plane, and it's z coordinate only increases or decreases, respectively.
|
||||
* route is the rest of the animation
|
||||
'''
|
||||
if len(self.original_frames) == 0:
|
||||
return
|
||||
self.land_index = len(self.original_frames) - 1
|
||||
frames = copy.deepcopy(self.original_frames)
|
||||
# Get takeoff index
|
||||
i = 0 # Moving index from the beginning
|
||||
# Select static begin frames
|
||||
while i < len(frames) - 1:
|
||||
self.static_begin_time += frames[i].delay
|
||||
self.takeoff_index = 0
|
||||
while i < len(frames):
|
||||
if moving(frames[i], frames[i+1], move_delta):
|
||||
break
|
||||
i += 1
|
||||
if i > 0:
|
||||
self.static_begin_frames = frames[:i+1]
|
||||
frames = frames[i+1:]
|
||||
i = 0
|
||||
else:
|
||||
self.static_begin_time = 0
|
||||
# Select takeoff frames
|
||||
while i < len(frames) - 1:
|
||||
self.takeoff_time += frames[i].delay
|
||||
self.takeoff_index = i+1
|
||||
# Get route index
|
||||
i = self.takeoff_index
|
||||
self.route_index = self.takeoff_index
|
||||
while i < len(frames):
|
||||
if moving(frames[i], frames[i+1], move_delta, z = False) or (frames[i+1].z - frames[i].z <= 0):
|
||||
break
|
||||
i += 1
|
||||
if i > 0:
|
||||
self.takeoff_frames = frames[:i+1]
|
||||
frames = frames[i+1:]
|
||||
else:
|
||||
self.takeoff_time = 0
|
||||
if i - self.route_index > 0:
|
||||
self.route_index = i+1
|
||||
# Get static end index
|
||||
i = len(frames) - 1 # Moving index from the end
|
||||
# Select static end frames
|
||||
self.static_end_index = len(self.original_frames) - 1
|
||||
while i >= 0:
|
||||
self.static_end_time += frames[i].delay
|
||||
if moving(frames[i], frames[i-1], move_delta):
|
||||
break
|
||||
i -= 1
|
||||
if i < len(frames) - 1:
|
||||
self.static_end_frames = frames[i+1:]
|
||||
frames = frames[:i+1]
|
||||
i = len(frames) - 1
|
||||
else:
|
||||
self.static_end_time = 0
|
||||
# Select land frames
|
||||
if self.static_end_index - i > 0:
|
||||
self.static_end_index = i
|
||||
# Get land index
|
||||
self.land_index = self.static_end_index
|
||||
while i >= 0:
|
||||
self.land_time += frames[i].delay
|
||||
if moving(frames[i], frames[i-1], move_delta, z = False) or (frames[i-1].z - frames[i].z <= 0):
|
||||
break
|
||||
i -= 1
|
||||
if i < len(frames) - 1:
|
||||
self.land_frames = frames[i+1:]
|
||||
frames = frames[:i+1]
|
||||
else:
|
||||
self.land_time = 0
|
||||
# Get route frames
|
||||
self.route_frames = frames
|
||||
for frame in self.route_frames:
|
||||
self.route_time += frame.delay
|
||||
if self.land_index - i > 0:
|
||||
self.land_index = i
|
||||
|
||||
def make_output_frames(self, static_begin, takeoff, route, land, static_end):
|
||||
def transform(self):
|
||||
try:
|
||||
x0, y0, z0 = numpy.array(self.config.animation_common_offset) + numpy.array(self.config.animation_private_offset)
|
||||
except (ValueError, KeyError):
|
||||
self.set_state("Can't transform animation: bad or empty config (offset in 'ANIMATION')", log_error=True)
|
||||
return
|
||||
try:
|
||||
x_ratio, y_ratio, z_ratio = self.config.animation_ratio
|
||||
except (ValueError, KeyError):
|
||||
self.set_state("Can't transform animation: bad or empty config (ratio in 'ANIMATION')", log_error=True)
|
||||
return
|
||||
self.transformed_frames = []
|
||||
self.transformed_frames = copy.deepcopy(self.original_frames)
|
||||
for frame in self.transformed_frames:
|
||||
frame.x = x_ratio*frame.x + x0
|
||||
frame.y = y_ratio*frame.y + y0
|
||||
frame.z = z_ratio*frame.z + z0
|
||||
|
||||
def mark_stand_frames(self):
|
||||
if not self.transformed_frames:
|
||||
return
|
||||
try:
|
||||
takeoff_level = self.config.animation_takeoff_level
|
||||
except (ValueError, KeyError):
|
||||
self.set_state("Can't set frame actions: bad or empty config (takeoff_level in 'ANIMATION')", log_error=True)
|
||||
return
|
||||
static_begin_action = "fly"
|
||||
static_end_action = "fly"
|
||||
|
||||
# Check first frame
|
||||
if self.transformed_frames[0].z < takeoff_level:
|
||||
static_begin_action = "stand"
|
||||
# Set action for static_begin frames
|
||||
for frame in self.transformed_frames[:self.takeoff_index]:
|
||||
frame.action = static_begin_action
|
||||
|
||||
# Check last frame
|
||||
if self.transformed_frames[-1].z < takeoff_level:
|
||||
static_end_action = "stand"
|
||||
# Set action for static_end frames
|
||||
for frame in self.transformed_frames[self.static_end_index:]:
|
||||
frame.action = static_end_action
|
||||
|
||||
def apply_flags(self):
|
||||
self.output_frames = []
|
||||
self.output_frames_min_z = None
|
||||
self.start_delay = 0.
|
||||
if not self.original_frames:
|
||||
self.output_frames_takeoff = []
|
||||
if not self.transformed_frames:
|
||||
return
|
||||
if not static_begin and not takeoff and not route and not land and not static_end:
|
||||
try:
|
||||
static_begin = self.config.animation_output_static_begin
|
||||
takeoff = self.config.animation_output_takeoff
|
||||
route = self.config.animation_output_route
|
||||
land = self.config.animation_output_land
|
||||
static_end = self.config.animation_output_static_end
|
||||
except (ValueError, KeyError):
|
||||
self.set_state("Can't get output flags: bad or empty config ('OUTPUT' in 'ANIMATION')", log_error=True)
|
||||
return
|
||||
try:
|
||||
takeoff_level = self.config.animation_takeoff_level
|
||||
except (ValueError, KeyError):
|
||||
self.set_state("Can't set frame actions: bad or empty config (takeoff_level in 'ANIMATION')", log_error=True)
|
||||
return
|
||||
output_frames = copy.deepcopy(self.transformed_frames)
|
||||
output_frames_takeoff = copy.deepcopy(self.transformed_frames)
|
||||
if static_begin:
|
||||
self.output_frames += self.static_begin_frames
|
||||
if not static_begin:
|
||||
self.start_delay += self.static_begin_time
|
||||
self.output_frames += output_frames[:self.takeoff_index]
|
||||
self.output_frames_takeoff += output_frames_takeoff[:self.takeoff_index]
|
||||
if takeoff:
|
||||
self.output_frames += self.takeoff_frames
|
||||
if not static_begin and not takeoff:
|
||||
self.start_delay += self.takeoff_time
|
||||
self.output_frames += output_frames[self.takeoff_index:self.route_index]
|
||||
if self.transformed_frames[self.takeoff_index].z >= takeoff_level:
|
||||
self.output_frames_takeoff += output_frames_takeoff[self.takeoff_index:self.route_index]
|
||||
if route:
|
||||
self.output_frames += self.route_frames
|
||||
if not static_begin and not takeoff and not route:
|
||||
self.start_delay += self.route_time
|
||||
self.output_frames += output_frames[self.route_index:self.land_index]
|
||||
self.output_frames_takeoff += output_frames_takeoff[self.route_index:self.land_index]
|
||||
if land:
|
||||
self.output_frames += self.land_frames
|
||||
if not static_begin and not takeoff and not route and not land:
|
||||
self.start_delay += self.land_time
|
||||
self.output_frames += output_frames[self.land_index:self.static_end_index]
|
||||
self.output_frames_takeoff += output_frames_takeoff[self.land_index:self.static_end_index]
|
||||
if static_end:
|
||||
self.output_frames += self.static_end_frames
|
||||
self.output_frames += output_frames[self.static_end_index:]
|
||||
self.output_frames_takeoff += output_frames_takeoff[self.static_end_index:]
|
||||
if self.output_frames:
|
||||
self.output_frames_min_z = min(self.output_frames, key = lambda p: p.z).z
|
||||
if self.output_frames_takeoff:
|
||||
self.output_frames_takeoff_min_z = min(self.output_frames_takeoff, key = lambda p: p.z).z
|
||||
|
||||
def update_frames(self, config, filepath):
|
||||
self.reset(filepath)
|
||||
self.load(filepath, config)
|
||||
self.make_output_frames(config.animation_output_static_begin,
|
||||
config.animation_output_takeoff,
|
||||
config.animation_output_route,
|
||||
config.animation_output_land,
|
||||
config.animation_output_static_end)
|
||||
def mark_flight(self):
|
||||
if not self.output_frames:
|
||||
return
|
||||
try:
|
||||
arming_time = self.config.flight_arming_time
|
||||
takeoff_time = self.config.flight_takeoff_time
|
||||
rfp_time = self.config.flight_reach_first_point_time
|
||||
land_delay = self.config.flight_land_delay
|
||||
except (ValueError, KeyError):
|
||||
self.set_state("Can't mark flight: bad or empty config ('FLIGHT' section)", log_error=True)
|
||||
return
|
||||
# add arm frame to output_frames
|
||||
i = 0
|
||||
while i < len(self.output_frames):
|
||||
if self.output_frames[i].action == 'fly':
|
||||
frame = copy.deepcopy(self.output_frames[i])
|
||||
frame.action = 'arm'
|
||||
frame.delay = arming_time
|
||||
self.output_frames.insert(i, frame)
|
||||
break
|
||||
i += 1
|
||||
# add takeoff frame to output_frames_takeoff
|
||||
i = 0
|
||||
while i < len(self.output_frames_takeoff):
|
||||
if self.output_frames_takeoff[i].action == 'fly':
|
||||
# set first fly frame action to reach point
|
||||
self.output_frames_takeoff[i].action = 'reach'
|
||||
self.output_frames_takeoff[i].delay = rfp_time
|
||||
# add takeoff action before reach point
|
||||
frame = copy.deepcopy(self.output_frames_takeoff[i])
|
||||
frame.action = 'takeoff'
|
||||
frame.delay = takeoff_time
|
||||
self.output_frames_takeoff.insert(i, frame)
|
||||
break
|
||||
i += 1
|
||||
# add land frame to output_frames
|
||||
i = len(self.output_frames) - 1
|
||||
while i >=0:
|
||||
if self.output_frames[i].action == 'fly':
|
||||
frame = copy.deepcopy(self.output_frames[i])
|
||||
frame.delay = land_delay
|
||||
self.output_frames[i].action = 'land'
|
||||
self.output_frames.insert(i, frame)
|
||||
break
|
||||
i -= 1
|
||||
i = len(self.output_frames_takeoff) - 1
|
||||
# add land frame to output_frames_takeoff
|
||||
while i >=0:
|
||||
if self.output_frames_takeoff[i].action == 'fly':
|
||||
frame = copy.deepcopy(self.output_frames_takeoff[i])
|
||||
frame.delay = land_delay
|
||||
self.output_frames_takeoff[i].action = 'land'
|
||||
self.output_frames_takeoff.insert(i, frame)
|
||||
break
|
||||
i -= 1
|
||||
self.start_frame_index = get_start_frame_index(self.output_frames)
|
||||
self.start_time = get_duration(self.output_frames[:self.start_frame_index])
|
||||
|
||||
def get_scaled_output(self, ratio=(1,1,1), offset=(0,0,0)):
|
||||
x0, y0, z0 = offset
|
||||
x_ratio, y_ratio, z_ratio = ratio
|
||||
scaled_frames = copy.deepcopy(self.output_frames)
|
||||
if scaled_frames:
|
||||
for frame in scaled_frames:
|
||||
frame.x = x_ratio*frame.x + x0
|
||||
frame.y = y_ratio*frame.y + y0
|
||||
frame.z = z_ratio*frame.z + z0
|
||||
return scaled_frames
|
||||
def on_animation_update(self, filepath="animation.csv", config=None):
|
||||
if config is None:
|
||||
config = self.config
|
||||
self.reset(filepath, config)
|
||||
self.load()
|
||||
if self.original_frames:
|
||||
self.on_config_update(self.config)
|
||||
|
||||
def get_scaled_output_min_z(self, ratio=(1,1,1), offset=(0,0,0)):
|
||||
if self.output_frames_min_z is None:
|
||||
def on_config_update(self, config):
|
||||
self.config = config
|
||||
self.transform()
|
||||
self.mark_stand_frames()
|
||||
self.apply_flags()
|
||||
self.mark_flight()
|
||||
|
||||
def get_start_frame(self, action):
|
||||
try:
|
||||
if action == 'fly':
|
||||
return self.output_frames[self.start_frame_index]
|
||||
if action == 'takeoff':
|
||||
return self.output_frames_takeoff[self.start_frame_index]
|
||||
except IndexError:
|
||||
return None
|
||||
z0 = offset[2]
|
||||
z_ratio = ratio[2]
|
||||
return self.output_frames_min_z*z_ratio + z0
|
||||
|
||||
def get_start_point(self, ratio=(1,1,1), offset=(0,0,0)):
|
||||
if not self.output_frames:
|
||||
return []
|
||||
x0, y0, z0 = offset
|
||||
x_ratio, y_ratio, z_ratio = ratio
|
||||
first_frame = self.output_frames[0]
|
||||
x = x_ratio*first_frame.x + x0
|
||||
y = y_ratio*first_frame.y + y0
|
||||
z = z_ratio*first_frame.z + z0
|
||||
return [x, y, z]
|
||||
def get_output_frames(self, action):
|
||||
if action == 'fly':
|
||||
return self.output_frames
|
||||
if action == 'takeoff':
|
||||
return self.output_frames_takeoff
|
||||
return []
|
||||
|
||||
def get_start_yaw(self):
|
||||
if not self.output_frames:
|
||||
return float('nan')
|
||||
return math.degrees(self.output_frames[0].yaw)
|
||||
def get_min_z(self, action):
|
||||
if action == 'fly':
|
||||
return self.output_frames_min_z
|
||||
if action == 'takeoff':
|
||||
return self.output_frames_takeoff_min_z
|
||||
return []
|
||||
|
||||
def check_ground(self, ground_level=0, ratio=(1,1,1), offset=(0,0,0)):
|
||||
return ground_level <= self.get_scaled_output_min_z(ratio, offset)
|
||||
|
||||
def get_start_action(self, start_action, current_height, takeoff_level,
|
||||
ground_level, ratio=(1,1,1), offset=(0,0,0), state="STANDBY"):
|
||||
def get_start_action(self, current_height=0, state="STANDBY", tolerance = 0.2):
|
||||
# Check output frames
|
||||
if not self.output_frames:
|
||||
return 'error: empty output frames'
|
||||
if math.isnan(current_height):
|
||||
return 'error: bad copter height'
|
||||
# Check that bottom point of animation is higher than ground level
|
||||
if ground_level == 'current':
|
||||
ground_level = current_height
|
||||
try:
|
||||
ground_level = float(ground_level)
|
||||
except ValueError:
|
||||
return 'error in [ANIMATION] ground_level parameter'
|
||||
if state != "ACTIVE" and ground_level > self.get_scaled_output_min_z(ratio, offset):
|
||||
return 'error: some animation points are lower than ground level for {:.2f}m (max)'.format(
|
||||
ground_level - self.get_scaled_output_min_z(ratio, offset)
|
||||
)
|
||||
# Check current_height
|
||||
if self.config.animation_check_ground:
|
||||
if math.isnan(current_height):
|
||||
return 'error: bad copter height'
|
||||
# Select start action
|
||||
try:
|
||||
start_action = self.config.animation_start_action
|
||||
except (ValueError, KeyError):
|
||||
return 'error in [ANIMATION] start_action'
|
||||
try:
|
||||
takeoff_level = self.config.animation_takeoff_level
|
||||
except (ValueError, KeyError):
|
||||
return 'error in [ANIMATION] takeoff_level'
|
||||
if start_action == 'auto':
|
||||
if self.get_start_point(ratio, offset)[2] - current_height > takeoff_level:
|
||||
return 'takeoff'
|
||||
if self.output_frames[0].z > takeoff_level:
|
||||
start_action = 'takeoff'
|
||||
else:
|
||||
return 'fly'
|
||||
start_action = 'fly'
|
||||
elif start_action in ('takeoff', 'fly'):
|
||||
return start_action
|
||||
pass
|
||||
else:
|
||||
return 'error in [ANIMATION] start_action parameter'
|
||||
|
||||
# Need for tests
|
||||
def save_corrected_animation(self):
|
||||
name, ext = os.path.splitext(self.filepath)
|
||||
filepath = name + '_corrected' + ext
|
||||
with open(filepath, mode='w+') as corrected_animation:
|
||||
csv_writer = csv.writer(corrected_animation, delimiter=',', quotechar='"', quoting=csv.QUOTE_MINIMAL)
|
||||
for frame in self.corrected_frames:
|
||||
csv_writer.writerow([frame.number, frame.x, frame.y, frame.z, frame.red, frame.green, frame.blue, frame.delay])
|
||||
# Check that bottom point of animation is higher than ground level
|
||||
if self.config.animation_check_ground:
|
||||
try:
|
||||
ground_level = self.config.animation_ground_level
|
||||
if ground_level == 'current':
|
||||
ground_level = current_height
|
||||
ground_level = float(ground_level)
|
||||
except (ValueError, KeyError):
|
||||
return 'error in [ANIMATION] ground_level parameter'
|
||||
if state != "ACTIVE" and ground_level - tolerance > self.get_min_z(start_action):
|
||||
return 'error: animation is lower than ground level for {:.2f}m'.format(
|
||||
ground_level - self.output_frames_min_z
|
||||
)
|
||||
return start_action
|
||||
|
||||
try:
|
||||
def execute_frame(frame, frame_id='aruco_map', use_leds=True,
|
||||
flight_func=flight.navto, auto_arm=False, flight_kwargs=None, interrupter=interrupt_event):
|
||||
if flight_kwargs is None:
|
||||
flight_kwargs = {}
|
||||
if frame.pose_is_valid():
|
||||
flight_func(x=frame.x, y=frame.y, z=frame.z, yaw=frame.yaw, frame_id=frame_id, auto_arm=auto_arm, interrupter=interrupt_event, **flight_kwargs)
|
||||
else:
|
||||
logger.error("Frame pose is not valid for flying")
|
||||
def execute_frame(frame, config, interrupter=interrupt_event):
|
||||
auto_arm = False
|
||||
use_leds = config.led_use
|
||||
frame_id = config.flight_frame_id
|
||||
if frame.action == 'takeoff':
|
||||
use_leds = use_leds & config.led_takeoff_indication
|
||||
takeoff(z=config.flight_takeoff_height, frame_id=frame_id, timeout=config.flight_takeoff_time, use_leds=use_leds, interrupter=interrupter)
|
||||
return
|
||||
if frame.action == 'land':
|
||||
use_leds = use_leds & config.led_land_indication
|
||||
land(frame_id=frame_id, timeout=config.flight_land_timeout, use_leds=use_leds, interrupter=interrupter)
|
||||
return
|
||||
if frame.action in ('fly', 'arm'):
|
||||
if frame.action == 'arm':
|
||||
auto_arm = True
|
||||
if frame.pose_is_valid():
|
||||
flight.navto(x=frame.x, y=frame.y, z=frame.z, yaw=frame.yaw, frame_id=frame_id, auto_arm=auto_arm, interrupter=interrupter)
|
||||
else:
|
||||
logger.error("Frame pose is not valid for flying")
|
||||
if use_leds:
|
||||
try:
|
||||
red, green, blue = frame.get_color()
|
||||
@@ -375,7 +536,10 @@ try:
|
||||
else:
|
||||
led.set_effect(r=red, g=green, b=blue)
|
||||
|
||||
def takeoff(z=1.5, safe_takeoff=True, frame_id='map', timeout=5.0, use_leds=True,
|
||||
def turn_off_led(interrupter=interrupt_event):
|
||||
led.set_effect(r=0, g=0, b=0)
|
||||
|
||||
def takeoff(z=1.5, safe_takeoff=False, frame_id='map', timeout=5.0, use_leds=True,
|
||||
interrupter=interrupt_event):
|
||||
if use_leds:
|
||||
led.set_effect(effect='wipe', r=255, g=0, b=0)
|
||||
@@ -387,8 +551,9 @@ try:
|
||||
led.set_effect(effect='blink_fast', r=0, g=255, b=0)
|
||||
|
||||
|
||||
def land(z=1.5, descend=False, timeout=5.0, frame_id='aruco_map', use_leds=True,
|
||||
def land(z=1.5, descend=False, timeout=5.0, frame_id='map', use_leds=True,
|
||||
interrupter=interrupt_event):
|
||||
led.set_effect(r=0, g=0, b=0)
|
||||
if use_leds:
|
||||
led.set_effect(effect='blink_fast', r=255, g=0, b=0)
|
||||
flight.land(z=z, descend=descend, timeout_land=timeout, frame_id_land=frame_id, interrupter=interrupter)
|
||||
|
||||
@@ -1,4 +1,13 @@
|
||||
import rospy
|
||||
import logging
|
||||
from clover.srv import SetLEDEffect
|
||||
|
||||
set_effect = rospy.ServiceProxy('led/set_effect', SetLEDEffect)
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
set_effect_service = rospy.ServiceProxy('led/set_effect', SetLEDEffect)
|
||||
|
||||
def set_effect(*args, **kwargs):
|
||||
try:
|
||||
set_effect_service(*args, **kwargs)
|
||||
except rospy.ServiceException:
|
||||
logger.error("Can't set led effect: service /led/set_effect is unavailable!")
|
||||
|
||||
@@ -88,7 +88,13 @@ class TaskManager(object):
|
||||
raise KeyError('Pop from an empty priority queue')
|
||||
|
||||
def get_last_task_name(self):
|
||||
return self._last_task
|
||||
try:
|
||||
name = self._last_task.func.__name__
|
||||
if name == 'execute_frame':
|
||||
return name.kwargs["frame"].action
|
||||
return name
|
||||
except AttributeError:
|
||||
return None
|
||||
|
||||
def get_current_task(self):
|
||||
try:
|
||||
@@ -180,12 +186,15 @@ class TaskManager(object):
|
||||
task.func(*task.args, interrupter=self._task_interrupt_event, **task.kwargs)
|
||||
|
||||
except Exception as e:
|
||||
logger.error("Error '{}' occurred in task {}".format(e, task))
|
||||
try:
|
||||
logger.error("Error '{}' occurred in task {}".format(e, task))
|
||||
#print("Error '{}' occurred in task {}".format(e, task))
|
||||
if str(e) == 'STOP':
|
||||
self.reset()
|
||||
logger.error("Return after STOP exception, can't arm!")
|
||||
return
|
||||
if str(e) == 'STOP':
|
||||
self.reset()
|
||||
logger.error("Return after STOP exception, can't arm!")
|
||||
return
|
||||
except (KeyError, TypeError):
|
||||
logger.error(e)
|
||||
else:
|
||||
logger.error("Task interrupted before execution")
|
||||
#print("Task interrupted before execution")
|
||||
@@ -207,7 +216,7 @@ class TaskManager(object):
|
||||
self.pop_task()
|
||||
except KeyError as e:
|
||||
logger.error(str(e))
|
||||
self._last_task = task.func.__name__
|
||||
self._last_task = task
|
||||
#try:
|
||||
#print("Pop {} function!".format(task.func.__name__))
|
||||
#except Exception as e:
|
||||
|
||||
@@ -2,4 +2,5 @@ selectors2
|
||||
psutil
|
||||
configobj
|
||||
watchdog
|
||||
geographiclib
|
||||
geographiclib
|
||||
tabulate
|
||||
|
||||
@@ -3,6 +3,15 @@ import sys
|
||||
import shutil
|
||||
from pytest import approx
|
||||
import pytest
|
||||
import logging
|
||||
|
||||
logging.basicConfig( # TODO all prints as logs
|
||||
level=logging.DEBUG, # INFO
|
||||
stream=sys.stdout,
|
||||
format="%(asctime)s [%(name)-7.7s] [%(threadName)-12.12s] [%(levelname)-5.5s] %(message)s",
|
||||
handlers=[
|
||||
logging.StreamHandler(sys.stdout),
|
||||
])
|
||||
|
||||
# Add parent dir to PATH to import messaging_lib and config_lib
|
||||
current_dir = (os.path.dirname(os.path.realpath(__file__)))
|
||||
@@ -36,111 +45,121 @@ assert config.config_name == "client"
|
||||
|
||||
import animation
|
||||
|
||||
a = animation.Animation()
|
||||
|
||||
assets_dir = os.path.realpath(os.path.join(root_dir, 'drone/tests/assets'))
|
||||
|
||||
def test_animation_1():
|
||||
a.update_frames(config, os.path.join(assets_dir, 'animation_1.csv'))
|
||||
def test_animation_1_2():
|
||||
a = animation.Animation(os.path.join(assets_dir, 'animation_1.csv'), config)
|
||||
assert a.id == 'basic'
|
||||
assert a.state == "OK"
|
||||
assert approx(a.original_frames[0].get_pos()) == [0,0,0]
|
||||
assert a.original_frames[0].get_color() == [204,2,0]
|
||||
assert a.original_frames[0].pose_is_valid()
|
||||
assert animation.get_numbers(a.static_begin_frames) == range(1,11)
|
||||
assert animation.get_numbers(a.takeoff_frames) == range(11,21)
|
||||
assert animation.get_numbers(a.route_frames) == range(21,31)
|
||||
assert animation.get_numbers(a.land_frames) == range(31, 41)
|
||||
assert animation.get_numbers(a.static_end_frames) == range(41, 51)
|
||||
assert animation.get_numbers(a.output_frames) == range(11,31)
|
||||
assert approx(a.static_begin_time) == 1
|
||||
assert approx(a.takeoff_time) == 1
|
||||
assert approx(a.output_frames_min_z) == 0.1
|
||||
assert approx(a.get_scaled_output(ratio=[1,2,3], offset=[4,5,6])[0].get_pos()) == [4.,5.,6.3]
|
||||
assert approx(a.get_scaled_output_min_z(ratio=[1,2,3], offset=[4,5,6])) == 6.3
|
||||
assert approx(a.get_start_point(ratio=[1,2,3], offset=[4,5,6])) == [4.,5.,6.3]
|
||||
assert a.takeoff_index == 10
|
||||
assert a.route_index == 20
|
||||
assert a.land_index == 29
|
||||
assert a.static_end_index == 39
|
||||
assert a.start_frame_index == a.takeoff_index
|
||||
assert approx(a.start_time) == 1
|
||||
assert a.output_frames[a.start_frame_index].action == 'arm'
|
||||
assert a.output_frames_takeoff[a.start_frame_index].action == 'takeoff'
|
||||
assert approx(a.output_frames_min_z) == 0
|
||||
assert a.get_start_action() == 'fly'
|
||||
config.set('ANIMATION', 'ratio', [1,2,3])
|
||||
config.set('ANIMATION', 'common_offset', [4,5,6])
|
||||
a.on_config_update(config)
|
||||
assert approx(a.output_frames[0].get_pos()) == [4.,5.,6.]
|
||||
assert approx(a.output_frames_min_z) == 6.
|
||||
assert approx(a.get_start_frame('fly').get_pos()) == [4.,5.,6.]
|
||||
assert a.get_start_action() == 'takeoff'
|
||||
config.set('ANIMATION', 'ratio', [1,1,1])
|
||||
config.set('ANIMATION', 'common_offset', [0,0,0])
|
||||
|
||||
def test_animation_2():
|
||||
a.update_frames(config, os.path.join(assets_dir, 'animation_2.csv'))
|
||||
a.on_animation_update(os.path.join(assets_dir, 'animation_2.csv'), config)
|
||||
assert a.id == 'parad'
|
||||
assert a.state == "OK"
|
||||
assert approx(a.original_frames[271].get_pos()) == [-1.00519,2.65699,0.24386]
|
||||
assert a.original_frames[271].get_color() == [7,255,0]
|
||||
assert a.original_frames[271].pose_is_valid()
|
||||
assert animation.get_numbers(a.static_begin_frames) == range(271)
|
||||
assert animation.get_numbers(a.takeoff_frames) == range(271,285)
|
||||
assert animation.get_numbers(a.route_frames) == range(285,1065)
|
||||
assert animation.get_numbers(a.land_frames) == []
|
||||
assert animation.get_numbers(a.static_end_frames) == []
|
||||
assert animation.get_numbers(a.output_frames) == range(271, 1065)
|
||||
assert approx(a.static_begin_time) == 27.1
|
||||
assert approx(a.takeoff_time) == 1.4
|
||||
assert approx(a.output_frames_min_z) == 0.24386
|
||||
assert approx(a.get_scaled_output(ratio=[1,2,3], offset=[4,5,6])[0].get_pos()) == [2.99481, 10.31398, 6.73158]
|
||||
assert approx(a.get_scaled_output_min_z(ratio=[1,2,3], offset=[4,5,6])) == 6.73158
|
||||
assert approx(a.get_start_point(ratio=[1,2,3], offset=[4,5,6])) == [2.99481, 10.31398, 6.73158]
|
||||
assert a.takeoff_index == 271
|
||||
assert a.route_index == 285
|
||||
assert a.land_index == 1064
|
||||
assert a.static_end_index == 1064
|
||||
assert a.start_frame_index == a.takeoff_index
|
||||
assert approx(a.start_time) == 27.1
|
||||
assert a.output_frames[a.start_frame_index].action == 'arm'
|
||||
assert a.output_frames_takeoff[a.start_frame_index].action == 'takeoff'
|
||||
assert approx(a.output_frames_min_z) == 0.21
|
||||
assert a.get_start_action() == 'fly'
|
||||
config.set('ANIMATION', 'ratio', [1,2,3])
|
||||
config.set('ANIMATION', 'common_offset', [4,5,6])
|
||||
a.on_config_update(config)
|
||||
assert approx(a.output_frames[0].get_pos()) == [2.99481, 10.31398, 6.63]
|
||||
assert approx(a.output_frames_min_z) == 6.63
|
||||
assert approx(a.get_start_frame('fly').get_pos()) == [2.99481, 10.31398, 6.63]
|
||||
assert a.get_start_action() == 'takeoff'
|
||||
config.set('ANIMATION', 'ratio', [1,1,1])
|
||||
config.set('ANIMATION', 'common_offset', [0,0,0])
|
||||
|
||||
def test_animation_3():
|
||||
a.update_frames(config, os.path.join(assets_dir, 'animation_3.csv'))
|
||||
a = animation.Animation(os.path.join(assets_dir, 'animation_3.csv'), config)
|
||||
assert a.id == 'route'
|
||||
assert a.state == "OK"
|
||||
assert approx(a.original_frames[9].get_pos()) == [0.97783,0.0,1.0]
|
||||
assert a.original_frames[9].get_color() == [0,204,2]
|
||||
assert a.original_frames[9].pose_is_valid()
|
||||
assert animation.get_numbers(a.static_begin_frames) == []
|
||||
assert animation.get_numbers(a.takeoff_frames) == []
|
||||
assert animation.get_numbers(a.route_frames) == range(20,31)
|
||||
assert animation.get_numbers(a.land_frames) == []
|
||||
assert animation.get_numbers(a.static_end_frames) == []
|
||||
assert approx(a.static_begin_time) == 0
|
||||
assert approx(a.takeoff_time) == 0
|
||||
assert a.takeoff_index == 0
|
||||
assert a.route_index == 0
|
||||
assert a.land_index == 10
|
||||
assert a.static_end_index == 10
|
||||
assert a.start_frame_index == a.takeoff_index
|
||||
assert approx(a.start_time) == 0
|
||||
assert a.output_frames[a.start_frame_index].action == 'arm'
|
||||
assert a.output_frames_takeoff[a.start_frame_index].action == 'takeoff'
|
||||
assert approx(a.output_frames_min_z) == 1
|
||||
assert approx(a.get_scaled_output(ratio=[1,2,3], offset=[4,5,6])[0].get_pos()) == [4,5,9]
|
||||
assert approx(a.get_scaled_output_min_z(ratio=[1,2,3], offset=[4,5,6])) == 9
|
||||
assert approx(a.get_start_point(ratio=[1,2,3], offset=[4,5,6])) == [4,5,9]
|
||||
assert a.get_start_action() == 'takeoff'
|
||||
config.set('ANIMATION', 'ratio', [1,2,3])
|
||||
config.set('ANIMATION', 'common_offset', [4,5,6])
|
||||
a.on_config_update(config)
|
||||
assert approx(a.output_frames[0].get_pos()) == [4,5,9]
|
||||
assert approx(a.output_frames_min_z) == 9
|
||||
assert approx(a.get_start_frame('fly').get_pos()) == [4,5,9]
|
||||
assert a.get_start_action() == 'takeoff'
|
||||
config.set('ANIMATION', 'ratio', [1,1,1])
|
||||
config.set('ANIMATION', 'common_offset', [0,0,0])
|
||||
|
||||
def test_animation_4():
|
||||
a.update_frames(config, os.path.join(assets_dir, 'animation_4.csv'))
|
||||
a = animation.Animation(os.path.join(assets_dir, 'animation_4.csv'), config)
|
||||
assert a.id == 'two_drones_test'
|
||||
assert a.state == "OK"
|
||||
assert approx(a.original_frames[11].get_pos()) == [0.21774,1.4,1.0]
|
||||
assert a.original_frames[11].get_color() == [0,0,0]
|
||||
assert a.original_frames[11].pose_is_valid()
|
||||
assert animation.get_numbers(a.static_begin_frames) == range(1,12)
|
||||
assert animation.get_numbers(a.takeoff_frames) == []
|
||||
assert animation.get_numbers(a.route_frames) == range(12,141)
|
||||
assert animation.get_numbers(a.land_frames) == []
|
||||
assert animation.get_numbers(a.static_end_frames) == range(141,161)
|
||||
assert animation.get_numbers(a.output_frames) == range(12,141)
|
||||
assert approx(a.static_begin_time) == 1.1
|
||||
assert approx(a.takeoff_time) == 0
|
||||
assert a.takeoff_index == 11
|
||||
assert a.route_index == 11
|
||||
assert a.land_index == 139
|
||||
assert a.static_end_index == 139
|
||||
assert a.start_frame_index == 0
|
||||
assert approx(a.start_time) == 0
|
||||
assert a.output_frames[a.start_frame_index].action == 'arm'
|
||||
assert a.output_frames_takeoff[a.start_frame_index].action == 'takeoff'
|
||||
assert approx(a.output_frames_min_z) == 1
|
||||
assert approx(a.get_scaled_output(ratio=[1,2,3], offset=[4,5,6])[0].get_pos()) == [4.21774,7.8,9]
|
||||
assert approx(a.get_scaled_output_min_z(ratio=[1,2,3], offset=[4,5,6])) == 9
|
||||
assert approx(a.get_start_point(ratio=[1,2,3], offset=[4,5,6])) == [4.21774,7.8,9]
|
||||
assert a.get_start_action() == 'takeoff'
|
||||
config.set('ANIMATION', 'ratio', [1,2,3])
|
||||
config.set('ANIMATION', 'common_offset', [4,5,6])
|
||||
a.on_config_update(config)
|
||||
assert approx(a.output_frames[0].get_pos()) == [4.2,7.8,9]
|
||||
assert approx(a.output_frames_min_z) == 9
|
||||
assert approx(a.get_start_frame('fly').get_pos()) == [4.2,7.8,9]
|
||||
assert a.get_start_action() == 'takeoff'
|
||||
config.set('ANIMATION', 'ratio', [1,1,1])
|
||||
config.set('ANIMATION', 'common_offset', [0,0,0])
|
||||
|
||||
def test_animation_no_file():
|
||||
a.update_frames(config, "zzz.csv")
|
||||
a = animation.Animation('zzz.csv', config)
|
||||
assert a.id == None
|
||||
assert a.state != "OK"
|
||||
assert a.original_frames == []
|
||||
assert a.output_frames == []
|
||||
assert animation.get_numbers(a.static_begin_frames) == []
|
||||
assert animation.get_numbers(a.takeoff_frames) == []
|
||||
assert animation.get_numbers(a.route_frames) == []
|
||||
assert animation.get_numbers(a.land_frames) == []
|
||||
assert animation.get_numbers(a.static_end_frames) == []
|
||||
assert a.static_begin_time == 0
|
||||
assert a.takeoff_time == 0
|
||||
assert a.output_frames_min_z is None
|
||||
assert a.get_scaled_output(ratio=[1,2,3], offset=[4,5,6]) == []
|
||||
assert a.get_scaled_output_min_z(ratio=[1,2,3], offset=[4,5,6]) is None
|
||||
assert a.get_start_point(ratio=[1,2,3], offset=[4,5,6]) == []
|
||||
|
||||
|
||||
# print animation.get_numbers(a.static_begin_frames)
|
||||
# print animation.get_numbers(a.takeoff_frames)
|
||||
# print animation.get_numbers(a.route_frames)
|
||||
# print animation.get_numbers(a.land_frames)
|
||||
# print animation.get_numbers(a.static_end_frames)
|
||||
# print animation.get_numbers(a.output_frames)
|
||||
# print a.static_begin_time
|
||||
# print a.takeoff_time
|
||||
# print a.output_frames_min_z
|
||||
|
||||
shutil.rmtree('animation_config')
|
||||
|
||||
BIN
examples/animations/basic/basic_real.gif
Normal file
BIN
examples/animations/basic/basic_real.gif
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 4.8 MiB |
@@ -1,113 +0,0 @@
|
||||
# This is generated config with default values
|
||||
# Modify to configure
|
||||
config_name = client
|
||||
config_version = 1.0
|
||||
|
||||
[SERVER]
|
||||
port = 25000
|
||||
host = 192.168.1.54
|
||||
buffer_size = 1024
|
||||
|
||||
[BROADCAST]
|
||||
use = True
|
||||
port = 8181
|
||||
|
||||
[TELEMETRY]
|
||||
transmit = True
|
||||
frequency = 1.0
|
||||
log_resources = False
|
||||
|
||||
[POSITION WATCHDOG]
|
||||
enabled = True
|
||||
log_state = True
|
||||
# Available options: emergency_land, land, disarm
|
||||
action = emergency_land
|
||||
# Time to get vision position after arm
|
||||
# No visual position will be checked
|
||||
# during this time after arming
|
||||
vision_pose_delay_after_arm = 3.0
|
||||
# Timeout for the last vision pose in /mavros/vision_pose/pose
|
||||
# Set 0 to disable vision pose check
|
||||
vision_pose_timeout = 0.0
|
||||
# Max delta between current position and setpoint
|
||||
# Set 0 to disable position delta check
|
||||
position_delta_max = 3.0
|
||||
# Time to disarm after action is triggered
|
||||
disarm_timeout = 10.0
|
||||
|
||||
[EMERGENCY LAND]
|
||||
thrust = 0.45
|
||||
decrease_thrust_after = 5.0
|
||||
|
||||
[COPTER]
|
||||
frame_id = floor
|
||||
arming_time = 0.5
|
||||
takeoff_height = 3.0
|
||||
takeoff_time = 10.0
|
||||
reach_first_point_time = 10.0
|
||||
land_delay = 1.0
|
||||
land_timeout = 10.0
|
||||
# __list__ x y z
|
||||
common_offset = 0.0, 0.0, 0.0
|
||||
|
||||
[FLOOR FRAME]
|
||||
parent = gps
|
||||
# Frame translation (x, y, z)
|
||||
# __list__ x y z
|
||||
translation = 0.0, 0.0, 5.0
|
||||
# Frame rotation (roll, pitch, yaw) in degrees
|
||||
# __list__ roll pitch yaw
|
||||
rotation = 0.0, 0.0, 0.0
|
||||
|
||||
[GPS FRAME]
|
||||
lat = 55.7032026
|
||||
lon = 37.7248114
|
||||
yaw = 0.0
|
||||
|
||||
[ANIMATION]
|
||||
# Available options:
|
||||
# 'auto' - automatic action selection from 'takeoff' or 'fly' based on current copter level
|
||||
# 'takeoff' - takeoff to first output animation point after static_begin_time then execute 'takeoff logic'
|
||||
# 'fly' - execute animation frames after static_begin_time
|
||||
start_action = auto
|
||||
takeoff_level = 0.5
|
||||
ground_level = -5.0
|
||||
frame_delay = 0.1
|
||||
# Animation ratio (x, y, z)
|
||||
# __list__ x y z
|
||||
ratio = 0.5, 0.5, 0.5
|
||||
# Available options:
|
||||
# 'animation' - take yaw from animation
|
||||
# 'nan' - don't change yaw during flight
|
||||
# or a number in degrees
|
||||
yaw = nan
|
||||
|
||||
[[OUTPUT]]
|
||||
static_begin = False
|
||||
takeoff = True
|
||||
route = True
|
||||
land = False
|
||||
static_end = False
|
||||
|
||||
[LED]
|
||||
use = False
|
||||
pin = 21
|
||||
count = 60
|
||||
takeoff_indication = True
|
||||
land_indication = True
|
||||
|
||||
[PRIVATE]
|
||||
# Available options: /hostname ; /default ; /ip ; any string 63 characters length
|
||||
id = /hostname
|
||||
# Drone's individual offset (x, y, z)
|
||||
# __list__ x y z
|
||||
offset = 0.0, 0.0, 0.0
|
||||
|
||||
[SYSTEM]
|
||||
change_hostname = True
|
||||
restart_after_rename = True
|
||||
|
||||
[NTP]
|
||||
use = False
|
||||
host = ntp1.stratum2.ru
|
||||
port = 123
|
||||
@@ -1,6 +1,9 @@
|
||||
1 1 ATT_EXT_HDG_M 1 6
|
||||
1 1 ATT_W_EXT_HDG 0.500000000000000000 9
|
||||
1 1 ATT_W_MAG 0.000000000000000000 9
|
||||
1 1 LPE_FLW_OFF_Z 0.000000000000000000 9
|
||||
1 1 LPE_FLW_QMIN 60 6
|
||||
1 1 LPE_FLW_R 0.200000002980232239 9
|
||||
1 1 LPE_FLW_RR 0.000000000000000000 9
|
||||
1 1 LPE_FLW_SCALE 1.000000000000000000 9
|
||||
1 1 LPE_FUSION 86 6
|
||||
1 1 LPE_FUSION 118 6
|
||||
|
||||
4
examples/positioning/aruco floor/aruco.params
Normal file
4
examples/positioning/aruco floor/aruco.params
Normal file
@@ -0,0 +1,4 @@
|
||||
1 1 ATT_EXT_HDG_M 1 6
|
||||
1 1 ATT_W_EXT_HDG 0.500000000000000000 9
|
||||
1 1 ATT_W_MAG 0.000000000000000000 9
|
||||
1 1 LPE_FUSION 20 6
|
||||
6
examples/positioning/aruco floor/client.ini
Normal file
6
examples/positioning/aruco floor/client.ini
Normal file
@@ -0,0 +1,6 @@
|
||||
[FLIGHT]
|
||||
frame_id = aruco_map
|
||||
|
||||
[ANIMATION]
|
||||
start_action = takeoff
|
||||
check_ground = False
|
||||
44
examples/positioning/aruco floor/launch/aruco.launch
Normal file
44
examples/positioning/aruco floor/launch/aruco.launch
Normal file
@@ -0,0 +1,44 @@
|
||||
<launch>
|
||||
<arg name="aruco_detect" default="true"/>
|
||||
<arg name="aruco_map" default="true"/>
|
||||
<arg name="aruco_vpe" default="true"/>
|
||||
|
||||
<!-- For additional help go to https://clover.coex.tech/aruco -->
|
||||
|
||||
<!-- aruco_detect: detect aruco markers, estimate poses -->
|
||||
<node name="aruco_detect" pkg="nodelet" if="$(arg aruco_detect)" type="nodelet" args="load aruco_pose/aruco_detect nodelet_manager" output="screen" clear_params="true">
|
||||
<remap from="image_raw" to="main_camera/image_raw"/>
|
||||
<remap from="camera_info" to="main_camera/camera_info"/>
|
||||
<remap from="map_markers" to="aruco_map/markers" if="$(arg aruco_map)"/>
|
||||
<param name="estimate_poses" value="true"/>
|
||||
<param name="send_tf" value="true"/>
|
||||
<param name="known_tilt" value="map"/>
|
||||
<param name="length" value="0.33"/>
|
||||
<!-- aruco detector parameters -->
|
||||
<param name="cornerRefinementMethod" value="2"/> <!-- contour refinement -->
|
||||
<param name="minMarkerPerimeterRate" value="0.075"/> <!-- 0.075 for 320x240, 0.0375 for 640x480 -->
|
||||
</node>
|
||||
|
||||
<!-- aruco_map: estimate aruco map pose -->
|
||||
<node name="aruco_map" pkg="nodelet" type="nodelet" if="$(arg aruco_map)" args="load aruco_pose/aruco_map nodelet_manager" output="screen" clear_params="true">
|
||||
<remap from="image_raw" to="main_camera/image_raw"/>
|
||||
<remap from="camera_info" to="main_camera/camera_info"/>
|
||||
<remap from="markers" to="aruco_detect/markers"/>
|
||||
<param name="map" value="$(find aruco_pose)/map/animation_map.txt"/>
|
||||
<param name="known_tilt" value="map"/>
|
||||
<param name="image_axis" value="true"/>
|
||||
<param name="frame_id" value="aruco_map_detected" if="$(arg aruco_vpe)"/>
|
||||
<param name="frame_id" value="aruco_map" unless="$(arg aruco_vpe)"/>
|
||||
<param name="markers/frame_id" value="aruco_map"/>
|
||||
<param name="markers/child_frame_id_prefix" value="aruco_"/>
|
||||
</node>
|
||||
|
||||
<!-- vpe publisher from aruco markers -->
|
||||
<node name="vpe_publisher" pkg="clover" type="vpe_publisher" if="$(arg aruco_vpe)" output="screen" clear_params="true">
|
||||
<remap from="~pose_cov" to="aruco_map/pose"/>
|
||||
<remap from="~vpe" to="mavros/vision_pose/pose"/>
|
||||
<param name="frame_id" value="aruco_map_detected"/>
|
||||
<param name="publish_zero" value="true"/>
|
||||
<param name="offset_frame_id" value="aruco_map"/>
|
||||
</node>
|
||||
</launch>
|
||||
85
examples/positioning/aruco floor/launch/clover.launch
Normal file
85
examples/positioning/aruco floor/launch/clover.launch
Normal file
@@ -0,0 +1,85 @@
|
||||
<launch>
|
||||
<arg name="fcu_conn" default="usb"/>
|
||||
<arg name="fcu_ip" default="127.0.0.1"/>
|
||||
<arg name="fcu_sys_id" default="1"/>
|
||||
<arg name="gcs_bridge" default="tcp"/>
|
||||
<arg name="web_video_server" default="true"/>
|
||||
<arg name="rosbridge" default="true"/>
|
||||
<arg name="main_camera" default="true"/>
|
||||
<arg name="optical_flow" default="false"/>
|
||||
<arg name="aruco" default="true"/>
|
||||
<arg name="rangefinder_vl53l1x" default="true"/>
|
||||
<arg name="led" default="true"/>
|
||||
<arg name="rc" default="true"/>
|
||||
<arg name="shell" default="true"/>
|
||||
|
||||
<!-- log formatting -->
|
||||
<env name="ROSCONSOLE_FORMAT" value="[${severity}] [${time}]: ${logger}: ${message}"/>
|
||||
|
||||
<!-- mavros -->
|
||||
<include file="$(find clover)/launch/mavros.launch">
|
||||
<arg name="fcu_conn" value="$(arg fcu_conn)"/>
|
||||
<arg name="fcu_ip" value="$(arg fcu_ip)"/>
|
||||
<arg name="fcu_sys_id" value="$(arg fcu_sys_id)"/>
|
||||
<arg name="gcs_bridge" value="$(arg gcs_bridge)"/>
|
||||
</include>
|
||||
|
||||
<!-- web video server -->
|
||||
<node name="web_video_server" pkg="web_video_server" type="web_video_server" if="$(arg web_video_server)" required="false" respawn="true" respawn_delay="5">
|
||||
<param name="default_stream_type" value="ros_compressed"/>
|
||||
<param name="publish_rate" value="1.0"/>
|
||||
</node>
|
||||
|
||||
<!-- aruco markers -->
|
||||
<include file="$(find clover)/launch/aruco.launch" if="$(arg aruco)"/>
|
||||
|
||||
<!-- optical flow -->
|
||||
<node pkg="nodelet" type="nodelet" name="optical_flow" args="load clover/optical_flow nodelet_manager" if="$(arg optical_flow)" clear_params="true" output="screen">
|
||||
<remap from="image_raw" to="main_camera/image_raw"/>
|
||||
<remap from="camera_info" to="main_camera/camera_info"/>
|
||||
<param name="calc_flow_gyro" value="true"/>
|
||||
<param name="roi_rad" value="0.8"/>
|
||||
</node>
|
||||
|
||||
<!-- main nodelet manager -->
|
||||
<node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" output="screen" clear_params="true">
|
||||
<param name="num_worker_threads" value="2"/>
|
||||
</node>
|
||||
|
||||
<node pkg="tf2_ros" type="static_transform_publisher" name="map_flipped_frame" args="0 0 0 3.1415926 3.1415926 0 map map_flipped"/>
|
||||
|
||||
<!-- simplified offboard control -->
|
||||
<node name="simple_offboard" pkg="clover" type="simple_offboard" output="screen" clear_params="true">
|
||||
<param name="reference_frames/body" value="map"/>
|
||||
<param name="reference_frames/base_link" value="map"/>
|
||||
<param name="reference_frames/navigate_target" value="map"/>
|
||||
</node>
|
||||
|
||||
<!-- main camera -->
|
||||
<include file="$(find clover)/launch/main_camera.launch" if="$(arg main_camera)"/>
|
||||
|
||||
<!-- rosbridge -->
|
||||
<include file="$(find rosbridge_server)/launch/rosbridge_websocket.launch" if="$(eval rosbridge or rc)"/>
|
||||
|
||||
<!-- tf2 republisher for web visualization -->
|
||||
<node name="tf2_web_republisher" pkg="tf2_web_republisher" type="tf2_web_republisher" output="screen" if="$(arg rosbridge)"/>
|
||||
|
||||
<!-- vl53l1x ToF rangefinder -->
|
||||
<node name="rangefinder" pkg="vl53l1x" type="vl53l1x_node" output="screen" if="$(arg rangefinder_vl53l1x)">
|
||||
<param name="frame_id" value="rangefinder"/>
|
||||
<param name="min_signal" value="0.4"/>
|
||||
<param name="pass_statuses" type="yaml" value="[0, 6, 7, 11]"/>
|
||||
</node>
|
||||
|
||||
<!-- led strip -->
|
||||
<include file="$(find clover)/launch/led.launch" if="$(arg led)"/>
|
||||
|
||||
<!-- rc backend -->
|
||||
<node name="rc" pkg="clover" type="rc" output="screen" if="$(arg rc)" clear_params="true">
|
||||
<!-- Send fake GCS heartbeats. Set to "true" for upstream PX4 -->
|
||||
<param name="use_fake_gcs" value="false"/>
|
||||
</node>
|
||||
|
||||
<!-- Shell access through ROS service -->
|
||||
<node name="shell" pkg="clover" type="shell" output="screen" if="$(arg shell)"/>
|
||||
</launch>
|
||||
4
examples/positioning/aruco floor/server.ini
Normal file
4
examples/positioning/aruco floor/server.ini
Normal file
@@ -0,0 +1,4 @@
|
||||
[CHECKS]
|
||||
check_current_position = False
|
||||
# in meters; set 0 to disable this check
|
||||
start_pos_delta_max = 0.0
|
||||
11
examples/positioning/gps/client.ini
Normal file
11
examples/positioning/gps/client.ini
Normal file
@@ -0,0 +1,11 @@
|
||||
[FLIGHT]
|
||||
frame_id = gps
|
||||
|
||||
[GPS FRAME]
|
||||
lat = 55.7032026
|
||||
lon = 37.7248114
|
||||
yaw = 0.0
|
||||
|
||||
[ANIMATION]
|
||||
start_action = auto
|
||||
check_ground = True
|
||||
5
examples/positioning/gps/gps.params
Normal file
5
examples/positioning/gps/gps.params
Normal file
@@ -0,0 +1,5 @@
|
||||
1 1 ATT_EXT_HDG_M 0 6
|
||||
1 1 ATT_W_ACC 0.200000002980232239 9
|
||||
1 1 ATT_W_EXT_HDG 0.100000001490116119 9
|
||||
1 1 ATT_W_MAG 0.100000001490116119 9
|
||||
1 1 LPE_FUSION 145 6
|
||||
43
examples/positioning/gps/launch/clover.launch
Normal file
43
examples/positioning/gps/launch/clover.launch
Normal file
@@ -0,0 +1,43 @@
|
||||
<launch>
|
||||
<arg name="fcu_conn" default="usb"/>
|
||||
<arg name="fcu_ip" default="127.0.0.1"/>
|
||||
<arg name="fcu_sys_id" default="1"/>
|
||||
<arg name="gcs_bridge" default="tcp"/>
|
||||
<arg name="rangefinder_vl53l1x" default="true"/>
|
||||
<arg name="led" default="true"/>
|
||||
|
||||
<!-- log formatting -->
|
||||
<env name="ROSCONSOLE_FORMAT" value="[${severity}] [${time}]: ${logger}: ${message}"/>
|
||||
|
||||
<!-- mavros -->
|
||||
<include file="$(find clover)/launch/mavros.launch">
|
||||
<arg name="fcu_conn" value="$(arg fcu_conn)"/>
|
||||
<arg name="fcu_ip" value="$(arg fcu_ip)"/>
|
||||
<arg name="gcs_bridge" value="$(arg gcs_bridge)"/>
|
||||
</include>
|
||||
|
||||
<!-- main nodelet manager -->
|
||||
<node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" output="screen" clear_params="true">
|
||||
<param name="num_worker_threads" value="2"/>
|
||||
</node>
|
||||
|
||||
<node pkg="tf2_ros" type="static_transform_publisher" name="map_flipped_frame" args="0 0 0 3.1415926 3.1415926 0 map map_flipped"/>
|
||||
|
||||
<!-- simplified offboard control -->
|
||||
<node name="simple_offboard" pkg="clover" type="simple_offboard" output="screen" clear_params="true">
|
||||
<param name="reference_frames/body" value="map"/>
|
||||
<param name="reference_frames/base_link" value="map"/>
|
||||
<param name="reference_frames/navigate_target" value="map"/>
|
||||
</node>
|
||||
|
||||
<!-- vl53l1x ToF rangefinder -->
|
||||
<node name="rangefinder" pkg="vl53l1x" type="vl53l1x_node" output="screen" if="$(arg rangefinder_vl53l1x)">
|
||||
<param name="frame_id" value="rangefinder"/>
|
||||
<param name="min_signal" value="0.4"/>
|
||||
<param name="pass_statuses" type="yaml" value="[0, 6, 7, 11]"/>
|
||||
</node>
|
||||
|
||||
<!-- led strip -->
|
||||
<include file="$(find clover)/launch/led.launch" if="$(arg led)"/>
|
||||
|
||||
</launch>
|
||||
4
examples/positioning/gps/server.ini
Normal file
4
examples/positioning/gps/server.ini
Normal file
@@ -0,0 +1,4 @@
|
||||
[CHECKS]
|
||||
check_current_position = True
|
||||
# in meters; set 0 to disable this check
|
||||
start_pos_delta_max = 1.0
|
||||
6
examples/positioning/optical flow/client.ini
Normal file
6
examples/positioning/optical flow/client.ini
Normal file
@@ -0,0 +1,6 @@
|
||||
[FLIGHT]
|
||||
frame_id = map
|
||||
|
||||
[ANIMATION]
|
||||
start_action = auto
|
||||
check_ground = True
|
||||
90
examples/positioning/optical flow/launch/clover.launch
Normal file
90
examples/positioning/optical flow/launch/clover.launch
Normal file
@@ -0,0 +1,90 @@
|
||||
<launch>
|
||||
<arg name="fcu_conn" default="usb"/>
|
||||
<arg name="fcu_ip" default="127.0.0.1"/>
|
||||
<arg name="fcu_sys_id" default="1"/>
|
||||
<arg name="gcs_bridge" default="tcp"/>
|
||||
<arg name="web_video_server" default="true"/>
|
||||
<arg name="rosbridge" default="true"/>
|
||||
<arg name="main_camera" default="true"/>
|
||||
<arg name="optical_flow" default="true"/>
|
||||
<arg name="aruco" default="false"/>
|
||||
<arg name="rangefinder_vl53l1x" default="true"/>
|
||||
<arg name="led" default="true"/>
|
||||
<arg name="rc" default="true"/>
|
||||
<arg name="shell" default="true"/>
|
||||
|
||||
<!-- log formatting -->
|
||||
<env name="ROSCONSOLE_FORMAT" value="[${severity}] [${time}]: ${logger}: ${message}"/>
|
||||
|
||||
<!-- mavros -->
|
||||
<include file="$(find clover)/launch/mavros.launch">
|
||||
<arg name="fcu_conn" value="$(arg fcu_conn)"/>
|
||||
<arg name="fcu_ip" value="$(arg fcu_ip)"/>
|
||||
<arg name="fcu_sys_id" value="$(arg fcu_sys_id)"/>
|
||||
<arg name="gcs_bridge" value="$(arg gcs_bridge)"/>
|
||||
</include>
|
||||
|
||||
<!-- web video server -->
|
||||
<node name="web_video_server" pkg="web_video_server" type="web_video_server" if="$(arg web_video_server)" required="false" respawn="true" respawn_delay="5">
|
||||
<param name="default_stream_type" value="ros_compressed"/>
|
||||
<param name="publish_rate" value="1.0"/>
|
||||
</node>
|
||||
|
||||
<!-- aruco markers -->
|
||||
<include file="$(find clover)/launch/aruco.launch" if="$(arg aruco)"/>
|
||||
|
||||
<!-- optical flow -->
|
||||
<node pkg="nodelet" type="nodelet" name="optical_flow" args="load clover/optical_flow nodelet_manager" if="$(arg optical_flow)" clear_params="true" output="screen">
|
||||
<remap from="image_raw" to="main_camera/image_raw"/>
|
||||
<remap from="camera_info" to="main_camera/camera_info"/>
|
||||
<param name="calc_flow_gyro" value="true"/>
|
||||
<param name="roi_rad" value="0.8"/>
|
||||
</node>
|
||||
|
||||
<!-- main nodelet manager -->
|
||||
<node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" output="screen" clear_params="true">
|
||||
<param name="num_worker_threads" value="2"/>
|
||||
</node>
|
||||
|
||||
<node pkg="tf2_ros" type="static_transform_publisher" name="map_flipped_frame" args="0 0 0 3.1415926 3.1415926 0 map map_flipped"/>
|
||||
|
||||
<!-- simplified offboard control -->
|
||||
<node name="simple_offboard" pkg="clover" type="simple_offboard" output="screen" clear_params="true">
|
||||
<param name="reference_frames/body" value="map"/>
|
||||
<param name="reference_frames/base_link" value="map"/>
|
||||
<param name="reference_frames/navigate_target" value="map"/>
|
||||
</node>
|
||||
|
||||
<!-- main camera -->
|
||||
<include file="$(find clover)/launch/main_camera.launch" if="$(arg main_camera)"/>
|
||||
|
||||
<!-- rosbridge -->
|
||||
<include file="$(find rosbridge_server)/launch/rosbridge_websocket.launch" if="$(eval rosbridge or rc)"/>
|
||||
|
||||
<!-- tf2 republisher for web visualization -->
|
||||
<node name="tf2_web_republisher" pkg="tf2_web_republisher" type="tf2_web_republisher" output="screen" if="$(arg rosbridge)"/>
|
||||
|
||||
<!-- vl53l1x ToF rangefinder -->
|
||||
<node name="rangefinder" pkg="vl53l1x" type="vl53l1x_node" output="screen" if="$(arg rangefinder_vl53l1x)">
|
||||
<param name="frame_id" value="rangefinder"/>
|
||||
<param name="min_signal" value="0.4"/>
|
||||
<param name="pass_statuses" type="yaml" value="[0, 6, 7, 11]"/>
|
||||
</node>
|
||||
|
||||
<!-- led strip -->
|
||||
<include file="$(find clover)/launch/led.launch" if="$(arg led)"/>
|
||||
|
||||
<!-- rc backend -->
|
||||
<node name="rc" pkg="clover" type="rc" output="screen" if="$(arg rc)" clear_params="true">
|
||||
<!-- Send fake GCS heartbeats. Set to "true" for upstream PX4 -->
|
||||
<param name="use_fake_gcs" value="false"/>
|
||||
</node>
|
||||
|
||||
<!-- Shell access through ROS service -->
|
||||
<node name="shell" pkg="clover" type="shell" output="screen" if="$(arg shell)"/>
|
||||
|
||||
<!-- Update static directory -->
|
||||
<node pkg="roswww_static" name="roswww_static" type="main.py" clear_params="true">
|
||||
<param name="default_package" value="clover"/>
|
||||
</node>
|
||||
</launch>
|
||||
9
examples/positioning/optical flow/optical_flow.params
Normal file
9
examples/positioning/optical flow/optical_flow.params
Normal file
@@ -0,0 +1,9 @@
|
||||
1 1 ATT_EXT_HDG_M 1 6
|
||||
1 1 ATT_W_EXT_HDG 0.500000000000000000 9
|
||||
1 1 ATT_W_MAG 0.000000000000000000 9
|
||||
1 1 LPE_FLW_OFF_Z 0.000000000000000000 9
|
||||
1 1 LPE_FLW_QMIN 60 6
|
||||
1 1 LPE_FLW_R 0.200000002980232239 9
|
||||
1 1 LPE_FLW_RR 0.000000000000000000 9
|
||||
1 1 LPE_FLW_SCALE 1.000000000000000000 9
|
||||
1 1 LPE_FUSION 118 6
|
||||
4
examples/positioning/optical flow/server.ini
Normal file
4
examples/positioning/optical flow/server.ini
Normal file
@@ -0,0 +1,4 @@
|
||||
[CHECKS]
|
||||
check_current_position = True
|
||||
# in meters; set 0 to disable this check
|
||||
start_pos_delta_max = 1.0
|
||||
@@ -157,7 +157,11 @@ class ConfigManager:
|
||||
keys = self.__dict__['_name_dict'][item]
|
||||
return self.get_chain(*keys)
|
||||
except (ValueError, KeyError):
|
||||
return self.__dict__[item]
|
||||
try:
|
||||
return self.__dict__[item]
|
||||
except KeyError:
|
||||
print("config: KeyError with item {}".format(item))
|
||||
return None
|
||||
|
||||
def __setattr__(self, key, value):
|
||||
try:
|
||||
|
||||
@@ -17,7 +17,7 @@ config_version = float(default='1.0')
|
||||
|
||||
[BROADCAST]
|
||||
send = boolean(default=True)
|
||||
listen = boolean(default=True)
|
||||
listen = boolean(default=False)
|
||||
port = integer(default=8181)
|
||||
send_ip = string(default=255.255.255.255)
|
||||
# delay for message sending in seconds
|
||||
|
||||
91
tools/animation_info.py
Normal file
91
tools/animation_info.py
Normal file
@@ -0,0 +1,91 @@
|
||||
import os
|
||||
import sys
|
||||
import shutil
|
||||
import logging
|
||||
import argparse
|
||||
from tabulate import tabulate
|
||||
|
||||
logging.basicConfig( # TODO all prints as logs
|
||||
level=logging.INFO, # INFO
|
||||
stream=sys.stdout,
|
||||
format="%(asctime)s [%(name)-7.7s] [%(threadName)-12.12s] [%(levelname)-5.5s] %(message)s",
|
||||
handlers=[
|
||||
logging.StreamHandler(sys.stdout),
|
||||
])
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
# Add parent dir to PATH to import messaging_lib and config_lib
|
||||
current_dir = (os.path.dirname(os.path.realpath(__file__)))
|
||||
root_dir = os.path.realpath(os.path.join(current_dir,'..'))
|
||||
lib_dir = os.path.realpath(os.path.join(root_dir, 'lib'))
|
||||
modules_dir = os.path.realpath(os.path.join(root_dir, 'drone/modules'))
|
||||
sys.path.insert(0, lib_dir)
|
||||
sys.path.insert(0, modules_dir)
|
||||
|
||||
from config import ConfigManager
|
||||
|
||||
def load_config(config):
|
||||
config_dir = 'animation_config/config'
|
||||
spec_path = os.path.join(config_dir,'spec')
|
||||
if not os.path.exists(spec_path):
|
||||
try:
|
||||
os.makedirs(spec_path)
|
||||
except OSError:
|
||||
logger.debug("Creation of the directory {} failed".format(spec_path))
|
||||
else:
|
||||
logger.debug("Successfully created the directory {}".format(spec_path))
|
||||
|
||||
client_config_dir = os.path.realpath(os.path.join(root_dir,"drone/config"))
|
||||
client_config_path = os.path.realpath(os.path.join(client_config_dir,"client.ini"))
|
||||
client_configspec_path = os.path.realpath(os.path.join(client_config_dir,"spec/configspec_client.ini"))
|
||||
shutil.copy(client_configspec_path, spec_path)
|
||||
if os.path.exists(client_config_path):
|
||||
shutil.copy(client_config_path, config_dir)
|
||||
config.load_config_and_spec(os.path.join(config_dir,'client.ini'))
|
||||
|
||||
config = ConfigManager()
|
||||
|
||||
if __name__ == "__main__":
|
||||
parser = argparse.ArgumentParser(description="Get animation info")
|
||||
parser.add_argument('animation', nargs='?', default='../examples/animations/basic/basic.csv',
|
||||
help="Path to animation. Default is ../examples/animations/basic/basic.csv.")
|
||||
parser.add_argument('--config', action='store_true',
|
||||
help="Set this option to print config info.")
|
||||
args = parser.parse_args()
|
||||
|
||||
if args.config:
|
||||
print("\nLoading config copy from drone")
|
||||
load_config(config)
|
||||
if args.config:
|
||||
print("\nConfig name: {} | version: {}".format(config.config_name, config.config_version))
|
||||
print("Config animation settings:")
|
||||
for key, value in config.animation.items():
|
||||
if key == 'OUTPUT':
|
||||
print('\tOUTPUT:')
|
||||
for key, value in config.animation_output.items():
|
||||
print("\t\t{}: {}".format(key, value))
|
||||
else:
|
||||
print("\t{}: {}".format(key, value))
|
||||
print("Config flight settings:")
|
||||
for key, value in config.flight.items():
|
||||
print("\t{}: {}".format(key, value))
|
||||
print("\nLoading animation {}".format(args.animation))
|
||||
import animation
|
||||
a = animation.Animation(args.animation, config)
|
||||
print("\nAnimation id: {} | state: {}".format(a.id, a.state))
|
||||
print("Frames separation:")
|
||||
print("\tStatic begin frames: {}".format(animation.get_numbers(a.transformed_frames[:a.takeoff_index])))
|
||||
print("\tTakeoff frames: {}".format(animation.get_numbers(a.transformed_frames[a.takeoff_index:a.route_index])))
|
||||
print("\tRoute frames: {}".format(animation.get_numbers(a.transformed_frames[a.route_index:a.land_index])))
|
||||
print("\tLand frames: {}".format(animation.get_numbers(a.transformed_frames[a.land_index:a.static_end_index])))
|
||||
print("\tStatic end frames: {}".format(animation.get_numbers(a.transformed_frames[a.static_end_index:])))
|
||||
header = animation.get_default_header()
|
||||
print("\nOutput frames on fly start action:")
|
||||
data = animation.get_table(a.output_frames, header)
|
||||
print (tabulate(data, headers=header))
|
||||
print("\nOutput frames on takeoff start action:")
|
||||
data = animation.get_table(a.output_frames_takeoff, header)
|
||||
print (tabulate(data, headers=header))
|
||||
|
||||
shutil.rmtree('animation_config')
|
||||
Reference in New Issue
Block a user