Длинный хвост питона - часть 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);

}

Ссылка на github