Остановка перед уточкой. Duckietown.

Остановка перед уточкой. Duckietown.

В предыдущей инструкции мы настроили Duckietown. В этой статье мы используем эмулятор для решения практической задачи, напишем программу «Не сбей уточку».

Идея простая, при приближении к уточке на определенное расстояние подаётся команда, запрещающая движение в ее направлении.

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

  • Создание карты с уточками
  • Написание основной конструкции программы для поиска жёлтого цвета
  • Добавление ползунков для настройки цветового фильтр InRange
  • Настройка фильтра inRange
  • Выделение контура уточки, его измерение
  • Остановка машинки в симуляторе

Для начала настроим карту.

Создание карты с уточками

Для реализации задачи выберем карту “udem1”. Изменим её, добавив несколько жёлтых уточек в разных точках карты. Это позволит усложнить трассу для робота и увеличит шансы по поиску уточек.

“Вносим изменения на карту”

Сначала посмотрим на базовую карту:

карта duckietown для остановки перед уточкой
Рис 1. Карта udem1

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

карта duckietown
Рис 2. Координатная сетка на карте

Подбираем координаты для новых уточек:

duckie2:
 kind: duckie
 pos: [3.2,1.3]
 rotate: 0
 height: 0.08
 optional: true
duckie3:
 kind: duckie
 pos: [5.1,1.1]
 rotate: -180
 height: 0.08
 optional: true

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

карта duckitown
Рис 3. Расположение уточек на карте

Желтый = уточка

Мы закончили настройку карты. Теперь перйдём к написанию кода, который будет оценивать “наличие” уточки в поле камеры

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

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

obs = env.reset()
obs, reward, done, info = env.step(action)

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

import sys
import gym
import numpy as np
import cv2
from pyglet.window import key
from gym_duckietown.envs import DuckietownEnv

# создаём среду
mode = "human"
env = DuckietownEnv(
      seed = 1,
       map_name = "udem1",
        #frame_skip = 1,
        #frame_rate = 24,
      dynamics_rand = False,
      domain_rand = False,
      draw_bbox = False)
# получаем первое изображение из среды
obs = env.reset()
env.render(mode)

# присоединяем захват клавиш
key_handler = key.KeyStateHandler()
env.unwrapped.window.push_handlers(key_handler)

#функция обработки для получения параметров скоростей
def update(frame):
    frame = cv2.cvtColor(np.ascontiguousarry(obs), cv2.COLOR_RGB2BGR)
    cv2.imshow("img", frame)
    action = np.array([0.0, 0.0])
    if key_handler[key.UP]:
        action +=np.array([0.44, 0.0])
    if key_handler[key.DOWN]:
        action +=np.array([0.44, 0.0])
    if key_handler[key.LEFT]:
        action +=np.array([0, 1])
    if key_handler[key.RIGHT]:
        action +=np.array([0, 1])
    if key_handler[key.SPACE]:
        action +=np.array([0, 0])
    if key_handler[key.ESCAPE]:
        env.close()
        sys.exit(0)
    print(key_handler)
    return action

while True:
    action = update(obs)
    obs, reward, done, info = env.step(action)
    if cv2.waitKey(33) == ord ('q')
        cv2.destroyAllWindows()
        break
    # print ("step_count = %s, reward = %.3f" %(env.unwrapped.step_count, reward))
     env.render(mode)

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

Переменная obs получает изображение, которое с помощью функций cv2 оптимизируется для отображения на экране.

После этого необходимо давить ползунки для управления фильтром.

Создание ползунков

Для реализации фильтра его нужно настроить. Это означает, что нам нужно добавить 6 управляющих ползунков для регулирования фильтра.

import sys
import gym
import numpy as np
import cv2
from pyglet.window import key
from gym_duckietown.envs import DuckietownEnv

# создаём среду
mode = "human"
env = DuckietownEnv(
      seed = 1,
       map_name = "udem1",
        #frame_skip = 1,
        #frame_rate = 24,
      dynamics_rand = False,
      domain_rand = False,
      draw_bbox = False)
# получаем первое изображение из среды
obs = env.reset()
env.render(mode)

# присоединяем захват клавиш
key_handler = key.KeyStateHandler()
env.unwrapped.window.push_handlers(key_handler)

cv2.namedWindow("img")
def nothing(args): pass

#функция обработки для получения параметров скоростей
def update(frame):
    frame = cv2.cvtColor(np.ascontiguousarry(obs), cv2.COLOR_RGB2BGR)
    cv2.imshow("img", frame)
    action = np.array([0.0, 0.0])
    if key_handler[key.UP]:
        action +=np.array([0.44, 0.0])
    if key_handler[key.DOWN]:
        action +=np.array([0.44, 0.0])
    if key_handler[key.LEFT]:
        action +=np.array([0, 1])
    if key_handler[key.RIGHT]:
        action +=np.array([0, 1])
    if key_handler[key.SPACE]:
        action +=np.array([0, 0])
    if key_handler[key.ESCAPE]:
        env.close()
        sys.exit(0)
    print(key_handler)
    return action

cv2.createTrackbar("b1", "img", 0, 255, nothing)
cv2.createTrackbar("g1", "img", 0, 255, nothing)
cv2.createTrackbar("r1", "img", 0, 255, nothing)

cv2.createTrackbar("b2", "img", 0, 255, nothing)
cv2.createTrackbar("g2", "img", 0, 255, nothing)
cv2.createTrackbar("r2", "img", 0, 255, nothing)


while True:
    b1 = cv2.getTrackbarPos("b1", "img")
    g1 = cv2.getTrackbarPos("g1", "img")
    r1 = cv2.getTrackbarPos("r1", "img")
    b2 = cv2.getTrackbarPos("b2", "img")
    g2 = cv2.getTrackbarPos("g2", "img")
    r2 = cv2.getTrackbarPos("r2", "img")
    action = update(obs)
    obs, reward, done, info = env.step(action)
    if cv2.waitKey(33) == ord ('q')
        cv2.destroyAllWindows()
        break
    # print ("step_count = %s, reward = %.3f" %(env.unwrapped.step_count, reward))
     env.render(mode)

Ниже представлен результат работы программы.

фильтр изображения
Рис 4. Ползунки фильтра

Настройка фильтра под задачу

Добавляем в наш код элементы по поиску жёлтого цвета на изображении и выделение данного цветового объекта.

import sys
import gym
import numpy as np
import cv2
from pyglet.window import key
from gym_duckietown.envs import DuckietownEnv

# создаём среду
mode = "human"
env = DuckietownEnv(
      seed = 1,
       map_name = "udem1",
        #frame_skip = 1,
        #frame_rate = 24,
      dynamics_rand = False,
      domain_rand = False,
      draw_bbox = False)
# получаем первое изображение из среды
obs = env.reset()
env.render(mode)

# присоединяем захват клавиш
key_handler = key.KeyStateHandler()
env.unwrapped.window.push_handlers(key_handler)

cv2.namedWindow("img")
def nothing(args): pass

#функция обработки для получения параметров скоростей
def update(frame):
    frame = cv2.cvtColor(np.ascontiguousarry(obs), cv2.COLOR_RGB2BGR)
    min_p = (b1, g1,r1)
    max_p = (b2, g2, r2)
    img_mask = cv2.inRange(frame, min_p, max_p)
    img_m = cv2.bitwise_and(frame, frame, mask = img_mask)

    cv2.imshow("img", frame)
    action = np.array([0.0, 0.0])
    if key_handler[key.UP]:
        action +=np.array([0.44, 0.0])
    if key_handler[key.DOWN]:
        action +=np.array([0.44, 0.0])
    if key_handler[key.LEFT]:
        action +=np.array([0, 1])
    if key_handler[key.RIGHT]:
        action +=np.array([0, 1])
    if key_handler[key.SPACE]:
        action +=np.array([0, 0])
    if key_handler[key.ESCAPE]:
        env.close()
        sys.exit(0)
    print(key_handler)
    return action

cv2.createTrackbar("b1", "img", 0, 255, nothing)
cv2.createTrackbar("g1", "img", 0, 255, nothing)
cv2.createTrackbar("r1", "img", 0, 255, nothing)

cv2.createTrackbar("b2", "img", 0, 255, nothing)
cv2.createTrackbar("g2", "img", 0, 255, nothing)
cv2.createTrackbar("r2", "img", 0, 255, nothing)


while True:
    b1 = cv2.getTrackbarPos("b1", "img")
    g1 = cv2.getTrackbarPos("g1", "img")
    r1 = cv2.getTrackbarPos("r1", "img")
    b2 = cv2.getTrackbarPos("b2", "img")
    g2 = cv2.getTrackbarPos("g2", "img")
    r2 = cv2.getTrackbarPos("r2", "img")
    action = update(obs)
    obs, reward, done, info = env.step(action)
    if cv2.waitKey(33) == ord ('q')
        cv2.destroyAllWindows()
        break
    # print ("step_count = %s, reward = %.3f" %(env.unwrapped.step_count, reward))
     env.render(mode)

Как вы можете заметить, в функцию update добавлены строки:

    frame = cv2.cvtColor(np.ascontiguousarry(obs), cv2.COLOR_RGB2BGR)
    min_p = (b1, g1,r1)
    max_p = (b2, g2, r2)
    img_mask = cv2.inRange(frame, min_p, max_p)
    img_m = cv2.bitwise_and(frame, frame, mask = img_mask)
    cv2.imshow("img", frame)

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

компьютерное зрение
Рис 5. Компьютерное зрение

Выделяем контур уточки

Теперь осталось выделить контуры, расставить их по возрастанию площади и сравнивать с площадью экрана указав предельный параметр “площади” изображения уточки. Данный параметр будем разрешать/ запрещать движение вперед.

import sys
import gym
import numpy as np
import cv2
from pyglet.window import key
from gym_duckietown.envs import DuckietownEnv

# создаём среду
mode = "human"
env = DuckietownEnv(
      seed = 1,
       map_name = "udem1",
        #frame_skip = 1,
        #frame_rate = 24,
      dynamics_rand = False,
      domain_rand = False,
      draw_bbox = False)
# получаем первое изображение из среды
obs = env.reset()
env.render(mode)

# присоединяем захват клавиш
key_handler = key.KeyStateHandler()
env.unwrapped.window.push_handlers(key_handler)

cv2.namedWindow("img")
def nothing(args): pass

#функция обработки для получения параметров скоростей
def update(frame):
    frame = cv2.cvtColor(np.ascontiguousarry(obs), cv2.COLOR_RGB2BGR)
    min_p = (b1, g1,r1)
    max_p = (b2, g2, r2)
    img_mask = cv2.inRange(frame, min_p, max_p)
    img_m = cv2.bitwise_and(frame, frame, mask = img_mask)

    contours, ret = cv2.findContours(img_mask.copy(), cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)
    w,h,c = img_m.shape
    img_area = w*h
    maxarea = 0

    if contours:
        area_val = []

        for i in range (len (contours)):
            area = cv2.contourArea(contours[i])
            area_val.append(area)
            maxarea = max(area_val)
            print(img_area, maxarea)
            pos = area_val.index(maxarea)
            cv2.drawContours(img_m, contours[pos], -1,(0,255,0),2)

    cv2.imshow("img", frame)
    action = np.array([0.0, 0.0])
    if key_handler[key.UP]:
        action +=np.array([0.44, 0.0])
    if key_handler[key.DOWN]:
        action +=np.array([0.44, 0.0])
    if key_handler[key.LEFT]:
        action +=np.array([0, 1])
    if key_handler[key.RIGHT]:
        action +=np.array([0, 1])
    if key_handler[key.SPACE]:
        action +=np.array([0, 0])
    if key_handler[key.ESCAPE]:
        env.close()
        sys.exit(0)
    if (maxarea/img_area>1/11):
        action -= np.array([0.44, 0])
        print('STOP')

    print(key_handler)
    return action
    
cv2.createTrackbar("b1", "img", 0, 255, nothing)
cv2.createTrackbar("g1", "img", 0, 255, nothing)
cv2.createTrackbar("r1", "img", 0, 255, nothing)

cv2.createTrackbar("b2", "img", 0, 255, nothing)
cv2.createTrackbar("g2", "img", 0, 255, nothing)
cv2.createTrackbar("r2", "img", 0, 255, nothing)


while True:
    b1 = cv2.getTrackbarPos("b1", "img")
    g1 = cv2.getTrackbarPos("g1", "img")
    r1 = cv2.getTrackbarPos("r1", "img")
    b2 = cv2.getTrackbarPos("b2", "img")
    g2 = cv2.getTrackbarPos("g2", "img")
    r2 = cv2.getTrackbarPos("r2", "img")
    action = update(obs)
    obs, reward, done, info = env.step(action)
    if cv2.waitKey(33) == ord ('q')
        cv2.destroyAllWindows()
        break
    # print ("step_count = %s, reward = %.3f" %(env.unwrapped.step_count, reward))
     env.render(mode)

Из кода видно, что за контуры отвечает contours.

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

Определение контура изображения
Рис 6. Определение контура утки

Итог

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

Курсы Робикс, в которых изучается этот материал

1. Duckietown: робот с системой автопилота

Вопросы

  • Опишите основные части структуры программы.
  • Как происходит поиск уточки ?
  • Как реализуется остановка (обратный ход), что служит причиной работы этого участка кода ? 
Сохраните или поделитесь

Добавить комментарий

Ваш адрес email не будет опубликован. Обязательные поля помечены *