331 lines
11 KiB
C++
331 lines
11 KiB
C++
#include <atomic>
|
|
#include <chrono>
|
|
#include <condition_variable>
|
|
#include <csignal>
|
|
#include <cstring>
|
|
#include <iostream>
|
|
#include <memory>
|
|
#include <mutex>
|
|
#include <queue>
|
|
#include <unordered_map>
|
|
#include <vector>
|
|
|
|
#include <EGL/egl.h>
|
|
#include <GLES2/gl2.h>
|
|
#include <libcamera/libcamera.h>
|
|
#include <libcamera/formats.h>
|
|
#include <netinet/in.h>
|
|
#include <netinet/tcp.h>
|
|
#include <sys/mman.h>
|
|
#include <sys/socket.h>
|
|
#include <unistd.h>
|
|
|
|
using namespace libcamera;
|
|
|
|
static std::atomic<bool> capturing{true};
|
|
static std::shared_ptr<Camera> camera;
|
|
static std::mutex frameMutex;
|
|
static std::condition_variable frameCv;
|
|
static std::queue<Request *> done;
|
|
static int clientFd = -1;
|
|
|
|
struct MappedBuffer { void *mem; size_t len; };
|
|
static std::unordered_map<FrameBuffer *, MappedBuffer> mapped;
|
|
|
|
enum class Mode { Motion, Raw };
|
|
|
|
static Mode parseMode(int argc, char **argv)
|
|
{
|
|
if (argc < 2)
|
|
return Mode::Motion;
|
|
if (!strcmp(argv[1], "--raw") || !strcmp(argv[1], "raw"))
|
|
return Mode::Raw;
|
|
if (!strcmp(argv[1], "--motion") || !strcmp(argv[1], "motion"))
|
|
return Mode::Motion;
|
|
std::cerr << "Usage: " << argv[0] << " [--motion|--raw]\n";
|
|
exit(1);
|
|
}
|
|
|
|
static int waitForClient(int port, const char *label)
|
|
{
|
|
int server = socket(AF_INET, SOCK_STREAM, 0);
|
|
int yes = 1;
|
|
setsockopt(server, SOL_SOCKET, SO_REUSEADDR, &yes, sizeof(yes));
|
|
|
|
sockaddr_in addr{};
|
|
addr.sin_family = AF_INET;
|
|
addr.sin_addr.s_addr = INADDR_ANY;
|
|
addr.sin_port = htons(port);
|
|
bind(server, reinterpret_cast<sockaddr *>(&addr), sizeof(addr));
|
|
listen(server, 1);
|
|
std::cerr << "tcp://0.0.0.0:" << port << " waiting...\n";
|
|
|
|
int client = accept(server, nullptr, nullptr);
|
|
close(server);
|
|
setsockopt(client, IPPROTO_TCP, TCP_NODELAY, &yes, sizeof(yes));
|
|
std::cerr << "streaming " << label << "\n";
|
|
return client;
|
|
}
|
|
|
|
static bool sendAll(const unsigned char *data, size_t size)
|
|
{
|
|
while (size && capturing) {
|
|
ssize_t n = send(clientFd, data, size, MSG_NOSIGNAL);
|
|
if (n <= 0)
|
|
return false;
|
|
data += n;
|
|
size -= n;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
static void requestComplete(Request *request)
|
|
{
|
|
if (request->status() == Request::RequestCancelled)
|
|
return;
|
|
std::lock_guard<std::mutex> lock(frameMutex);
|
|
done.push(request);
|
|
frameCv.notify_one();
|
|
}
|
|
|
|
// Pi libcamera RGB888 is BGR in memory; BGR888 gives true RGB for ffplay rgb24.
|
|
static void packRgb24(void *src, unsigned char *dst, int w, int h, int stride)
|
|
{
|
|
auto *p = static_cast<unsigned char *>(src);
|
|
for (int y = 0; y < h; ++y)
|
|
memcpy(dst + y * w * 3, p + y * stride, w * 3);
|
|
}
|
|
|
|
static void uploadFrame(GLuint tex, const unsigned char *rgb, int w, int h)
|
|
{
|
|
glBindTexture(GL_TEXTURE_2D, tex);
|
|
glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, w, h, GL_RGB, GL_UNSIGNED_BYTE, rgb);
|
|
}
|
|
|
|
static GLuint makeProgram()
|
|
{
|
|
const char *vs =
|
|
"attribute vec2 pos; attribute vec2 tex; varying vec2 uv;"
|
|
"void main(){uv=tex; gl_Position=vec4(pos,0.0,1.0);}";
|
|
// Luminance diff avoids false motion from AWB/color shifts (common GLES motion trick).
|
|
const char *fs =
|
|
"precision mediump float; varying vec2 uv;"
|
|
"uniform sampler2D prevFrame, currFrame;"
|
|
"const vec3 lum = vec3(0.299, 0.587, 0.114);"
|
|
"void main(){"
|
|
" float a = dot(texture2D(prevFrame, uv).rgb, lum);"
|
|
" float b = dot(texture2D(currFrame, uv).rgb, lum);"
|
|
" float d = clamp(abs(b - a) * 4.0, 0.0, 1.0);"
|
|
" gl_FragColor = vec4(vec3(d), 1.0);"
|
|
"}";
|
|
|
|
auto compile = [](GLenum type, const char *src) {
|
|
GLuint s = glCreateShader(type);
|
|
glShaderSource(s, 1, &src, nullptr);
|
|
glCompileShader(s);
|
|
return s;
|
|
};
|
|
GLuint p = glCreateProgram();
|
|
glAttachShader(p, compile(GL_VERTEX_SHADER, vs));
|
|
glAttachShader(p, compile(GL_FRAGMENT_SHADER, fs));
|
|
glLinkProgram(p);
|
|
return p;
|
|
}
|
|
|
|
int main(int argc, char **argv)
|
|
{
|
|
Mode mode = parseMode(argc, argv);
|
|
std::signal(SIGINT, [](int) { capturing = false; });
|
|
|
|
auto cm = std::make_unique<CameraManager>();
|
|
cm->start();
|
|
if (cm->cameras().empty()) {
|
|
std::cerr << "no camera\n";
|
|
return 1;
|
|
}
|
|
|
|
camera = cm->get(cm->cameras()[0]->id());
|
|
camera->acquire();
|
|
|
|
auto config = camera->generateConfiguration({ StreamRole::Viewfinder });
|
|
StreamConfiguration &sc = config->at(0);
|
|
Size full = sc.size;
|
|
sc.size.width = (full.width / 2) & ~1u;
|
|
sc.size.height = (full.height / 2) & ~1u;
|
|
sc.pixelFormat = formats::BGR888;
|
|
sc.colorSpace = ColorSpace::Sycc;
|
|
sc.bufferCount = 4;
|
|
if (config->validate() == CameraConfiguration::Invalid || sc.pixelFormat != formats::BGR888) {
|
|
std::cerr << "BGR888 config failed\n";
|
|
return 1;
|
|
}
|
|
camera->configure(config.get());
|
|
|
|
const int w = sc.size.width, h = sc.size.height, stride = sc.stride;
|
|
std::cerr << "mode=" << (mode == Mode::Raw ? "raw" : "motion")
|
|
<< " " << w << "x" << h << " (half of " << full.toString() << ")"
|
|
<< " stride=" << stride << "\n";
|
|
|
|
std::vector<unsigned char> rgbFrame(w * h * 3), flipRow(w * 3);
|
|
|
|
GLuint program = 0, prevTex = 0, currTex = 0, fbo = 0, vbo = 0;
|
|
GLint uPrev = -1, uCurr = -1, aPos = -1, aTex = -1;
|
|
bool havePrev = false;
|
|
EGLDisplay dpy = EGL_NO_DISPLAY;
|
|
EGLSurface surface = EGL_NO_SURFACE;
|
|
EGLContext ctx = EGL_NO_CONTEXT;
|
|
|
|
if (mode == Mode::Motion) {
|
|
dpy = eglGetDisplay(EGL_DEFAULT_DISPLAY);
|
|
eglInitialize(dpy, nullptr, nullptr);
|
|
EGLint cfgAttr[] = { EGL_SURFACE_TYPE, EGL_PBUFFER_BIT, EGL_RENDERABLE_TYPE, EGL_OPENGL_ES2_BIT,
|
|
EGL_RED_SIZE, 8, EGL_GREEN_SIZE, 8, EGL_BLUE_SIZE, 8, EGL_NONE };
|
|
EGLConfig cfg;
|
|
EGLint n;
|
|
eglChooseConfig(dpy, cfgAttr, &cfg, 1, &n);
|
|
EGLint pb[] = { EGL_WIDTH, w, EGL_HEIGHT, h, EGL_NONE };
|
|
surface = eglCreatePbufferSurface(dpy, cfg, pb);
|
|
EGLint ctxAttr[] = { EGL_CONTEXT_CLIENT_VERSION, 2, EGL_NONE };
|
|
ctx = eglCreateContext(dpy, cfg, EGL_NO_CONTEXT, ctxAttr);
|
|
eglMakeCurrent(dpy, surface, surface, ctx);
|
|
|
|
auto makeTex = [&](int tw, int th) {
|
|
GLuint t;
|
|
glGenTextures(1, &t);
|
|
glBindTexture(GL_TEXTURE_2D, t);
|
|
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
|
|
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
|
|
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, tw, th, 0, GL_RGB, GL_UNSIGNED_BYTE, nullptr);
|
|
return t;
|
|
};
|
|
program = makeProgram();
|
|
prevTex = makeTex(w, h);
|
|
currTex = makeTex(w, h);
|
|
GLuint outTex;
|
|
glGenTextures(1, &outTex);
|
|
glBindTexture(GL_TEXTURE_2D, outTex);
|
|
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, w, h, 0, GL_RGBA, GL_UNSIGNED_BYTE, nullptr);
|
|
glGenFramebuffers(1, &fbo);
|
|
glBindFramebuffer(GL_FRAMEBUFFER, fbo);
|
|
glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, outTex, 0);
|
|
|
|
float quad[] = { -1,-1, 0,1, 1,-1, 1,1, -1,1, 0,0, 1,1, 1,0 };
|
|
glGenBuffers(1, &vbo);
|
|
glBindBuffer(GL_ARRAY_BUFFER, vbo);
|
|
glBufferData(GL_ARRAY_BUFFER, sizeof(quad), quad, GL_STATIC_DRAW);
|
|
|
|
uPrev = glGetUniformLocation(program, "prevFrame");
|
|
uCurr = glGetUniformLocation(program, "currFrame");
|
|
aPos = glGetAttribLocation(program, "pos");
|
|
aTex = glGetAttribLocation(program, "tex");
|
|
}
|
|
|
|
clientFd = waitForClient(5000, mode == Mode::Raw ? "raw rgb24" : "motion rgb24");
|
|
|
|
FrameBufferAllocator *allocator = new FrameBufferAllocator(camera);
|
|
Stream *stream = sc.stream();
|
|
if (allocator->allocate(stream) < 0) {
|
|
std::cerr << "buffer alloc failed\n";
|
|
return 1;
|
|
}
|
|
|
|
for (const auto &buffer : allocator->buffers(stream)) {
|
|
const auto &plane = buffer->planes()[0];
|
|
mapped[buffer.get()] = {
|
|
mmap(nullptr, plane.length, PROT_READ, MAP_SHARED, plane.fd.get(), plane.offset),
|
|
plane.length
|
|
};
|
|
}
|
|
|
|
std::vector<std::unique_ptr<Request>> requests;
|
|
for (const auto &buffer : allocator->buffers(stream)) {
|
|
auto request = camera->createRequest();
|
|
if (!request || request->addBuffer(stream, buffer.get()) < 0)
|
|
return 1;
|
|
requests.push_back(std::move(request));
|
|
}
|
|
|
|
camera->requestCompleted.connect(requestComplete);
|
|
camera->start();
|
|
for (auto &r : requests)
|
|
camera->queueRequest(r.get());
|
|
|
|
while (capturing) {
|
|
Request *request = nullptr;
|
|
{
|
|
std::unique_lock<std::mutex> lock(frameMutex);
|
|
frameCv.wait_for(lock, std::chrono::milliseconds(50), [] { return !done.empty(); });
|
|
if (done.empty())
|
|
continue;
|
|
request = done.front();
|
|
done.pop();
|
|
}
|
|
|
|
FrameBuffer *buffer = request->buffers().begin()->second;
|
|
if (buffer->metadata().status == FrameMetadata::FrameSuccess) {
|
|
void *mem = mapped[buffer].mem;
|
|
|
|
if (mode == Mode::Raw) {
|
|
packRgb24(mem, rgbFrame.data(), w, h, stride);
|
|
if (!sendAll(rgbFrame.data(), rgbFrame.size()))
|
|
capturing = false;
|
|
} else {
|
|
packRgb24(mem, rgbFrame.data(), w, h, stride);
|
|
uploadFrame(currTex, rgbFrame.data(), w, h);
|
|
if (!havePrev) {
|
|
uploadFrame(prevTex, rgbFrame.data(), w, h);
|
|
havePrev = true;
|
|
} else {
|
|
glViewport(0, 0, w, h);
|
|
glUseProgram(program);
|
|
glActiveTexture(GL_TEXTURE0);
|
|
glBindTexture(GL_TEXTURE_2D, prevTex);
|
|
glUniform1i(uPrev, 0);
|
|
glActiveTexture(GL_TEXTURE1);
|
|
glBindTexture(GL_TEXTURE_2D, currTex);
|
|
glUniform1i(uCurr, 1);
|
|
|
|
glBindBuffer(GL_ARRAY_BUFFER, vbo);
|
|
glEnableVertexAttribArray(aPos);
|
|
glEnableVertexAttribArray(aTex);
|
|
glVertexAttribPointer(aPos, 2, GL_FLOAT, GL_FALSE, 4 * sizeof(float), (void *)0);
|
|
glVertexAttribPointer(aTex, 2, GL_FLOAT, GL_FALSE, 4 * sizeof(float), (void *)(2 * sizeof(float)));
|
|
glDrawArrays(GL_TRIANGLE_STRIP, 0, 4);
|
|
|
|
glReadPixels(0, 0, w, h, GL_RGB, GL_UNSIGNED_BYTE, rgbFrame.data());
|
|
for (int y = 0; y < h / 2; ++y) {
|
|
unsigned char *top = rgbFrame.data() + y * w * 3;
|
|
unsigned char *bot = rgbFrame.data() + (h - 1 - y) * w * 3;
|
|
memcpy(flipRow.data(), top, w * 3);
|
|
memcpy(top, bot, w * 3);
|
|
memcpy(bot, flipRow.data(), w * 3);
|
|
}
|
|
if (!sendAll(rgbFrame.data(), rgbFrame.size()))
|
|
capturing = false;
|
|
std::swap(prevTex, currTex);
|
|
}
|
|
}
|
|
}
|
|
|
|
request->reuse(Request::ReuseBuffers);
|
|
camera->queueRequest(request);
|
|
}
|
|
|
|
camera->stop();
|
|
camera->requestCompleted.disconnect();
|
|
if (clientFd >= 0)
|
|
close(clientFd);
|
|
for (auto &[_, m] : mapped)
|
|
munmap(m.mem, m.len);
|
|
allocator->free(stream);
|
|
delete allocator;
|
|
if (mode == Mode::Motion) {
|
|
eglDestroyContext(dpy, ctx);
|
|
eglDestroySurface(dpy, surface);
|
|
eglTerminate(dpy);
|
|
}
|
|
camera->release();
|
|
cm->stop();
|
|
return 0;
|
|
}
|