понедельник, 11 июня 2018 г.

Длинный хвост питона - часть 2

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


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


Алгоритм распознавания знаков "Влево", "Вправо", "Прямо" заключается в следующем:
  • отрезаем правую часть кадра (знак должен быть только справа от дороги)
  • накладываем цветовую маску, отбрасывая все не синие объекты
  • проверяем, что в кадре есть синие объекты
  • ищем среди объектов эллипсы встроенной функцией OpenCV
  • если найден эллипс с диаметром, лежащем в заданном диапазоне (знак на удобном для распознавания расстоянии) - выполняем сравнение с сохраненными образцами
  • результат сравнения с наиболее "похожим" знаком накапливается, пока модель движется на удобном для распознавания расстоянии от знака
  • на основе накопленной информации принимается решение о направлении движения на следующей развязке
Пример прохождения трассы (вид из бортовой камеры):


Обновленный алгоритм распознавания черной линии:
  • берем очередной кадр из объекта в памяти
  • отрезаем от кадра полоску высотой 20 пикселей в нижней его части
  • переводим в цветовое пространство HSV
  • используя функцию cv2.findContours проводим контурный анализ полоски, выделяя объекты
  • отбрасываем объекты со слишком малой площадью, которые не могут быть линиями
  • используя функцию вычисления моментов cv2.moments, вычисляем горизонтальное смещение каждого из оставшихся объектов относительно центра
  • выбираем из найденных линии ту, по которой мы продолжим движение, опираясь на распознанные знаки 
  • передаем данные о смещении относительно линии процессу ПИД-регулятора

Исходный код программы для робота:
#!/usr/bin/env python3

from flask import Flask, render_template, Response
import serial, time
import cv2
import threading
import numpy as np
from imutils.video import WebcamVideoStream

port = "/dev/ttyACM0"
ser = serial.Serial(port , 115200, timeout = 1)
time.sleep(5)

cx = []
speed_go = 255
porog = 1000
porog1 = 0
porog2 = 0
speed_blue = 0
speed = 255
K = 1
see_red = 0
see_blue = 0
x = 0
out_old = 0
i_main = 0
i_mail2arduino = 0
i_see_sign = 0
i_camera2inet = 0
time_main = time.time()
time_mail2arduino = time.time()
time_see_sign = time.time()
time_camera2inet = time.time()
fps_main = 0
fps_mail2arduino = 0
fps_see_sign = 0
fps_camera2inet = 0
pixel_ellips = 0
pixel_ellips_blue = 0

class sign:
    def __init__(self, way_in):
        self.sign_img = cv2.imread(way_in)
        self.sign_img_hsv = cv2.cvtColor(self.sign_img, cv2.COLOR_BGR2HSV)
        self.sign_img_inRange = cv2.inRange(self.sign_img_hsv, (0, 0, 0), (254, 254, 254))

sign_right = sign("/var/www/html/sign_right.png")
sign_left = sign("/var/www/html/sign_left.png")
sign_forward = sign("/var/www/html/sign_forward.png")

sign_direct = sign_right
direct = 0

cap = WebcamVideoStream(src=0).start()
for i in range(5): frame = cap.read()
frame = cap.read()
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
frame_gray = cv2.inRange(hsv[420:,:, :], (0, 0, 0), (255, 255, 100))
frame_red = cv2.inRange(hsv[:, 320:, :], (0, 150, 30), (10, 255, 80))
frame_blue = cv2.inRange(hsv[:, 320:, :], (100, 220, 60), (150, 255, 255))

cv2.imwrite('/var/www/html/dgip_frame_all.png', frame)

def mail2arduino_pr1():
    print("Start arduino thread")
    global K, x, out_old, speed_go, speed, i_mail2arduino, fps_mail2arduino, time_mail2arduino
    while 1:
        local_x = x
        if(local_x >= 0): out = "L"
        else: out = "R"
        local_x = abs(int(local_x*K))

        if(local_x > 90): local_x = 90
        if(local_x < 10): out += "0" + str(local_x)
        else: out += str(local_x)

        if(speed >= 100): out += "F" + str(speed)
        elif(speed >= 10): out += "F0" + str(speed)
        else: out += "F00" + str(speed)

        if(out != out_old):
            ser.write(out.encode())
            time_out = time.time()

        if(time.time() - time_out > 2):
            ser.write("0000000".encode())
            time_out = time.time()
        out_old = out
        i_mail2arduino += 1
        if(time.time() - time_mail2arduino > 1):
            time_mail2arduino = time.time()
            fps_mail2arduino = i_mail2arduino
            i_mail2arduino = 0

def image2jpeg(image):
    ret, jpeg = cv2.imencode('.jpg', image)
    return jpeg.tobytes()

def camera2inet_pr2():
    print("Start inet thread")
    global frame_gray, frame, frame_red, hsv, i_camera2inet, time_camera2inet,  fps_camera2inet, direct, pixel_ellips_blue
    global fps_main, fps_see_sign, fps_mail2arduino, fps_camera2inet, pixel_ellips, see_red, sign_direct
    global porog1, porog2, see_blue
    app = Flask(__name__)

    @app.route('/')
    def index():
        return render_template('index.html')

    def gen_gray():
        while True:
            frame_inet = cv2.inRange(hsv[460:, :, :], (0, 0, 0), (150, 255, 80))
            cv2.putText(frame_inet,"fps: "+str(fps_main)+" "+str(fps_see_sign)+" "+str(fps_mail2arduino), (0,10), cv2.FONT_HERSHEY_SIMPLEX, 0.3, (255,255,255), 1)
            frameinet = image2jpeg(frame_inet)
            yield (b'--frame\r\n'
                    b'Content-Type: image/jpeg\r\n\r\n' + frameinet + b'\r\n\r\n')
    def gen_frame():
        while True:
            frame_inet = frame
            frameinet = image2jpeg(frame_inet)
            yield (b'--frame\r\n'
                    b'Content-Type: image/jpeg\r\n\r\n' + frameinet + b'\r\n\r\n')
    def gen_red():
        while True:
            cv2.putText(frame_red,"red_see: "+str(see_red), (0,10), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (255,255,255), 1)
            cv2.putText(frame_red,"red_pixel: "+str(pixel_ellips), (0,20), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (255,255,255), 1)
            frameinet = image2jpeg(frame_red)
            yield (b'--frame\r\n'
                    b'Content-Type: image/jpeg\r\n\r\n' + frameinet + b'\r\n\r\n')
    def gen_direct():
        while True:
            frameinet = image2jpeg(sign_direct)
            yield (b'--frame\r\n'
                    b'Content-Type: image/jpeg\r\n\r\n' + frameinet + b'\r\n\r\n')
    def gen_blue():
        while True:
            cv2.putText(frame_blue,"direct: "+str(direct), (0,10), cv2.FONT_HERSHEY_SIMPLEX, 0.3, (255,255,255), 1)
            cv2.putText(frame_blue,"pixel_ellips_blue: "+str(pixel_ellips_blue), (0,20), cv2.FONT_HERSHEY_SIMPLEX, 0.3, (255,255,255), 1)
            frameinet = image2jpeg(frame_blue)
            yield (b'--frame\r\n'
                    b'Content-Type: image/jpeg\r\n\r\n' + frameinet + b'\r\n\r\n')

    @app.route('/video_frame')
    def video_frame():
        return Response(gen_frame(),
                        mimetype='multipart/x-mixed-replace; boundary=frame')
    @app.route('/video_line')
    def video_line():
        return Response(gen_gray(),
                        mimetype='multipart/x-mixed-replace; boundary=frame')
    @app.route('/video_red')
    def video_red():
        return Response(gen_red(),
                        mimetype='multipart/x-mixed-replace; boundary=frame')
    @app.route('/video_direct')
    def video_direct():
        return Response(gen_direct(),
                        mimetype='multipart/x-mixed-replace; boundary=frame')
    @app.route('/video_blue')
    def video_blue():
        return Response(gen_blue(),
                        mimetype='multipart/x-mixed-replace; boundary=frame')
    time.sleep(0.3)
    i_camera2inet += 1
    if(time.time() - time_camera2inet > 1):
        time_camera2inet = time.time()
        fps_camera2inet = i_camera2inet
        i_camera2inet = 0

    app.run(host='0.0.0.0', debug=False,threaded=True)

def see_sign_pr3():
    print("Start see red thread")
    global K, see_blue, pixel_ellips, speed_blue, pixel_ellips_blue, see_red, hsv, frame_red, i_see_sign, time_see_sign, fps_see_sign, direct, sign_direct, frame_blue
    print(frame_red.dtype)
    time_last_see_red = time.time()-10
    search_red = True
    time_last_see_blue = time.time()-10
    search_blue = True
    time_see_sign_forward = time.time()-10
    pre_direct = 0
    while 1:
        frame_red = cv2.inRange(hsv[:, 320:, :], (10, 10, 30), (10, 255, 255))
        frame_blue = cv2.inRange(hsv[:, 320:, :], (90, 200, 50), (150, 255, 255))
        if(search_red):
            frame_copy = frame_red.copy()
            _, con, hierarchy = cv2.findContours(frame_copy, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
            for i in con:
                if len(i)>10:
                    ellipse = cv2.fitEllipse(i)
                    _, y1 = ellipse[0]
                    _, y2 = ellipse[1]
                    pixel_ellips = abs(y2-y1)*0.3 + 0.7 * pixel_ellips
                    if(pixel_ellips > 1000):
                        pixel_ellips = 0
                        see_red = 1
                        search = False
                        time_last_see_red = time.time()
                        cv2.ellipse(frame_red, ellipse, (255,0,0), 2)
                    else: see_red = 0
                    break
                else:
                    see_red = 0
        if(time.time() - time_last_see_red <= 10):
            see_red = 1
        if(time.time() - time_last_see_red > 10):
            see_red = 0
            K = 0.1
        if(time.time() - time_last_see_red >= 12):
            K = 1
        if(time.time() - time_last_see_red >= 20):
            search_red = True

        if(search_blue and np.sum(frame_blue)>1200000):
            see_blue = 1
            frame_copy = frame_blue.copy()
            _, con, hierarchy = cv2.findContours(frame_copy, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
            max_con = []
            for i in range(len(con)):
                if len(con[i])>len(max_con):
                    max_con = con[i]

            ellipse = cv2.fitEllipse(max_con)

            x1, y1 = ellipse[0]
            x2, y2 = ellipse[1]

            t1 = ((int(x1)-int(x2/2)), (int(y1)-int(y2/2)))
            t2 = ((int(x1)+int(x2/2)), (int(y1)+int(y2/2)))

            pixel_ellips_blue = abs((int(y1)-int(y2/2))-(int(y1)+int(y2/2)))*0.15 + 0.85 * pixel_ellips_blue
            cv2.rectangle(frame_blue, t1, t2, (100, 100, 100), 3)

            if(pixel_ellips_blue < 90 and pixel_ellips_blue > 70):
                time_last_see_blue = time.time()

                pre_sign_direct = frame_blue[int(y1)-int(y2/2):int(y1)+int(y2/2), int(x1)-int(x2/2):int(x1)+int(x2/2)]
                if(pre_sign_direct.shape[0]>0 and pre_sign_direct.shape[1]>0): sign_direct = cv2.resize(pre_sign_direct, (64, 64), interpolation=cv2.INTER_AREA)

                sum_matr = [np.sum(sign_direct*sign_left.sign_img_inRange),
                            np.sum(sign_direct*sign_forward.sign_img_inRange),
                            np.sum(sign_direct*sign_right.sign_img_inRange)]

                if(np.argmax(sum_matr) == 2):
                    direct += 1
                elif(np.argmax(sum_matr) == 0):
                    direct -= 1
                elif(np.argmax(sum_matr) == 1):
                    direct == 0
                    K = 0.1
                    time_see_sign_forward = time.time()
        else:
            see_blue = 0
        if(time.time() - time_last_see_blue > 15):
            direct = 0
        if(time.time() - time_see_sign_forward > 15):
            K = 1
        i_see_sign += 1
        if(time.time() - time_see_sign > 1):
            time_see_sign = time.time()
            fps_see_sign = i_see_sign
            i_see_sign = 0

pr2 = threading.Thread(target=camera2inet_pr2)
pr2.daemon = True
pr2.start()
time.sleep(5)

pr1 = threading.Thread(target=mail2arduino_pr1)
pr1.daemon = True
pr1.start()
pr3 = threading.Thread(target=see_sign_pr3)
pr3.daemon = True
pr3.start()

while 1:
    frame = cap.read()
    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
    frame_gray = cv2.inRange(hsv[460:, :, :], (0, 0, 0), (150, 255, 80))
    if(np.sum(frame_gray) > porog and see_red != 1):
        if(see_blue != 1): speed = 255
        else: speed = 120
        cx.clear()
        min = 1000
        _, contours, hierarchy = cv2.findContours(frame_gray.copy(), cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
        for i in contours:
            if(i is not None):
                area = cv2.contourArea(i)
                if(area>=250):
                    M = cv2.moments(i)
                    pre_x = int(M['m10']/M['m00'])
                    cx.append(pre_x)
                    if(direct == 0):
                        if(abs(pre_x-320)<min):
                            min = abs(pre_x-320)
                            x = 320 - pre_x
                    elif(direct > 0):
                        if(abs(pre_x-640)<min):
                            min = abs(pre_x-640)
                            x = 320 - pre_x
                    elif(direct < 0):
                        if(pre_x<min):
                            min = pre_x
                            x = 320 - pre_x
    else:
        speed = 0
    i_main += 1
    if(time.time() - time_main > 1):
        time_main = time.time()
        fps_main = i_main
        i_main = 0

воскресенье, 3 июня 2018 г.

Страшная снаружи, добрая внутри

После установки натяжного потолка в одной из комнат нам пришлось распрощаться с 5-рожковой люстрой 5x100 Вт и диммером "Сапфир" c плавным управлением яркостью с любого пульта ДУ. Лампы накаливания сильно греются и потолку от них нехорошо, поэтому мы начали искать замещающее решение с использованием светодиодных ламп. Света в сумме хотелось не меньше, такого же лампово-теплого по спектру и по-прежнему подвластного пульту.
Оставлю за кадром решения, которые мы рассматривали, но в итоге направления осталось два:
  • диммер для светодиодных ламп и использование недешевых диммируемых ламп
  • некий китайский контроллер, монтируемый в люстру и позволяющий последовательно зажигать до 3 каналов, повторным нажатием выключателя. Главный его недостаток - он не помнит установленным последним уровень освешенности. Достоинство решения - использование любых светодиодных ламп.
Параллельно с этим руки зачесались спаять что-нибудь самодельно-кустарное, получив все желаемые функции быстро и уже начать пользоваться люстрой, продолжая поиск более красивого и функционального аналога.

Итак, для очередного проекта выходного дня нам понадобились:
  • 4 настенных светильника для ванной комнаты. Для самоделки удобны тем, что будучи соединенными попарно винтами образуют корпуса для электроники между подставками
  • 4 светодиодные лампы с патроном e27 на 5, 10, 20 и 20 Вт
  • алюминиевая трубка, алюминиевая полоса, винты и гайки м4
  • клеммы винтовые 2,5мм, провода
  • зарядка от телефона USB, которую мы лишили корпуса
  • arduino nano
  • 4 реле модуля, управляемых 5В логикой
  • ИК-приемник VS1838B
  • микродинамик

Собранная люстра обладает следующими функциями:
  • включение-выключение обычным клавишным выключателем - вся электроника "нормально-разомкнута"
  • управление с любого пульта ДУ (используется 2 кнопки - прибавить яркость, убавить яркость)
  • 11 уровней яркости: 5Вт, 10Вт, 15Вт(5+10), 20Вт, 25Вт(20+5), 30 Вт(20+10), 35Вт(20+10+5), 40Вт(20+20), 45Вт(20+20+5), 50Вт(20+20+10), 55Вт(20+20+10+5)
  • "плавный старт" - если в люстре в последний раз были включены, например, 3 лампы, то при повторном включении они будут последовательно включены с интервалом в 2 секунды - по нарастанию их мощности - чтобы не ослепить входящего в комнату человека
  • обучение под любой пульт ДУ - троекратное нажатие последовательности любых трех кнопок на любом пульте вводит люстру в режим обучения - издается звуковой сигнал. В режиме обучения ожидается нажатие любых двух кнопок - они будут запомнены как новые кнопки управления яркостью
  • запоминание установленного уровня яркости в энергонезависимой памяти через 30 сек после последней регулировки
  • автовыключение света через 6 часов
  • звуковое подтверждение всех операций по регулировке, обучению, сохранении настроек

Код программы выглядит так:


#include <IRremote.h> 
#include <EEPROM.h>

int relay1 = 5; // 5W
int relay2 = 6; // 10W
int relay3 = 7; // 20W
int relay4 = 8; // 20W

int buzzer = 9; // speaker or led

unsigned long history[9];

unsigned long power_up = 0xFFA05F;
unsigned long power_down = 0xFF20DF;

unsigned long timer = millis();
boolean not_rec = false;

int receiverPin = 11; 
IRrecv irrecv(receiverPin);
  
decode_results results;

byte power;
int mem_addr = 0;

long EEPROMReadlong(long address) {
  long four = EEPROM.read(address);
  long three = EEPROM.read(address + 1);
  long two = EEPROM.read(address + 2);
  long one = EEPROM.read(address + 3);
  
  return ((four << 0) & 0xFF) + ((three << 8) & 0xFFFF) + ((two << 16) & 0xFFFFFF) + ((one << 24) & 0xFFFFFFFF);
}

void EEPROMWritelong(int address, long value) {
  byte four = (value & 0xFF);
  byte three = ((value >> 8) & 0xFF);
  byte two = ((value >> 16) & 0xFF);
  byte one = ((value >> 24) & 0xFF);
  
  EEPROM.write(address, four);
  EEPROM.write(address + 1, three);
  EEPROM.write(address + 2, two);
  EEPROM.write(address + 3, one);
}

void set_power(int p) {
  timer = millis();
  not_rec = true;
  Serial.print("Power: ");
  Serial.println(p);
  boolean r1, r2, r3, r4;
  if (p >= 40) {r4 = LOW; p -= 20;} else r4 = HIGH;
  if (p >= 20) {r3 = LOW; p -= 20;} else r3 = HIGH;
  if (p >= 10) {r2 = LOW; p -= 10;} else r2 = HIGH;
  if (p >= 5)  {r1 = LOW; p -= 5;} else r1 = HIGH;
  
  digitalWrite(relay1, r1);
  digitalWrite(relay2, r2);
  digitalWrite(relay3, r3);
  digitalWrite(relay4, r4);  
  
  Serial.print("Relay 5W: ");
  Serial.println(!r1);
  Serial.print("Relay 10W: ");
  Serial.println(!r2);
  Serial.print("Relay 20W: ");
  Serial.println(!r3);
  Serial.print("Relay 20W: ");
  Serial.println(!r4);  
}

void init_power(int p) {
  
  int init_delay = 2000;
  Serial.print("Init power: ");
  Serial.println(p);
  boolean r1, r2, r3, r4;
  if (p >= 40) {r4 = LOW; p -= 20;} else r4 = HIGH;
  if (p >= 20) {r3 = LOW; p -= 20;} else r3 = HIGH;
  if (p >= 10) {r2 = LOW; p -= 10;} else r2 = HIGH;
  if (p >= 5)  {r1 = LOW; p -= 5;} else r1 = HIGH;
  
  boolean light = false;
  digitalWrite(relay1, r1);
  if (!r1) light = true;
  Serial.print("Relay 5W: ");
  Serial.println(!r1);
  if (light && !r2) delay(init_delay);
  digitalWrite(relay2, r2);
  if (!r2) light = true;
  Serial.print("Relay 10W: ");
  Serial.println(!r2);
  if (light && !r3) delay(init_delay);
  digitalWrite(relay3, r3);
  if (!r3) light = true;
  Serial.print("Relay 20W: ");
  Serial.println(!r3);
  if (light && !r4) delay(init_delay);
  digitalWrite(relay4, r4);  
  Serial.print("Relay 20W: ");
  Serial.println(!r4);  
}


void learn (unsigned long code) {
  delay(100);
  for (int i=0; i<8; i++) history[i] = history[i+1];
  history[8] = code;
  if (history[0] == history[3] && history[0] == history[6] &&
      history[1] == history[4] && history[1] == history[7] &&
      history[2] == history[5] && history[2] == history[8] &&
      history[0] != history[1] && history[1] != history[2]) {
      Serial.println("Learn mode enabled");  
      tone(buzzer, 880, 200);      
      delay(1000);
      irrecv.resume();
      
      while (!irrecv.decode(&results));
      tone(buzzer, 523, 200);
      power_up = results.value;
      delay(100);      
      irrecv.resume();
            
      delay(1000);

      while (!irrecv.decode(&results));
      tone(buzzer, 440, 200);
      power_down = results.value;
      delay(100);      
      irrecv.resume();

      EEPROMWritelong(mem_addr+1,power_up);
      EEPROMWritelong(mem_addr+5,power_down);      
  }
}  
  
void setup() {
  pinMode(relay1, OUTPUT);
  pinMode(relay2, OUTPUT);
  pinMode(relay3, OUTPUT);
  pinMode(relay4, OUTPUT);

  digitalWrite(relay1, HIGH);
  digitalWrite(relay2, HIGH);
  digitalWrite(relay3, HIGH);
  digitalWrite(relay4, HIGH);

  pinMode(buzzer, OUTPUT);   
  tone(buzzer, 659, 500);
  
  Serial.begin(9600);
  
  power = EEPROM.read(mem_addr);
  init_power(int(power));
  
  irrecv.enableIRIn();
  for (int i=0; i<9; i++) history[i] = i;

  power_up = EEPROMReadlong(mem_addr+1);
  power_down = EEPROMReadlong(mem_addr+5);
}

void loop() {
  if (irrecv.decode(&results)) { 
    if(results.value == power_up){
      if (power < 55) {
        tone(buzzer, 523, 200);
        power += 5;
        set_power(int(power));
      }
      else {
        tone(buzzer, 329, 200);
      }  
    }
    if(results.value == power_down){
      if (power > 5) {
        tone(buzzer, 440, 200);
        power -= 5;
        set_power(int(power));
      }
      else {
        tone(buzzer, 329, 200);
      }   
    }
    learn (results.value);
    irrecv.resume();
  }
  if (not_rec && millis()-timer > 30000) {
    tone(buzzer, 262, 100);
    EEPROM.write(mem_addr, power); 
    Serial.println("Save to EEPROM: ");
    Serial.println(power);
    timer = millis();
    not_rec = false;  
  } 
  if (millis()-timer > 6*60*60*1000) {
    power = 0;
    set_power(int(power));
    tone(buzzer, 659, 1000);
    delay(1000);
    tone(buzzer, 440, 1000);
    delay(1000);       
  }  
}





воскресенье, 6 мая 2018 г.

Длинный хвост питона - часть 1

Последние полтора года наша команда изучает Python и почти все наши проекты, использующие LEGO Mindstorms EV3 в качестве платформы, были разработаны с использованием этого языка программирования. Изучение Python и, параллельно, ОС Linux, велось не случайно, а имело далеко идущие планы. Дело в том, что Питон должен стать "переходным мостиком", протянув свой длинный хвост между LEGO-роботами и более серьезной робототехникой, в которой компонентов от LEGO должно становиться все меньше и меньше.


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

Конструкция модели автомобиля, которую мы будем использовать, во многом повторяет модель LEGO Technic #9398, имеет полноприводное шасси и рулевой сервомотор.


В проекте мы снова будем использовать Raspberry Pi, а вот в качестве контроллера двигателей, роль которого в прошлом проекте "Следуй за белым кроликом" выполнял LEGO Mindstroms EV3, применим Arduino с установленным Adafruit Motor Shield. C LEGO Technic двигателями постоянного тока все понятно, они штатно подключаюся к клеммам M1 и M2 шилда и управляются функциями изменения скорости и направления вращения.


А вот с LEGO сервомотором пришлось поэкспериментировать. Как оказалось, управляется он посредством ШИМ по двум средним проводам, крайние же обеспечивают питание. Соответственно подключить его можно точно также как обычный DC-мотор к, например, клеммам M3, однако необходимо дополнительно обеспечить ему питание 9В, которое можно взять с клемм питания шилда PWR_EXT. Для управления углом поворота сервомотора используется обычная ШИМ-модуляция, соответственно встроенные в библиотеку Adafruit Motor Shield функции для управления скоростью и направлением вращения DC-мотора начинают управлять углом поворота сервомотора.


Как вы уже наверное успели заметить на фото выше - мы используем новую камеру - это камера Sony Playstation Eye. Ее основные преимущества - высокая частота кадров (до 180 fps), достаточно широкий, изменяемый физическим регулятором угол зрения и отличная поддержка со стороны Linux.

Основные моменты, которые мы проработаем в первой части проекта:
  • Организуем передачу данных между Raspberry Pi и Arduino, используя помехозащищенный "протокол", основанный на передаче текстовых строк
  • Настроим многопоточную обработку данных для разумного распределения вычислительных ресурсов
  • Запустим веб-трансляцию обработанного видеопотока для визуального мониторинга прохождения трассы
  • Научимся распознавать один знак - "Стоп", расположенный справа от дороги и остановку на заданном расстоянии до него.
Для взаимодействия Raspberry Pi и Arduino мы используем стандартное serial-соединение поверх USB и соответствующие библиотеки с обоих сторон. Так как нам нужно передавать различные данные - скорость и направление вращения ходового мотора и угол поворота сервомотора, то просто передавать число через serial не выйдет - принимающая сторона должна понимать что она получает. Мы организовали передачу пакета данных в виде текстовой строки в формате "L90F255", где:
  • первый символ L/R - указывает направление поворота рулевого сервомотора
  • следующие два символа - угол поворота рулевого сервомотора в данном направлении, в диапазоне 0..90
  • четвертый символ - F/B - направление вращение ходового мотора (вперед-назад)
  • следующие три символа - скорость вращения ходовых моторов в данном направлении, в диапазоне 0..255.
Последний переданный пакет (строка) запоминается, и новый пакет, чтобы не забивать канал передачи данных, передается только при изменении углов/скоростей, либо раз в секунду подтверждается повторением, если последние переданные угол и скорость все еще актуальны. Необходимость в подтверждении связана с реализацией на принимающей стороне автоматического отключения двигателей, если за последние 3 секунды не приходило никаких "правильных" пакетов. Это очень удобно - можно прерывать программу на Raspberry Pi не опасаясь что Arduino продолжит крутить двигателями в соответствии с последней переданной командой.


На стороне Raspberry Pi у нас выполняется целый ряд задач, каждая из которых должна работать "в своем темпе", не тормозя работу остальных:
  • взаимодействие с Arduino
  • чтение данных с камеры в памяти
  • анализ нижней части кадра, поиск линии, формирование отклонения от линии
  • ПИД регулирование (ошибка - отклонение от линии, управляющее воздействие - угол поворота рулевого сервомотора)
  • анализ правой части кадра, поиск знака "стоп"
  • трансляция видеопотока, формирующегося из нижней части кадров при поиске линии
Для параллельного выполнения задач в программе на стороне Raspberry Pi используется модуль threading.


Веб-трансляция видеопотока, формирующегося в ходе анализа положения линии относительно модели автомобиля, происходит с использованием модуля flask. Конечно, использование wi-fi дает некоторую задержку при трансляции, но при невысокой скорости движения это не так важно. Гораздо важнее иметь возможность "видеть как робот", понимать на каком участке трассы какие есть проблемы с распознаванием линии (засветка, радиусы поворота и т.п.).

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


Исходный код программы для Raspberry Pi:

#!/usr/bin/env python3

from flask import Flask, render_template, Response
import serial, time
import cv2
import threading
import numpy as np
from imutils.video import WebcamVideoStream

port = "/dev/ttyACM0"
ser = serial.Serial(port , 115200, timeout = 1)
time.sleep(5)

speed_go = 0
porog = 500
speed = 155
K = 2
see_red = 0
x = 0
out_old = 0
i_main = 0
i_mail2arduino = 0
i_see_red = 0
i_camera2inet = 0
time_main = time.time()
time_mail2arduino = time.time()
time_see_red = time.time()
time_camera2inet = time.time()
fps_main = 0
fps_mail2arduino = 0
fps_see_red = 0
fps_camera2inet = 0
pixel_ellips = 0

cap = WebcamVideoStream(src=0).start()
for i in range(5): frame = cap.read()
frame = cap.read()
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
frame_gray = cv2.inRange(hsv[420:,:, :], (0, 0, 0), (255, 255, 100))
frame_red = cv2.inRange(hsv[:, 320:, :], (0, 150, 30), (10, 255, 80))

cv2.imwrite('/var/www/html/dgip_gray360.png', frame_gray)
cv2.imwrite('/var/www/html/dgip_frame_all.png', frame)

def mail2arduino_pr1():
    print("Start arduino thread")
    global x, out_old, speed_go, speed, i_mail2arduino, fps_mail2arduino, time_mail2arduino
    while 1:
        local_x = x
        if(local_x >= 0): out = "L"
        else: out = "R"
        local_x = abs(int(local_x*K))

        if(local_x > 90): local_x = 90
        if(local_x < 10): out += "0" + str(local_x)
        else: out += str(local_x)

        if(speed >= 100): out += "F" + str(speed)
        elif(speed >= 10): out += "F0" + str(speed)
        else: out += "F00" + str(speed)

        if(out != out_old):
            ser.write(out.encode())
            time_out = time.time()

        if(time.time() - time_out > 2):
            ser.write("0000000".encode())
            time_out = time.time()
        out_old = out
        i_mail2arduino += 1
        if(time.time() - time_mail2arduino > 1):
            time_mail2arduino = time.time()
            fps_mail2arduino = i_mail2arduino
            i_mail2arduino = 0

def image2jpeg(image):
    ret, jpeg = cv2.imencode('.jpg', image)
    return jpeg.tobytes()

def camera2inet_pr2():
    print("Start inet thread")
    global frame_gray, frame, frame_red, i_camera2inet, time_camera2inet,  fps_camera2inet
    global fps_main, fps_see_red, fps_mail2arduino, fps_camera2inet, pixel_ellips, see_red
    app = Flask(__name__)

    @app.route('/')
    def index():
        return render_template('index.html')

    def gen_gray():
        while True:
            cv2.putText(frame_gray,"fps_main: "+str(fps_main), (0,10), cv2.FONT_HERSHEY_SIMPLEX, 0.3, (255,255,255), 1)
            cv2.putText(frame_gray,"fps_see_red: "+str(fps_see_red), (0,20), cv2.FONT_HERSHEY_SIMPLEX, 0.3, (255,255,255), 1)
            cv2.putText(frame_gray,"fps_mail2arduino: "+str(fps_mail2arduino), (0,30), cv2.FONT_HERSHEY_SIMPLEX, 0.3, (255,255,255), 1)
            cv2.putText(frame_gray,"fps_camera2inet: "+str(fps_camera2inet), (0,40), cv2.FONT_HERSHEY_SIMPLEX, 0.3, (255,255,255), 1)

            frameinet = image2jpeg(frame_gray)
            yield (b'--frame\r\n'
                    b'Content-Type: image/jpeg\r\n\r\n' + frameinet + b'\r\n\r\n')
    def gen_red():
        while True:
            cv2.putText(frame_red,"red_see: "+str(see_red), (0,10), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (255,255,255), 1)
            cv2.putText(frame_red,"red_pixel: "+str(pixel_ellips), (0,20), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (255,255,255), 1)
            frameinet = image2jpeg(frame_red)
            yield (b'--frame\r\n'
                    b'Content-Type: image/jpeg\r\n\r\n' + frameinet + b'\r\n\r\n')

    @app.route('/video_line')
    def video_line():
        return Response(gen_gray(),
                        mimetype='multipart/x-mixed-replace; boundary=frame')
    @app.route('/video_red')
    def video_red():
        return Response(gen_red(),
                        mimetype='multipart/x-mixed-replace; boundary=frame')
    time.sleep(0.15)
    i_camera2inet += 1
    if(time.time() - time_camera2inet > 1):
        time_camera2inet = time.time()
        fps_camera2inet = i_camera2inet
        i_camera2inet = 0

    app.run(host='0.0.0.0', debug=False,threaded=True)

def see_red_pr3():
    print("Start see red thread")
    global K, pixel_ellips, see_red, hsv, frame_red, i_see_red, time_see_red, fps_see_red
    print(frame_red.dtype)
    time_last_see_red = time.time()-20
    search = True
    while 1:
        frame_red = cv2.inRange(hsv[:, 320:, :], (0, 120, 30), (10, 255, 255))
        if(search):
            frame_copy = frame_red.copy()
            _, con, hierarchy = cv2.findContours(frame_copy, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
            for i in con:
                if len(i)>10:
                    ellipse = cv2.fitEllipse(i)
                    _, y1 = ellipse[0]
                    _, y2 = ellipse[1]
                    pixel_ellips = abs(y2-y1)*0.15 + 0.85 * pixel_ellips
                    if(pixel_ellips > 200):
                        pixel_ellips = 0
                        see_red = 1
                        search = False
                        time_last_see_red = time.time()
                    else: see_red = 0
                    cv2.ellipse(frame_red, ellipse, (255,0,0), 2)
                    break
                else:
                    see_red = 0
        if(time.time() - time_last_see_red <= 10):
            see_red = 1
        if(time.time() - time_last_see_red > 10):
            see_red = 0
            K = 0.1
        if(time.time() - time_last_see_red >= 12):
            K = 1
        if(time.time() - time_last_see_red >= 20):
            search = True

        i_see_red += 1
        if(time.time() - time_see_red > 1):
            time_see_red = time.time()
            fps_see_red = i_see_red
            i_see_red = 0


pr1 = threading.Thread(target=mail2arduino_pr1)
pr1.daemon = True
pr1.start()
pr2 = threading.Thread(target=camera2inet_pr2)
pr2.daemon = True
pr2.start()
pr3 = threading.Thread(target=see_red_pr3)
pr3.daemon = True
pr3.start()

while 1:
    frame = cap.read()
    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
    frame_gray = cv2.inRange(hsv[420:,:, :], (0, 0, 0), (255, 255, 80))
    if(np.sum(frame_gray) > porog and see_red != 1):
        speed = 255
        moments = cv2.moments(frame_gray, 1)
        dM01 = moments['m01']
        dM10 = moments['m10']
        dArea = moments['m00']

        x = 320 - int(dM10 / dArea)
        y = 120 - int(dM01 / dArea)
    else:
        speed = 0
    i_main += 1
    if(time.time() - time_main > 1):
        time_main = time.time()
        fps_main = i_main
        i_main = 0

Исходный код программы для Arduino:

#include <AFMotor.h>

char in_command_array[7]; 
int napr = 0;
int ugol = 0;
int error = 0;
int speed_ = 0;

AF_DCMotor motor1(1);
AF_DCMotor motor2(2);
AF_DCMotor motor3(3);

void setup() {
  Serial.begin(115200);           

  motor1.setSpeed(200);
  motor1.run(RELEASE);
  motor2.setSpeed(200);
  motor2.run(RELEASE);
  motor3.setSpeed(0);
  motor3.run(FORWARD);
  delay(5000);
}

void loop() {
  String inString = "";
  String ServoNapr = "";
  String ServoUgol = "";
  String MotorNapr = "";
  String MotorSpeed = "";
  char in_command_array[7];
  
  while (Serial.available()) { 
    in_command_array[0] = Serial.read();
    if (ServoNapr == 'L' || ServoNapr == 'R') { 
      inString = "";
      for (int i=0;i<3;i++) {
        delay(1);
        char inChar = Serial.read(); 
        if (isDigit(inChar)) in_command_array[i+1] = (char)inChar; 
      }  
      int ServoUgol = inString.toInt();

    in_command_array[3] = Serial.read(); 
    inString = "";
    for (int i=0;i<3;i++) {
      delay(1);
      char inChar = Serial.read(); 
      if (isDigit(inChar)) in_command_array[i+4] = (char)inChar; 
    }  
    int MotorSpeed = inString.toInt();

    Serial.println(in_command_array);
    }
  }  

    if(in_command_array[0] == 76) { napr = -1; }
    else if(in_command_array[0] == 82) { napr = 1; }
    else if(in_command_array[0] == 48) { napr = 0; }
    else { error++; }

    Serial.println();
    Serial.print("napr = ");
    Serial.println(napr); 
    Serial.print("error = ");
    Serial.println(error); 

    int ugol_num1 = in_command_array[1] - 48;
    int ugol_num2 = in_command_array[2] - 48;

    Serial.print("ugol_num1 = ");
    Serial.println(ugol_num1); 
    Serial.print("ugol_num2 = ");
    Serial.println(ugol_num2); 

    if(ugol_num1 == 0) { ugol = ugol_num2; }
    else if(ugol_num1 != 0) { ugol = ugol_num1*10 + ugol_num2; }
    else { error++; }

    Serial.print("ugol = ");
    Serial.println(ugol);
    Serial.print("error = ");
    Serial.println(error);

    int speed_num1 = in_command_array[4] - 48;
    int speed_num2 = in_command_array[5] - 48;
    int speed_num3 = in_command_array[6] - 48;

    Serial.print("speed_num1 = ");
    Serial.println(speed_num1); 
    Serial.print("speed_num2 = ");
    Serial.println(speed_num2); 
    Serial.print("speed_num3 = ");
    Serial.println(speed_num3);

    if(speed_num1 == 0 && speed_num2 == 0) { speed_ = speed_num3; }
    else if(speed_num1 == 0) { speed_ = ugol_num2*10 + speed_num3; }
    else if(speed_num1 != 0) { speed_ = speed_num1*100 + speed_num2*10 + speed_num3; }
    else { error++; }

    Serial.print("speed = ");
    Serial.println(speed_);
    Serial.print("error = ");
    Serial.println(error);

    if(speed_ >= 255) {
      speed_ = speed_ - 255;
      motor1.run(FORWARD);
      motor1.setSpeed(speed_);
      motor2.run(FORWARD);
      motor2.setSpeed(speed_);
    }
    else {
      motor1.run(BACKWARD);
      motor1.setSpeed(speed_);
      motor2.run(BACKWARD);
      motor2.setSpeed(speed_);
    }

    if(speed_ == 255) {
      motor1.run(RELEASE);
      motor2.run(RELEASE);
    }

    if(napr == 1) { motor3.run(FORWARD); }
    else if(napr == -1) { motor3.run(BACKWARD); }

    motor3.setSpeed(ugol*2.83);
}

Самое популярное