qing
7e44025671
- 工具循环：capture_image / get_robot_status / spawn_subagent - 子代理：技能超集（run_command 带超时截断、read_file / write_file） - V4L2 摄像头抓帧：MJPEG 优先零转码，YUYV+libjpeg 回退，/dev/video* 自动探测 - 时间戳注入：system prompt 会话级当前时间 + 工具结果执行时间 - CLI：--prompt-file / --camera / --camera-device / --model / --max-turns
250 lines
9.2 KiB
C++
250 lines
9.2 KiB
C++
#include "camera.hpp"
|
||
|
||
#include <jpeglib.h>
|
||
|
||
#include <fcntl.h>
|
||
#include <glob.h>
|
||
#include <linux/videodev2.h>
|
||
#include <poll.h>
|
||
#include <sys/ioctl.h>
|
||
#include <sys/mman.h>
|
||
#include <unistd.h>
|
||
|
||
#include <algorithm>
|
||
#include <chrono>
|
||
#include <cstring>
|
||
#include <stdexcept>
|
||
|
||
namespace {
|
||
|
||
constexpr size_t CAPTURE_BUFFER_COUNT = 3;
|
||
constexpr int FRAME_TIMEOUT_MS = 1500; // 单帧出队超时
|
||
|
||
// 扫描 /dev/video*，返回第一个支持视频采集的设备
|
||
std::string auto_detect_device() {
|
||
glob_t g;
|
||
std::vector<std::string> candidates;
|
||
if (glob("/dev/video*", 0, nullptr, &g) == 0) {
|
||
for (size_t i = 0; i < g.gl_pathc; ++i)
|
||
candidates.emplace_back(g.gl_pathv[i]);
|
||
}
|
||
globfree(&g);
|
||
std::sort(candidates.begin(), candidates.end());
|
||
|
||
for (const auto& dev : candidates) {
|
||
int fd = open(dev.c_str(), O_RDWR | O_NONBLOCK);
|
||
if (fd < 0) continue;
|
||
v4l2_capability cap{};
|
||
bool ok = ioctl(fd, VIDIOC_QUERYCAP, &cap) == 0 &&
|
||
(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE);
|
||
close(fd);
|
||
if (ok) return dev;
|
||
}
|
||
throw std::runtime_error("未找到可用的视频采集设备（已扫描 /dev/video*，"
|
||
"可用 --camera-device 指定）");
|
||
}
|
||
|
||
void xioctl(int fd, unsigned long request, void* arg, const char* what) {
|
||
if (ioctl(fd, request, arg) != 0)
|
||
throw std::runtime_error(std::string(what) + " 失败: " + strerror(errno));
|
||
}
|
||
|
||
// YUYV → RGB24 → JPEG（libjpeg 内存压缩）
|
||
std::vector<unsigned char> yuyv_to_jpeg(const std::vector<unsigned char>& yuyv,
|
||
unsigned w, unsigned h) {
|
||
std::vector<unsigned char> rgb(static_cast<size_t>(w) * h * 3);
|
||
const unsigned char* in = yuyv.data();
|
||
unsigned char* out = rgb.data();
|
||
for (unsigned y = 0; y < h; ++y) {
|
||
for (unsigned x = 0; x + 1 < w; x += 2) {
|
||
int y0 = in[0], u = in[1] - 128, y1 = in[2], v = in[3] - 128;
|
||
in += 4;
|
||
int c0 = y0 - 16, c1 = y1 - 16;
|
||
int r0 = (298 * c0 + 409 * v + 128) >> 8;
|
||
int g0 = (298 * c0 - 100 * u - 208 * v + 128) >> 8;
|
||
int b0 = (298 * c0 + 516 * u + 128) >> 8;
|
||
int r1 = (298 * c1 + 409 * v + 128) >> 8;
|
||
int g1 = (298 * c1 - 100 * u - 208 * v + 128) >> 8;
|
||
int b1 = (298 * c1 + 516 * u + 128) >> 8;
|
||
*out++ = static_cast<unsigned char>(std::clamp(r0, 0, 255));
|
||
*out++ = static_cast<unsigned char>(std::clamp(g0, 0, 255));
|
||
*out++ = static_cast<unsigned char>(std::clamp(b0, 0, 255));
|
||
*out++ = static_cast<unsigned char>(std::clamp(r1, 0, 255));
|
||
*out++ = static_cast<unsigned char>(std::clamp(g1, 0, 255));
|
||
*out++ = static_cast<unsigned char>(std::clamp(b1, 0, 255));
|
||
}
|
||
if (w % 2) { // 奇数宽度：最后一像素复用前一对的 U/V
|
||
int y0 = in[0], u = in[1] - 128, v = in[2] - 128;
|
||
in += 4;
|
||
int c0 = y0 - 16;
|
||
*out++ = static_cast<unsigned char>(std::clamp((298 * c0 + 409 * v + 128) >> 8, 0, 255));
|
||
*out++ = static_cast<unsigned char>(std::clamp((298 * c0 - 100 * u - 208 * v + 128) >> 8, 0, 255));
|
||
*out++ = static_cast<unsigned char>(std::clamp((298 * c0 + 516 * u + 128) >> 8, 0, 255));
|
||
}
|
||
}
|
||
|
||
jpeg_compress_struct cinfo;
|
||
jpeg_error_mgr jerr;
|
||
cinfo.err = jpeg_std_error(&jerr);
|
||
jpeg_create_compress(&cinfo);
|
||
unsigned char* outbuf = nullptr;
|
||
unsigned long outsize = 0;
|
||
jpeg_mem_dest(&cinfo, &outbuf, &outsize);
|
||
cinfo.image_width = w;
|
||
cinfo.image_height = h;
|
||
cinfo.input_components = 3;
|
||
cinfo.in_color_space = JCS_RGB;
|
||
jpeg_set_defaults(&cinfo);
|
||
jpeg_set_quality(&cinfo, 85, TRUE);
|
||
jpeg_start_compress(&cinfo, TRUE);
|
||
while (cinfo.next_scanline < h) {
|
||
JSAMPROW row = rgb.data() + static_cast<size_t>(cinfo.next_scanline) * w * 3;
|
||
jpeg_write_scanlines(&cinfo, &row, 1);
|
||
}
|
||
jpeg_finish_compress(&cinfo);
|
||
std::vector<unsigned char> result(outbuf, outbuf + outsize);
|
||
free(outbuf);
|
||
jpeg_destroy_compress(&cinfo);
|
||
return result;
|
||
}
|
||
|
||
} // namespace
|
||
|
||
CameraCapture::CameraCapture(const std::string& device) : device_(device) {
|
||
if (device_.empty()) device_ = auto_detect_device();
|
||
open_device();
|
||
init_format();
|
||
request_buffers();
|
||
}
|
||
|
||
CameraCapture::~CameraCapture() {
|
||
std::lock_guard<std::mutex> lock(mtx_);
|
||
if (fd_ < 0) return;
|
||
try {
|
||
stream_off();
|
||
} catch (...) {
|
||
}
|
||
for (size_t i = 0; i < buf_count_; ++i)
|
||
munmap(buffers_[i], buffer_sizes_[i]);
|
||
close(fd_);
|
||
}
|
||
|
||
void CameraCapture::open_device() {
|
||
fd_ = open(device_.c_str(), O_RDWR | O_NONBLOCK);
|
||
if (fd_ < 0)
|
||
throw std::runtime_error("无法打开摄像头 " + device_ + ": " + strerror(errno));
|
||
|
||
v4l2_capability cap{};
|
||
if (ioctl(fd_, VIDIOC_QUERYCAP, &cap) != 0)
|
||
throw std::runtime_error("VIDIOC_QUERYCAP 失败: " + std::string(strerror(errno)));
|
||
if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) ||
|
||
!(cap.capabilities & V4L2_CAP_STREAMING))
|
||
throw std::runtime_error(device_ + " 不支持视频捕获或 mmap 流式传输");
|
||
}
|
||
|
||
void CameraCapture::init_format() {
|
||
v4l2_format fmt{};
|
||
fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
||
fmt.fmt.pix.width = 640;
|
||
fmt.fmt.pix.height = 480;
|
||
fmt.fmt.pix.field = V4L2_FIELD_ANY;
|
||
|
||
// 优先 MJPEG：摄像头硬件编码，抓帧即用，无需转码
|
||
fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG;
|
||
if (ioctl(fd_, VIDIOC_S_FMT, &fmt) == 0 && fmt.fmt.pix.pixelformat == V4L2_PIX_FMT_MJPEG) {
|
||
jpeg_format_ = true;
|
||
} else {
|
||
fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
|
||
xioctl(fd_, VIDIOC_S_FMT, &fmt, "设置 YUYV 格式");
|
||
if (fmt.fmt.pix.pixelformat != V4L2_PIX_FMT_YUYV)
|
||
throw std::runtime_error(device_ + " 不支持 MJPEG 或 YUYV");
|
||
jpeg_format_ = false;
|
||
}
|
||
width_ = fmt.fmt.pix.width;
|
||
height_ = fmt.fmt.pix.height;
|
||
}
|
||
|
||
void CameraCapture::request_buffers() {
|
||
v4l2_requestbuffers req{};
|
||
req.count = CAPTURE_BUFFER_COUNT;
|
||
req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
||
req.memory = V4L2_MEMORY_MMAP;
|
||
xioctl(fd_, VIDIOC_REQBUFS, &req, "申请缓冲");
|
||
if (req.count < 2)
|
||
throw std::runtime_error("摄像头缓冲不足");
|
||
buf_count_ = req.count;
|
||
|
||
buffers_.resize(buf_count_);
|
||
buffer_sizes_.resize(buf_count_);
|
||
for (size_t i = 0; i < buf_count_; ++i) {
|
||
v4l2_buffer buf{};
|
||
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
||
buf.memory = V4L2_MEMORY_MMAP;
|
||
buf.index = i;
|
||
xioctl(fd_, VIDIOC_QUERYBUF, &buf, "查询缓冲");
|
||
buffers_[i] = mmap(nullptr, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, fd_,
|
||
buf.m.offset);
|
||
if (buffers_[i] == MAP_FAILED)
|
||
throw std::runtime_error("mmap 失败: " + std::string(strerror(errno)));
|
||
buffer_sizes_[i] = buf.length;
|
||
xioctl(fd_, VIDIOC_QBUF, &buf, "缓冲入队");
|
||
}
|
||
}
|
||
|
||
void CameraCapture::stream_on() {
|
||
int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
||
xioctl(fd_, VIDIOC_STREAMON, &type, "开启视频流");
|
||
}
|
||
|
||
void CameraCapture::stream_off() {
|
||
int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
||
if (ioctl(fd_, VIDIOC_STREAMOFF, &type) != 0 && errno != EINVAL)
|
||
throw std::runtime_error("关闭视频流失败: " + std::string(strerror(errno)));
|
||
}
|
||
|
||
std::vector<unsigned char> CameraCapture::dequeue_frame(int timeout_ms) {
|
||
auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(timeout_ms);
|
||
while (true) {
|
||
v4l2_buffer buf{};
|
||
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
||
buf.memory = V4L2_MEMORY_MMAP;
|
||
if (ioctl(fd_, VIDIOC_DQBUF, &buf) == 0) {
|
||
std::vector<unsigned char> frame(static_cast<unsigned char*>(buffers_[buf.index]),
|
||
static_cast<unsigned char*>(buffers_[buf.index]) +
|
||
buf.bytesused);
|
||
xioctl(fd_, VIDIOC_QBUF, &buf, "缓冲重新入队");
|
||
return frame;
|
||
}
|
||
if (errno != EAGAIN)
|
||
throw std::runtime_error("出队帧失败: " + std::string(strerror(errno)));
|
||
auto now = std::chrono::steady_clock::now();
|
||
if (now >= deadline)
|
||
throw std::runtime_error("等待摄像头画面超时");
|
||
int remain_ms = static_cast<int>(
|
||
std::chrono::duration_cast<std::chrono::milliseconds>(deadline - now).count());
|
||
struct pollfd pfd{fd_, POLLIN, 0};
|
||
poll(&pfd, 1, std::min(remain_ms, 200));
|
||
}
|
||
}
|
||
|
||
std::string CameraCapture::grab_jpeg() {
|
||
std::lock_guard<std::mutex> lock(mtx_);
|
||
stream_on();
|
||
try {
|
||
dequeue_frame(FRAME_TIMEOUT_MS); // 丢弃第一帧（曝光未稳定）
|
||
auto frame = dequeue_frame(FRAME_TIMEOUT_MS); // 取第二帧
|
||
stream_off();
|
||
if (!jpeg_format_) {
|
||
auto jpeg = yuyv_to_jpeg(frame, width_, height_);
|
||
return std::string(reinterpret_cast<char*>(jpeg.data()), jpeg.size());
|
||
}
|
||
return std::string(reinterpret_cast<char*>(frame.data()), frame.size());
|
||
} catch (...) {
|
||
try {
|
||
stream_off();
|
||
} catch (...) {
|
||
}
|
||
throw;
|
||
}
|
||
}
|