DroneSimClient/ImageServer/imageserver.cpp - master - vasandi/DroneControl

DroneSimClient/ImageServer/imageserver.cpp
282 строки9 KB
Андрей Васильченко
управление дроном в Airsim
08 июл 2026, 13:29
08 июл 2026, 13:29d1a17d2

#include <QFile>
#include <QDebug>
#include <QDateTime>
#include <QImage>
#include <QBuffer>
#include <QPixmap>
#include <QTemporaryFile>
#include <QtConcurrent>
#include <QPainter>
#include <asio.hpp>
#include <nlohmann/json.hpp>
#include "imageserver.h"

using namespace std::literals::chrono_literals;
using json = nlohmann::json;

// ASIO io_context
asio::io_context ctxAsio;
// Сокет на отправку в AI
asio::ip::tcp::socket *socketAsio = nullptr;

const uint8_t MAGIC[] = { 'V', '1' };
//const asio::ip::tcp::endpoint endpoint(asio::ip::make_address("192.168.255.12"), 10000);
const asio::ip::tcp::endpoint endpoint(asio::ip::make_address("127.0.0.1"), 10000);

QByteArray createImageWithText(const QByteArray &imageData, const QString &text)
{
    QPixmap originalPixmap;
    if (!originalPixmap.loadFromData(imageData, "JPEG")) {
        return QByteArray();
    }

    // Создаем новое изображение такого же размера
    QPixmap resultPixmap(originalPixmap.size());
    resultPixmap.fill(Qt::transparent);

    QPainter painter(&resultPixmap);

    // исходное изображение
    painter.drawPixmap(0, 0, originalPixmap);

    // шрифт
    QFont font = painter.font();
    font.setPointSize(32);
    font.setBold(true);
    painter.setFont(font);

    // Настраиваем цвет текста
    painter.setPen(Qt::white);

    // Добавляем тень для лучшей читаемости
    painter.setPen(QPen(Qt::black, 2));
    painter.drawText(resultPixmap.rect().adjusted(2, 2, 2, 2), Qt::AlignBottom | Qt::AlignRight, text);

    painter.setPen(Qt::white);
    painter.drawText(resultPixmap.rect(), Qt::AlignBottom | Qt::AlignRight, text);

    painter.end();

    // обратно в QByteArray
    QByteArray resultData;
    QBuffer buffer(&resultData);
    buffer.open(QIODevice::WriteOnly);
    resultPixmap.save(&buffer, "JPEG", 85); // 85 - качество JPEG

    return resultData;
}

void sendFrame(const QByteArray &buffer, asio::ip::tcp::socket &socket)
{
    try
    {
        auto len_bytes = htonl(buffer.size());

        // Отправка magic bytes, длина кадра и сам кадр
        asio::write(socket, asio::buffer(MAGIC));
        asio::write(socket, asio::buffer(&len_bytes, sizeof(uint32_t)));
        asio::write(socket, asio::buffer(buffer.data(), buffer.size()));

        qDebug() << "sendFrame: Successfully sent" << buffer.size() << "bytes";
    }
    catch (const asio::system_error& e) {
        qWarning() << "sendFrame: ASIO system error - code:" << e.code().value()
        << ", category:" << e.code().category().name()
        << ", message:" << e.what()
        << "(attempted to send:" << buffer.size() << "bytes)";
    }
    catch (const std::exception& e) {
        qWarning() << "sendFrame: Standard exception -" << e.what()
        << "(attempted to send:" << buffer.size() << "bytes)";
    }
    catch (...) {
        qWarning() << "sendFrame: Unknown exception (attempted to send:"
                   << buffer.size() << "bytes)";
    }
}

std::pair<bool, json> receiveResponse(asio::ip::tcp::socket &socket)
{
    uint32_t response_length;
    char raw_data[sizeof(response_length)];

    try
    {
        // Чтение 4 bytes поля длины
        asio::read(socket, asio::buffer(raw_data, sizeof(response_length)));
        response_length = ntohl(*reinterpret_cast<const uint32_t*>(raw_data));

        const uint32_t MAX_RESPONSE_SIZE = 1024 * 1024; // 1 MB
        if (response_length == 0) {
            qWarning() << "receiveResponse: Пустой ответ (length = 0)";
            return {false, json()};
        }

        if (response_length > MAX_RESPONSE_SIZE) {
            qWarning() << "receiveResponse: Большой ответ large:" << response_length
                       << "bytes (max:" << MAX_RESPONSE_SIZE << ")";
            return {false, json()};
        }

        // Буфер под ответ JSON payload
        std::vector<char> response_buffer(response_length);
        asio::read(socket, asio::buffer(response_buffer.data(), response_length));

        // Парсинг JSON
        json parsed_json = json::parse(response_buffer.begin(), response_buffer.end());
        return {true, parsed_json};
    }
    catch (const asio::system_error& e) {
        qWarning() << "receiveResponse: ASIO system error - code:" << e.code().value()
                   << ", message:" << e.what();
        return {false, json()};
    }
    catch (const json::parse_error& e)
    {
        qWarning() << "receiveResponse: JSON parse error - byte:" << e.byte
                   << ", message:" << e.what();
        return {false, json()};
    }
    catch (const std::bad_alloc& e)
    {
        qWarning() << "receiveResponse: Memory allocation error -" << e.what()
                   << "(requested size:" << response_length << "bytes)";
        return {false, json()};
    }
    catch (const std::exception& e)
    {
        qWarning() << "receiveResponse: Standard exception -" << e.what();
        return {false, json()};
    }
    catch (...)
    {
        qWarning() << "receiveResponse: Unknown exception";
        return {false, json()};
    }
}

ImageServer::ImageServer(QObject *parent)
    : QObject(parent)
{
    // Порт для видео сервера
    const quint16 streamPort = 8000;
    _mjpegStreamer = QSharedPointer<MjpegStreamer>(new MjpegStreamer(streamPort));
    connect(this, &ImageServer::signalShowImage, _mjpegStreamer.data(), &MjpegStreamer::slotNextFrame);
    _mjpegStreamer-> startServer();
}

ImageServer::~ImageServer()
{
    _isStarted = false;
    if (socketAsio != nullptr) {
        socketAsio->close();
        delete socketAsio;
    }
}

void ImageServer::slotSave(const QByteArray &buffer)
{
    if (!_futureConnect.isRunning() && !_isConnectedToAi) {
        _futureConnect = QtConcurrent::run(this, &ImageServer::connectToAi);
        qDebug() << "Попытка соединения с AI сервисом";
    }

    if (_isConnectedToAi && !_futureSendImage.isRunning()) {
        _futureSendImage = QtConcurrent::run(this, &ImageServer::sendImageToAi, buffer);
        _isStarted = true;
        qDebug() << "Отправка изображения в AI сервис";
    }

    if (_isConnectedToAi && !_futureResponse.isRunning()) {
        _futureResponse = QtConcurrent::run(this, &ImageServer::responseFromAi);
        qDebug() << "Запуск приёма json от AI сервиса";
    }

    QPixmap pixmap;
    pixmap.loadFromData(buffer);
    QImage image = pixmap.toImage();

    // Создаем массив байтов для хранения результата в формате JPEG
    QByteArray baJpeg;
    QBuffer qbuf(&baJpeg);
    qbuf.open(QIODevice::WriteOnly);

    // Сохраняем изображение в буфер в формате JPEG
    bool success = image.save(&qbuf, "JPEG");
    if (!success) {
        qWarning() << "Ошибка сохранения изображения в JPEG";
        return;
    }

    // Отправка кадра в видео поток
    // emit signalShowImage(baJpeg);
    emit signalShowImage(createImageWithText(baJpeg, _textAiData));

#ifdef SAVE_IMAGES
    QFile file(_fileImagesPath + QString::number(QDateTime::currentMSecsSinceEpoch()) + ".jpg");
    if (!file.open(QIODevice::WriteOnly)) {
        qWarning() << "Ошибка открытия файла изображения для сохранения";
        return;
    }

    file.write(baJpeg);
    file.flush();
    file.close();
#endif
}

void ImageServer::connectToAi()
{
    _isConnectedToAi = false;
    try {
        socketAsio = new asio::ip::tcp::socket(ctxAsio);
        socketAsio->connect(endpoint);
        _isConnectedToAi = true;
        qDebug() << "Успешное соединение с AI сервисом";
    } catch (...) {
        qWarning() << "[ERROR] Ошибка соединение с AI сервисом";
        if (socketAsio != nullptr) {
            socketAsio->close();
            delete socketAsio;
        }
    }
}

void ImageServer::sendImageToAi(const QByteArray &buffer)
{
    if (socketAsio != nullptr) {
        // В сокет AI
        sendFrame(buffer, *socketAsio);
    }
}

void ImageServer::responseFromAi()
{
    while (_isStarted) {
        // Ожидание ответа от AI
        auto [ok, data] = receiveResponse(*socketAsio);
        if (ok && !data.empty()) {
            int width = data["image_size"]["width_px"];
            int height = data["image_size"]["height_px"];
            double center_x = data["center_px"]["x"];
            double center_y = data["center_px"]["y"];
            double object_x = data["object_px"]["x"];
            double object_y = data["object_px"]["y"];
            double polar_r = data["polar_coordinates"]["r_px"];
            double polar_theta = data["polar_coordinates"]["theta_deg"];

            qDebug() << "Ответ от AI";
            emit signalAiDataResponse(QPoint(object_x, object_y),
                                      QPoint(center_x, center_y),
                                      QSize(width, height),
                                      polar_r,
                                      polar_theta);

            _textAiData = QString("obj_x: %1, obj_y: %2, polar_r: %3, polar_theta: %4")
                            .arg((int)object_x)
                            .arg((int)object_y)
                            .arg((int)polar_r)
                            .arg((int)polar_theta);
        }
    }
}