zmv/vision-anti-pinch
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Integrate CAN-aware CMS risk pipeline and relocatable Orin runtime

Browse Source
This commit is contained in:
zmv
2026-09-23 18:54:11 +08:00
parent 26c5bfb88d
commit 9aadf9cbda
119 changed files with 13940 additions and 152 deletions
Download Patch File Download Diff File Expand all files Collapse all files
main.cpp
+344
View File
@@ -0,0 +1,344 @@
// vision-anti-pinch - V4L2 MMAP 取图 + USBCANFD CAN 收发 (OpenCV 版本)
//
// 使用 OpenCV 进行 UYVY/YUYV -> BGR 转换，避免手动转换公式问题
// CAN 收发默认开启, --no-can 关闭
//
// 用法:
// ./vision-anti-pinch -d /dev/video5 -n 10 -s 2 -t jpg -o ./out
#include "v4l2_capture.h"
#include "can_comm.h"
#include "send.hpp"
#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/imgcodecs.hpp>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <string>
#include <vector>
#include <bitset>
#include <atomic>
#include <chrono>
#include <csignal>
#include <linux/videodev2.h>
std::atomic<uint64_t> g_Feedback_Of_The_Actual_Position_Of_The_Left_Front_Door_391_S;
std::atomic<uint64_t> g_Feedback_Of_The_Actual_Position_Of_The_Right_Front_Door_391_S;
std::atomic<uint64_t> g_Feedback_Of_The_Actual_Position_Of_The_Left_Rear_Door_391_S;
std::atomic<uint64_t> g_Feedback_Of_The_Actual_Position_Of_The_Right_Rear_Door_391_S;
// Sensing GMSL2 驱动自定义控制 ID
#define CID_SENSOR_MODE 0x009a2008
#define CID_TRIG_PIN 0x009a2084
#define CID_TRIG_MODE 0x009a2085
#define CID_GAIN 0x009a2009
#define CID_EXPOSURE 0x009a200a
static volatile sig_atomic_t g_stop = 0;
static void onSignal(int) { g_stop = 1; }
static void printUsage(const char *prog) {
printf(
"Usage: %s [options]\n"
"V4L2 MMAP capture demo for Sensing GMSL2 camera on Jetson Orin.\n"
"\n"
"Capture options:\n"
" -d <device> V4L2 device node (default: /dev/video5)\n"
" -W <width> frame width (default: 1920)\n"
" -H <height> frame height (default: 1536)\n"
" -f <fps> target frame rate (default: 30)\n"
" -F <format> pixel format: UYVY | YUYV (default: UYVY)\n"
" -m <mode> sensor_mode control value\n"
" -p <trig_pin> trig_pin control value (hex/dec)\n"
" --gain <val> sensor gain\n"
" --exposure <val> sensor exposure\n"
"\n"
"Output options:\n"
" -n <count> frames to capture, 0 = run forever\n"
" -s <count> frames to save\n"
" -o <dir> output directory (default: ./frames)\n"
" -t <type> file type: jpg | png | bmp | raw (default: jpg)\n"
" --quality <1..100> JPEG quality (default: 95)\n"
" -v verbose output\n"
" -h print this help\n"
"\n"
"CAN options:\n"
" --no-can disable USBCANFD CAN/CANFD receive (enabled by default)\n"
"\n"
"Example:\n"
" %s -d /dev/video5 -n 100 -s 5 -t jpg -o ./out\n"
" %s -d /dev/video5 -n 0 -v\n",
prog, prog, prog);
}
struct Args {
std::string device = "/dev/video5";
uint32_t width = 1920;
uint32_t height = 1536; // SG3S-ISX031C-GMSL2F 默认分辨率
uint32_t fps = 30;
std::string format = "UYVY";
long sensor_mode = -1;
long trig_pin = -1;
long trig_mode = -1;
long gain = -1;
long exposure = -1;
long count = 0;
long save = 0;
std::string out_dir = "./frames";
std::string save_type = "jpg";
int quality = 95;
bool verbose = false;
bool can_enabled = true; // 默认启用 CAN 收发, --no-can 关闭
};
// 手动解析参数
static bool parseArgs(int argc, char **argv, Args &a) {
auto need_val = [&](int &i) -> std::string {
if (i + 1 >= argc) {
printf("missing value for %s\n", argv[i]);
std::exit(1);
}
return argv[++i];
};
for (int i = 1; i < argc; ++i) {
std::string arg = argv[i];
if (arg == "-d") a.device = need_val(i);
else if (arg == "-W") a.width = (uint32_t)std::atoi(need_val(i).c_str());
else if (arg == "-H") a.height = (uint32_t)std::atoi(need_val(i).c_str());
else if (arg == "-f") a.fps = (uint32_t)std::atoi(need_val(i).c_str());
else if (arg == "-F") a.format = need_val(i);
else if (arg == "-m") a.sensor_mode = std::strtol(need_val(i).c_str(), nullptr, 0);
else if (arg == "-p") a.trig_pin = std::strtol(need_val(i).c_str(), nullptr, 0);
else if (arg == "-M") a.trig_mode = std::strtol(need_val(i).c_str(), nullptr, 0);
else if (arg == "-n") a.count = std::strtol(need_val(i).c_str(), nullptr, 0);
else if (arg == "-s") a.save = std::strtol(need_val(i).c_str(), nullptr, 0);
else if (arg == "-o") a.out_dir = need_val(i);
else if (arg == "-t") a.save_type = need_val(i);
else if (arg == "-v") a.verbose = true;
else if (arg == "-h") { printUsage(argv[0]); std::exit(0); }
else if (arg == "--gain") a.gain = std::strtol(need_val(i).c_str(), nullptr, 0);
else if (arg == "--exposure") a.exposure = std::strtol(need_val(i).c_str(), nullptr, 0);
else if (arg == "--quality") a.quality = std::atoi(need_val(i).c_str());
else if (arg == "--no-can") a.can_enabled = false;
else {
printf("unknown option: %s\n", arg.c_str());
printUsage(argv[0]);
return false;
}
}
return true;
}
// 使用 OpenCV 将 V4L2 帧 (UYVY/YUYV) 转为 BGR Mat
static cv::Mat frameToBgr(const V4l2Capture::Frame &frame) {
// OpenCV 原生支持 UYVY/YUYV -> BGR 转换
// UYVY = COLOR_YUV2BGR_UYVY, YUYV = COLOR_YUV2BGR_YUYV
int code = (frame.pixelformat == V4L2_PIX_FMT_UYVY)
? cv::COLOR_YUV2BGR_UYVY
: cv::COLOR_YUV2BGR_YUYV;
cv::Mat yuv((int)frame.height, (int)frame.width, CV_8UC2, (void *)frame.data);
cv::Mat bgr;
cv::cvtColor(yuv, bgr, code);
return bgr;
}
/* =====================================================================
* CAN 报文接收解析 (由 can_comm 接收线程回调)
* ===================================================================== */
// 512 位报文位域, deal_msg 中按 msg_id 拷贝后可按位解析信号
static std::bitset<512> g_bitMsg;
static uint64_t get_bit_value(int star_bit, int bit_len)
{
uint64_t signal_value = 0;
for (int i = 0; i < bit_len; ++i)
{
signal_value |= g_bitMsg[star_bit + i] << i;
}
return signal_value;
}
// 收到报文的回调点: 目前仅解析 0x391, 后续信号解析在此扩展
static void deal_msg(int64_t time, uint32_t msg_id, const uint8_t *data, uint32_t len)
{
if (msg_id == 0x391 && len >= 4 && len <= 64)
{
memcpy(&g_bitMsg, data, len);
g_Feedback_Of_The_Actual_Position_Of_The_Left_Front_Door_391_S = data[0];
g_Feedback_Of_The_Actual_Position_Of_The_Right_Front_Door_391_S = data[1];
g_Feedback_Of_The_Actual_Position_Of_The_Left_Rear_Door_391_S = data[2];
g_Feedback_Of_The_Actual_Position_Of_The_Right_Rear_Door_391_S = data[3];
// Rear_BCM_0x391,0x391,64,1.0-1.7,8,Feedback_Of_The_Actual_Position_Of_The_Left_Front_Door_391_S
// Rear_BCM_0x391,0x391,64,2.0-2.7,8,Feedback_Of_The_Actual_Position_Of_The_Right_Front_Door_391_S
// Rear_BCM_0x391,0x391,64,3.0-3.7,8,Feedback_Of_The_Actual_Position_Of_The_Left_Rear_Door_391_S
// Rear_BCM_0x391,0x391,64,4.0-4.7,8,Feedback_Of_The_Actual_Position_Of_The_Right_Rear_Door_391_S
}
}
int main(int argc, char **argv) {
Args args;
if (!parseArgs(argc, argv, args)) {
return 1;
}
std::signal(SIGINT, onSignal);
std::signal(SIGTERM, onSignal);
uint32_t pixfmt = (args.format == "UYVY") ? V4L2_PIX_FMT_UYVY : V4L2_PIX_FMT_YUYV;
// 启动 CAN 收发(失败不致命, 可继续仅取图)
if (args.can_enabled) {
CanComm::set_msg_callback(deal_msg); // 注册报文解析回调
if (!CanComm::can_start()) {
printf("CAN start failed, continue with camera only.\n");
args.can_enabled = false;
}
}
V4l2Capture cap;
if (!cap.open(args.device)) {
return 1;
}
// 设置相机控制
if (args.sensor_mode >= 0) cap.setControl(CID_SENSOR_MODE, (int32_t)args.sensor_mode);
if (args.trig_pin >= 0) cap.setControl(CID_TRIG_PIN, (int32_t)args.trig_pin);
if (args.trig_mode >= 0) cap.setControl(CID_TRIG_MODE, (int32_t)args.trig_mode);
if (args.gain >= 0) cap.setControl(CID_GAIN, (int32_t)args.gain);
if (args.exposure >= 0) cap.setControl(CID_EXPOSURE, (int32_t)args.exposure);
if (!cap.setFormat(args.width, args.height, pixfmt)) {
return 1;
}
cap.setFps(args.fps);
if (!cap.initMmap(4)) {
return 1;
}
if (!cap.startStreaming()) {
return 1;
}
printf("=========================================================\n");
printf("Capturing %s %ux%u @ %u fps from %s\n",
args.format.c_str(), cap.width(), cap.height(), args.fps, args.device.c_str());
printf("Press Ctrl+C to stop.\n");
printf("=========================================================\n");
// 创建输出目录
if (args.save > 0) {
std::string mkcmd = "mkdir -p " + args.out_dir;
std::system(mkcmd.c_str());
}
using clock = std::chrono::steady_clock;
auto t_start = clock::now();
auto t_last_report = t_start;
uint64_t frame_count = 0;
uint64_t save_count = 0;
uint32_t timeout_consecutive = 0;
while (!g_stop) {
if (args.count > 0 && frame_count >= (uint64_t)args.count) {
break;
}
V4l2Capture::Frame frame;
if (!cap.getFrame(frame, 2000)) {
timeout_consecutive++;
if (timeout_consecutive > 5) {
printf("getFrame: timeout/empty for %u consecutive tries, aborting\n",
timeout_consecutive);
break;
}
continue;
}
timeout_consecutive = 0;
frame_count++;
// 保存帧
if (save_count < (uint64_t)args.save) {
bool ok = false;
char fname[512];
// 使用 OpenCV 转换并保存
cv::Mat bgr = frameToBgr(frame);
if (!bgr.empty())
{
std::snprintf(fname, sizeof(fname), "%s/frame_%06lu.%s",
args.out_dir.c_str(), save_count, args.save_type.c_str());
std::vector<int> params;
if (args.save_type == "jpg" || args.save_type == "jpeg")
{
params = {cv::IMWRITE_JPEG_QUALITY, args.quality};
}
else if (args.save_type == "png")
{
params = {cv::IMWRITE_PNG_COMPRESSION, 3};
}
ok = cv::imwrite(fname, bgr, params);
}
if (ok) {
printf("[saved] %s\n", fname);
save_count++;
} else {
printf("[save failed] frame %lu\n", save_count);
}
}
DebugInfo *pData = new DebugInfo();
// pData->CameraId = (ctx->cam_no == 1 ? 0 : 1);
// pData->TimeStampMs = epochTimeStamp_ms;
memcpy(pData->Img, frame.data, frame.size);
bool res = send_debuginfo(pData);
//printf("send %ld %d %d\n", pData->TimeStampMs, pData->CameraId, res);
delete pData;
pData = nullptr;
cap.releaseFrame(frame);
// if (args.verbose) {
// printf("frame #%lu seq=%u size=%zu\n",
// frame_count, frame.sequence, frame.size);
// }
auto now = clock::now();
auto since_report = std::chrono::duration_cast<std::chrono::milliseconds>(now - t_last_report);
if (since_report.count() >= 1000) {
double total_sec = std::chrono::duration_cast<std::chrono::milliseconds>(now - t_start).count() / 1000.0;
double avg_fps = total_sec > 0 ? frame_count / total_sec : 0;
printf("[stats] frames=%lu avg_fps=%.2f door_position[%ld %ld %ld %ld]\n", frame_count, avg_fps, g_Feedback_Of_The_Actual_Position_Of_The_Left_Front_Door_391_S.load(), g_Feedback_Of_The_Actual_Position_Of_The_Right_Front_Door_391_S.load(), g_Feedback_Of_The_Actual_Position_Of_The_Left_Rear_Door_391_S.load(), g_Feedback_Of_The_Actual_Position_Of_The_Right_Rear_Door_391_S.load());
t_last_report = now;
}
}
auto t_end = clock::now();
double total_sec = std::chrono::duration_cast<std::chrono::milliseconds>(t_end - t_start).count() / 1000.0;
printf("=========================================================\n");
printf("Capture finished. frames=%lu duration=%.2fs avg_fps=%.2f\n",
frame_count, total_sec, total_sec > 0 ? frame_count / total_sec : 0);
if (args.save > 0) {
printf("Saved %lu frames to %s/\n", save_count, args.out_dir.c_str());
}
if (args.can_enabled && CanComm::is_open()) {
printf("CAN received: %d frames\n", CanComm::receive_count());
}
printf("=========================================================\n");
cap.stopStreaming();
cap.close();
if (args.can_enabled) {
CanComm::can_stop();
}
return 0;
}