v4l2-cpp jpeg frame reader

file.cpp

#pragma ide diagnostic ignored "hicpp-signed-bitwise"

#include "CameraExtractor.h"

#include <boost/log/trivial.hpp>
#include <boost/format.hpp>
#include <boost/chrono/chrono.hpp>

#include <fcntl.h> /* low-level i/o */

#include <sys/stat.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <thread>
#include <iostream>


char const *CameraExtractor::DEVICE_NAME = "/dev/video0";

static int xioctl(int fh, int request, void *arg) {
    int r;
    do {
        r = ioctl(fh, request, arg);
    } while (-1 == r && EINTR == errno);
    return r;
}

static void mainLoop(const CameraExtractor &cameraExtractor) {
    cameraExtractor.captureImages();
}

CameraExtractor::CameraExtractor(FrameQueue &fq)
        : frameQueue(fq), mainLoopThread(nullptr), buffers(nullptr), v4l2Buffer(new v4l2_buffer()),
          totalTimer(nullptr), // perFrameTimer(nullptr),
          bufferCnt(0), camera_fd(-1), running(false), frameCount(0), status(0) {

    v4l2Buffer->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    v4l2Buffer->memory = V4L2_MEMORY_MMAP;

    status = initialise();

    BOOST_LOG_TRIVIAL(info) << boost::format{"Created Camera Extractor [%1%: %2%x%3%@%4%, %5% buffers], status = %6% "}
                               % CameraExtractor::DEVICE_NAME
                               % static_cast<int>(CameraExtractor::IMG_WIDTH)
                               % static_cast<int>(CameraExtractor::IMG_HEIGHT)
                               % (1000 / CameraExtractor::CAPTURE_SLEEP_TIME_MILLIS)
                               % bufferCnt
                               % status;
}

CameraExtractor::~CameraExtractor() {
    if (buffers != nullptr) {
        delete[] buffers;
        buffers = nullptr;
    }
//    if (perFrameTimer != nullptr) {
//        delete perFrameTimer;
//        perFrameTimer = nullptr;
//    }
    if (totalTimer != nullptr) {
        delete totalTimer;
        totalTimer = nullptr;
    }
    delete v4l2Buffer;
    v4l2Buffer = nullptr;
    BOOST_LOG_TRIVIAL(info) << "Destroyed Camera extractor";
}

int CameraExtractor::initialise() {
    if (openDevice() != 0) {
        BOOST_LOG_TRIVIAL(error) << "openDevice(): FAILED";
        return -1;
    }
    if (initDevice() != 0) {
        BOOST_LOG_TRIVIAL(error) << "initDevice(): FAILED";
        return -1;
    }
    if (initMmap() != 0) {
        BOOST_LOG_TRIVIAL(error) << "initMmap(): FAILED";
        return -1;
    }
    if (initCapturing() != 0) {
        BOOST_LOG_TRIVIAL(error) << "initCapturing(): FAILED";
        return -1;
    }
    return 0;
}

void CameraExtractor::start() {
    if (status == 0) {
        totalTimer = new boost::timer::auto_cpu_timer();
//        perFrameTimer = new boost::timer::cpu_timer();
        running = true;
        mainLoopThread = new std::thread(mainLoop, std::ref(*this));
    }
}

void CameraExtractor::stop() {
    running = false;
    if (mainLoopThread != nullptr) {
        mainLoopThread->join();
        delete mainLoopThread;
        mainLoopThread = nullptr;
    }
    if (unInitCapturing() != 0) {
        BOOST_LOG_TRIVIAL(error) << "unInitCapturing(): FAILED";
    }
    if (unInitMmap() != 0) {
        BOOST_LOG_TRIVIAL(error) << "unInitMmap(): FAILED";
    }
    if (closeDevice() != 0) {
        BOOST_LOG_TRIVIAL(error) << "closeDevice(): FAILED";
    }
}

int CameraExtractor::openDevice() {
    struct stat st{};
    if (-1 == stat(CameraExtractor::DEVICE_NAME, &st)) {
        BOOST_LOG_TRIVIAL(error) << boost::format{"Cannot identify '%1%': %2%, %3%"}
                                    % CameraExtractor::DEVICE_NAME
                                    % errno
                                    % strerror(errno);
        return -1;
    }

    if (!S_ISCHR(st.st_mode)) {
        BOOST_LOG_TRIVIAL(error) << boost::format{"%1% is no device"}
                                    % CameraExtractor::DEVICE_NAME;
        return -1;
    }
    camera_fd = open(CameraExtractor::DEVICE_NAME, O_RDWR /* required */ | O_NONBLOCK, 0);
    if (-1 == camera_fd) {
        BOOST_LOG_TRIVIAL(error) << boost::format{"Cannot open '%1%': %2%, %3%"}
                                    % CameraExtractor::DEVICE_NAME
                                    % errno
                                    % strerror(errno);
        return -1;
    }
    return 0;
}

int CameraExtractor::closeDevice() {
    if (-1 == close(camera_fd)) {
        BOOST_LOG_TRIVIAL(error) << boost::format{"Cannot close %1%"}
                                    % CameraExtractor::DEVICE_NAME;
        return -1;
    }
    return 0;
}

int CameraExtractor::initDevice() {
    struct v4l2_capability cap{};
    struct v4l2_cropcap crop_cap{};
    struct v4l2_crop crop{};
    struct v4l2_format fmt{};
    unsigned int min;

    if (-1 == xioctl(camera_fd, VIDIOC_QUERYCAP, &cap)) {
        if (EINVAL == errno) {
            BOOST_LOG_TRIVIAL(error) << boost::format{"%1% is no V4L2 device"}
                                        % CameraExtractor::DEVICE_NAME;
        } else {
            BOOST_LOG_TRIVIAL(error) << boost::format{"VIDIOC_QUERYCAP: %1%"}
                                        % errno;
        }
        return -1;
    }

    if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
        BOOST_LOG_TRIVIAL(error) << boost::format{"%1% is not a video capture device"}
                                    % CameraExtractor::DEVICE_NAME;
        return -1;
    }

    if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
        BOOST_LOG_TRIVIAL(error) << boost::format{"%1% does not support streaming i/o"}
                                    % CameraExtractor::DEVICE_NAME;
        return -1;
    }



    /* Select video input, video standard and tune here. */
    crop_cap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

    if (0 == xioctl(camera_fd, VIDIOC_CROPCAP, &crop_cap)) {
        crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        crop.c = crop_cap.defrect; /* reset to default */

        if (-1 == xioctl(camera_fd, VIDIOC_S_CROP, &crop)) {
            switch (errno) {
                case EINVAL:
                    /* Cropping not supported. */
                    break;
                default:
                    /* Errors ignored. */
                    break;
            }
        }
    } else {
        /* Errors ignored. */
    }

    //    CLEAR(fmt);
    fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

    /* Note VIDIOC_S_FMT may change width and height. */
    fmt.fmt.pix.width = CameraExtractor::IMG_WIDTH;
    fmt.fmt.pix.height = CameraExtractor::IMG_HEIGHT;
    fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG;
    fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;

    if (-1 == xioctl(camera_fd, VIDIOC_S_FMT, &fmt)) {
        BOOST_LOG_TRIVIAL(error) << boost::format{"VIDIOC_S_FMT: %1%"}
                                    % errno;
    }

    /* Buggy driver paranoia. */
    min = fmt.fmt.pix.width * 2;
    if (fmt.fmt.pix.bytesperline < min)
        fmt.fmt.pix.bytesperline = min;
    min = fmt.fmt.pix.bytesperline * fmt.fmt.pix.height;
    if (fmt.fmt.pix.sizeimage < min)
        fmt.fmt.pix.sizeimage = min;

    return 0;
}

int CameraExtractor::initMmap() {
    struct v4l2_requestbuffers req{};

    req.count = CameraExtractor::WANTED_BUFFER_COUNT;
    req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    req.memory = V4L2_MEMORY_MMAP;

    if (-1 == xioctl(camera_fd, VIDIOC_REQBUFS, &req)) {
        if (EINVAL == errno) {
            BOOST_LOG_TRIVIAL(error) << boost::format{"%1% does not support memory mapping"}
                                        % CameraExtractor::DEVICE_NAME;
        } else {
            BOOST_LOG_TRIVIAL(error) << boost::format{"VIDIOC_REQBUFS: %1% "}
                                        % errno;
        }
        return -1;
    }

    if (req.count < 2) {
        BOOST_LOG_TRIVIAL(error) << boost::format{"Insufficient buffer memory on %1%"}
                                    % CameraExtractor::DEVICE_NAME;
        return -1;
    }

    bufferCnt = req.count;
    buffers = new avp::buffer[bufferCnt];

    if (!buffers) {
        BOOST_LOG_TRIVIAL(error) << "OOM Buffers";
        return -1;
    }

    for (int i = 0; i < bufferCnt; ++i) {
        v4l2Buffer->index = i;
        if (-1 == xioctl(camera_fd, VIDIOC_QUERYBUF, v4l2Buffer)) {
            BOOST_LOG_TRIVIAL(error) << "VIDIOC_QUERYBUF: " << i;
            return -1;
        }
        buffers[i].length = v4l2Buffer->length;
        buffers[i].start =
                mmap(nullptr /* start anywhere */,
                     v4l2Buffer->length,
                     PROT_READ | PROT_WRITE /* required */,
                     MAP_SHARED /* recommended */,
                     camera_fd,
                     v4l2Buffer->m.offset);

        if (MAP_FAILED == buffers[i].start) {
            BOOST_LOG_TRIVIAL(error) << "MMAP_FAILED: " << i;
            return -1;
        }
    }

//    v4l2_streamparm streamparm{};
//    if (-1 == xioctl(camera_fd, VIDIOC_G_PARM, &streamparm)) {
//        BOOST_LOG_TRIVIAL(error) << boost::format{"VIDIOC_G_PARM: %1%"}
//                                    % errno;
//    }
//
//
//    v4l2_standard standard{};
//    if (-1 == xioctl(camera_fd, VIDIOC_ENUMSTD, &standard)) {
//        BOOST_LOG_TRIVIAL(error) << boost::format{"VIDIOC_ENUMSTD: %1%"}
//                                    % errno;
//    }

    return 0;
}

int CameraExtractor::unInitMmap() {
    for (int i = 0; i < bufferCnt; ++i) {
        if (-1 == munmap(buffers[i].start, buffers[i].length)) {
            BOOST_LOG_TRIVIAL(error) << "UN_MMAP_FAILED: " << i;
            return -1;
        }
    }
    return 0;
}

int CameraExtractor::initCapturing() {
    for (int i = 0; i < bufferCnt; ++i) {
        v4l2Buffer->index = i;
        if (-1 == xioctl(camera_fd, VIDIOC_QBUF, v4l2Buffer)) {
            BOOST_LOG_TRIVIAL(error) << "VIDIOC_QBUF: " << i << " errno: " << errno;
            return -1;
        }
    }

    enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    if (-1 == xioctl(camera_fd, VIDIOC_STREAMON, &type)) {
        BOOST_LOG_TRIVIAL(error) << "VIDIOC_STREAMON: " << errno;
        return -1;
    }
    return 0;
}

int CameraExtractor::unInitCapturing() {
    //TODO: v4l2_buf_type extract field?
    enum v4l2_buf_type type;
    type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    if (-1 == xioctl(camera_fd, VIDIOC_STREAMOFF, &type)) {
        BOOST_LOG_TRIVIAL(error) << "VIDIOC_STREAMOFF: " << errno;
        return -1;
    }
    return 0;
}

void CameraExtractor::captureImages() const {
    struct timeval tv{};
    /* Timeout. */
    fd_set fds;
    int r;

    while (running) {
        tv.tv_sec = 2;
        tv.tv_usec = 0;
        FD_ZERO(&fds);
        FD_SET(camera_fd, &fds);

        // select returns num of available descriptors man
        r = select(camera_fd + 1, &fds, nullptr, nullptr, &tv);

        if (r < 1) {
            // Do i need to restart capture?
            BOOST_LOG_TRIVIAL(error) << "[main loop] fd select: " << errno << " r: " << r;
            continue;
        }

        if (-1 == xioctl(camera_fd, VIDIOC_DQBUF, v4l2Buffer)) {
            switch (errno) {
                case EAGAIN:
                    continue;
                case EIO:
                    /* Could ignore EIO, see spec. */
                    /* fall through */
                default:
                    BOOST_LOG_TRIVIAL(error) << "[main loop] VIDIOC_DQBUF: " << errno;
                    continue;
            }
        }

        // sanity check, change to update status
//        assert(v4l2Buffer->index < bufferCnt);
//        auto x = v4l2Buffer->flags & V4L2_BUF_FLAG_TIMECODE;

        processImage(buffers[v4l2Buffer->index].start,
                     v4l2Buffer->bytesused);

        if (-1 == xioctl(camera_fd, VIDIOC_QBUF, v4l2Buffer)) {
            BOOST_LOG_TRIVIAL(error) << "[main loop] VIDIOC_QBUF: " << errno;
        }
    }
}

void CameraExtractor::processImage(const void *p, int size) const {
    frameCount++;

    auto* data = new unsigned int[size];
    /* using memcpy to copy string: */
    memcpy(data, p, size);
    auto *b = new struct avp::buffer;
    b->length = size;
    b->start = data;

//
//    std::cout << "hashedChars: ";
//    for (int i = 0; i < 32; i++) {
//        std::cout << std::hex << data[i];
//    }
//    std::cout << std::endl;

    if(!frameQueue.push(b)){
        BOOST_LOG_TRIVIAL(error) << "frameQueue is full";
        delete[] data;
        delete b;
    }

//    size_t *image = new
//    [size]();
//
//    person.age = 46;
//
//    /* using memcpy to copy structure: */
//    memcpy(&person_copy, &person, sizeof(person));
//
//    printf("person_copy: %s, %d \n", person_copy.name, person_copy.age);


//    char buf[120];
//    snprintf(buf, 120, "out/data.jpg");
//    FILE *f = fopen(buf, "wb");
//    fwrite(p, 1, size, f);
//    fclose(f);
//    f = NULL;
    if (frameCount % 100 == 0) {
        auto nanoseconds = boost::chrono::nanoseconds(totalTimer->elapsed().wall);
        auto seconds = boost::chrono::duration_cast<boost::chrono::seconds>(nanoseconds).count();
        auto fps = frameCount / (seconds + 1);
        BOOST_LOG_TRIVIAL(trace) << "FPS = " << fps;
//        perFrameTimer->stop();
//        nanoseconds = boost::chrono::nanoseconds(perFrameTimer->elapsed().wall);
//        auto milliseconds = boost::chrono::duration_cast<boost::chrono::milliseconds>(nanoseconds).count();
//        BOOST_LOG_TRIVIAL(trace) << boost::format{"Frame took %1% millis "} % milliseconds;
//        perFrameTimer->elapsed().clear();
//        perFrameTimer->start();
    }
}

file.hpp

#ifndef AVP_MJPEG_SERVER_CAMERAEXTRACTOR_H
#define AVP_MJPEG_SERVER_CAMERAEXTRACTOR_H

#include <linux/videodev2.h>
#include <string>
#include <thread>
#include <boost/atomic.hpp>
#include <boost/timer/timer.hpp>
#include "../AvpBuffer.h"

class CameraExtractor {
private:
    static char const *DEVICE_NAME;
    static int const WANTED_BUFFER_COUNT = 4;
    static int const CAPTURE_SLEEP_TIME_MILLIS = 40;
    static int const IMG_WIDTH = 848;
    static int const IMG_HEIGHT = 480;
private:
    FrameQueue &frameQueue;
    std::thread *mainLoopThread;
    struct avp::buffer *buffers;
    struct v4l2_buffer *v4l2Buffer;
    boost::timer::auto_cpu_timer *totalTimer;
//    boost::timer::cpu_timer *perFrameTimer;
    int bufferCnt;
    int camera_fd;
    boost::atomic<bool> running;
    mutable long int frameCount;
public:
    boost::atomic<int> status;
public:
    explicit CameraExtractor(FrameQueue &fq);

    virtual ~CameraExtractor();

    void start();

    void stop();

    void captureImages() const;

private:
    int initialise();

    int openDevice();

    int closeDevice();

    int initDevice();

    int initMmap();

    int unInitMmap();

    int initCapturing();

    int unInitCapturing();

    void processImage(const void *p, int size) const;
};


#endif //AVP_MJPEG_SERVER_CAMERAEXTRACTOR_H