Инструкция по настройке симуляции робота TurtleBot3 в ROS

Установка и настройка ПО

Загрузка и установка

Для полноценной работы с фреймворком ROS необходим дистрибутив операционной системы с ядром linux, для этого можно использовать виртуальную машину. То есть по сути оболочку для запуска операционной системы внутри операционной системы, для этого понадобится программа VirualBox. Также необходимо скачать сам образ операционной системы с предустановленным ROS, который можно будет открыть с помощью программы VirtualBox. Для этого необходимо скачать архив по этой ссылке на гугл-диск
После скачивания его нужно разархивировать в удобную вам папку. После разархивирования можно увидеть следующую структуру папок и файлов Ubuntu 64-bit:

Файлы виртуальной машины
Данные файлы нельзя как либо модифицировать и изменять во избежании всевозможных поломок ПО. В дальнейшем с помощью программы virtualBox будет открываться файл Ubuntu 64-bit с расширением Virtual Machine Disk Format (.vmdk)

Настройка

Для первого запуска виртуальной машины необходимо:
1) Запустить программу virtualBox
2) Нажать кнопку создать, чтобы создать новую виртуальную машину

3) В высветившимся окне нажать кнопку экспертный режим

4) Настраиваем виртуальную машину:

4.1) Задать любое имя машины

4.2) Выбрать папку в которой будут храниться данные виртуальной машины, предложенную папку можно не изменять

4.3) Тип машины - Linux, версия Ubuntu (64-bit)

4.4) Выбрать количество оперативной памяти, которое будет доступно виртуальной машине. ДЛЯ КОРРЕКТНОЙ РАБОТЫ НЕОБХОДИМО НЕ МЕНЕЕ 6 ГБ

4.5) В графе жесткий диск выбрать пункт использовать существующий виртуальный диск, и в качестве виртуального диска выбрать файл Ubuntu 64-bit.vmdk описанный в предыдущем пункте

5) Когда все настройки выставлены в соответствии с пунктом 4 необходимо нажать кнопку создать.
6) Перед запуском виртуальной машины, необходимо настроить количество ядер процессора, которые она сможет использовать (для комфортной работы рекомендуется не менее 4). Для этого:

6.1) Необходимо нажать кнопку настроить

6.2) Перейти в графу система

6.3) Перейти во вкладку процессор и разрешить использование 4 или более ядер (количество разрешенных к использованию ядер зависит от общего количества ядер процессора)

7) После всех настроек можно запустить виртуальную машину, для этого необходимо:

7.1) Нажать кнопку запустить

7.2) После загрузки системы появится следующее окно:

7.3) В данном окне нажать на пользователя tb3, после чего высветится окно для ввода пароля:

7.4) Ввести пароль tb3, после чего откроется окно системы:

Note: если окно системы слишком маленькое для работы, то в настройках ubuntu необходимо изменить разрешение экрана:

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

Симулятор Gazebo

Введение

Gazebo 3Dразрабатываемый некоммерческой организацией OSRF (Open Source Robotics Foundation), имеет ряд преимуществ по сравнению с другими робототехническими симуляторами. Во-первых, он бесплатный и имеет открытый код. Во-вторых, он очень популярен среди мирового робототехнического сообщества и является официальным симулятором соревнований DARPA. В-третьих, Gazebo отлично интегрируется с программной платформой ROS (Robot Operating System), а значит разработанную программу управления виртуальным роботом в Gazebo и ROS будет относительно несложно перенести на реального робота.
В данном симуляторе будет запускаться робот turtlebot3, на поле соревнований turtlebot3 autorace:

Суть данных соревнований в том, что робот должен проехать по размеченной линии параллельно выполняя различные миссии:

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

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

Соревновательное поле

Рассмотрим соревновательное поле и миссии подробнее. Поле представляет из себя квадратную поверхность на которую нанесены желтая и белая линии, которые формируют дорожное полотно. По регламенту соревнований робот не должен выезжать за пределы дорожной полосы, иначе ему будут начислены штрафные баллы. Поэтому очень важно реализовать максимально точное следование линии.
Также разберем все миссии:
1) Остановка на светофоре
Первой миссией является остановка на светофоре. Изначально на светофоре горит зеленый сигнал. Когда робот подъезжает к нему, зеленый сигнал сменяется красным и робот должен остановиться. Затем робот должен ждать повторного появления зеленого сигнала и только после этого продолжать движение. Пример того, что видит робот при подъезде к светофору:

2) Парковка
Второй миссией, которую робот должен пройти является парковка. В рамках данной миссии, робот должен заехать в одно из двух отведенных мест парковки (другое место занимает второй робот). В качестве ориентира для начала миссии используется знак парковки:

3) Шлагбаум
Третьей миссией является остановка перед шлагбаумом. При подъезде робота к перекрестку шлагбаум опустится. У робота есть два варианта действий, либо остановиться перед шлагбаумом и ждать пока тот поднимется, либо объехать его по прилегающей дороге. Также рядом с шлагбаумом расположен знак stop:

4) Туннель
Последней миссией для робота является замкнутый лабиринт, имеющий один вход и один выход. Для ориентации в тоннеле робот не может использовать камеры, но может использовать остальные имеющиеся датчики. Главная задача - выехать из туннеля через зону выхода. Зона входа же в свою очередь помечена знаком:

Конечной целью робота является прохождение всей миссий за наименьшее время.

Запуск симулятора с моделями и базовые действия

Для запуска симулятора, с моделью робота и соревновательным полем необходимо открыть терминал (ctr+alt+t) и написать следующую команду: $roslaunch turtlebot3_gazebo turtlebot3_autorace.launch При первой загрузке симулятору потребуется время, чтобы настроить окружение, это может занять примерно 5-10 минут.
Окно запущенного симулятора выглядит так:

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

После чего левой кнопкой мыши нажать на объект, который необходимо передвинуть (категорически не рекомендуется передвигать что то кроме робота!). На объекте высветится система координат:

Чтобы передвинуть объект вдоль какой-либо оси, необходимо потянуть за эту ось с нажатой левой кнопкой мыши.
Аналогичным способом можно вращать объекты, в режиме вращения. Для этого необходимо нажать на иконку вращения в панели управления:

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

Запуск миссий

Изначально при запуске мира, миссии не запускаются. То есть светофор не горит, оба места для парковки остаются не занятыми и шлагбаум не опускается. Для того, чтобы активировать миссии в терминале необходимо написать следующую команду: $roslaunch turtlebot3_gazebo turtlebot3_autorace_mission.launch. Соответственно, до запуска данной команды соревновательное поле выглядело так:

После запуска команды поле выглядит вот так:

Видно, что зеленый сигнал светофора загорелся и одно из парковочных мест оказалось занято другим роботом.
Миссии активируются, исходя из показаний топика odom робота (топик, в котором хранятся данные колесной одометрии, подробнее будет рассмотрен далее). То есть каждая миссия будет активироваться только тогда, когда робот самостоятельно доехал до нее, если просто передвигать робота в какую-либо точку то миссии активироваться не будут.

Модель робота

В симуляторе запускается точная копия робота turtlebot3, включая все топики реального робота. Чаще всего использоваться будут следующие топики:
● scan: топик в который публикуются данные лазерного дальномера, в формате сообщения sensor_msgs/LaserScan. Дальномер делает 360 замеров на один оборот, то есть его разрешающая способность один градус. Все расстояния хранятся в массиве ranges, который входит в сообщение LaserScan, всего там 360 элементов. Ниже на рисунке можно увидеть как отсчитываются углы:

● odom: топик в который публикуется колесная одометрия робота, то есть его перемещение от точки старт рассчитанное, исходя из показаний датчиков оборотов, установленных в привода. Так как в симулятора Gazebo также симулируется трение, показания в данном топике со временем будут накапливать ошибку, и отличаться от реального перемещения робота. В данный топик публикуются сообщения типа: nav_msgs/Odometry.
● cmd_vel: топик который управляет скоростью движения робота, подробнее его работа рассмотрена в пункте Езда по линии.
Кроме основных элементов, на модель робота также установлено две камеры. Графического отображения они не имеют, схематичное расположение камер показано на рисунке ниже:

Таким образом одна из камер направлена на линию (camera_line), а вторая на дорожные знаки. Изображение полученные с камер выглядят следующим образом (слева с камеры линии, справа с камеры направленной на знаки):

Изображение с камеры линии поступает в топик: /camera_line/image_line. Изображение с камеры направленной на знаки поступает в топик: /camera/image.
В данные топики поступает сообщение типа: sensor_msgs/Image, которое не получится визуализировать с помощью библиотеки OpenCV, поэтому для визуализации используется утилита rqt.

Визуализация изображений при помощи утилиты rqt

Чтобы визуализировать изображение необходимо запустить данную утилиту написав следующую команду в терминале:
$ rqt
На экране должно появиться пустое окно, а на панели управления иконка утилиты:

После в панели управления необходимо выбрать пункт: Plugins->Visualization->Image View
После этого откроется окно визуализации, в котором необходимо выбрать топик, информацию с которого нужно визуализировать (красным выделено окно выбора топика):

В результате изображение будет в онлайн режиме транслироваться на экран в следующем виде:

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

Детектирование

Детектирования дорожной разметки

Для детектирования линий дорожной разметки используется довольно таки простой алгоритм - бинаризация по цветовому порогу.
Изображение напрямую с виртуальной камеры, направленной на дорогу публикуется в закодированном виде в топик /camera_line/image_line. Поиск цвета на изображении происходит по цветовой модели HSV. Данная модель очень полезна, если вам необходимо оперировать всем спектром цветов при помощи одной переменной. Название модели состоит из первых букв от слов Hue – тон, Saturation – насыщенность и Value – значение (Brightness – яркость). Тон варьируется от 0 до 255 , насыщенность и яркость так же – от 0 до 255.

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

Рассмотрим код программы подробнее (файл line_detect.py):

#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
import numpy as np
import cv2
from sensor_msgs.msg import Image, CompressedImage
from cv_bridge import CvBridge
from time import sleep
from std_msgs.msg import Float64

Здесь подключаем необходимые библиотеки: библиотеку rospy для подключения ros окружения, numpy для работы с массивами, opencv для обработки изображений, cv_bridge для конвертирования изображения из ros сообщения в numpy массив, также модуль задержки sleep из библиотеки time. Для полноценного общения с ros окружением необходимо также добавить используемые сообщения: изображение и сжатое изображения и float64 (дробное число).

pub_image = rospy.Publisher('image', Image, queue_size=1)
pub_error = rospy.Publisher('line_error', Float64, queue_size=1)
cvBridge = CvBridge()

Создаем необходимые глобальные переменные: объект publisher для публикации обработанного изображения (это необходимо для отладки), объект publisher для публикации отклонения от идеального положения и объект cvBridge для конвертирования сообщения типа image в numpy массив.

def cbImageProjection(data):
    cv_image_original = cvBridge.imgmsg_to_cv2(data, "bgr8")
    cv_image_original = cv2.GaussianBlur(cv_image_original, (5, 5), 0)
    yellow_detect, yellow_array = mask_yellow(cv_image_original)
    white_detect, white_array = mask_white(cv_image_original)
    detected = cv2.add(white_detect, yellow_detect)
    pub_image.publish(cvBridge.cv2_to_imgmsg(detected, "bgr8"))
    error = Float64()
    error.data = calculate_error(yellow_array, white_array)
    pub_error.publish(error)

Данная функция является колбэк функцией для изображения, приходящего с камеры, она выполняет все необходимые преобразования, рассчитывает отклонение от идеального положения и отсылает это отклонение в топик “line_error”. Разберем ее поэтапно:

1) Переводим изображение из сообщения Image в numpy array, чтобы с ним можно было работать функциями библиотеки OpenCV

2) “Блюрим” изображение, то есть немного размываем его, чтобы избавиться от различных шумов и артефактов. Выполняется это с помощ Гауссиан блюра, смысл работы которого в том, что значения какой то группы соседних пикселей усредняется, таким образом мы можем добиться плавного размытия.

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

4) Затем необходимо создать изображение, использующееся для отладки, для этого объединяем изображения с белой и желтой линии. После этого публикуем данное изображение в топик “image”.

5) Финальным этапом является расчет отклонения от идеального положения и публикация этого отклонения в топик “line_error”.

def mask_yellow(img):
    hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
    Hue_l = 27
    Hue_h = 41
    Saturation_l = 130
    Saturation_h = 255
    Lightness_l = 160
    Lightness_h = 255
    # define range of yellow color in HSV
    lower_yellow = np.array([Hue_l, Saturation_l, Lightness_l])
    upper_yellow = np.array([Hue_h, Saturation_h, Lightness_h])
    # Threshold the HSV image to get only yellow colors
    mask = cv2.inRange(hsv, lower_yellow, upper_yellow)
    # Bitwise-AND mask and original image
    res = cv2.bitwise_and(img, img, mask = mask)
    fraction_num = np.count_nonzero(mask)
    point_arr = []
    stop_flag = False
    if fraction_num > 50:
   	 k = 0
   	 jold = 0
   	 for i in range(mask.shape[0]-1,0,-15):
   		 if stop_flag == True:
   			 break
   		 for j in range(0,mask.shape[1],15):
if mask[i,j] > 0:
   				 point_arr.append([j,i])
   				 k+=1
   				 if abs(j-jold) > 80 and k > 1:
   					 point_arr.pop()
   					 stop_flag = True
   				 jold = j
   				 break
   	 if(len(point_arr) > 0):
   		 point_before = point_arr[0]
   		 for point in point_arr:
   			 res = cv2.line(res, (point[0], point[1]), (point_before[0],point_before[1]), (0,0,255),8)
   			 point_before = point
    return res, point_arr
    

def mask_white(img):
    hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
    Hue_l = 0
    Hue_h = 25
    Saturation_l = 0
    Saturation_h = 36
    Lightness_l = 180
    Lightness_h = 255
    # define range of yellow color in HSV
    lower_white = np.array([Hue_l, Saturation_l, Lightness_l])
    upper_white = np.array([Hue_h, Saturation_h, Lightness_h])
    # Threshold the HSV image to get only yellow colors
    mask = cv2.inRange(hsv, lower_white, upper_white)
    # Bitwise-AND mask and original image
    res = cv2.bitwise_and(img, img, mask = mask)
    fraction_num = np.count_nonzero(mask)
    point_arr = []
    stop_flag = False
    if fraction_num > 50:
   	 k = 0
   	 jold = 0
   	 for i in range(mask.shape[0]-1,0,-20):
   		 if stop_flag == True:
   			 break
   		 for j in range(mask.shape[1]-1,0,-20):
   			 if mask[i,j] > 0:
   				 point_arr.append([j,i])
   				 k+=1
   				 if abs(j-jold) > 80 and k > 1:
   					 point_arr.pop()
   					 stop_flag = True
   				 jold = j
   				 break
   	 if len(point_arr) > 0:
   		 point_before = point_arr[0]
   		 for point in point_arr:
   			 res = cv2.line(res, (point[0], point[1]), (point_before[0],point_before[1]), (0,0,255),8)
   			 point_before = point
    return res, point_arr

Выше описаны функции детектирования белой и желтой линии соответственно. Они практически идентичны, поэтому разберем только одну из них, а именно функцию для белой линии. Сначала необходимо перевести изображение в HSV формат, о том что это за формат говорилось ранее. После чего создаются массивы пороговых значений цветов для бинаризации, для белого цвета это: Минимальные значения цвета по каналам H, S и V = 0,0,180 Максимальные значения цвета по каналам H, S и V = 25, 36, 255 Затем используя эти пороги бинаризуем изображение и получаем его “маску”.
Теперь используя эту маску можно найти все точки принадлежащие линии, но перед этим создается изображение для отладки. В данном изображении белым изображены выделенные пиксели, а остальные пиксели имеют свой обычный цвет в rgb формате. Проверив, что на изображении достаточно белых пикселей можно перейти к алгоритму нахождения точек принадлежащих линии на изображении.
Искать все точки, принадлежащие линии очень затратно и не имеет смысла, поэтому используются следующие принципы:

1) Только одна точка в строке

2) Точка ищется раз в 20 строчек (то есть если изображение размерностью 320x240, то для одной линии найдется примерно 12 точек)

Для поиска точек, необходимо с помощью двух циклов пройти по всему изображению с шагом 20 (важным является то, что для белой линии поиск осуществляется справа налево и снизу вверх, а для желтой слева направо и снизу вверх). Для этого запускается 2 цикла и если значение пиксела больше нуля (то есть если пиксель белый), то он добавляется в массив точек. Также есть защита от разрыва, если на протяжении более 80 пикселей точек не обнаружено, значит линия разорвалась и необходимо прекратить поиск.
После поиска необходимо отрисовать полученные точки на изображении, для отладки. Для этого проверяем нашлась ли хоть одна точка, после чего соединяем все точки красными линиями. Затем функция возвращает полученные точки и изображение.

def calculate_error(yellow_array, white_array):
    
    error_yell = 0
    error_white = 0
    weight = 0
    i = 1
    for yel in yellow_array:
   	 #when yel[2] = 600 then weight = 0 and if yel[2] = 0 wheight = 1
   	 weight = yel[1]*0.0017 + 1
   	 error_yell = weight*(30 - yel[0]) + error_yell
   	 i+=1
    error_yell = error_yell/i
    for white in white_array:
   	 weight = white[1]*0.0017 + 1
   	 error_white = weight*(300 - white[0]) + error_white
   	 i+=1
    error_white = error_white/i
    print("white "+ str(error_white) + " yellow "+ str(error_yell))
    if error_white < 30:
   	 return error_yell
    elif error_yell < 30:
   	 return error_white
    else:
   	 return (error_white + error_yell)/2

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

1) Отклонение для желтой и белой линии считаются по отдельности

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

Таким образом сначала вычисляется суммарное отклонение от желтой линии (идеальное положение точки по оси y - 30), а затем от белой линии (идеальное положение точки по оси y - 300). После этого если ошибка для белой линии мала, то возвращается ошибка для желтой линии и наоборот. Если же обе ошибки велики, то возвращается среднее арифметическое между ними.

if __name__ == '__main__':
    rospy.init_node('image_projection')
    sub_image = rospy.Subscriber('/camera_line/image_line', Image, cbImageProjection, queue_size=1)
    while not rospy.is_shutdown():
   	 try:
   		 rospy.sleep(0.1)
   	 except KeyboardInterrupt:
   		 break
   		 cv2.destroyAllWindows()

Выше функция main, которая вызывается при старте программы. В ней:

1) Инициализируется нода

2) Происходит подписка на топик, содержащий в себе изображение с камеры и указание, что в качестве колбэк функции использовать функцию cbImageProjection

Детектирование и распознавание дорожных знаков

Для детектирования дорожных знаков используется алгоритм SIFT (Scale-invariant feature transform). SIFT - очень мощный инструмент для распознавания необходимых объектов на изображении. Его суть заключается в том, что на изображении ищутся “особые точки”, после чего найденные точки, можно сравнить с такими же точками на идеальном изображении и сделать вывод о наличии/отсутствии необходимого объекта на изображении.
Ниже представлено изображение и найденные на нем особые точки:

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

В итоге получается следующая схема решения задачи детектирования дорожных знаков:

1. Изображение переводится в черно-белый формат, для уменьшения количества вычислений.

2. На идеальном изображении выделяются особые точки, а дескрипторы этих точек сохраняются в массив

3. Выделяются особые точки на текущем изображении с камеры

4. По совпадению дескрипторов выделяются соответствующие друг другу ключевые точки.

5. На основе набора совпадающих ключевых точек строится вывод о наличии в данный момент времени дорожного знака на изображении.

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

Таким образом нода постоянно публикует в топик “/sign”, имя текущего наблюдаемого дорожного знака, если он есть на изображении и строку “none” если не видит ничего.
Имена знаков:

• “stop”
• “parking”
• “tunnel”

Перейдем к программе (файл sign_detect.py):

#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
import numpy as np
import cv2
from sensor_msgs.msg import Image, CompressedImage
from cv_bridge import CvBridge
import math
import os
from time import sleep
from std_msgs.msg import String

Здесь подключаются основные библиотеки и используемые сообщения. Для работы с изображением все также используется OpenCV.

pub_image = rospy.Publisher('sign_image', Image, queue_size=1)
pub_sign = rospy.Publisher('sign', String, queue_size=1)
cvBridge = CvBridge()
counter = 1
FLANN_INDEX_KDTREE = 0
index_params = dict(algorithm = FLANN_INDEX_KDTREE, trees = 5)
search_params = dict(checks = 50)
flann = cv2.FlannBasedMatcher(index_params, search_params)

Здесь создаются глобальные переменные, необходимые паблишеры и объявляются необходимые классы. Класс для перевода изображения из сообщения типа Image в numpy массив. Также создается элемент класса FlannBasedMatcher, данный класс используется для автоматического сравнивания дескрипторов ключевых точек. Соответственно переменные index_params и search_params необходимы для инициализации элемента класса.

def cbImageProjection(data):
	global kp_ideal, des_ideal, sift, counter, flann
# drop the frame to 1/5 (6fps) because of the processing speed
	if counter % 3 != 0:
		counter += 1
		return
     else:
           counter = 1
	cv_image_original = cvBridge.imgmsg_to_cv2(data, "bgr8")
	cv_image_gray = cv2.cvtColor(cv_image_original, cv2.COLOR_BGR2GRAY)
	kp,des = sift.detectAndCompute(cv_image_gray, None)
	cv_image_original = cv2.drawKeypoints(cv_image_gray,kp,None,(255,0,0),4)
	for i in range(0,3):
		matches = flann.knnMatch(des,des_ideal[i],k=2)
		result = compare_matches(kp, kp_ideal[i], matches)
		sign_msg = String()
		if result == True:
			if i == 0:
				sign_msg.data = "stop"
			elif i == 1:
				sign_msg.data = "parking"
			else:
				sign_msg.data = "tunnel"
			break
		else:
			sign_msg.data = "none"
	print sign_msg.data
	pub_sign.publish(sign_msg)
	pub_image.publish(cvBridge.cv2_to_imgmsg(cv_image_original, "rgb8"))

Выше - основная колбэк функция для изображения. Так как процесс детектирования ключевых точек изображения требует много вычислительных мощностей, а изображение публикуется 30 раз в секунду некоторые кадры изображения необходимо отбрасывать. Для этого будет рассматриваться только каждые третий приходящий кадр.
После отбрасывания лишних кадров, сначала полученное изображение переводится в numpy формат, с которым может работать библиотека OpenCV. После этого изображение переводится в черно белый формат.
Затем функция detectAndCompute класса sift (объявление элемента класса происхоидит в функции ниже) находит ключевые точки и их дескрипторы. После этого найденные ключевые точки визуализируются на изображении для отладки. Теперь в цикле необходимо сопоставить найденные ключевые точки с идеальными ключевыми точками, после чего если найдено много совпадений, то в топик “sign” отправляется имя знака, если же ничего не найдено, то отправляется строка “none”. Цикл необходим для того, чтобы по очереди сопоставить найденные ключевые точки с ключевыми точками идеальных знаков.
Также в конце исполнительной функции в топик “sign_image” публикуется изображение с отмеченным на нем ключевыми точками.

def compare_matches(kp,kp_ideal,matches):
	MATCHES_ERR = 50000
	MATCHES_DIST_MIN = 7
	# print("matches count: ",len(matches))
	good = []
	for m,n in matches:
		if m.distance < 0.7*n.distance:
			good.append(m)
	if len(good) > MATCHES_DIST_MIN:
		src_pts = np.float32([kp[m.queryIdx].pt for m in good])
		dst_pts = np.float32([kp_ideal[m.trainIdx].pt for m in good])
		# print("distance matches count: ",len(good))
		mse_err = find_mse(src_pts,dst_pts)
		print("mse_err: ", mse_err)
		if mse_err < MATCHES_ERR:
			return True
	return False

Выше функция, которая сравнивает дескрипторы ключевых точек. Есть два основных параметра для сравнивания. Первый - это расстояние между соседними ключевыми точками (ведь ключевые точки могут совпадать и отдельным фактором для идентификации является именно расстояние между соседними ключевыми точками). Второй - суммарная квадратичная ошибка в расстояниях между точками MATCHES_ERR. Функция принимает: kp - найденные на изображении ключевые точки, kp_ideal - ключевые точки с идеального изображения, matches - набор совпавших ключевых точек (совпадения рассчитываются knnMaycher-ом в функции cbImageProjection).
Сначала необходимо найти какие точки подходят нам по параметру расстояния, для этого сравниваем расстояния для каждой ключевой точки изображения и соответствующей совпавшей точки на идеальном изображении. Удовлетворяющие точки заносятся в список good.
Затем если таких совпавших точек оказалось больше 7, то идет расчет среднеквадратичной ошибки между дескрипторами совпадающих ключевых точек. Если эта ошибка оказывается меньше порогового значения, значит мы можем сделать вывод о том что в данный момент знак есть на изображении.

def find_mse(arr1, arr2):
	err = (arr1-arr2)**2
	sum_err = err.sum()
	size = arr1.shape[0]
	sum_err = sum_err/size
	return sum_err

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

def standart_signs():
	dir_path = os.path.dirname(os.path.realpath(__file__))
	dir_path = dir_path.replace('myRace/src', 'myRace/')
	dir_path += 'data_set/detect_sign/'
	img1 = cv2.imread(dir_path + 'stop.png',0)         # trainImage1
	img2 = cv2.imread(dir_path + 'parking.png',0)      # trainImage2
	img3 = cv2.imread(dir_path + 'tunnel.png',0)    
	sift = cv2.xfeatures2d.SIFT_create()
	kp1,des1 = sift.detectAndCompute(img1, None)
	kp2, des2 = sift.detectAndCompute(img2,None)
	kp3, des3 = sift.detectAndCompute(img3,None)
	# img1 = cv2.drawKeypoints(img1,kp1,None,(255,0,0),4)
	kp_ideal = [kp1,kp2,kp3]
	des_ideal = [des1,des2,des3]
	return kp_ideal, des_ideal, sift#, img1

Выше представлена функция вычисления ключевых точек и их дескрипторов из идеальных изображений. Сначала из папки data_set подгружаются необходимые изображения, после чего создается элемент класса sift, который впоследствии будет использоваться в функции cbImageProjection. Затем для идеальных изображений вычисляются ключевые точки и их дескрипторы и записываются в списки kp_ideal - ключевые точки (0 элемент - знак стоп, 1 элемент - знак парковки, 2 элемент - знак въезда в туннель), des_ideal - дескрипторы идеальных точек.

if __name__ == '__main__':
	rospy.init_node('image_projection')
	sub_image = rospy.Subscriber('/camera/image', Image, cbImageProjection, queue_size=1)
	kp_ideal, des_ideal, sift = standart_signs()
	while not rospy.is_shutdown():
		try:
			rospy.sleep(0.1)
			# cv2.imshow('image',img1)
			# if cv2.waitKey(0) == 27:
				# cv2.destroyAllWindows()
				# break
		except KeyboardInterrupt:
			break

Main функция, вызываемая при запуске ноды. Здесь происходит инициализация ноды, создаются необходимые объекты подписчики и указываются колбэк функции, а также запускается цикл работы ROS-а.

Детектирование огней светофора

Алгоритм детектирования сигнала светофора очень похож на алгоритм детектирования линии. Также проводится бинаризация по различным порогам (для желтого, зеленого и красного цветов). Но так как таких цветов на поле достаточно много (например, вездесущая желтая линия), то необходимо также проверять форму задетектированнного бинаризованного объекта. Для детектирования окружности используется Hough circle detector, использующий преобразования Хафа. В простейшем случае преобразование Хафа является линейным преобразованием для обнаружения прямых. Прямая может быть задана уравнением y = mx + b и может быть вычислена по любой паре точек (x, y) на изображении. Главная идея преобразования Хафа — учесть характеристики прямой не как уравнение, построенное по паре точек изображения, а в терминах её параметров, то есть m — углового коэффициента и b — точки пересечения с осью ординат. Исходя из этого прямая, заданная уравнением y = mx + b, может быть представлена в виде точки с координатами (b, m) в пространстве параметров.
Принцип детектирования окружности с помощью детектирования линий очень прост: если прямые перпендикулярные какой либо поверхности пересекаются в одной точке, то скорее всего эта поверхность является окружностью.

Естественно это очень упрощенное представления, ведь вряд ли на реальном изображении линии сойдутся в одной точке, поэтому рассматривается некоторая окрестность возможных пересечений и проверяется насколько в этой окрестности линии близки друг к другу.
Рассмотрим подробнее функцию HoughCircles(image, method, dp, minDist, param1, param2, minRadius, maxRadius)

● image: бесцветное черно-белое изображение, на котором необходимо найти окружности.

● method: Определяет метод с помощью которого будет происходить детектирование, в нашем случае используется cv2.HOUGH_GRADIENT

● dp: Параметр характеризующий насколько идеальной должна быть найденная окружность.

● minDist: минимальное расстояние между центрами задетектированных окружностей (в пикселях).

● param1, param2: параметры с помощью которых регулируется точность детектирования.

● minRadius: минимальный радиус детектируемой окружности.

● maxRadius: максимальный радиус детектируемой окружности.

Задетектированный сигнал светофора выглядит следующим образом:

Перейдем к программной части (файл traffic_light_controller.py):

#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
import numpy as np
import cv2
from sensor_msgs.msg import Image, CompressedImage
from cv_bridge import CvBridge
from time import sleep
from std_msgs.msg import String
pub_image = rospy.Publisher('image_traffic_light', Image, queue_size=1)
pub_traffic_light = rospy.Publisher('traffic_light', String, queue_size=1)
cvBridge = CvBridge()
counter = 1

Здесь подключаются необходимые библиотеки. Также создается два паблишера в топики: image_traffic_light и топик traffic_light. В данные топики публикуется обработанное изображение для отладки и цвет сигнала светофора, который видится в данный момент (публикуется строка, если никакой их сигналов не виден публикуется none). По аналогии с обработкой дорожных знаков данная нода также затрачивает много вычислительных мощностей, поэтому частоту обработки кадров необходимо ограничить. Будет обрабатываться каждый третий кадр, для этого создается переменная counter.

def cbImageProjection(data):
	global counter
	if counter % 3 != 0:
		counter += 1
		return
     else: 
           couner = 1	
	cv_image_original = cvBridge.imgmsg_to_cv2(data, "bgr8")
	cv_image_original = cv2.GaussianBlur(cv_image_original, (3, 3), 0)
	cv_image_gray = cv2.cvtColor(cv_image_original, cv2.COLOR_BGR2GRAY)
	circles = cv2.HoughCircles(cv_image_gray, cv2.HOUGH_GRADIENT, 1, 50, param2 = 20, minRadius = 8, maxRadius = 15 ) 
	green_x, green_y  = mask_green(cv_image_original)
	yellow_x, yellow_y = mask_yellow(cv_image_original)
	red_x, red_y  = mask_red(cv_image_original)
	light_msg = String()
	light_msg.data = "None"
	if circles is not None:
		circles = np.round(circles[0,:]).astype("int")
		for x,y,r in circles:
			cv2.circle(cv_image_gray, (x,y),r,(0,255,0), 3)
			green_err = calc_err(green_x, green_y, x, y) 
			red_err = calc_err(red_x, red_y, x, y)
			yellow_err =  calc_err(yellow_x, yellow_y, x, y)
			if green_err < 400:
				light_msg.data = "green" 
			if red_err < 400:
				light_msg.data = "red"
			if yellow_err < 400:
				light_msg.data = "yellow"
	pub_traffic_light.publish(light_msg)
	temp = np.hsplit(cv_image_gray,2) 
	cv_image_gray = temp[1]
	pub_image.publish(cvBridge.cv2_to_imgmsg(cv_image_gray, "8UC1")) 

Выше описана основная колбэк функция топика изображения с камеры. Рассмотрим ее работу поэтапно:

1) Проверяем какой по счету кадр пришел в обработку, если третий то обрабатываем, иначе завершаем работу функции

2) Размываем изображение с помощью GaussianBlur

3) Переводим изображение в серый формат

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

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

6) Проводится сравнение координат найденных окружностей и координат найденных белых пикселей, если есть совпадения то делается вывод о том что наблюдается конкретный сигнал светофора

7) Распознанные окружности переносятся на изображение для отладки

8) Объекты паблишеры публикуют рассчитанные данные

Перейдем к детальному рассмотрению каждой функции в отдельности.

def calc_err(color_x, color_y, circle_x, circle_y):
	if color_x*color_y*circle_y*circle_x > 0:
		x_err = (color_x - circle_x)**2
		y_err = (color_y - circle_y)**2
		err = x_err+y_err / 2
		return err
	return 100000

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

def mask_yellow(img):
	hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
	Hue_l = 20
	Hue_h = 35
	Saturation_l = 100
	Saturation_h = 255
	Lightness_l = 50
	Lightness_h = 255
	# define range of yellow color in HSV
	lower_yellow = np.array([Hue_l, Saturation_l, Lightness_l])
	upper_yellow = np.array([Hue_h, Saturation_h, Lightness_h])
	# Threshold the HSV image to get only yellow colors
	mask = cv2.inRange(hsv, lower_yellow, upper_yellow)
	temp = np.hsplit(mask,2)
	mask = temp[1]
	fraction_num = np.count_nonzero(mask)
	if fraction_num > 100:
		for y in range(0, mask.shape[0]-1,5):
			for x in range(0,mask.shape[1]-1,5):
				if mask[y,x]>0:
					return(x+160,y) 
	return 0, 0

def mask_red(img):
	hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
	Hue_l = 0
	Hue_h = 10
	Saturation_l = 30
	Saturation_h = 255
	Lightness_l = 48
	Lightness_h = 255
	# define range of yellow color in HSV
	lower_red = np.array([Hue_l, Saturation_l, Lightness_l])
	upper_red = np.array([Hue_h, Saturation_h, Lightness_h])
	# Threshold the HSV image to get only yellow colors
	mask = cv2.inRange(hsv, lower_red, upper_red)
	temp = np.hsplit(mask,2) 
	mask = temp[1]
	fraction_num = np.count_nonzero(mask)
	if fraction_num > 100:
		for y in range(0, mask.shape[0]-1,5):
			for x in range(0,mask.shape[1]-1,5):
				if mask[y,x]>0:
					return(x+160,y) 
	return 0, 0

def mask_green(img):
	hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
	Hue_l = 46
	Hue_h = 76
	Saturation_l = 86
	Saturation_h = 255
	Lightness_l = 50
	Lightness_h = 255
	# define range of yellow color in HSV
	lower_green = np.array([Hue_l, Saturation_l, Lightness_l])
	upper_green = np.array([Hue_h, Saturation_h, Lightness_h])
	# Threshold the HSV image to get only yellow colors
	mask = cv2.inRange(hsv, lower_green, upper_green)
	temp = np.hsplit(mask,2) 	mask = temp[1]
	fraction_num = np.count_nonzero(mask)
	if fraction_num > 100:
		for y in range(0, mask.shape[0]-1,5):
			for x in range(0,mask.shape[1]-1,5):
				if mask[y,x]>0:
					return(x+160,y) 
	return 0, 0

Выше представлены функции бинаризации и поиска белых пикселей: для желтого, красного и зеленого сигналов светофора соответственно. Рассмотрим подробнее одну из них, остальные являются похожими за исключением порогов бинаризации. Сама бинаризация проводится по аналогии с бинаризацией в случае детектирования линии. После бинаризации изображение разбивается на две одинаковые части, в дальнейшем будет рассматриваться только правая часть изображения, так как светофор расположен справа. Затем проводится проверка на количество белых пикселей, если их больше 100, то проводится поиск по всему изображению с шагом 5. Если белый пиксель найден, то его координаты (уже в изображении нормального размера) возвращаются функцией.

if __name__ == '__main__':
	rospy.init_node('traffic_light_detector')
	sub_image = rospy.Subscriber('/camera/image', Image, cbImageProjection, queue_size=1)
	while not rospy.is_shutdown():
		try:
			rospy.sleep(0.1)
		except KeyboardInterrupt:
			break

Здесь происходит инициализация ноды и объявление объекта подписчика.

Детектирование шлагбаума

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

Перейдем к коду программы (файл bar_detect.py):

#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
import numpy as np
import cv2
from sensor_msgs.msg import Image, CompressedImage
from cv_bridge import CvBridge
from time import sleep
from std_msgs.msg import Bool
pub_image = rospy.Publisher('image_bar', Image, queue_size=1)
pub_bar = rospy.Publisher('bar', Bool, queue_size=1)
cvBridge = CvBridge()
counter = 1

Выше происходит подключение необходимых библиотек и объявление паблишеров. В данном случае один из них в топик bar публикует True если шлагбаум в данный момент виден и False если шлагбаума перед роботом нет. В топик image_bar публикуется бинаризованное изображение для отладки.

def cbImageProjection(data):
	global counter
	if counter % 3 != 0:
		counter += 1
		return
      else:
           counter = 1
	cv_image_original = cvBridge.imgmsg_to_cv2(data, "bgr8")
	cv_image_original = cv2.GaussianBlur(cv_image_original, (3, 3), 0)
	bar_msg = Bool()
	bar_msg.data, res = mask_red(cv_image_original)
	pub_bar.publish(bar_msg)
	pub_image.publish(cvBridge.cv2_to_imgmsg(res, "8UC1"))

Выше основная колбэк функция. Первой идет проверка на кадр, если данный кадр третий, то начинается его обработка. Сначала изображение переводится из сообщения в тип numpy array. Затем изображение размывается с помощью GaussianBlur, после чего проводится бинаризация и делается вывод о том есть ли шлагбаум на изображении. После чего публикуется бинаризованное изображение и наличие шлагбаума.

def mask_red(img):
	hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
	Hue_l = 0
	Hue_h = 10
	Saturation_l = 30
	Saturation_h = 255
	Lightness_l = 48
	Lightness_h = 255
	# define range of yellow color in HSV
	lower_red = np.array([Hue_l, Saturation_l, Lightness_l])
	upper_red = np.array([Hue_h, Saturation_h, Lightness_h])
	# Threshold the HSV image to get only yellow colors
	mask = cv2.inRange(hsv, lower_red, upper_red)
	mask = cv2.erode(mask, (4,4), iterations = 6) #for detection of big rectangle
	fraction_num = np.count_nonzero(mask)
	if fraction_num > 3000:
		return True, mask
	else:
		return False, mask

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

if __name__ == '__main__':
	rospy.init_node('bar_detector')
	sub_image = rospy.Subscriber('/camera/image', Image, cbImageProjection, queue_size=1)
	while not rospy.is_shutdown():
		try:
			rospy.sleep(0.1)
		except KeyboardInterrupt:
			break
			cv2.destroyAllWindows()

Здесь инициализируется ноды и подписчик на топик камеры.

Выполнение миссий

Как говорилось ранее выделяется 5 основных миссии:

● Остановка на светофоре

● Парковка

● Остановка перед шлагбаумом

● Лабиринт

● Езда по линии

Архитектура программ, с помощью которых выполняются миссии практически одинаковы, блок схема представлена ниже (не относится к езде по линии):

Нода каждой миссии подписывается на необходимый ей топик результата ноды детектирования. После того как “инициатор” ноды задетектирован, отключается движение по линии и начинается исполнение миссии. Затем робот снова начинает двигаться по линии, а нода миссии отключается.

Езда по линии

Управление моделью робота осуществляется при помощи задания необходимой линейной и угловой скорости движения. В топик cmd_vel, публикуется сообщение типа geometry_msgs/Twist, которое состоит из двух векторов:
linear: - линейная скорость (м/c)

x - по оси Х

y - по оси У

z - по оси Z

angular: - угловая скорость (скорость вращения вокруг своей оси, рад/c)

x - вокруг оси х

y - вокруг оси У

z -вокруг оси Z

Оси координат модели робота, расположены следующим образом: