Урок 11
Введение: Данная задача, является первой из группы задач на использование полезной нагрузки нашего полигона. На этом уроке мы рассмотрим детектирование нагревательного элемента при помощи тепловизора. Эта задача слегка оторвана от остальных задач на использование полезной нагрузки, потому что она тематически связанна с использованием Arduino и стоит рассказать ее именно сейчас, перед темой компьютерного зрения так как прибор которым мы будем детектировать нагрев - это тепловизионная камера (тепловизор) подключенная к Arduino.
Теория: Для детектирования нагревательных элементов, мы будем использовать установленный на роботе тепловизор, он измеряет температуру по 8х8=64 пикселям в температурном диапазоне от 0°C до 80°C +- 2.5°C, он может определять температуру объектов на расстоянии до 7 метров, в зависимости от разности температур объекта и окружающей среды. Сама камера тепловизора имеет довольно маленькое фокусное расстояние, т.е. она широкоугольная. Следовательно для устойчивого определения температуры робота надо подводить достаточно близко к нагревателю. Максимальная частота считывания данных с тепловизора до 10Hz. Массив пикселей типа данных float32, содержит температуру нагрева в градусах Цельсия. Всю предобработку данных осуществляет сам тепловизор, так что нам надо просто считывать массив и получать данные по нагреву. В принципе, данный массив является аналогом черно-белого изображения, где интенсивность цвета соответствует температуре объекта.
В целом обработка данных полученных с тепловизора может быть представлена в виде элементарной задачи компьютерного зрения . В общем случае алгоритмы компьютерного зрения решают две задачи: Семантическую - что именно находится на изображении и Метрическую - какого размера, ориентации, цвета, формы и т.д. то, что находится на изображении (подробнее о решении задач компьютерного зрения в следующих уроках). В нашем случае применяя алгоритм сравнения температуры пикселей с пороговым значением мы можем получить решение семантической задачи - Находится ли нагретый элемент на изображении тепловизора. В принципе, можно сразу решить и метрическую задачу - до какой температуры нагрет элемент на изображении, но в связи со своей тривиальностью данная задача не представляет интереса.
Для ответа на вопрос, находится ли нагретый элемент на изображении, мы будем применять один из двух семантических алгоритмов. Первый будет определять есть ли нагретый объект на изображении, если хотя бы один из пикселей нагрет выше порога, а второй будет давать положительный результат если среднее арифметическое значение температуры всех пикселей больше порога. Мы будем последовательно применять оба эти алгоритма и посмотрим, какой из них выдает более приемлемое для нас решение семантической задачи.
Подготовка: Данную задачу нагляднее всего решать в комбинации полигона - "Офис". Попросите учащихся непосредственно перед выполнением написанной программы, при помощи управления через веб-интерфейс, поставить робота в первую комнату офиса - спиной к зарядке. По внешней камере полигона убедитесь, чтобы перед роботом было достаточно свободного пространства для движения по прямой. Если программа написана некорректно, и робот делает что-то несогласованное, попросите учащихся прервать выполнение программы, остановить робота при при помощи управления через веб-интерфейс и вернуть его в первоначальную позицию. Перед запуском программы, включите какие-либо из нагревательных элементов на полигоне для контроля их состояния используйте веб-интерфейс портала полигона.
Задача: При направлении робота на нагревательный элемент с температурой выше порогового значения, робот должен выдать в топик ROS - сообщение об обнаружении перегрева.
Общий алгоритм решения:
1. Создаем прошивку для ардуино нашего робота, которая будет считывать данные температуры пикселей напрямую с тепловизора, собирать их в массив и публиковать в топик ROS на распбери робота.
2. Создаем обработчик данных массива на Python. Он будет решать семантическую задачу поиска на изображении с тепловизора элемента с температурой выше заданного порогового значения
3. Результат решения семантической задачи будет публиковаться в другой топик, откуда мы сможем его прочитать при помощи утилиты rostopic echo.
Решение: Для начала напишем прошивку для Arduino:
Для работы с драйвером тепловизора, вам необходимо установить в Arduino IDE библиотеку https://github.com/adafruit/Adafruit_AMG88xx Установка библиотеки происходит стандартным образом. Для работы с ROS вам необходимо установить библиотеку ros_lib. Как это сделать описано в инструкции к роботу и в уроках ранее.
Перейдем к написанию самого скетча. Импортируем все библиотеки и структуры данных, которые нам понадобятся для перебрасывания данных в ROS. Wire.h - нам нужна для связи с тепловизором через интерфейс I2C, ros.h - библиотека реализующая связь с ROS, и Adafruit_AMG88xx.h - это библиотека непосредственно драйвера тепловизора.
#include <Wire.h>
#include <Adafruit_AMG88xx.h>
#include <ros.h>
Создадим экземпляр класса для работы с тепловизором и массив пикселей, которые мы будем читать с тепловизора. Константа AMG88xx_PIXEL_ARRAY_SIZE равна 64 и определяется в заголовочном файле библиотеки драйвера тепловизора Adafruit_AMG88xx.h
Adafruit_AMG88xx amg;
float pixels[AMG88xx_PIXEL_ARRAY_SIZE];
Далее объявляем связь с ROS, на распберри, ноду, тип используемого в топике массива данных - std_msgs::Float32MultiArray и Издателя этого типа для топика "amg88xx_pixels".
class NewHardware : public ArduinoHardware
{
public:
NewHardware():ArduinoHardware(&Serial1, 115200){};
};
ros::NodeHandle_<NewHardware> nh;
std_msgs::Float32MultiArray f_array_msg;
ros::Publisher amg88xx_pixels_pub("amg88xx_pixels", &f_array_msg);
Опишем стандартную функцию setup() для arduino.
void setup()
{
bool status;
status = amg.begin();
if (!status) {
while (1);
}
delay(100);
f_array_msg.data = (float*)malloc(sizeof(float) * AMG88xx_PIXEL_ARRAY_SIZE);
f_array_msg.data_length = AMG88xx_PIXEL_ARRAY_SIZE;
nh.initNode();
nh.advertise(amg88xx_pixels_pub);
}
В начале мы пытаемся подключиться к тепловизору через экземпляр класса драйвера используя функцию amg.begin() и если нам удалось это сделать, то инициализируем массив f_array_msg, ROS-ноду и топик, котрые мы объявили ранее.
В функции loop(), мы читаем массив пикселей при помощи функции amg.readPixels(pixels); затем в цикле заполняем ROS-массив этими прочитанными значениями. Потом публикуем этот заполненный ROS-массив, и делаем паузу на 500мс.
void loop()
{
amg.readPixels(pixels);
for (int i = 0; i < AMG88xx_PIXEL_ARRAY_SIZE; ++i)
{
f_array_msg.data[i] = pixels[i];
}
amg88xx_pixels_pub.publish(&f_array_msg);
nh.spinOnce();
delay(500);
}
В итоге у вас должна появиться вот такая прошивка:
#include <Wire.h>
#include <Adafruit_AMG88xx.h>
#include <ros.h>
#include <std_msgs/Float32MultiArray.h>
Adafruit_AMG88xx amg;
float pixels[AMG88xx_PIXEL_ARRAY_SIZE];
class NewHardware : public ArduinoHardware
{
public:
NewHardware():ArduinoHardware(&Serial1, 115200){};
};
ros::NodeHandle_<NewHardware> nh;
std_msgs::Float32MultiArray f_array_msg;
ros::Publisher amg88xx_pixels_pub("amg88xx_pixels", &f_array_msg);
void setup()
{
bool status;
status = amg.begin();
if (!status) {
while (1);
}
delay(100);
f_array_msg.data = (float*)malloc(sizeof(float) * AMG88xx_PIXEL_ARRAY_SIZE);
f_array_msg.data_length = AMG88xx_PIXEL_ARRAY_SIZE;
nh.initNode();
nh.advertise(amg88xx_pixels_pub);
}
void loop()
{
amg.readPixels(pixels);
for (int i = 0; i < AMG88xx_PIXEL_ARRAY_SIZE; ++i)
{
f_array_msg.data[i] = pixels[i];
}
amg88xx_pixels_pub.publish(&f_array_msg);
nh.spinOnce();
delay(500);
}
Залейте ее на ардуино вашего робота и убедитесь что все работает при помощи команды rostopic echo /amg88xx_pixels выполнив ее на вашем роботе. В топике должен появиться массив данных с тепловизора состоящий из 64-х значений температуры по каждому пикселю.
Теперь перейдем к обработке полученных данных и применению семантических алгоритмов. Создадим новый питоновский файл:
Для начала импортируем все библиотеки и структуры данных, которые нам понадобятся. Это rospy, массив Float32 - данных температуры и Bool для публикации результата анализа.
import rospy
from std_msgs.msg import Float32MultiArray, Bool
Сделаем класс, который будет реализовывать решение нашей семантической задачи.
class HeatSensor(object):
def __init__(self):
self._current_pixel_array = None
self._threshold = 55 #temp of pixel
self._output_pub = rospy.Publisher('heat_sensor_output', Bool, queue_size=10)
self._heat_sub = rospy.Subscriber('amg88xx_pixels', Float32MultiArray, self._heat_callback)
self._run()
Создадим массив в который будем передавать данные полученные с тепловизора и объявим пороговое значение температуры, выше которого мы будем считать элемент изображения нагретым - 55 градусов.
Дальше создадим экземпляры Подписчика и Издателя. Подписчик будет получать от Arduino нашего робота данные тепловизора, а Издатель публиковать в топик 'heat_sensor_output', результат решения семантической задачи - находится ли перед тепловизором нагретый объект или нет.
Далее вызовем функцию _run(), которая и будет содержать основной цикл нашей программы.
Опишем функцию обратного вызова подписчика:
def _heat_callback(self, heat_msg):
self._current_pixel_array = heat_msg.data
Она будет брать данные, которые ей передаст Подписчик и складывать в переменную массива, которую мы создали ранее.
Теперь опишем две функции поиска нагретого объекта - по среднему значению и по максимальному значению.
def _mean_detector(self):
if not len(self._current_pixel_array):
current_mean_temp = (sum(self._current_pixel_array)/
len(self._current_pixel_array))
if current_mean_temp >= self._threshold:
return True
else:
return False
else:
return False
Эта функция определяет нагретый объект по среднему арифметическому значений всех пикселе тепловизора. В начале нее мы проверяем не является ли длина массива пикселей нулем, и если не является, то суммируем значения температуры по всему массиву и делим на количество элементов в массиве - тем самым мы получим среднюю температуру по массиву. А дальше сравниваем эту среднюю температуру с пороговым значением и если она выше порога, то возвращаем True, что значит что мы видим нагретый объект, а если меньше, то False - означающий что нагретый объект не найден.
Далее напишем функцию определяющую нагретый объект по одному из максимально нагретых пикселей.
def _max_detector(self):
current_max_temp = max(self._current_pixel_array)
if current_max_temp >= self._threshold:
return True
else:
return False
Она проще - просто сравнивает максимальное значение среди всего массива с пороговым значением, и если оно больше порога то True, если меньше то False. True и False определяют обнаружен ли нагретый элемент перед роботом, как и в функции по среднему значению.
Теперь напишем функцию-обертку для Издателя, которая будет заполнять структуру ответа тем значением, которое мы ей передадим и публиковать ее при помощи экземпляра Издателя который мы создали в функции __init__().
def _send_info_msg(self, state):
alert_msg = Bool()
alert_msg.data = state
self._output_pub.publish(alert_msg)
Осталось описать функцию _run()
def _run(self):
while not rospy.is_shutdown():
self._send_info_msg(self._max_detector())
rospy.sleep(0.1)
Она содержит бесконечный цикл, в котором с частотой 10 герц, определенной в rospy.sleep(0.1) вызывается одна из функций семантического анализа, результаты которой передаются в функцию обертку, которая и будет публиковать результаты анализа.
Дальше инициализируем ноду и создадим экземпляр только что описанного класса.
rospy.init_node('heat_sensor')
hd = HeatSensor()
В итоге мы получим вот такой вот код:
import rospy
from std_msgs.msg import Float32MultiArray, Bool
class HeatSensor(object):
def __init__(self):
self._current_pixel_array = None
self._threshold = 55 #temp of pixel
self._output_pub = rospy.Publisher('heat_sensor_output', Bool, queue_size=10)
self._heat_sub = rospy.Subscriber('amg88xx_pixels', Float32MultiArray, self._heat_callback)
self._run()
def _heat_callback(self, heat_msg):
self._current_pixel_array = heat_msg.data
def _mean_detector(self):
if not len(self._current_pixel_array):
current_mean_temp = (sum(self._current_pixel_array)/
len(self._current_pixel_array))
if current_mean_temp >= self._threshold:
return True
else:
return False
else:
return False
def _max_detector(self):
current_max_temp = max(self._current_pixel_array)
if current_max_temp >= self._threshold:
return True
else:
return False
def _send_info_msg(self, state):
alert_msg = Bool()
alert_msg.data = state
self._output_pub.publish(alert_msg)
def _run(self):
while not rospy.is_shutdown():
self._send_info_msg(self._mean_detector())
rospy.sleep(0.1)
rospy.init_node('heat_sensor')
hd = HeatSensor()
Проверка решения:
Попросите учащихся перенести программу на робота и запустить, затем открыть еще одно окно терминала с ssh на робота и вывести данные топика heat_sensor_output в консоль, и дальше при помощи веб-интерфейса подвести робота к нагретому элементу. Если программа написана правильно в консоли будет выведена 1 для включенного нагревательного элемента и 0 для выключенного.
После запуска программы попросите учащихся провести робота по полигону вручную направляя его на нагревательные элементы и сообщить вам какие из нагревательных элементов включены.
Убедитесь, что в данные в топике с нуля сменились на единицу, при появлении нагревателя в поле зрения тепловизора. Попросите учащихся поменять функцию семантического анализа на _max_detector и посмотрите как меняется характер определения нагретого объекта. Проконтролируйте работу учащихся в режиме трансляции их экрана через Discord.
В следующих уроках, мы продолжим тему компьютерного зрения, но начнем уже анализировать данные видеопотока с камеры робота.