Слежение за объектом на EV3

package ru.proghouse.ev3.project;
 
import java.io.IOException;
import java.io.OutputStream;
import java.net.ServerSocket;
import java.net.Socket;
import java.util.List;
import java.util.Vector;
 
import org.opencv.core.Core;
import org.opencv.core.Mat;
import org.opencv.core.MatOfByte;
import org.opencv.core.MatOfPoint;
import org.opencv.core.MatOfPoint2f;
import org.opencv.core.Point;
import org.opencv.core.Rect;
import org.opencv.core.Scalar;
import org.opencv.core.Size;
import org.opencv.highgui.Highgui;
import org.opencv.highgui.VideoCapture;
import org.opencv.imgproc.Imgproc;
 
import lejos.hardware.BrickFinder;
import lejos.hardware.Button;
import lejos.hardware.lcd.GraphicsLCD;
import lejos.hardware.lcd.LCD;
import lejos.hardware.motor.EV3MediumRegulatedMotor;
import lejos.robotics.RegulatedMotor;
import lejos.utility.Delay;
 
public class BallTrackingBot {
 
    public static void main(String[] args) throws Exception {
        //Загружаем библиотеку OpenCV.
        System.loadLibrary(Core.NATIVE_LIBRARY_NAME);
        //Создаём вспомогательный класс для захвата видео.
        VideoCapture videoCapture = new VideoCapture(0);
        //Задаём ширину и высоту кадра, с которым будем работать. 
        final int WIDTH = 160, HEIGHT = 120;
        videoCapture.set(Highgui.CV_CAP_PROP_FRAME_WIDTH, WIDTH);
        videoCapture.set(Highgui.CV_CAP_PROP_FRAME_HEIGHT, HEIGHT);
        //Задаём ширину и высоту центральной области.
        //Когда за рамки этой области будет выходить объект, будем двигать робота.
        final int CENTER_WIDTH = 50, CENTER_HEIGHT = 50;
        //Считаем координаты центра кадра.
        Point imageCenter = new Point(WIDTH / 2, HEIGHT / 2);
        //Захватываем камеру для работы с видео.
        if (videoCapture.open(0))
            //Выдаём сообщение, что камера готова.
            LCD.drawString("Camera is ready.", 0, 0);
        else {
            //Выдаём сообщение, что камера не готова. Может быть не подключена?
            LCD.drawString("Camera is not ready.", 0, 0);
            Delay.msDelay(5000);
            return;
        }
 
        final int MOTOR_A_SPEED = 100; //Скорость вращения мотора A.
        final int MOTOR_BC_SPEED = 100; //Скорость вращения моторов B и C.
 
        //Робот двигается шажками, поэтому задаём здесь размеры шагов.
        final int MOTOR_A_STEP = 20; //Размер шага для мотора A.
        final int MOTOR_BC_STEP_ROTATION = 20; //Размер шага для моторов B и C при вращении робота.
        final int MOTOR_BC_STEP_MOVE = 90; //Размер шага для моторов B и C при движении робота.
 
        //Инициализируем моторы.
        RegulatedMotor motorA = new EV3MediumRegulatedMotor(BrickFinder.getDefault().getPort("A"));
        RegulatedMotor motorB = new EV3MediumRegulatedMotor(BrickFinder.getDefault().getPort("B"));
        RegulatedMotor motorC = new EV3MediumRegulatedMotor(BrickFinder.getDefault().getPort("C"));
 
        //Задаём скорость вращения моторов.
        motorA.setSpeed(MOTOR_A_SPEED);
        motorB.setSpeed(MOTOR_BC_SPEED);
        motorC.setSpeed(MOTOR_BC_SPEED);
 
        //Выводим на экран приглашение выбрать режим (обычный или отладка).
        LCD.drawString("Choose the mode:", 0, 1);
        LCD.drawString("Up - normal;", 0, 2);
        LCD.drawString("Down - debug.", 0, 3);
 
        boolean debug = false;
        int debugMode = 0;
 
        //Пока ждём выбор режима, считываем изображения с камеры.
        Mat image = new Mat();
        while (true) {
            videoCapture.read(image);
            if (Button.ESCAPE.isDown())
                return; //Esc - выход из программы.
            else if (Button.UP.isDown())
                break; //Кнопка "вверх" - выбран обычный режим.
            else if (Button.DOWN.isDown()) {
                debug = true; //Кнопка "вниз" - выбран режим отладки.
                break;
            }
        }
 
        //Берём пробу цвета в центре кадра.
        double[] sampleColor = getSampleColor(videoCapture, WIDTH, HEIGHT);
        //Выводим на экран результаты взятия пробы цвета.
        LCD.drawString("Sample color:", 0, 4);
        LCD.drawString(" " + Double.toString(sampleColor[0])
                + " " + Double.toString(sampleColor[1])
                + " " + Double.toString(sampleColor[2]), 0, 5);
        //Определяем цвет по цветовому тону и выводим его на экран.
        double h = sampleColor[0];
        String colorName = "";
        if (h < 22)
            colorName = "orange";
        else if (h < 38)
            colorName = "yellow";
        else if (h < 75)
            colorName = "green";
        else if (h < 130)
            colorName = "blue";
        else if (h < 160)
            colorName = "violet";
        else
            colorName = "red";
        LCD.drawString("Is it " + colorName + "?", 0, 6);
 
        Delay.msDelay(1000);
 
        final Scalar RED = new Scalar(0, 0, 255);
 
        ServerSocket serverSocket = null;
        Socket socket = null;
        String boundary = "Thats it folks!";
        try {
            if (debug) {
                //Выводим на экран приглашение подключиться к EV3 с помощью браузера.
                LCD.clear();
                LCD.drawString("Connect to", 0, 0);
                LCD.drawString("http://10.0.1.1:8080", 0, 1);
                //Создаём серверный сокет.
                serverSocket = new ServerSocket(8080);
                //Ждём подключения к серверу.
                socket = serverSocket.accept();
                //Клиент (браузер) подключен, выводим информацию об этом на экран.
                LCD.drawString("Connected!", 0, 2);
                //Отдаём клиенту (браузеру) HTTP-заголовок.
                writeHeader(socket.getOutputStream(), boundary);
            }
 
            //Вычисляем интервал цветов, близких к цвету пробы.
            double hMin = sampleColor[0] - 10;
            double hMax = sampleColor[0] + 10;
            if (hMin < 0)
                hMin = 0;
            if (hMax > 179)
                hMax = 179;
            double sMin = sampleColor[1] - 50;
            double sMax = sampleColor[1] + 50;
            if (sMin < 0)
                sMin = 0;
            if (sMax > 255)
                sMax = 255;
            double vMin = sampleColor[2] - 50;
            double vMax = sampleColor[2] + 50;
            if (vMin < 0)
                vMin = 0;
            if (vMax > 255)
                vMax = 255;
            Scalar hsvMin = new Scalar(hMin, sMin, vMin);
            Scalar hsvMax = new Scalar(hMax, sMax, vMax);
 
            GraphicsLCD lcd = BrickFinder.getDefault().getGraphicsLCD();
 
            Mat hsv = new Mat();
            Mat mask = new Mat();
            Mat hierarchy = new Mat();
            Size ksize  = new Size(5, 5);
            List<MatOfPoint> contours = new Vector<MatOfPoint>();
            double contourArea, maxArea;
            MatOfPoint biggestContour;
            MatOfPoint2f contours2f = new MatOfPoint2f();
            Point center = new Point();
            float[] radius = new float[1];
            MatOfByte buffer = new MatOfByte();
 
            boolean motorBCMoving = true;
            boolean motorAMoving = true;
 
            while (!Button.ESCAPE.isDown()) {
                //Меняем режим отладки, если нажата клавиша Enter на модуле EV3.
                if (Button.ENTER.isDown()) {
                    debugMode++;
                    if (debugMode >= 4)
                        debugMode = 0;
                }
                //Запоминаем, крутятся ли моторы перед поиском объекта.
                motorBCMoving = motorB.isMoving() || motorC.isMoving();
                motorAMoving = motorA.isMoving();
 
                //Считываем кадр с камеры.
                if (videoCapture.read(image) && !image.empty()) {
 
                    //Если включена отладка и режим отладки 2 (source),
                    //отдаём полученный кадр браузеру.
                    if (debug && debugMode == 2) {
                        Highgui.imencode(".jpeg", image, buffer);
                        writeJpg(socket.getOutputStream(), buffer, boundary);
                    }
 
                    //Imgproc.blur(image, image, ksize); //Размытие отключено, для ускорения.
                    //Полученный кадр формата BGR. Переводим его в формат HSV.
                    Imgproc.cvtColor(image, hsv, Imgproc.COLOR_BGR2HSV);
                    //Ищем цвета в нужном нам интервале. В результате получится чёрно-белое изображение,
                    //где белый цвет обозначает цвет, попавший в интервал, а чёрный - всё остальное.
                    Core.inRange(hsv, hsvMin, hsvMax, mask);
 
                    //Если включена отладка и режим отладки 1 (mask),
                    //отдаём результат работы функции inRange браузеру.
                    if (debug && debugMode == 1) {
                        Highgui.imencode(".jpeg", mask, buffer);
                        writeJpg(socket.getOutputStream(), buffer, boundary);
                    }
 
                    //Ищем контуры, т.е. объекты нужного нам цвета.
                    contours.clear();
                    Imgproc.findContours(mask, contours, hierarchy,
                            Imgproc.RETR_LIST, Imgproc.CHAIN_APPROX_SIMPLE);
 
                    //Среди найденных контуров ищем контур наибольшего размера.
                    biggestContour = null;
                    maxArea = 0;
                    for (MatOfPoint contour : contours) {
                        contourArea = Imgproc.contourArea(contour);
                        if (contourArea > maxArea) {
                            biggestContour = contour;
                            maxArea = contourArea;
                        }
                    }
 
                    //Очищаем экран.
                    lcd.clear();
 
                    //Если контур есть и его размер больше или равен 100...
                    if (biggestContour != null && maxArea >= 100) {
 
                        //1-й способ поиска круга на изображении.
                        //Здесь ищется окружность в указанном контуре.
                        //В результате получаем координату центра окружности и её радиус.
                        //Этот способ работает лучше, чем 2-й способ, но медленнее.
                        //Чтобы работал этот способ откомментируйте его и закомментируйте 2-й.
                        /*biggestContour.convertTo(contours2f, CvType.CV_32FC2);
                        Imgproc.minEnclosingCircle(contours2f, center, radius);*/
 
                        //2-й способ поиска круга на изображении.
                        //На самом деле здесь круг не ищется.
                        //Здесь просто считаем, что радиус - это наибольший размер контура / 2,
                        //а центр контура - это центр нашего круга.
                        Rect rect = Imgproc.boundingRect(biggestContour);
                        center.x = rect.x + rect.width / 2;
                        center.y = rect.y + rect.height / 2;
                        radius[0] = Math.max(rect.width, rect.height) / 2;
 
                        //Если включен режим отладки, выводим на экран радиус круга и режим отладки.
                        if (debug) {
                            LCD.drawString("Radius: " + Double.toString(radius[0]), 0, 6);
                            LCD.drawString("Debug mode:"
                                    + (debugMode == 0 ? "contour"
                                     : debugMode == 1 ? "mask"
                                     : debugMode == 2 ? "source"
                                     : "none"), 0, 7);
                        }
                        //Рисуем найденный круг на экране EV3.
                        lcd.drawRoundRect((int)center.x - (int)radius[0],
                                (int)center.y - (int)radius[0], 
                                (int)radius[0] * 2, (int)radius[0] * 2,
                                (int)radius[0] * 2, (int)radius[0] * 2);
 
                        //Считаем смещение центра круга от центра кадра.
                        double dy = center.y - imageCenter.y;
                        //Если мотор не двигается сейчас и не двигался перед взятием кадра,
                        //можно двигать камеру.
                        if ((!motorA.isMoving()) && (!motorAMoving)) {
                            //Если круг вышел за центральную область двигаем камеру.
                            if (dy < -CENTER_HEIGHT / 2) 
                                motorA.rotate(-MOTOR_A_STEP, true);
                            else if (dy > CENTER_HEIGHT / 2)
                                motorA.rotate(MOTOR_A_STEP, true);
                        }
 
                        //Считаем смещение центра круга от центра кадра.
                        double dx = center.x - imageCenter.x;
                        //Если моторы не двигаются сейчас и не двигались перед взятием кадра,
                        //можно двигать робота.
                        if ((!motorB.isMoving()) && (!motorC.isMoving()) && (!motorBCMoving)) {
                            //Если круг вышел за центральную область, поворачиваем робота.
                            if (dx < -CENTER_WIDTH / 2) {
                                motorB.rotate(-MOTOR_BC_STEP_ROTATION, true);
                                motorC.rotate(MOTOR_BC_STEP_ROTATION, true);
                            } else if (dx > CENTER_WIDTH / 2) {
                                motorB.rotate(MOTOR_BC_STEP_ROTATION, true);
                                motorC.rotate(-MOTOR_BC_STEP_ROTATION, true);
                            }
                            //Если круг слишком мал, подъезжаем ближе.
                            else if (radius[0] < 20) {
                                motorB.rotate(MOTOR_BC_STEP_MOVE, true);
                                motorC.rotate(MOTOR_BC_STEP_MOVE, true);
                            }
                            //Если круг слишком велик, отъезжаем дальше.
                            else if (radius[0] > 30) {
                                motorB.rotate(-MOTOR_BC_STEP_MOVE, true);
                                motorC.rotate(-MOTOR_BC_STEP_MOVE, true);
                            }
 
                        }
 
                        //Если включена отладка и режим отладки 0 (contour)...
                        if (debug && debugMode == 0) {
                            contours.clear();
                            contours.add(biggestContour);
                            //Рисуем найденный контур на кадре.
                            Imgproc.drawContours(image, contours, 0, RED);
                            //Рисуем найденный круг на кадре.
                            Core.circle(image, center, (int)radius[0], RED);
                        }
                    }
                    else { //Если контур не найден или его размер меньше 100, останавливаем двигатели.
                        if (motorA.isMoving())
                            motorA.stop(true);
                        if (motorB.isMoving() || motorC.isMoving()) {
                            motorB.stop(true);
                            motorC.stop(true);
                        }
                    }
 
                    //Если включена отладка и режим отладки 0 (contour),
                    //отдаём браузеру кадр, полученный с камеры,
                    //с подрисованными контуром и кругом.
                    if (debug && debugMode == 0) {
                        Highgui.imencode(".jpeg", image, buffer);
                        writeJpg(socket.getOutputStream(), buffer, boundary);
                    }
                }
            }
            lcd.clear();
        } finally {
            //Закрываем сокеты.
            if (socket != null)
                socket.close();
            if (serverSocket != null)
                serverSocket.close();
        }
    }
 
    //Функция получает пробу цвета с камеры.
    private static double[] getSampleColor(VideoCapture videoCapture,
            int imageWidth, int imageHeight) {
        Mat image = new Mat();
        Size ksize  = new Size(5, 5);
        double h = 0, s = 0, v = 0;
        int sampleCount = 7; //Количество проб.
        for (int i = 0; i < sampleCount; ) {
            if (videoCapture.read(image) && !image.empty()) {
                Rect snippetROI = new Rect(image.width() / 2 - 5,
                        image.height() / 2 - 5, 10, 10);
                Mat snippet    = image.submat(snippetROI);
                Imgproc.blur(snippet, snippet, ksize);
                Imgproc.cvtColor(snippet, snippet, Imgproc.COLOR_BGR2HSV);
                double[] color = snippet.get(5, 5);
                h += color[0];
                s += color[1];
                v += color[2];
                i++;
                Delay.msDelay(100);
            }
        }
        return new double[] {Math.round(h / sampleCount),
                Math.round(s / sampleCount),
                Math.round(v / sampleCount)};
    }
 
    //Функция отдаёт браузеру HTTP-заголовок.
    private static void writeHeader(OutputStream stream, String boundary) throws IOException {
        stream.write(("HTTP/1.0 200 OK\r\n" +
                "Connection: close\r\n" +
                "Max-Age: 0\r\n" +
                "Expires: 0\r\n" +
                "Cache-Control: no-store, no-cache, must-revalidate, pre-check=0, post-check=0, max-age=0\r\n" +
                "Pragma: no-cache\r\n" + 
                "Content-Type: multipart/x-mixed-replace; " +
                "boundary=" + boundary + "\r\n" +
                "\r\n" +
                "--" + boundary + "\r\n").getBytes());
    }
 
    //Функция отдаёт браузеру изображение.
    private static void writeJpg(OutputStream stream, MatOfByte image, String boundary) throws IOException {
        byte[] imageBytes = image.toArray();
        stream.write(("Content-type: image/jpeg\r\n" +
                "Content-Length: " + imageBytes.length + "\r\n" +
                "\r\n").getBytes());
        stream.write(imageBytes);
        stream.write(("\r\n--" + boundary + "\r\n").getBytes());
    }
 
}

Camera is ready.
Choose the mode:
Up - normal;
Down - debug.

Sample color:
 109.0 208.0 251.0
Is it blue?

Connect to
http://10.0.1.1:8080

<html>
<body>
<img src="http://10.0.1.1:8080"/>
</body>
</html>

Radius: 21.4
Debug mode: contour