Робот-попрошайка на ROS и нейросетках07.05.2020 20:37

Обычно к таким поделкам возникает два вопроса: «как?» и «для чего?» Первому вопросу посвящена сама публикация, а на второй я отвечу сразу:

Этот проект я затеял для того, чтобы освоить робототехнику, начиная с Raspberry Pi и камеры. Как известно, один из лучших способов чему-нибудь научиться — это придумать себе техзадание и попытаться его выполнить, по ходу получая необходимые навыки.

На тот момент у меня еще не было светлых идей в области робототехники, поэтому я решил сделать исключительно фановый проект — робота-попрошайку. В итоге получился автономный робот на Raspberry Pi и ROS, использующий Movidius Neural Cumpute Stick для обнаружения лиц. Он бродит по помещению, ищет людей, и трясет перед ними банкой. Вот как выглядит этот робот:

Робот случайным образом двигается по помещению, а если замечает человека — подкатывает к нему и трясет банкой для мелочи. Смеха ради я добавил ему немного мимики — он умеет двигать бровями:

После первой попытки робот пытается снова найти лицо в поле зрения, поворачивается к человеку и трясет банкой еще раз. А вот что случится, если в этот момент уйти:

Робот

Идею робота-попрошайки я взял из журнала «Популярная Механика». Прообраз за авторством Криса Экерта под названием Gimme выглядит весьма эстетично.

Я же больше хотел сконцентрироваться на функциональности, поэтому корпус был собран из подручных материалов. В частности, ПВХ уголки показали себя наиболее универсальным материалом, с помощью которого можно соединить практически любые две детали. Кажется, что на текущий момент робот процентов на пять состоит из ПВХ уголков и винтов М3. Сам корпус представляет собой три платформы из ламината, на которые монтируется голова и вся электроника.

Основой робота является Raspberry Pi 2B, а код написан на C++ и лежит на GitHub.

Зрение

Для восприятия реальности робот использует камеру Paspberry Pi Camera Module v2, которой можно управлять с помощью библиотеки RaspiCam.

Для обнаружения лиц я попробовал несколько разных подходов. Качество классических детекторов из OpenCV меня не удовлетворило, поэтому в итоге я пришел к довольно нестандартному решению. Детекцией лиц занимается нейронная сеть, работающая на устройстве Movidius Neural Compute Stick (NCS) под управлением фреймворка OpenVINO.

NCS — это такая железка для эффективного запуска нейронных сетей, внутри которой находятся несколько векторных процессоров, специально заточенных под это. Устройство подключается по USB и потребляет всего 1 Ватт мощности. Таким образом, NCS выступает в роли со-процессора для Raspberry Pi, которая нейросети не тянет. Пока NCS обрабатывает очередной кадр, процессор Paspberry свободен для других операций. Стоит заметить, что для оптимальной работы устройства требуется интерфейс USB версии 3.0, которого на старых версиях Raspberry нет; с USB 2.0 тоже работает, просто медленнее. Также, чтобы не загораживать USB-разъемы Raspberry, я подключаю к ней NCS через короткий USB-кабель. Я подробно писал о работе с Neural Compute Stick в своей предыдущей статье.

Сначала я пробовал обучить свой детектор лиц с архитектурой MobileNet + SSD на открытых датасетах. Детектор действительно работал, но не очень устойчиво: при неизбежном ухудшении условий съемки (засветка и смазанные кадры) качество детектора сильно проседало. Однако спустя некоторое время в OpenVINO появились готовые детекторы лиц, я и переключился на детектор с архитектурой SqueezeNet light + SSD, который не только лучше работал в самых разных условиях съемки, но и был быстрее.

Перед тем, как загружать изображение на NCS для получения предсказаний детектора, изображение нужно предобработать. Выбранный мной детектор работает с цветными изображениями , поэтому изображение нужно сначала сжать. Для этого я использую самый легковесный алгоритм масштабирования — методом ближайшего соседа (INTER_NEAREST в библиотеке OpenCV). Работает чуточку быстрее, чем методы интерполяции, а на результат почти не влияет. Также стоит обратить внимание на порядок каналов изображения: детектор ожидает порядок BGR, поэтому такой же нужно выставить у камеры.

Еще я пробовал разделять обработку видео на два потока, один из которых получал с камеры очередной кадр и обрабатывал его, а другой в это время загружал предыдущий кадр на NCS и ждал результатов детектора. При такой схеме, технически, скорость обработки увеличивается, но также увеличивается задержка между получением кадра и получением детекций для него. Из-за этого отставания от реальности следить за лицом становится только сложнее, поэтому в итоге от этой схемы я отказался.

Помимо собственно обнаружения лиц, их нужно еще отслеживать, чтобы избежать ошибок детектора. Для этого я использую легковесный трекер Simple Online Realtime Tracker (SORT). Этот простой трекер состоит из двух частей: для сопоставления объектов на соседних кадрах применяется Венгерский алгоритм, а для предсказания траектории объекта, если он внезапно пропадает — фильтр Калмана. Пока я игрался с отслеживанием лиц, я обнаружил, что траектории, предсказанные фильтром Калмана, могут быть очень неправдоподобными при резких движениях, что опять же только усложняет процесс.

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

Верхняя платформа, слева направо: камера, сервоприводы для управления головой и бровями, выключатель, клеммы питания, Большая Красная Кнопка.

Движение

Для движения у робота есть пять сервоприводов: две сервы непрерывного вращения FS5103R крутят колеса; есть еще две обычных FS5109M, одна из которых вращает головy, а вторая трясет банкой; наконец, маленькая SG90 двигает бровями.

Если честно, мини-сервы SG90 показались мне хламом — у одной из моих серв была неправильная ширина управляющего пульса, а среди остальных четырех выжила только одна. Справедливости ради, одну из серв я случайно снес локтем, но остальные две просто не выдержали нагрузки (раньше я их использовал для головы и банки). Даже ту последнюю серву, которой досталась самая простая работа — двигать бровями, приходится время от времени тыкать палкой, чтобы ее не клинило. С другими сервоприводами я проблем не заметил. Правда, сервоприводы непрерывного вращения иногда приходится калибровать, чтобы в неактивном состоянии они не крутились — для этого на них есть маленький регулятор, который можно крутить часовой отверткой.

Управлять сервоприводами с Raspberry, оказывается, не так просто. Во-первых, они управляются при помощи широтно-импульсной модулиции (ШИМ / PWM), а на Raspberry всего два пина, на которых ШИМ поддерживается аппаратно. Во-вторых, конечно, от Raspberry не получится запитать сервоприводы, она этого не выдержит. К счастью, эти проблемы решаются с помощью внешнего ШИМ-контроллера.

Adafruit PCA9685 — это 16-канальный ШИМ-контроллер, которым можно управлять по интерфейсу I2C. Также очень удобно, что на нем есть клеммы для подвода питания для сервоприводов. Мало того, [теоретически] можно соединить в цепочку до 62 контроллеров, получив при этом до 992 управляющих пинов — для этого нужно каждому контроллеру назначить уникальный адрес при помощи специальных перемычек. Так что если вам вдруг понадобится армия сервоприводов — вы знаете, что делать.

Для управления PCA9685 есть высокоуровневая библиотека, работающая как расширение WiringPi. Работать с этой штукой довольно удобно — при инициализации она создает 16 виртуальных пинов, в которые можно записать ШИМ-сигнал, но сначала придется рассчитать число тиков. Чтобы повернуть рычаг сервопривода до определенного угла в диапазоне [0, 180], нужно сначала перевести этот угол в диапазон длин управляющего пульса в миллисекундах [SERVO_MS_MIN, SERVO_MS_MAX]. Для всех моих серв эти значения примерно равны 0.6 мс и 2.4 мс соответственно. Вообще эти значения можно найти в даташите сервопривода, но практика показала, что они могут отличаться, поэтому их, возможно, придется подбирать. Затем полученное значение делим на 20 мс (стандартное значение длины управляющего цикла) и умножаем на максимальное число тиков PCA9685 (4096):

void driveDegs(float angle, int pin) {
    int ticks = (int) (PCA_MAX_PWM * (angle/180.0f*(SERVO_MS_MAX-SERVO_MS_MIN) + SERVO_MS_MIN) / 20.0f); 
    pwmWrite(pin, ticks);
}

Аналогично это делается с сервами непрерывного вращения — вместо угла задаем скорость в диапазоне [-1,1].

Шасси робота, как и корпус, я собирал из подручных средств: на сервоприводы непрерывного вращения поставил мебельные колеса, а в качестве третьего колеса выступает мебельная шаровая опора. Раньше вместо нее стояло колесо на вращающейся опоре, но с таким шасси было сложно делать точные повороты, поэтому пришлось заменить. Под банкой тоже стоит маленькое колесико, чтобы перенести часть веса с сервопривода на корпус. Простая вещь, которая для меня не была очевидной изначально — рычаги сервоприводов нужно обязательно закреплять винтом, особенно для колес, чтобы они не отваливались по пути. Из-за такой глупости пришлось один раз переделать шасси. Также я сделал роботу широкий бампер из ПВХ уголков, чтобы он не так часто застревал.

Теперь о том, что можно с этим делать. Во-первых, можно трясти банкой и двигать бровями — для этого нужно просто поворачивать рычаг сервопривода на заранее подобранные углы.

Во-вторых, можно вращать головой. Я не хотел, чтобы голова крутилась с максимальной скоростью вращения сервопривода, потому что на ней стоит камера. Поэтому я решил программно снизить скорость: нужно повернуть рычаг на маленький угол, затем подождать несколько миллисекунд — и так пока желаемый угол не будет достигнут. При этом нужно запоминать текущее абсолютное положение головы и каждый раз проверять, не вышла ли она за допустимые границы (на моем роботе это в диапазоне [10, 90] градусов).

В-третьих, можно менять направление движения, меняя скорость вращения колес. Таким же образом можно вращать платформу, например, чтобы следить за лицом. Угловая скорость вращения зависит как от самих сервоприводов, так и от их расположения на шасси, поэтому ее проще измерить один раз и потом учитывать при поворотах. Чтобы найти необходимую задержку между включением моторов для вращения и их выключением, нужно модуль угла поделить на угловую скорость.

Наконец, вращать голову и шасси можно одновременно и асинхронно, чтобы не тратить время. Я это делаю вот так:

auto waitRotation = std::async(std::launch::async, rotatePlatform, platformAngle);
success = driveHead(headAngle);
waitRotation.wait();

Центральная платформа, слева направо: PCA9685, шина питания, Raspberry Pi, АЦП MCP3008

Навигация

Тут я не стал ничего усложнять, поэтому робот для навигации использует всего два инфракрасных дальномера Sharp GP2Y0A02YK. Это тоже не так просто, потому что датчики аналоговые, но у Raspberry, в отличие от Arduino, нет аналоговых входов. Эта проблема решается аналого-цифровым преобразователем (АЦП / ADC) — я использую 10-битный, 8-канальный MCP3008. Он продается в виде отдельной микросхемы, поэтому его пришлось припаять на печатную плату и туда же еще напаять пинов, чтобы было удобнее подключаться. Также по совету моего бати, который больше шарит в схемотехнике, я припаял два конденсатора (керамический и электролитический) между ножками питания и земли, чтобы гасить помехи от цифровой части всей схемы. Датчики выдают на выходе не более трех вольт, поэтому в качестве эталонного напряжения АЦП (VREF) можно подключить 3.3v с Raspberry — такое же, как и для питания MCP3008 (VDD).

MCP3008 можно управлять по интерфейсу SPI, и для этого даже несложно найти готовый код на GitHub.

unsigned int analogRead(mcp3008Spi &adc, unsigned char channel)
{
    unsigned char spi_data[3];
    unsigned int val = 0;

    spi_data[0] = 1;  // start bit
    spi_data[1] = 0b10000000 | ( channel << 4); // mode and channel
    spi_data[2] = 0; // anything
    adc.spiWriteRead(spi_data, sizeof(spi_data));
  
    // read value, combine last two bits of second byte with whole third byte
    val = (spi_data[1]<< 8) & 0b1100000000; 
    val |= (spi_data[2] & 0xff);
    return val;
}

На MCP3008 нужно отправить три байта, где в первом байте записан стартовый бит, а во втором — режим и номер канала (0–7). Обратно получаем тоже три байта, после чего нужно склеить два младших бита второго байта со всеми битами третьего.

Теперь, когда мы можем получить значения с датчиков, их нужно откалибровать, потому что два датчика могут между собой немного различаться. Вообще отображение из расстояния в силу сигнала у этих датчиков нелинейное и не очень простое (подробнее в даташите, pdf). Поэтому достаточно только подобрать два коэффициента, при умножении на которые датчики будут выдавать значение 1.0 на каком-нибудь осмысленном, одинаковом расстоянии.

Показания датчиков могут быть довольно шумными, особенно на сложных препятствиях, поэтому я использую экспоненциально взвешенное скользящее среднее (EWMA) для сглаживания сигнала каждого датчика. Параметры сглаживания я подобрал на глаз, чтобы сигнал не шумел и при этом не сильно отставал от реальности.

Вид спереди: банка, дальномеры и бампер.

Питание

Для начала оценим, какой ток будет потреблять робот (про потребление тока Raspberry и периферией):

• Raspberry Pi 2B: не менее 350 мА, но больше под нагрузкой (до 750–820 мА (?));
• Камера: порядка 250 мА;
• Neural Compute Stick: заявленная потребляемая мощность 1 ватт, при напряжении 5 вольт на USB это 200 мА;
• ИК датчики: по 33 мА каждый (даташит, pdf);
• MCP3008: очень мало, порядка 0.5 мА (даташит, pdf);
• PCA9685: тоже мало, 6 мА (даташит, pdf);
• Сервоприводы: от ~150–200 мА до 1500–2000 мА при максимальной нагрузке (stall current), но в роботе они так сильно нагружаться не будут (даташит FS5109M, pdf)
• HDMI (подключение монитора для отладки): 50 мА;
• Клавиатура + мышь (для отладки): ~200 мА.

Итого можно прикинуть, что 1.5–2.5 ампера должно хватить при условии, что все сервоприводы не движутся одновременно под большой нагрузкой. При этом для Raspberry нужны условные 5 вольт напряжения, а для сервоприводов — 4.8–6 вольт. Осталось найти источник питания, который удовлетворяет этим требованиям.

В итоге я решил запитать робота от аккумуляторов формата 18650. Если взять два аккумулятора ROBITON 3.4/Li18650 (3.6 вольт, 3400 мАч, максимальный ток разряда 4875 мА) и соединить их последовательно, то они смогут выдавать до 4.8 ампер при напряжении 7.2 вольт. При токе потребления 1.5–2.5 ампера их должно хватить на час или два.

У аккумуляторов, кстати, есть подвох: несмотря на указанный форм-фактор 18650, их размеры далеко не  мм — они на несколько миллиметров длиннее из-за встроенной схемы управления зарядкой. Из-за этого мне пришлось ножом подхачить батарейный отсек, чтобы они туда поместились.

Осталось только понизить напряжение до 5 вольт. Для этого я использую два отдельных понижающих DC-DC конвертера DFRobot Power Module. Эта железка позволяет понижать напряжение при входном напряжении 3.6–25 вольт и разнице напряжений не менее 0.6 вольт. Для удобства на ней есть переключатель, позволяющий выбрать ровно 5 вольт на выходе, либо можно настроить произвольное выходное напряжение при помощи специального регулятора. Я настроил оба конвертера на 5 вольт; один из них питает Raspberry через Micro-USB разъем, а второй питает сервоприводы через клеммы PCA9685. Это нужно для того, чтобы максимально развязать питание логической и силовой частей робота, чтобы они не мешали друг другу.

На этапе отладки я использовал вместо аккумуляторов китайский блок питания на 9 вольт, 2 ампера, и его хватало для работы робота — я подключал его так же, как и аккумуляторы, к двум DC-DC преобразователям. Поэтому для удобства я сделал клеммы на роботе, к которым можно подключить блок питания или батарейный отсек на выбор. Это очень помогло, когда я полностью переписывал весь код на ROS, и мне приходилось подолгу отлаживать робота, в том числе сервоприводы.

Для удобства еще пришлось сделать «шину питания» — по сути, просто кусок платы с тремя рядами соединенных пинов для земли, 3.3v и 5v соответственно. Шина подключается к соответствующим пинам Raspberry. От 5v шины питаются только ИК дальномеры, а от 3.3v шины — MCP3008 и PCA9685.

Ну и конечно, по старой доброй традиции я поставил на робота Большую Красную Кнопку — она при нажатии просто прерывает всю цепь питания. Для экстренной остановки ее использовать не приходилось, но включать робота с помощью кнопки и правда удобнее.

Нижняя платформа, слева направо: батарейный отсек, NCS, DC-DC преобразователи, сервоприводы с колесами, дальномеры.

Управление роботом

На Raspberry Pi 2B нет Wi-Fi, поэтому мне приходится подключаться по ssh через Ethernet кабель (кстати, это можно делать и напрямую от ноутбука, не используя роутер). В получается такая схема: подключаемся по ssh через кабель, запускаем робота и сдергиваем кабель. Затем его можно будет вернуть на место, чтобы снова получить доступ к Raspberry. Есть и более элегантные решения, но я решил не усложнять.

Чтобы робота можно было легко остановить, не выключая, я добавил на него массивный советский переключатель (с подводной лодки?), при выключении которого программа завершается, а робот останавливается.

Переключатель подключается к земле и к одному из GPIO пинов Raspberry, а читать с него можно при помощи библиотеки WiringPi:

wiringPiSetup();
pinMode(PIN_SWITCH, INPUT);
pullUpDnControl(PIN_SWITCH, PUD_UP);
bool value = digitalRead(BB_PIN_SWITCH);

Стоит заметить, что при таком подключении напряжение на пине нужно подтянуть до 3.3v, и при этом он будет выдавать высокий сигнал в разомкнутом состоянии, и низкий сигнал в замкнутом.

Собираем все вместе

Потоки

Теперь все вышеперечисленное нужно объединить в одну программу, управляющую роботом. В первой версии робота я сделал это при помощи потоков (pthread). Эта версия находится в ветке master, но код там довольно страшный.

Программа работает в четыре потока: один поток забирает кадры с камеры и запускает детектор на NCS; второй поток читает данные с дальномеров; третий поток следит за переключателем и выставляет глобальную переменную is_running в false, если он выключен; главный поток отвечает за поведение робота и управление сервоприводами. У потоков есть общие с главным потоком указатели, по которым они пишут результаты своей работы. Вектора, в которых хранится информация о лицах, найденных детектором, я ограничил мьютексом, а другие, более простые общие переменные, объявил как атомарные. Для координации потока детектора лиц с главным потоком есть флаг face_processed, который сбрасывается, когда с детектора приходит новый результат, и поднимается, когда главный поток использует этот результат для выбора поведения — это нужно для того, чтобы не обрабатывать старые данные, которые могут быть не актуальны после перемещения.

ROS

Версия с потоками прекрасно работала, однако я все это затеял для того, чтобы чему-нибудь научиться, так почему бы заодно не освоить ROS? Этот фреймворк давно был у меня на слуху, и мне даже приходилось немного работать с ним на хакатоне, поэтому в итоге я решил переписать весь код на ROS. Эта версия кода лежит в дефолтной ветке ros и выглядит гораздо приличнее. Понятно, что реализация на ROS почти наверное будет медленнее реализации на потоках из-за накладных расходов на пересылку сообщений и все остальное — вопрос только в том, насколько?

Для своего робота я не использовал каких-либо готовых пакетов с алгоритмами из ROS, я только оформил код робота в виде отдельного пакета, состоящего из пяти узлов, общающихся между собой при помощи сообщений и сервисов ROS.

Самый простой узел, switch_node, следит за состоянием переключателя. Как только переключатель оказывается выключен, узел начинает спамить неинформативные сообщения типа bool в топик terminator. Это — сигнал главному узлу о том, что пора завершать работу.

Второй узел, sensor_node, периодически читает показания обоих ИК дальномеров и отправляет их в топик sensor_state одним сообщением. Также этот узел отвечает за обработку сигнала: масштабирование калибровочными коэффициентами и скользящее среднее.

Третий узел, camera_node, отвечает за все, что связано с лицами: берет изображения с камеры, обрабатывает их, получает результаты детектора, пропускает их через трекер, а затем находит ближайшее к центру кадра лицо — остальные робот все равно не использует, а сообщения хочется делать поменьше. Сообщения, которые узел отправляет в топик camera_state, содержат номер кадра, факт наличия лица (потому что об отсутствии лица тоже нужно знать), относительные координаты левого верхнего угла, ширину и высоту лица. Вот так в итоге выглядит описание типа сообщения в файле DetectionBox.msg:

int64 count
bool present
float32 x
float32 y
float32 width
float32 height

Четвертый узел, servo_node, отвечает за сервоприводы. Во-первых, он поддерживает сервис servo_action, который позволяет выполнить одно из действий сервоприводами по его номеру: перевести весь узел в начальное состояние (брови, банку, голову, остановить шасси); перевести голову в начальное состояние; потрясти банкой; изобразить бровями одно из трех выражений (доброе, нейтральное, злое). Во-вторых, с помощью сервиса servo_speed можно установить новые скорости для обоих колес, отправив их в запросе. Оба сервиса ничего не возвращают. Наконец, есть сервис servo_head_platform, который позволяет вращать голову и/или шасси на определенный угол относительно текущего положения. Этот сервис возвращает true, если голову удалось повернуть хотя бы частично, и false иначе — в случае, когда голова уже находится на границе допустимого угла, а мы пытаемся повернуть ее еще дальше. Если в запросе оба угла ненулевые, сервис совершает вращение асинхронно, как было указано выше. В главном цикле серво-узел ничего не делает.

Вот так, например, выглядит описание сервиса servo_head_platform:

float32 head_delta
float32 platform_delta
---
bool head_success

Каждый из перечисленных узлов поддерживает сервис terminate_{switch, camera, sensor, servo} с пустым запросом-ответом, который останавливает работу узла. Реализовано это таким образом:

...
std::atomic_bool is_running; // global

bool terminate_node(std_srvs::Empty::Request &req, std_srvs::Empty::Response &ignored) {
    is_running = false;
    return true;
}

int main(int argc, char **argv) {
    is_running = true;
    ...
    while (is_running && ros::ok()) {
        // do stuff
    }
    ...
}

У узла есть глобальная переменная is_running, от значения которой зависит главный цикл узла. Сервис просто сбрасывает эту переменную, и главный цикл прерывается.

Есть еще главный узел beggar_bot, в котором реализована основная логика робота. До начала главного цикла он подписывается на топики sensor_state и camera_state и в функциях-коллбэках сохраняет содержимое сообщений в глобальные переменные. Еще он подписан на топик terminator, коллбэк для которого сбрасывает флаг is_running, прерывая главный цикл. Также перед началом цикла узел объявляет интерфейсы для сервисов серво-узла и ждет несколько секунд, чтобы другие узлы успели запуститься. После окончания главного цикла этот узел вызывает сервисы terminate_{switch, camera, sensor, servo}, таким образом выключая все остальные узлы, а затем выключается сам. То есть, при выключении переключателя все пять узлов завершают работу.

Переход на ROS вынудил меня довольно сильно изменить структуру программы. Например, раньше можно было с большой частотой менять скорость колес, и это нормально работало, но ROS-сервис работает на порядок медленнее, поэтому пришлось переписать код так, чтобы сервис вызывался только тогда, когда скорость реально меняется (в «ленивом режиме»).

ROS также позволяет довольно удобно запускать все узлы робота. Для этого нужно написать файл запуска .launch, перечисляющий все узлы и другие атрибуты робота в формате xml, а затем выполнить команду:

roslaunch beggar_bot robot.launch

ROS против pthread

Теперь, наконец, можно сравнить скорость работы ROS-версии и pthread-версии. Я это делаю таким образом: поток / узел, отвечающий за работу с камерой, считает свой FPS (как наиболее медленный элемент) при условии того, что все остальное тоже работает. Для pthread-версии я стабильно получал FPS 9.99 или около того, для ROS-версии получилось примерно 8.3. На самом деле, этого вполне достаточно для такой игрушки, но накладные расходы весьма заметны.

Поведение робота

Затея вполне простая: если робот видит человека, он должен подъехать к нему и потрясти банкой. Трясти банкой довольно просто и весело, но сначала нужно до человека добраться.

Есть функция follow_face, которая при наличии в кадре лица поворачивает шасси и голову робота в его сторону (при этом учитывается только ближайшее к центру лицо). Это нужно для того, чтобы робот всегда держал курс на человека, если он есть в кадре, а также смотрел прямо в лицо, когда трясет банкой.

Для синхронизации этой функции с топиком camera_state используется такая же переменная face_processed, как и в версии с потоками. Идея все та же — мы хотим обрабатывать данные только один раз, потому что робот постоянно движется. Функция сначала ждет, пока коллбэк топика с детекциями не опустит флаг о том, что последний кадр обработан. Пока она ждет, она постоянно вызывает ros::spinOnce(), чтобы получать новые сообщения (вообще это нужно делать везде, где программа ожидает новые данные). Если лицо в кадре есть, рассчитываются углы, на которые нужно повернуть платформу и голову — это можно сделать, зная относительные координаты центра лица и поле зрения камеры по горизонтали и вертикали. После этого можно вызывать сервис servo_head_platform и двигать робота.

Тут есть один тонкий момент: информация о положении лица отстает от реального перемещения лица и может отставать от перемещений самого робота. Поэтому робот может переоценивать угол поворота, из-за чего голова начинает двигаться туда-сюда с нарастающей амплитудой. Чтобы это предотвратить, я делаю задержки после перемещения (300 мс), а также пропускаю один кадр, следующий за перемещением. Для этой же цели углы поворота шасси и головы умножаются на коэффициент 0.8 (имеет смысл P-компоненты PID-регулятора).

Функция follow_face возвращает статус лица. Лицо может: отсутствовать, быть достаточно близко к центру, находиться на слишком большом расстоянии от робота; еще один вариант — когда мы повернули робота и не знаем, что стало с лицом (в процессе поиска); есть еще редкий случай, когда голова находится на границе, из-за чего к лицу повернуться нельзя.

В главном цикле происходит довольно простая вещь:

1. Вызывать follow_face до тех пор, пока у лица не будет определенный статус (любой, кроме «в процессе поиска»). По окончании этого шага робот будет смотреть прямо на лицо.
2. Если лицо найдено и оно близко:
   
   1. Потрясти банкой;
   2. Найти лицо еще раз;
   3. Если лицо на месте, сделать доброе выражение бровями и потрясти банкой еще раз;
   4. Если же лицо исчезло, сделать сердитое выражение бровями;
   5. Развернуться, переход в начало цикла.
   
   
3. Если лица нет (или оно далеко) — навигация по помещению:
   
   1. Если с обеих сторон до препятствий далеко — ехать вперед (если лицо было найдено, но оказалось слишком далеко, робот поедет к человеку);
   2. Если с обеих сторон препятствия близко, совершить случайный поворот в диапазоне ;
   3. Если препятствие только с одной стороны, повернуться в противоположную сторону на случайный угол ;
   4. Если движение вперед продолжается слишком долго (возможно, застрял), отъехать немного назад и совершить случайный поворот в диапазоне ;
   
   

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

Заключение

Несмотря на кажущуюся простоту, этот проект покрывает много нетривиальных тем: работу с аналоговыми датчиками, работу с ШИМ, компьютерное зрение, координацию асинхронных задач. К тому же, это просто безумно весело. Вероятно, дальше я займусь чем-нибудь более осмысленным, но больше с уклоном в глубокое обучение.

В качестве бонуса — галерея:

© Habrahabr.ru