воскресенье, 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);
}

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