diff --git a/docs/ru/animation.md b/docs/ru/animation.md new file mode 100644 index 0000000..ea0809c --- /dev/null +++ b/docs/ru/animation.md @@ -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. +``` + +Данная утилита выводит полную информацию о настройках анимации и конфигурации клиента (опционально), а также выводит оба возможных варианта воспроизведения анимации, что позволяет проанализировать действия коптеров перед реальным полётом. diff --git a/docs/ru/client.md b/docs/ru/client.md index 3c53f05..616aaf3 100644 --- a/docs/ru/client.md +++ b/docs/ru/client.md @@ -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) diff --git a/docs/ru/positioning.md b/docs/ru/positioning.md new file mode 100644 index 0000000..510c352 --- /dev/null +++ b/docs/ru/positioning.md @@ -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` из верхнего меню сервера. diff --git a/docs/ru/start-tutorial.md b/docs/ru/start-tutorial.md index 20601d2..6880572 100644 --- a/docs/ru/start-tutorial.md +++ b/docs/ru/start-tutorial.md @@ -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 ``` -* Теперь при запуске серверного приложения настроенные коптеры будут отображаться в виде таблицы. Также можно подключаться к 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): + +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 метр вправо и начнёт опускаться вниз. После касания земли в анимации коптер перейдёт в режим посадки, заглушит двигатели, и продолжит менять цвет светодиодной ленты до окончания анимации. + +Результат выполнения анимации должен выглядеть так (с точностью до настройки коптера): + +basic animation + +**Подробная информация по работе модуля анимации находится [здесь](animation.md).** diff --git a/drone/client.py b/drone/client.py index 258c483..154581a 100644 --- a/drone/client.py +++ b/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__": diff --git a/drone/config/spec/configspec_client.ini b/drone/config/spec/configspec_client.ini index 0e00840..0287009 100644 --- a/drone/config/spec/configspec_client.ini +++ b/drone/config/spec/configspec_client.ini @@ -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) diff --git a/drone/modules/animation.py b/drone/modules/animation.py index c805fac..18231e3 100644 --- a/drone/modules/animation.py +++ b/drone/modules/animation.py @@ -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) diff --git a/drone/modules/led.py b/drone/modules/led.py index 4e00542..cccfc27 100644 --- a/drone/modules/led.py +++ b/drone/modules/led.py @@ -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!") diff --git a/drone/modules/tasking.py b/drone/modules/tasking.py index c64dbcb..ed1e910 100644 --- a/drone/modules/tasking.py +++ b/drone/modules/tasking.py @@ -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: diff --git a/drone/requirements.txt b/drone/requirements.txt index 61ece20..5267e38 100644 --- a/drone/requirements.txt +++ b/drone/requirements.txt @@ -2,4 +2,5 @@ selectors2 psutil configobj watchdog -geographiclib \ No newline at end of file +geographiclib +tabulate diff --git a/drone/tests/animation_test.py b/drone/tests/animation_test.py index 2291e30..534509f 100644 --- a/drone/tests/animation_test.py +++ b/drone/tests/animation_test.py @@ -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') diff --git a/examples/animations/basic/basic_real.gif b/examples/animations/basic/basic_real.gif new file mode 100644 index 0000000..07a78e5 Binary files /dev/null and b/examples/animations/basic/basic_real.gif differ diff --git a/examples/configurations/gps_sim/client.ini b/examples/configurations/gps_sim/client.ini deleted file mode 100644 index 5f61403..0000000 --- a/examples/configurations/gps_sim/client.ini +++ /dev/null @@ -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 diff --git a/examples/fcu_parameters/clever_lpe_flow.params b/examples/fcu_parameters/clever_lpe_flow.params index 04a358b..a0de7d3 100644 --- a/examples/fcu_parameters/clever_lpe_flow.params +++ b/examples/fcu_parameters/clever_lpe_flow.params @@ -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 \ No newline at end of file +1 1 LPE_FUSION 118 6 diff --git a/examples/positioning/aruco floor/aruco.params b/examples/positioning/aruco floor/aruco.params new file mode 100644 index 0000000..7caf8cf --- /dev/null +++ b/examples/positioning/aruco floor/aruco.params @@ -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 diff --git a/examples/positioning/aruco floor/client.ini b/examples/positioning/aruco floor/client.ini new file mode 100644 index 0000000..4d1d16b --- /dev/null +++ b/examples/positioning/aruco floor/client.ini @@ -0,0 +1,6 @@ +[FLIGHT] +frame_id = aruco_map + +[ANIMATION] +start_action = takeoff +check_ground = False diff --git a/examples/positioning/aruco floor/launch/aruco.launch b/examples/positioning/aruco floor/launch/aruco.launch new file mode 100644 index 0000000..39f2ad0 --- /dev/null +++ b/examples/positioning/aruco floor/launch/aruco.launch @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/positioning/aruco floor/launch/clover.launch b/examples/positioning/aruco floor/launch/clover.launch new file mode 100644 index 0000000..2cc8de1 --- /dev/null +++ b/examples/positioning/aruco floor/launch/clover.launch @@ -0,0 +1,85 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/positioning/aruco floor/server.ini b/examples/positioning/aruco floor/server.ini new file mode 100644 index 0000000..c7145e4 --- /dev/null +++ b/examples/positioning/aruco floor/server.ini @@ -0,0 +1,4 @@ +[CHECKS] +check_current_position = False +# in meters; set 0 to disable this check +start_pos_delta_max = 0.0 diff --git a/examples/positioning/gps/client.ini b/examples/positioning/gps/client.ini new file mode 100644 index 0000000..7fe2aad --- /dev/null +++ b/examples/positioning/gps/client.ini @@ -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 diff --git a/examples/positioning/gps/gps.params b/examples/positioning/gps/gps.params new file mode 100644 index 0000000..c67b018 --- /dev/null +++ b/examples/positioning/gps/gps.params @@ -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 diff --git a/examples/positioning/gps/launch/clover.launch b/examples/positioning/gps/launch/clover.launch new file mode 100644 index 0000000..39e5803 --- /dev/null +++ b/examples/positioning/gps/launch/clover.launch @@ -0,0 +1,43 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/positioning/gps/server.ini b/examples/positioning/gps/server.ini new file mode 100644 index 0000000..c33bb4e --- /dev/null +++ b/examples/positioning/gps/server.ini @@ -0,0 +1,4 @@ +[CHECKS] +check_current_position = True +# in meters; set 0 to disable this check +start_pos_delta_max = 1.0 diff --git a/examples/positioning/optical flow/client.ini b/examples/positioning/optical flow/client.ini new file mode 100644 index 0000000..c8c427f --- /dev/null +++ b/examples/positioning/optical flow/client.ini @@ -0,0 +1,6 @@ +[FLIGHT] +frame_id = map + +[ANIMATION] +start_action = auto +check_ground = True diff --git a/examples/positioning/optical flow/launch/clover.launch b/examples/positioning/optical flow/launch/clover.launch new file mode 100644 index 0000000..41b704c --- /dev/null +++ b/examples/positioning/optical flow/launch/clover.launch @@ -0,0 +1,90 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/positioning/optical flow/optical_flow.params b/examples/positioning/optical flow/optical_flow.params new file mode 100644 index 0000000..a0de7d3 --- /dev/null +++ b/examples/positioning/optical flow/optical_flow.params @@ -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 diff --git a/examples/positioning/optical flow/server.ini b/examples/positioning/optical flow/server.ini new file mode 100644 index 0000000..c33bb4e --- /dev/null +++ b/examples/positioning/optical flow/server.ini @@ -0,0 +1,4 @@ +[CHECKS] +check_current_position = True +# in meters; set 0 to disable this check +start_pos_delta_max = 1.0 diff --git a/lib/config.py b/lib/config.py index e820f0d..0596e91 100644 --- a/lib/config.py +++ b/lib/config.py @@ -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: diff --git a/server/config/spec/configspec_server.ini b/server/config/spec/configspec_server.ini index 6d5d014..fff51d2 100644 --- a/server/config/spec/configspec_server.ini +++ b/server/config/spec/configspec_server.ini @@ -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 diff --git a/tools/animation_info.py b/tools/animation_info.py new file mode 100644 index 0000000..dcc1fc8 --- /dev/null +++ b/tools/animation_info.py @@ -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') \ No newline at end of file