Урок 16

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

Подготовка: Данную задачу можно решать в любой комбинации полигона, подойдет и "Офис" и "Дорога". Попросите учащихся непосредственно перед выполнением написанной программы, при помощи управления через веб-интерфейс, поставьте робота в центр полигона. По внешней камере полигона убедитесь, чтобы перед роботом было достаточно свободного пространства. Попросите техника на полигоне поставить желтый шарик на расстоянии около метра рядом с роботом. Если программа написана некорректно, и робот делает что-то несогласованное, попросите учащихся прервать выполнение программы, остановить робота при при помощи управления через веб-интерфейс и вернуть его в первоначальную позицию.

Убедимся что настройки не сбились: Для реализации того, алгоритма который разобран в уроке, необходимо убедиться, что видео-поток с камеры перенаправлен в ROS (По умолчанию он направлен в web-интерфейс). В связи с тем, что два потока не могут работать с камерой одновременно, поэтому необходимо переконфигурировать файл запуска /etc/ros/turtlebro.d/turtlebro.launch таким образом, чтобы по умолчанию видео-поток шел в ROS:

<arg name="run_turtlebro_web" default="false"/>
<arg name="run_ros_camera" default="true"/>

Задача: После запуска программы, основываясь на изображении получаемой с фронтальной камеры, робот должен найти желтый шарик, и подъехать к нему поближе. Расстояние подъезда к шарику будет определяться учащимися индивидуально для каждого робота.

Общий алгоритм решения:

• Получить изображение с камеры робота
• Преобразовать его в HSV
• Наложить цветовую маску - желтого цвета (взять алгоритм из предыдущего урока)
• Применить размытие к полученной маске, для того, чтобы убрать мелкие пиксели совпадающего цвета.
• Применить к полученной маске функцию поиска контуров на изображении
• Описать вокруг найденного контура окружность, для оценки координат центра и радиуса шарика
• Передать найденные параметры окружности в функцию, которая будет управлять движением робота
• Написать функцию управляющую движением робота, которая будет анализировать где на изображении находится шарик и поворачивать робота таким образом, чтобы камера робота смотрела строго на робота. Кроме того робот будет ехать вперед до тех пор пока радиус описанной окружности не будет больше заданного, т.е. робот подъедет к шарику на требуемое расстояние.

Решение: Основные подходы к решению данной задачи мы уже изучили на предыдущих уроках, так что сейчас мы будем применять полученные знания и познакомимся с некоторыми стандартными функциями библиотеки OpenCV, реализующими уже известные нам функции.

Для начала импортируем все нужные библиотеки и структуры данных

import rospy
import cv2
import numpy as np
from sensor_msgs.msg import Image, CompressedImage
from geometry_msgs.msg import Twist

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

class RoboTurner():

    def __init__(self):
        rospy.Subscriber("/front_camera/compressed", CompressedImage, self.cb_video_capture)
        rospy.sleep(1)
        rospy.loginfo("init done")

    yellowLower = (14, 180, 80) # dark
    yellowUpper = (34, 255, 255) # light
    ball_reached = False
    vel = Twist()
    ANGULAR_SPEED = 1.
    LIN_SPEED = 0.10
    DES_BALL_SIZE = 50
    MIN_BALL_SIZE = 10
    pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)

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

Введем константу угловой скорости и линейной скорости. И константу желаемого размера шарика на изображении.

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

    def cb_video_capture(self, image_msg):
        np_arr = np.frombuffer(image_msg.data, np.uint8)
        self.image_from_ros_camera = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)

Теперь напишем функцию, которая будет делать предобработку изображения, и применение семантики

    def process(self):
        frame = np.copy(self.image_from_ros_camera)
        hsv = cv2.cvtColor(frame, cv2.COLOR_RGB2HSV)
        mask = cv2.inRange(hsv, self.yellowLower, self.yellowUpper)
        mask = cv2.erode(mask, None, iterations=2)
        mask = cv2.dilate(mask, None, iterations=2)
        cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,
            cv2.CHAIN_APPROX_SIMPLE)[-2]
        center = None

        self.current_data = [0,0,0]
        # only proceed if at least one contour was found
        if len(cnts) > 0:
            c = max(cnts, key=cv2.contourArea)
            ((x, y), radius) = cv2.minEnclosingCircle(c)
            if radius > 10:
                self.current_data = [x,y,radius]
        return self.current_data

Давайте разберем ее подробно:

Создаем копию нашего массива изображения

frame = np.copy(self.image_from_ros_camera)

Переводим RGB в HSV, эта функция делает ровно, то что делала наша самописная функция на одном из прошлых уроков. Т.е. меняет цветовую модель изображения из RGB в HSV.

hsv = cv2.cvtColor(frame, cv2.COLOR_RGB2HSV)

Накладываем маску на HSV, в соответствии с теми границами, которые мы определили на предыдущих уроках.

mask = cv2.inRange(hsv, self.yellowLower, self.yellowUpper)

Далее применим пару функций размывающих мелкие детали на изображении, это позволит избавится от цветового шума и в целом сделает картинку шарика более однородной. Данные функции мы не писали на предыдущих уроках, их реализацию вы можете посмотреть в документации к библиотеке OpenCV (cv.dilate И cv.erode).

        mask = cv2.erode(mask, None, iterations=2)
        mask = cv2.dilate(mask, None, iterations=2)

Все! С предобработкой закончили. Теперь применим семантическую функцию, которая найдет контуры на изображении:

        cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,
            cv2.CHAIN_APPROX_SIMPLE)[-2]

Функция вернет нам массив результатов, в том числе и найденные контуры (см. документацию), а [-2] после функции означает, что мы возьмем только второй с конца элемент в этом массиве результатов.

Далее опишем, как именно мы будем обрабатывать найденные контуры:

if len(cnts) > 0:

В общем случае контуров на изображении может быть не один, а много, и поэтому договоримся, что нам нужен самый большой по площади контур. Для этого применим связку функций max и cv2.contourArea к массиву найденных контуров.

У функции max, есть именованный аргумент "key", в который мы можем передать функцию, которая будет применяться ко всем элементам переданным в max, на сравнение, и далее max вернет уже тот элемент, результат обработки функцией которого будет наибольший. Звучит сложновато, но давайте на примере:

Передадим в функцию max последовательность чисел, к примеру:

s = max(1,2,3,4,5)

В результате, в s передастся максимальное значение т.е. 5.

Если теперь вместо чисел, мы передадим в функцию какие-то другие объекты, в нашем случае "контуры", то нам надо как-то сравнить их. Контуры в отличие от чисел имеют много характеристик и чтобы сравнивать их, мы должны выбрать ту характеристику по которой мы будем их сравнивать, это может быть размер или яркость или количество углов или длина периметра. Для этого и есть та функция, которую мы применим к объектам, т.е. берем функцию которая вычислит длину периметра, применим ее ко всем контурам и получим какие-то значения, эти значения и будем сравнивать. В данном случае мы будем сравнивать контуры по площади. Для этого используем функцию cv2.contourArea после применения этой функции к каждому найденному контуру, мы получим площади каждого из контуров, и уже эти площади передадим в функцию max. Подробнее про аргументы функции max, в документации.

c = max(cnts, key=cv2.contourArea)

Далее к самому большому контуру с - который нам вернет функция max, применим функцию cv2.minEnclosingCircle(c), она обернет этот контур окружностью наименьшего радиуса, таким образом чтобы весь контур оказался внутри этой окружности, и вернет два параметра: координаты x,y - центра и радиус окружности.

((x, y), radius) = cv2.minEnclosingCircle(c)

Дальше если радиус больше 10 пикселей (размер определяем на глаз в константе MIN_BALL_SIZE), мы вернем список current_data, заполненный значениями x,y и radius. Если радиус контура меньше 10 пикселей или вообще никаких контуров не найдено, то список current_data будет содержать нули.

     current_data = [0,0,0]  
        if len(cnts) > 0:
            c = max(cnts, key=cv2.contourArea)
            ((x, y), radius) = cv2.minEnclosingCircle(c)
            if radius > MIN_BALL_SIZE:
                current_data = [x,y,radius]

    return current_data

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

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

    def go_to_ball(self, data):
        self.vel.angular.z = -self.ANGULAR_SPEED*((data[0]-320)/320)
        if data[2] < self.DES_BALL_SIZE:
            self.vel.linear.x = self.LIN_SPEED
        else:
            self.vel.linear.x = 0
            self.ball_reached = True
        self.pub.publish(self.vel)

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

self.vel.angular.z = -self.ANGULAR_SPEED*((data[0]-320)/320)

Опишем еще одну функцию, которая будет управлять всем процессом. Ее логика проста. Если шарик не найден, то ищем, если найден, то останавливаем робота и больше не реагируем, на изменения состояния.

    def search_the_ball(self):
        data = self.process()
        if data == [0,0,0] or not self.ball_reached:
            self.go_to_ball(data)
        else:
            self.pub.publish(Twist())
            print("done")

В итоге у вас должна получиться вот такая программа:

import rospy
import cv2
import numpy as np
from sensor_msgs.msg import Image, CompressedImage
from geometry_msgs.msg import Twist

class RoboTurner():

    def __init__(self):
        rospy.Subscriber("/front_camera/compressed", CompressedImage, self.cb_video_capture)
        rospy.sleep(1)
        rospy.loginfo("init done")

    yellowLower = (14, 180, 80) # dark
    yellowUpper = (34, 255, 255) # light
    ball_reached = False
    vel = Twist()
    ANGULAR_SPEED = 1.
    LIN_SPEED = 0.10
    DES_BALL_SIZE = 50
    MIN_BALL_SIZE = 10
    pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)


    def cb_video_capture(self, image_msg):
        np_arr = np.frombuffer(image_msg.data, np.uint8)
        self.image_from_ros_camera = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)

    def process(self):
        frame = np.copy(self.image_from_ros_camera)
        hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
        mask = cv2.inRange(hsv, self.yellowLower, self.yellowUpper)
        mask = cv2.erode(mask, None, iterations=2)
        mask = cv2.dilate(mask, None, iterations=2)
        cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,
            cv2.CHAIN_APPROX_SIMPLE)[-2]
        center = None

        self.current_data = [0,0,0]
        # only proceed if at least one contour was found
        if len(cnts) > 0:
            c = max(cnts, key=cv2.contourArea)
            ((x, y), radius) = cv2.minEnclosingCircle(c)
            if radius > MIN_BALL_SIZE:
                current_data = [x,y,radius]

        return current_data

    def go_to_ball(self, data):
        self.vel.angular.z = -self.ANGULAR_SPEED*((data[0]-320)/320)
        if data[2] < self.DES_BALL_SIZE:
            self.vel.linear.x = self.LIN_SPEED
        else:
            self.vel.linear.x = 0
            self.ball_reached = True
        self.pub.publish(self.vel)

    def search_the_ball(self):
        data = self.process()
        if data == [0,0,0] or not self.ball_reached:
            self.go_to_ball(data)
        else:
            self.pub.publish(Twist())
            print("done")

rospy.init_node("ball_searcher")

r = RoboTurner()
while not rospy.is_shutdown():
    r.search_the_ball()
    rospy.sleep(0.1)

Проверка решения:

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

Попросите учащихся запустить программу и по камере полигона проконтролировать, что робот довернул на центр шарика, подъехал к нему поближе и остановился, написав в консоль сообщение "done". Попросите учащихся показать вывод сообщения в консоли и езду робота в режиме демонстрации экрана в Discord.

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