Инструкция по настройке симуляции робота 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

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

Здесь синяя ось - ось Z, красная - ось X, зеленая - ось Y.
Из-за того что робот имеет дифференциальную колесную базу, он может перемещаться только вдоль оси Х и вокруг оси Z. Соответственно, для того, чтобы робот поехал вперед, необходимо отправить следующее сообщение:
msg:

linear:

x = 0.1

y = 0

z = 0

angular:

x = 0

y = 0

z = 0

Для того, чтобы робот крутился на месте:
msg:

linear:

x = 0

y = 0

z = 0

angular:

x = 0

y = 0

z = 0.1

А также для того чтобы крутился по дуге:
msg:

linear:

x = 0.1

y = 0

z = 0

angular:

x = 0

y = 0

z = 0.1

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

Так как при работе данной ноды, ничто больше не может управлять скоростью робота (канал управления скоростью занят), необходимо иметь флаг, с помощью которого ноду можно будет включать и выключать. При запуске нода является включенной, для отключения необходимо в топик line_move_flag отправить сообщение типа std_msgs Bool, содержащее False. Для повторного запуска в этот же топик нужно отправить True.
Перейдем к коду программы (файл line_control.py):

#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
from time import sleep
from std_msgs.msg import Float64, Bool
from geometry_msgs.msg import Twist
pub_vel = rospy.Publisher('cmd_vel', Twist, queue_size=1)
integral = 0
move_flag = True

Итак, здесь подключаются необходимые библиотеки и сообщения. Создается объект паблишер, для топика cmd_vel. Который будет отправлять скорость на робота. Создается переменная integral хранящая в себе интегральную составляющую регулятора. Данную переменную необходимо сделать глобальной, для того, чтобы она не обнулялась после каждого исполнения функции. Также создается переменная move_flag, которая в свою очередь и является переключателем между рабочим и выключенными состояниями. Изначально ее значение True, поэтому робот сразу начнет движение (если запущена нода детектирования линии).

def cbError(error):
	global integral, move_flag
	if(move_flag == True):
		velocity = Twist()
		integral = integral + 0.000005*error.data
		proportional = 0.01*error.data
		up = proportional+integral
		velocity.angular.z = up
		velocity.linear.x = 0.22 - 0.09*abs(up)
		pub_vel.publish(velocity)

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

def cb_flag(data):
	global move_flag
	move_flag = data.data

Данная функция - колбэк включения/выключения ноды. Здесь значение пришедшего сообщения переписывается в переменную move_flag

if __name__ == '__main__':
	rospy.init_node('line_control')
	sub_image = rospy.Subscriber('line_error', Float64, cbError, queue_size=1)
	sub_move_flag = rospy.Subscriber('line_move_flag', Bool, cb_flag, queue_size=1)
	while not rospy.is_shutdown():
		try:
			rospy.sleep(0.1)
		except KeyboardInterrupt:
			break

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

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

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

#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
from time import sleep
from std_msgs.msg import  String, Bool
from geometry_msgs.msg import Twist
light = False

Здесь подключаются необходимые библиотеки. Также создается глобальная переменная light. Если значение переменной False, то робот продолжает ехать по линии. Если же значение True, то активируется миссия светофора.

def cb_traffic_light(data):
    global light
    if(data.data == "yellow" or data.data == "red"):
   	 light = True
    elif(data.data == "green"):
   	 light = False

Выше колбэк топика traffic_light в который публикуется информация о том какой сигнал светофора виден в данный момент. Если виден желтый или красный сигнал то значение переменной light становится равным True и активируется миссия.

def pub_velocity(x, z, time):
    pub_vel = rospy.Publisher('cmd_vel', Twist, queue_size=1)
    vel = Twist()
    for i in range(0, int(time*10)):
   	 vel.linear.x = x
   	 vel.angular.z = z
   	 pub_vel.publish(vel)
   	 rospy.sleep(0.1)

Здесь описана функция публикации заданной скорости какое то количество времени, она принимает необходимую линейную, угловую скорости и время в течении которого необходимо поддерживать данную скорость. В ней объявляется объект паблишер в топик cmd_vel (топик скорости). Затем создается сообщение типа Twist. Для стабильности работы необходимая скорость будет отправляться в цикле. Количество итеарций цикла рассчитывается исходя из времени в течении которого нужно ехать, каждая итерация длится 0.1 секунду.

def do_traffic_light():
    global light
    pub_line_move = rospy.Publisher('line_move_flag', Bool, queue_size=1)
    flag_move_line = Bool()
    flag_move_line.data = False
    rospy.sleep(0.1)
    pub_line_move.publish(flag_move_line)
    print("published stop msg")
    while( light == True):
   	 pub_velocity(0, 0, 0.1)
    flag_move_line.data = True
    pub_line_move.publish(flag_move_line)

Выше функция, описывающая выполнение миссии светофора. Сначала в топик line_move_flag отправляется значение False — это останавливает процесс движения по линии и топик cmd_vel становится не занятым. Затем в топик cmd_vel посылаются нулевые значения и робот останавливается, начинается процесс ожидания смены сигнала светофора. Как только значение переменной light сменяется с True на False, в топик line_move_flag отправляется значение True и продолжается движение по линии.

if __name__ == '__main__':
    rospy.init_node('traffic_light_controller')
    sub_sign = rospy.Subscriber('traffic_light', String, cb_traffic_light, queue_size=1)
    while not rospy.is_shutdown():
   	 try:
   		 if(light == True):
   			 print("start traffic light mission")
   			 do_traffic_light()
   			 break
   	 except KeyboardInterrupt:
   		 break

Здесь происходит инициализация ноды, и подписка на топик traffic_light. В основном цикле, постоянно происходит проверка переменной light, если ее значение True (то есть замечены желтый или красный сигнал светфоора), то вызывается функция исполнения мисии. После ее завершения цикл завершается вызовом break и нода уничтожается.

Парковка

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

Проверка на то свободно ли место, осуществляется с помощью лидара - датчика расстояния. Если расстояния меньше критического значения, значит место свободно.
Первое детектирование знака происходит в этом месте:

На этом фото видно, что первое парковочное место было свободно и робот занял его:

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

#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
from time import sleep
from std_msgs.msg import Float64, Bool, String
from geometry_msgs.msg import Twist
from sensor_msgs.msg import 
distance = 0
parking = False

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

def cb_sign(data):
    global parking
    if(data.data == "parking"):
   	 parking = True

Выше колбэк функция топика “sign”, если задетектирован знак парковки, то значение переменной parking изменяется с False на True.

def cb_scan(data):
    global distance
    counter = 0
    for i in range(200,300):
   	 if(data.ranges[i] < 1):
   		 distance = distance + data.ranges[i]
   		 counter = counter + 1
    if(counter != 0):
   	 distance = distance/counter
    else:
   	 distance = 0

Данная функция является колбэком топика “scan”, в который приходят измерения лазерного дальномера. В данный топик приходит сообщения типа LaserScan, в данном сообщении хранится множество переменных, но нас интересует массив ranges в котором и хранятся расстояния. Расстояния до объектов расположенных справа от робота хранятся в позициях ranges[200] - ranges[300]. В цикле выполняется проход по этому диапазону списка и если текущий элемент меньше 1 (то есть расстояние замеренное данным лучем меньше 1 метра), данное расстояние учитывается. Таким образом если хоть одно расстояние из всего диапазона будет меньше 1, значение переменной distance будет ненулевое.

def pub_velocity(x, z, time):
    pub_vel = rospy.Publisher('cmd_vel', Twist, queue_size=1)
    vel = Twist()
    for i in range(0, int(time*10)):
   	 vel.linear.x = x
   	 vel.angular.z = z
   	 pub_vel.publish(vel)
   	 rospy.sleep(0.1)

Данная функция отправки сообщений в топик cmd_vel уже была рассмотрена выше.

def do_parking():
    pub_line_move = rospy.Publisher('line_move_flag', Bool, queue_size=1)
    flag_move_line = Bool()
    flag_move_line.data = False
    rospy.sleep(0.1)
    pub_line_move.publish(flag_move_line)    
    print("published stop msg")
    pub_velocity(0, 0, 0.5)
    print("published vel")
    pub_velocity(0, -0.4,4)
    pub_velocity(0, 0, 0.3)
    pub_velocity(0.13, 0,2)
    pub_velocity(0, 0, 0.3)
    pub_velocity(0, 0.4,4)
    pub_velocity(0, 0, 0.5)
    pub_velocity(0, -0.4,4)
    pub_velocity(0, 0, 0.3)
    pub_velocity(-0.13, 0,2)
    pub_velocity(0, 0, 0.3)
    pub_velocity(0, 0.4,4)
    pub_velocity(0,0,0.5)
    flag_move_line.data = True
    pub_line_move.publish(flag_move

Здесь описывается выполнение самой миссии, рассмотрим эту функцию поэтапно:

● отключение движения по линии

● остановка

● поворот направо на 90°

● остановка (остановка после каждого движения нужна для того, чтобы движения робота получались наиболее четкими)

● проезд вперед на 0.26 м

● остановка

● поворот налево на 90° (этим движением фиксируется элемент парковки, так как робот должен быть направлен не в сторону линии)

● остановка

● поворот направо на 90°

● остановка

● проезд назад на 0.26 м

● поворот налево на 90° (поворот по направлению линии)

● возврат к движению по линии

if __name__ == '__main__':
    rospy.init_node('parking')
    sub_sign = rospy.Subscriber('sign', String, cb_sign, queue_size=1)
    sub_scan = rospy.Subscriber('scan', LaserScan, cb_scan, queue_size=1)
    while not rospy.is_shutdown():
   	 try:
   		 if(parking == True):
   			 print("start parking mission")
   			 rospy.sleep(9)
   			 print(distance)
   			 if(distance > 1 or distance == 0):    			           	 do_parking()
   			 else:
   				 rospy.sleep(2)
   				 do_parking()
   			 break
   	 except KeyboardInterrupt:
   		 break

Здесь происходит инициализация ноды и подписка на топик sign и scan. В основном цикле постоянно производится проверка переменной parking, если ее значение True, то начинается выполнение миссии по алгоритму:

● 9-секундное ожидание, того что робот доехал до первого парковочного места

● если переменная distance больше 1 или равна нулю (то есть если препятствий не обнаружено), то вызывается функция do_parking()

● если же первое место было занято, то необходимо подождать 2 секунды (столько занимает доезд до второго места) и вызвать функцию do_parking

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

В данной миссии есть два варианта действий:

1. объехать шлагбаум по объездному пути

2. остановиться перед шлагбаумом и проехать прямо

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

#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
from time import sleep
from std_msgs.msg import Float64, Bool, String
from geometry_msgs.msg import Twist
stop_bar = False

Подключение необходимых библиотек, объявление глобальной переменной stop_bar. Выполнение миссии начнется когда stop_bar примет значение True.

def cb_bar(data):
	global stop_bar
	stop_bar = data.data

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

def pub_velocity(x, z, time):
	pub_vel = rospy.Publisher('cmd_vel', Twist, queue_size=1)
	vel = Twist()
	for i in range(0, int(time*10)):
		vel.linear.x = x
		vel.angular.z = z
		pub_vel.publish(vel)
		rospy.sleep(0.1)

Функция отправки сообщений в топик cmd_vel, описывалась ранее.

def do_stop():
	pub_line_move = rospy.Publisher('line_move_flag', Bool, queue_size=1)
	flag_move_line = Bool()
	flag_move_line.data = False
	rospy.sleep(0.1)
	pub_line_move.publish(flag_move_line)
	# pub_velocity(0.2,0,1)
	for i in range(20,0, -2):
		pub_velocity(i/100,0,0.2)
	while stop_bar == True:
		pub_velocity(0,0,0.1)
	flag_move_line.data = True
	pub_line_move.publish(flag_move_line)

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

• отключение движение по линии

● плавная остановка (необходима для того, чтобы при остановке из за инерции робота не повело в сторону)

● пока значение переменной stop_bar - True, то есть шлагбаум опущен, стоим на месте

● продолжение движения по линии

if __name__ == '__main__':
	rospy.init_node('stop_bar')
	sub_bar = rospy.Subscriber('bar', Bool, cb_bar, queue_size=1)
	while not rospy.is_shutdown():
		try:
			if(stop_bar == True):
				print("stop detected")
				do_stop()
				break
		except KeyboardInterrupt:
			break

Здесь инициализируется нода, и происходит подписка на топик bar. В основном цикле производиться постоянная проверка переменной stop_bar. Если ее значение меняется на True, то начинается исполнение миссии. После того как миссия выполнена, исполнительный файл завершается.

Лабиринт

Исполнение этой миссии разделяется на два этапа:

1) Детектирование знака и доезд до лабиринта

2) Нахождение выхода из лабиринта

Первый этап выполняется довольно прост, после того как знак туннеля задетектирован робот продолжает движение вперед в течении 3 секунд.

Второй этап заключается в создании алгоритма для прохождения лабиринта. Лабиринт представляет из себя коробку с цилиндрическими препятствиями внутри:

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

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

#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
from time import sleep
from std_msgs.msg import Float64, Bool, String
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
ranges = list()
tunnel = False
in_tunnel = False

Здесь подключаются необходимые библиотеки и создаются глобальные переменные. Переменная ranges будет хранить показания лазерного дальномера в формате списка. Переменная tunnel активирует начало миссии.
Переменная in_tunnel активирует алгоритм прохождения лабиринта.

def cb_sign(data):
	global tunnel
	if(data.data == "tunnel"):
		tunnel = True

Выше описана колбэк функция топика sign. Если задетектирован знак въезда в туннель, то есть в топик пришло сообщение содержащее строку “tunnel”, то значение переменной tunnel изменяется с False на True.

def cb_scan(data):
	global ranges
	ranges = data.ranges

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

def pub_velocity(x, z, time):
	pub_vel = rospy.Publisher('cmd_vel', Twist, queue_size=1)
	vel = Twist()
	for i in range(0, int(time*10)):
		vel.linear.x = x
		vel.angular.z = z
		pub_vel.publish(vel)
		rospy.sleep(0.1)

Выше описана функция отправки сообщений в топик cmd_vel.

def do_tunnel():
	pub_line_move = rospy.Publisher('line_move_flag', Bool, queue_size=1)
	flag_move_line = Bool()
	flag_move_line.data = False
	rospy.sleep(3)
	rospy.sleep(0.1)
	pub_line_move.publish(flag_move_line)
	pub_velocity(0.2,0,2)
	for i in range(20,0, -2):
		pub_velocity(i/100,0,0.3)

Здесь описана функция активации миссии туннеля. Разберем ее поэтапно:

● происходит отключение движения по линии

● робот движется вперед 2 секунды, чтобы заехать в лабиринт

● плавная остановка

def sign(data):
	if(data >= 0):
		return 1
	else:
		return -1

Функция sign является функцией определения “знака” числа. То есть если в нее отправить число -29, то она вернет -1, если же в нее отправить положительное число, то она вернет 1.

def in_tunnel_go():
	global ranges
	vel_x = 0.12
	vel_z = 0
	error = 4*(0.23 - ranges[300])
	if(abs(error) > 1.5):
		error = sign(error)*1.5
	vel_z = error
	for i in range(0,360,1):
		if(ranges[i] < 0.15):
			if(i <= 30 or i >= 330):
				vel_x = -0.09
				vel_z = 0.4		
	pub_velocity(vel_x, vel_z, 0.1)

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

Луч, смотрящий точно направо не подходит для реализации регулятора, так как система имеет небольшое запаздывание. Поэтому выбирается луч, смотрящий направо и немного вперед.
Робот должен держаться на расстоянии 0.23 м от стены, а коэффициент усиления равен 4, поэтому ошибка рассчитывается по следующей формуле:
error = 4*(0.23 - ranges[300])
После расчета ошибки проверяется нет ли препятствий вокруг робота. Если они есть, то роботу необходимо отъехать назад, поворачивая направо, если же препятствий нет, то скорости не изменяются.
На последнем этапе рассчитанные скорости публикуются в топик cmd_vel.

if __name__ == '__main__':
	rospy.init_node('tunnel')
	sub_sign = rospy.Subscriber('sign', String, cb_sign, queue_size=1)
	sub_bar = rospy.Subscriber('scan', LaserScan, cb_scan, queue_size=1)
	while not rospy.is_shutdown():
		try:
			if(tunnel == True and in_tunnel == False):
				print("tunnel detected")
				do_tunnel()
				in_tunnel = True
			elif(in_tunnel == True):
				in_tunnel_go()
		except KeyboardInterrupt:
			break

Здесь происходит инициализация ноды, а также подписка на топики sign и scan. В основном цикле идет проверка переменных tunnel и in_tunnel. Если знак туннеля задетектирован (то есть значение переменной tunnel True, а переменной in_tunnel False), то выполняется подъезд к туннелю и значение переменной in_tunnel меняется на True. При следующих итерациях цикла вызывается функция in_tunnel_go(). Таким образом она продолжит вызываться, пока нода не будет выключена принудительно.

Создание и запуск launch файлов, исправление возможных ошибок

Launch файлы

Запускать все вышеописанные файлы по отдельности очень долго, поэтому для запуска нескольких нод одновременно используются launch файлы. Создано два лаунч файла:

● запуск нод детектирования (detection.launch)

● запуск нод выполнения миссий (control.launch)

Рассмотрим структуру launch файла, на примере detection.launch:

<launch>
    <node pkg="myRace" name="bar_detect" type="bar_detect.py"/>

    <node pkg="myRace" name="line_detect" type="line_detect.py"/>

    <node pkg="myRace" name="sign_detect" type="sign_detect.py"/>

    <node pkg="myRace" name="traffic_light_detect" type="traffic_light_detector.py"/>

</launch>

Первая строка, объявляет launch файл, а последняя его закрывает.
Вызов необходимого исполнительного файла производится по следующей структуре:

● pkg - имя пакета, в котором расположен файл

● name - то как будет называться нода при инициализации (то есть по какому имени к ней обращаться с помощью команды rosnode)

● type - имя исполнительного файла

Launch файлы хранятся в отдельной папке launch внутри пакета myRace и имеют расширение .launch. Для их запуска используется команда roslaunch, например: $ roslaunch myRace detection.launch Одной из распространенных ошибок при запуске лаунч файла, который в свою очередь запускает ноды написанные на языке python, является следующая: ERROR: cannot launch node of type [turtlebot3_teleop/turtlebot3_teleop_key]: can't locate node [turtlebot3_teleop_key] in package [turtlebot3_teleop]
Данная ошибка говорит о том, что команда roslaunch не смогла найти указанной в лаунч файле ноды, это может быть вызвано двумя причинами:

1. неправильно указано имя исполнительного файла ноды

2. окружение ROS не знает о существовании данного исполнительного файла с расширением .py (то есть написанного на языке python)

Первая ошибка исправляется, соответствующим изменением имени файла.
Чтобы исправить вторую ошибку необходимо дать окружению ROS знать, о существовании исполнительного файла, для этого необходимо перейти в директорию, в которой находится исполнительный файл:
$ cd ~/tb_ws/src/myRace/src
И затем выполнить команду chmod +x на данный исполнительный файл:
$ chmod +x “имя файла”
Например:
$chmod +x traffic_light_controller.py

Использований rqt_graph для отслеживания ошибок

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

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

Описание последовательного запуска всех элементов

Итак, для того, чтобы запустить робота для выполнения всего задания целиком необходимо:

1) Запустить симулятор, с моделью робота и соревновательного поля

команда: $ roslaunch turtlebot3_gazebo turtlebot3_autorace.launch

2) Активировать миссии

команда: $ roslaunch turtlebot3_gazebo turtlebot3_autorace_mission.launch

3) Активировать все ноды детектирования

команда: $ roslaunch myRace detection.launch

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

команда: $ roslaunch myRace control.launch