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

Compare commits

merge into: zmv/vision-anti-pinch:use-can
Branches Tags
zmv/vision-anti-pinch:main zmv/vision-anti-pinch:inference-only zmv/vision-anti-pinch:use-can zmv/vision-anti-pinch:lazy-inference
..
pull from: zmv/vision-anti-pinch:inference-only
Branches Tags
zmv/vision-anti-pinch:inference-only zmv/vision-anti-pinch:use-can zmv/vision-anti-pinch:lazy-inference zmv/vision-anti-pinch:main

8 Commits
use-can .. inference-only

Author SHA1 Message Date
zmv 69d565fda5 feat: add low-light brightness gate 2026-10-09 16:02:15 +08:00
zmv 4eff3825be feat: add mask tool 2026-09-30 17:04:29 +08:00
zmv f87e5aad40 Fix live CAN feedback with CH0-only asynchronous risk transmission 
Send 0x1F6 risk updates through a bounded latest-result worker on CH0 and disable CH1 initialization, reception, transmission and live display. Preserve the four actual-position bytes from 0x391 and recorded-session compatibility.

Include regression coverage, Orin camera/TensorRT/CAN validation, and the verified standalone binary with refreshed checksums.
 2026-09-28 13:59:26 +08:00
zmv 515a9f32d8 Add USBCANFD transport and visualize 0x391 door feedback 2026-09-24 15:41:49 +08:00
zmv 475fe2440b Enable visualization by default for inference and replay 2026-09-24 15:15:51 +08:00
zmv daab1db58f Add H.264 inference session recording and replay 2026-09-24 15:05:44 +08:00
zmv 0192fccbf0 Recover transient camera failures within six timeout windows 2026-09-23 21:59:08 +08:00
zmv c1e2ea6577 Add optional original, annotated and JSON frame outputs 2026-09-23 20:37:55 +08:00
94 changed files with 2991 additions and 1798 deletions
Expand all files Collapse all files
.gitattributes
+5 -4
View File
@@ -1,4 +1,5 @@
*.engine binary
*.onnx binary
*.so binary
*.ko binary
*.sh text eol=lf
*.engine -text
*.ko -text
/orin/** -text
/config/door_mask.json -text
.gitignore
+4 -20
View File
@@ -1,23 +1,7 @@
# Generated build/cache/runtime files
/build*/
__pycache__/
*.py[cod]
*.o
*.a
.venv/
venv/
outputs/
logs/
*.log
*.pid
*.mp4
*.avi
/models/
*.onnx
# Local credentials
.env
.env.*
!.env.example
/build/
/engines/
/logs/
*.tmp
# Optional per-frame output, including standalone Orin runs
output/
CMakeLists.txt
+23 -8
View File
@@ -1,24 +1,39 @@
cmake_minimum_required(VERSION 3.24)
project(vision_anti_pinch LANGUAGES CXX CUDA)
if(NOT DEFINED CMAKE_CUDA_ARCHITECTURES)
set(CMAKE_CUDA_ARCHITECTURES native)
endif()
project(camera_risk LANGUAGES CXX CUDA)
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CUDA_STANDARD 17)
set(CMAKE_CUDA_ARCHITECTURES 87)
find_package(CUDAToolkit REQUIRED)
find_package(OpenCV REQUIRED COMPONENTS core imgproc imgcodecs highgui videoio)
find_package(Threads REQUIRED)
find_path(TENSORRT_INCLUDE_DIR NvInfer.h PATH_SUFFIXES aarch64-linux-gnu REQUIRED)
find_path(TENSORRT_INCLUDE_DIR NvInfer.h PATH_SUFFIXES aarch64-linux-gnu x86_64-linux-gnu REQUIRED)
find_library(TENSORRT_LIBRARY nvinfer REQUIRED)
add_library(camera_risk_core
src/trt_engine.cpp src/pose_engine.cu src/gpu_preprocess.cu src/uyvy_converter.cu
src/detector.cpp src/v4l2_capture.cpp src/camera.cpp src/can_comm.cpp src/can_state.cpp
src/door_risk.cpp src/door_zone.cpp src/camera_pipeline.cpp src/output_writer.cpp src/result_renderer.cpp src/config.cpp src/serialization.cpp)
src/can_bus.cpp src/detector.cpp src/door_risk.cpp src/brightness_gate.cpp src/camera.cpp src/camera_pipeline.cpp src/inference_session.cpp src/serialization.cpp src/output_writer.cpp src/result_renderer.cpp src/config.cpp)
target_include_directories(camera_risk_core PUBLIC include third_party ${TENSORRT_INCLUDE_DIR} ${OpenCV_INCLUDE_DIRS})
target_link_libraries(camera_risk_core PUBLIC ${TENSORRT_LIBRARY} ${OpenCV_LIBS} Threads::Threads CUDA::cudart CUDA::nppc CUDA::nppig CUDA::nppidei CUDA::nppicc
"${CMAKE_SOURCE_DIR}/orin/lib/libusbcanfd.so" "${CMAKE_SOURCE_DIR}/orin/lib/libusb-1.0.so")
target_link_libraries(camera_risk_core PUBLIC ${TENSORRT_LIBRARY} ${OpenCV_LIBS} Threads::Threads CUDA::cudart CUDA::nppc CUDA::nppig CUDA::nppidei CUDA::nppicc)
target_include_directories(camera_risk_core SYSTEM PRIVATE third_party/zlg)
target_compile_options(camera_risk_core PRIVATE $<$<COMPILE_LANGUAGE:CXX>:-Wall;-Wextra;-Wpedantic>)
add_library(camera_risk_visualizer src/visualizer.cpp)
target_link_libraries(camera_risk_visualizer PUBLIC camera_risk_core)
add_library(camera_risk_mask_annotator src/door_mask_annotator.cpp)
target_include_directories(camera_risk_mask_annotator PUBLIC include third_party ${OpenCV_INCLUDE_DIRS})
target_link_libraries(camera_risk_mask_annotator PUBLIC ${OpenCV_LIBS})
target_compile_options(camera_risk_mask_annotator PRIVATE -Wall -Wextra -Wpedantic)
add_executable(camera_risk src/main.cpp)
target_link_libraries(camera_risk PRIVATE camera_risk_visualizer)
set_target_properties(camera_risk PROPERTIES BUILD_WITH_INSTALL_RPATH TRUE INSTALL_RPATH "$ORIGIN/../lib;$ORIGIN/../orin/lib;/usr/local/cuda/lib64")
add_executable(door_mask_tool src/door_mask_tool.cpp)
target_link_libraries(door_mask_tool PRIVATE camera_risk_core camera_risk_mask_annotator)
target_compile_options(door_mask_tool PRIVATE -Wall -Wextra -Wpedantic)
# Reference USBCANFD SDK is shipped with the standalone Orin package.
target_link_libraries(camera_risk_core PUBLIC "${CMAKE_SOURCE_DIR}/orin/lib/libusbcanfd.so"
"-Wl,--no-as-needed" "${CMAKE_SOURCE_DIR}/orin/lib/libusb-1.0.so" "-Wl,--as-needed")
set_target_properties(camera_risk door_mask_tool PROPERTIES BUILD_WITH_INSTALL_RPATH TRUE INSTALL_RPATH "$ORIGIN/../lib;$ORIGIN/../orin/lib;/usr/local/cuda/lib64")
# Keep the bundled SDK/libusb pair together even with a desktop LD_LIBRARY_PATH.
target_link_options(camera_risk PRIVATE "-Wl,--disable-new-dtags")
target_link_options(door_mask_tool PRIVATE "-Wl,--disable-new-dtags")
Makefile
-9
View File
@@ -1,9 +0,0 @@
.PHONY: all run package clean
all:
./scripts/build.sh
run: all
./run_camera.sh
package: all
./scripts/package_orin.sh
clean:
cmake --build build --target clean
README.md
+168 -65
View File
@@ -1,99 +1,114 @@
# Vision Anti-Pinch — use-can
# Vision Anti-Pinch
相机默认每 **2000 ms** 无有效帧记一次失败，连续 **6 次**才停止流水线（纯超时约 **12 秒**）；成功取得有效帧后重新计数。每个等待周期内仍以 100 ms 间隔检查停止请求，CAN 接收不受重试影响。取流错误或坏帧也计入重试期限；设备失效时尝试重开相机，恢复后继续推理。单次等待时间由 `config/camera.json` 的 `camera.timeout_ms` 配置。
C++ / TensorRT 实时摄像头风险推理，算法来自最新 run_risk.py。
持续取图、推理并更新内存中的 FrameResult；实时推理、录制和回灌默认开启可视化。
## 编译与运行
相机每次取帧默认等待 **2000 ms**，连续超时 **6 次**才停止流水线（纯超时约 **12 秒**）；成功取得有效帧后清零超时计数。单次等待时间由 `config/camera.json` 的 `camera.timeout_ms` 配置。运行中的坏帧、取流错误共用该重试期限；设备失效时尝试重新打开相机，恢复有效帧后继续推理。GPU 错误仍按原有异常处理退出。
目标环境：Jetson AGX Orin、JetPack 5.1.2 / L4T R35.4.1、CUDA 11.4、TensorRT 8.5.x、OpenCV 4.5.4。
源码编译另需 CMake 3.24+ 和相应开发库。
原始画面在 YOLO 前经过低照度门控。启动前 5 帧不输出有效风险；明显极暗连续 15 帧后暂停人体、姿态和风险推理，亮度满足恢复阈值连续 15 帧后自动恢复。低照度结果标记 `risk_valid=false`、`risk_level=unavailable`，不会转换成 `clear`。阈值和 ROI 可在 `brightness_gate` 配置段调整，`enabled=false` 可关闭门控。阈值依据研发原始视频验证说明设置，适用边界仍包括黄昏、局部强光和镜头污染等未覆盖场景。
## Orin 直接运行
适用环境：Jetson AGX Orin、JetPack 5.1.2 / L4T R35.4.1、CUDA 11.4、TensorRT 8.5.2、OpenCV 4.5.4。
仓库已包含该板端编译的程序和重新构建的 TensorRT 引擎。
```bash
./scripts/build.sh # 或 make
./scripts/initialize_camera.sh # 开机后摄像头尚未初始化时执行
./run_camera.sh # 执行 build/camera_risk
./run_camera.sh --visualize # 可选 OpenCV 本地窗口
git clone https://git.kendeji.fun/zmv/vision-anti-pinch.git
cd vision-anti-pinch/orin
./scripts/initialize_camera.sh # 开机后初始化 /dev/video5 摄像头链路
./run.sh # 持续推理并显示预览，结果只保存在内存
```
预编译 Orin 包：
默认显示本地预览，叠加门区、检测框、手臂/手部关键点及风险信息。需要关闭预览时：
```bash
cd orin
./scripts/initialize_camera.sh
./run.sh # 脚本直接 exec C++ ELF 程序
# 也可以直接执行：
./build/camera_risk --config config/camera.json
./run.sh --no-visualize
```
默认无窗口、不保存文件；Ctrl+C / SIGTERM 停止。`--visualize` 使用 Orin 的图形桌面，关闭窗口仅停止显示、推理继续。没有桌面环境时保持无窗口推理。
`--visualize` 仍可显式开启；没有 DISPLAY / WAYLAND_DISPLAY 时自动跳过窗口并继续推理。
## 流水线与结果变量
Ctrl+C / SIGTERM 停止程序。
关闭预览窗口或按 q/Esc 只关闭预览，推理继续。
```
V4L2 MMAP 采集 / NPP UYVY 转换（线程 1）
→ YOLO TensorRT 检测（线程 2）
→ RTMW TensorRT / 隔离推理 / C++ 时序融合（线程 3）
→ 前后门多特征归属 / 因果中值 / mask 风险 / CAN 过滤
→ shared_ptr<const FrameResult> 最新结果变量
摄像头为 /dev/video5，1920×1536 UYVY，30 FPS。
当前 door mask 是指定的 constant_door_mask.json 副本，标定状态为 false；装车后应更换与实际画面匹配的门区并更新标定状态。
## Door mask 标注工具
先停止正在占用摄像头的推理程序，再运行：
```bash
./annotate_door_mask.sh --camera --output config/door_masks/driver-side.json
```
摄像头使用有界队列并丢弃积压旧帧；可视化独立读取最新结果。
下游直接链接 `camera_risk_core`，在自己的线程中读取：
工具按 `config/camera.json` 抓取一张参考画面并打开标注窗口。左键依次添加门区多边形顶点，右键或 `U` 撤销一个点，`R` 清空，`S` 或 Enter 保存，Esc 或 `Q` 取消。保存时生成 x-anylabeling JSON，并在同一目录保存 `<名称>_reference.jpg`；已有文件默认不会覆盖，需要覆盖时显式添加 `--force`。
也可以使用已有图片，不打开摄像头：
```bash
./annotate_door_mask.sh --image /path/to/frame.jpg --output config/door_masks/driver-side.json
```
输入已经采集的视频时，工具会先打开帧选择窗口。可拖动滑块，使用 `A/D` 逐帧移动，或使用 `J/L` 前后跳过 30 帧；按 `S` 或 Enter 选择当前帧并进入标注：
```bash
./annotate_door_mask.sh --video /path/to/video.mp4 --output config/door_masks/driver-side.json
```
已知帧号时可以跳过帧选择窗口，帧号从 0 开始：
```bash
./annotate_door_mask.sh --video /path/to/video.mp4 --frame 244 --output config/door_masks/driver-side.json
```
`--image` 和 `--video` 模式不会打开或检查摄像头。视频生成的 JSON 会额外记录 `sourceVideo` 和 `sourceFrameIndex`。
该工具仅生成 mask 文件，暂不改变 `camera.json` 或推理程序当前使用的默认 mask。
## 下游获取结果
main.cpp 注释中也包含以下示例。下游通过自己的调用位置或线程读取：
```cpp
#include "camera_pipeline.hpp"
#include "config.hpp"
cms::CameraPipeline pipeline(cms::loadConfig("config/camera.json"));
pipeline.start();
// 下游自己的循环中读取；首帧前 result 为空。
auto result = pipeline.latest();
if (result) {
const auto& risk = result->risk;
const auto& front = result->door_risks.at("front");
const auto& rear = result->door_risks.at("rear");
const auto& people = result->persons; // 每人 selected/smoothed/temporal 各 133 点
const auto& assignments = result->door_assignments;
const auto& can = result->can;
// risk.risk_type / risk.risk_level / risk.risk_score（std::optional<double>）
const auto& people = result->persons;
// result->frame_index / track_ids / boxes
// result->image / door_mask_pixels / capture / timing
// people[i].selected / smoothed / temporal：各 133 个关键点
}
// 退出时：
pipeline.requestStop();
pipeline.join();
```
`latest()` 返回拥有数据的只读快照，读者互不消费结果，旧引用不会被新帧修改。慢读者可以跳过中间帧；异常通过 `join()` 传播。停止后的快照只代表最后结果，持续控制应检查运行状态和时间戳。
公共结构定义见 `include/frame_result.hpp`，接口见 `include/camera_pipeline.hpp`。
latest() 返回 shared_ptr<const FrameResult>；首帧尚未完成时为空。
公共定义位于 include/camera_pipeline.hpp 和 include/frame_result.hpp。
## CAN 与风险
- 0x391 的 data[0..3]：左前、右前、左后、右后。
- 默认左侧，`--can-side right` 切到右侧；值为 0 时对应门活跃。
- 缺失或过期（默认 1000 ms）报文输出 `CAN_UNKNOWN`，分数为空，不能解释为 clear。
- `--no-can --active-door both` 显式模拟门状态；也可选择 front/rear/none。
- 优先使用有效手部凸包，无有效手部时用人体框兜底；transition 保留 unknown 语义。
- 最终使用 `risk` 或 `door_risks`；`ungated_risk` 仅用于诊断。当前只接收门状态，不发送执行器指令。
- mask 和前后门参数来自研发机位，默认 calibration_verified=false；装车后需要重新标定。
配置位于 `config/camera.json`，模型位于 `orin/models/engines/`。详细参数：
## 从源码构建
```bash
./run_camera.sh --help
./run_camera.sh --duration 30
./run_camera.sh --save-results /path/results.jsonl
./scripts/build.sh
./scripts/initialize_camera.sh
./run_camera.sh
```
显式保存时每帧写一行 JSON，核心字段保留 frame_index、track_ids、boxes、skeletons、door_assignments、door_risks、risk、ungated_risk、can；文件由用户指定路径管理。`--video` 可使用同一 C++ 流程处理视频文件，尺寸需匹配配置。
源码构建要求 CMake 3.24+、CUDA/NPP、TensorRT 和 OpenCV 开发依赖。
构建脚本会优先使用 Orin 上已有的 ~/miniconda3/envs/yolo/bin/cmake。
源码配置直接使用 orin/engines/ 中的板端引擎。该引擎仅适用于上述 Orin 环境；仓库不包含 ONNX 或模型转换脚本。
独立库目标为 camera_risk_core；可视化在 camera_risk_visualizer。
## 目录与打包
## 目录
- `src/`：C++ / CUDA 源文件及 main.cpp。
- `include/`：头文件及下游数据结构。
- `third_party/nlohmann/`：C++ JSON 依赖及许可证。
- `config/`、`annotations/`：运行配置与标定。
- `scripts/`：编译、摄像头初始化、打包。
- `orin/`：可单独复制的 ELF 程序、模型、驱动、CAN 库和配置。
- src/、include/：采集、推理、结果变量及可选绘制。
- config/：源码构建程序的配置和固定门区。
- scripts/：构建和摄像头初始化脚本。
- orin/：已编译程序、TensorRT 引擎、配置、当前 GMSL 驱动及启动脚本。
- third_party/：nlohmann JSON 及其许可证。
修改源码后，在 Orin 重新编译并执行 `./scripts/package_orin.sh`，或执行 `make package`。运行、打包均不使用 Python。打包后可在 orin 目录运行 `sha256sum -c SHA256SUMS`。
NPP 用于 UYVY 转换、YOLO 缩放填充、RTMW 仿射裁剪；CUDA 用于通道转换与归一化。
风险几何计算仍包含 CPU/OpenCV 操作；算法内部的临时 JSON 对象只在内存中使用。
当前摄像头驱动启动时偶发首帧 ERROR，应用会丢弃并计数；持续错误、采集超时或推理错误会明确报错并退出。
现有 GMSL 驱动停止采集时可产生 videobuf2 缓冲区告警，v4l2-ctl 同样复现；需在驱动侧完成量产修复和验证。
## 可选保存（默认全部关闭）
@@ -117,3 +132,91 @@ pipeline.join();
相机超时回归测试：`./tests/run_camera_timeout.sh`；加 `--wall-clock` 验证默认 6 次超时约 12 秒。测试直接编译实际相机代码，使用模拟采集和转换器，无需摄像头、GPU 或 OpenCV；不覆盖视频回放路径。
板端真实摄像头 / TensorRT 恢复与停止测试记录见 [Orin 测试报告](tests/ORIN_TEST_REPORT.md)。
## 推理过程录制与视频回灌
当前 `inference-only` 分支支持 H.264/MP4 会话；默认关闭，不再为会话逐帧保存图片。
```bash
# 录制实际完成推理的帧，Ctrl+C / SIGTERM 正常停止并封装视频
./run_camera.sh --record-session output/sessions/drive-001
# 视频解码后重新运行 YOLO、RTMW、融合与风险推理；文件结束自动退出
./run_camera.sh --replay-session output/sessions/drive-001 --save-results
# 同时保存回灌后的新视频及推理过程，便于对比
./run_camera.sh --replay-session output/sessions/drive-001 \
--record-session output/sessions/drive-001-replay
```
独立包使用同样参数，例如 `./orin/run.sh --record-session output/sessions/drive-001`。
会话路径相对于当前工作目录；录制目录必须尚不存在，避免覆盖原始记录。
会话内容：
- `frames.mp4`：H.264 有损压缩视频，按完成推理的顺序编码，尺寸与原图一致。
- `frames/*.json`：对应视频帧序号、原推理帧号、完整采集元数据、检测框、原始/隔离关键点、融合记录、最终结果与耗时。
- `frames.txt`：视频帧与 JSON 的顺序索引。
- `session.json`、`door_mask.json`：录制时的推理参数、分辨率、名义帧率、门区副本、模型路径与大小。
- `complete.json`：正常停止后写入的帧数与视频大小，用于检查会话是否完成。
回灌不打开摄像头。它恢复会话中的分辨率、检测阈值、人数上限、风险阈值和门区，使用 `--config` 指定的当前 TensorRT 引擎，按顺序尽快推理，不按墙钟时间限速。视频使用配置的固定名义帧率；真实采集间隔和断流间隔以 JSON 中的纳秒时间戳为准。原始采集时间在再次录制时保存为 `source_capture`，回灌耗时使用本次运行的单调时钟。
编码使用 OpenCV 的 FFmpeg H.264 编码器；需 OpenCV `videoio` 与可用的 H.264 编码支持，宽高必须为正偶数。不支持时明确报错。当前使用软件编解码，会增加 CPU、编码延迟和磁盘开销；保存对象是完成推理的帧，并非相机全部原始帧。H.264 有损压缩和不同引擎可能改变检测、关键点与风险结果，不承诺逐位一致。模型只记录来源和大小，不复制引擎，也不保证大小相同即模型相同；精确对比应使用同一组引擎。
Ctrl+C / SIGTERM 会封装已写入的视频，`CameraPipeline::join()` 返回前完成收尾。强制杀进程、断电或写入失败可能留下 `frames.pending.mp4`；缺少完成标记的会话拒绝回灌。编码器封装后检查帧数，回灌遇到缺帧、缺失 JSON、损坏索引或解码错误会报错，不静默跳帧。录制没有自动限时或磁盘轮转，请按可用空间控制录制时长。
编解码及会话回归：`./tests/run_session.sh`，无需摄像头或 TensorRT。真实摄像头与 TensorRT 验证脚本：`python3 tests/orin_session_test.py --binary build/camera_risk --config config/camera.json --output /tmp/new-session-test`；输出目录必须不存在，脚本不会停止已有部署进程。
本次实机结果与未覆盖范围见 [H.264 会话测试报告](tests/ORIN_SESSION_REPORT.md)。
## CAN/CANFD 收发与四门位置显示
实时运行默认打开参考工程 `/home/orin/vision-anti-pinch` 使用的 USBCANFD（设备类型 33、设备索引 0），启用 CH0 和 CH1：60 MHz 时钟、仲裁域 500 kbps、数据域 2 Mbps。一个接收线程只接收 CH0，所有发送只使用 CH1；两个通道默认启用终端电阻。可在 `config/camera.json` 的 `can` 中设置 `enabled`、`device_index`、`timeout_ms`（默认 1000 ms）和 `termination`；`--no-can` 显式关闭实时 CAN。设备不可用时显示 UNAVAILABLE，摄像头推理继续；接收 API 失败显示 ERROR，需检查设备后重启程序。
首次使用 USB-CAN 时，若节点为 root:root 且普通用户不可写，可运行 `./scripts/enable_can_access.sh`（需要 sudo）。它只为 `3068:0009` 适配器安装 `plugdev` 组读写规则并刷新设备权限；运行用户须属于 `plugdev`，新加组后需重新登录。无需以 root 运行整个推理程序。
`0x391` 四个信号按参考工程注释中的四个连续字节解析：
| 字节（从 0 开始） | 预览标签 | 信号 |
|---|---|---|
| 0 | LF | 左前门实际位置反馈 |
| 1 | RF | 右前门实际位置反馈 |
| 2 | LR | 左后门实际位置反馈 |
| 3 | RR | 右后门实际位置反馈 |
显示值是无符号原始字节 0–255，尚无物理单位、缩放或无效值定义，因此不标为百分比、不把 255 擅自视为无效。CH0 接收 CAN/CANFD 数据帧，过滤远程帧、错误帧和发送回显；少于 4 字节的 `0x391` 不更新信号。修正了参考代码 `get_bit_value(0/1/2/3, 8)` 导致的位域重叠，字节起始位应为 0/8/16/24。
默认预览显示 CH0 四门实际位置反馈、LIVE/STALE/WAIT、新鲜度及帧数，并显示总 RX/TX 和错误计数。尚未接收到值时显示 `--`，超时保留最后值但标记 STALE；CH1 专用于发送，不接收也不在实时预览中显示。即使相机正在重试或暂无新推理帧，CAN 预览仍约每 100 ms 刷新。CAN 只提供状态显示和下游数据，本次没有新增基于门位置的推理启停或风险门控。
下游通过 `pipeline.canStatus()` 读取当前状态；`pipeline.latest()->can` 是该推理结果生成时的快照。按需发送使用线程安全的 `pipeline.sendCan()`：
```cpp
cms::CanFrame frame;
frame.channel = 1; // 发送仅使用 CH1
frame.id = 0x123; // 按实际协议设置
frame.data = {0x01, 0x02}; // 示例数据，应替换为实际报文
bool sent = pipeline.sendCan(frame);
// CANFD：frame.fd = true；可按协议设置 extended、bitrate_switch。
// CAN 数据长度 0–8；CANFD 长度 0–8、12、16、20、24、32、48、64。
// 标准 ID 最大 0x7FF，扩展 ID 最大 0x1FFFFFFF。
```
发送通道不是 CH1，或 ID/长度/标志非法时抛出 `std::invalid_argument`；设备未就绪或 SDK 发送失败返回 false，发送计数以 SDK 返回为准。`sendCan()` 接口不自动填充/截断数据。发送调用与设备关闭互斥；CH0 接收保持独立线程运行。
实时推理首次取得风险状态时发送一次，此后仅在二值风险发生变化时，由后台 CAN 发送线程通过 CH1 发送一帧标准 CAN 数据报文 `0x1F6`，DLC 固定为 8。`data[0]` 的低四位（bit0–3）和高四位（bit4–7）同时填写相同的二值风险：`risk_level` 为 `clear`、`watch` 或 `unavailable` 时各为 0，`warning`、`danger` 以及未知状态时各为 1；后七字节全部为 0。即 `clear`、`watch` 和无人或证据不足导致的 `unavailable` 发送 `00 00 00 00 00 00 00 00`，其余发送 `11 00 00 00 00 00 00 00`（十六进制）。
发送按报文二值状态变化触发：`clear/watch/unavailable ↔ warning/danger/未知状态` 才产生新报文；相同编码的等级切换不重复发送。没有独立周期，风险不变时不重发；发送失败计数后也不因后续相同状态自动重试，等待新的二值状态变化。SDK 发送等待不占用推理线程；后台最多保留一个待发结果，发送变慢时用最新结果替换尚未发送的结果，避免积压旧风险；等待期间若从 A 变为 B 又回到正在发送的 A，会取消冗余待发报文，因此不保证每个短暂中间状态都上总线。CH1 发送失败通过现有 TX 错误计数呈现；结果中的 CAN 快照记录采样时已完成的发送计数，不保证包含当前帧的发送。停止时丢弃待发结果，并等待已经进入 SDK 的调用返回后释放设备。`--no-can` 或 `can.enabled=false` 禁用自动发送，视频回灌始终不发送。按需 `sendCan()` 仍为同步 API。
结果 JSON 和录制会话自动包含每帧 `can` 快照。视频回灌不打开 USB-CAN，不发送报文，直接恢复录制时的状态并标记 `source=replay`；旧会话没有 CAN 字段时显示 NOT_RECORDED。回灌调用 `sendCan()` 会明确拒绝。
依赖 `third_party/zlg/zcan.h` 和 `orin/lib/` 中的参考工程 SDK 库（含 SONAME 链接），独立包已包含它们。纯模拟收发回归：`./tests/run_can.sh`；会话回归同时覆盖 CAN 快照保存。模拟测试不代表物理总线收发验证。
本次检查范围与延期项目见 [CAN 实现记录](tests/ORIN_CAN_REPORT.md)；CH0 实机排查与单通道修复见 [0x391 实际位置反馈检查](tests/ORIN_391_REPORT.md)。
新增 `0x1F6` 二值风险发送的检查范围及部署状态见 [风险发送检查记录](tests/RISK_CAN_REPORT.md)。
风险发送现已改为首次发送及二值变化触发，验证结果见 [风险变化触发发送检查](tests/ORIN_RISK_CHANGE_REPORT.md)。
CAN 通道现为 CH0 接收、CH1 发送，板端检查与未覆盖项目见 [CH1 发送切换检查](tests/ORIN_CH1_TX_REPORT.md)。
annotate_door_mask.sh
Executable
+4
View File
@@ -0,0 +1,4 @@
#!/usr/bin/env bash
set -euo pipefail
root="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" && pwd)"
exec "$root/build/door_mask_tool" --config "$root/config/camera.json" "$@"
annotations/constant_door_mask.json
-77
View File
@@ -1,77 +0,0 @@
{
"version": "x-anylabeling",
"flags": {},
"shapes": [
{
"label": "door",
"points": [
[
212.86,
145.919
],
[
378.891,
851.857
],
[
468.293,
996.446
],
[
536.408,
1294.131
],
[
553.437,
1404.7
],
[
561.951,
1532.279
],
[
51.086,
1532.279
],
[
8.514,
1515.268
],
[
0.0,
1468.489
],
[
8.514,
26.845
],
[
25.543,
5.582
],
[
131.973,
-2.924
],
[
191.574,
1.329
],
[
208.603,
145.919
]
],
"group_id": null,
"shape_type": "polygon",
"flags": {}
}
],
"imagePath": "constant_door_mask_reference.jpg",
"imageData": null,
"imageHeight": 1536,
"imageWidth": 1920,
"sourceVideo": "D:\\huang.xuewei\\CMS\\video\\20260921104502\\segment_009.mp4",
"sourceFrameIndex": 244,
"constantDoorMask": true
}
annotations/door_zone_multifeature.json
-47
View File
@@ -1,47 +0,0 @@
{
"version": "v9.10-research-calibration-1",
"description": "Temporary-camera calibration from seg_01 and seg_08; recalibrate for the production camera.",
"front_probability_min": 0.72,
"rear_probability_max": 0.28,
"minimum_person_height": 0.4,
"minimum_pose_features": 4,
"history_frames": 5,
"stale_track_frames": 120,
"features": {
"shoulder_center_x": {
"rear": 0.53,
"front": 0.64,
"weight": 1.2
},
"shoulder_center_y": {
"rear": 0.54,
"front": 0.46,
"weight": 1.3
},
"shoulder_width": {
"rear": 0.14,
"front": 0.2,
"weight": 1.4
},
"torso_height": {
"rear": 0.35,
"front": 0.47,
"weight": 1.5
},
"hip_center_y": {
"rear": 0.89,
"front": 0.93,
"weight": 0.6
},
"box_center_x": {
"rear": 0.48,
"front": 0.58,
"weight": 0.35
},
"box_height": {
"rear": 0.7,
"front": 0.82,
"weight": 0.45
}
}
}
config/camera.json
+40 -4
View File
@@ -1,6 +1,42 @@
{
"camera": {"device":"/dev/video5","width":1920,"height":1536,"fps":30,"buffers":4,"timeout_ms":2000},
"inference": {"engine_directory":"../orin/models/engines","detector_confidence":0.30,"max_people":3,"queue_capacity":2},
"door": {"mask":"../annotations/constant_door_mask.json","zone_config":"../annotations/door_zone_multifeature.json","warning_relative":0.30,"release_relative":0.45,"calibration_verified":false},
"can": {"enabled":true,"side":"left","timeout_ms":1000}
"camera": {
"device": "/dev/video5",
"width": 1920,
"height": 1536,
"fps": 30,
"buffers": 4,
"timeout_ms": 2000
},
"inference": {
"engine_directory": "../orin/engines",
"detector_confidence": 0.3,
"max_people": 3,
"queue_capacity": 2
},
"door": {
"mask": "door_mask.json",
"warning_relative": 0.3,
"release_relative": 0.45,
"calibration_verified": false
},
"brightness_gate": {
"enabled": true,
"roi": [0.02, 0.05, 0.88, 0.98],
"dark_pixel_luma": 40.0,
"enter_median_luma": 18.0,
"enter_dark_ratio": 0.85,
"enter_dark_tile_ratio": 0.75,
"exit_median_luma": 24.0,
"exit_dark_ratio": 0.80,
"exit_dark_tile_ratio": 0.70,
"startup_frames": 5,
"enter_frames": 15,
"exit_frames": 15
},
"can": {
"enabled": true,
"device_index": 0,
"timeout_ms": 1000,
"termination": true
}
}
config/door_mask.json
+51
View File
@@ -0,0 +1,51 @@
{
"version": "x-anylabeling",
"flags": {},
"shapes": [
{
"label": "door",
"points": [
[
1908,
235
],
[
1874,
267
],
[
1787,
803
],
[
1735,
873
],
[
1716,
944
],
[
1682,
1275
],
[
1654,
1531
],
[
1919,
1533
]
],
"group_id": null,
"shape_type": "polygon",
"flags": {}
}
],
"imagePath": "driver-side-2_reference.jpg",
"imageData": null,
"imageHeight": 1536,
"imageWidth": 1920,
"constantDoorMask": true
}
config/door_masks/driver-side-1.json
+51
View File
@@ -0,0 +1,51 @@
{
"version": "x-anylabeling",
"flags": {},
"shapes": [
{
"label": "door",
"points": [
[
314,
4
],
[
425,
572
],
[
495,
713
],
[
534,
822
],
[
542,
1138
],
[
538,
1522
],
[
6,
1535
],
[
0,
9
]
],
"group_id": null,
"shape_type": "polygon",
"flags": {}
}
],
"imagePath": "driver-side-1_reference.jpg",
"imageData": null,
"imageHeight": 1536,
"imageWidth": 1920,
"constantDoorMask": true
}
config/door_masks/driver-side-1_reference.jpg
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 980 KiB

config/door_masks/driver-side-2.json
+51
View File
@@ -0,0 +1,51 @@
{
"version": "x-anylabeling",
"flags": {},
"shapes": [
{
"label": "door",
"points": [
[
1908,
235
],
[
1874,
267
],
[
1787,
803
],
[
1735,
873
],
[
1716,
944
],
[
1682,
1275
],
[
1654,
1531
],
[
1919,
1533
]
],
"group_id": null,
"shape_type": "polygon",
"flags": {}
}
],
"imagePath": "driver-side-2_reference.jpg",
"imageData": null,
"imageHeight": 1536,
"imageWidth": 1920,
"constantDoorMask": true
}
config/door_masks/driver-side-2_reference.jpg
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 1.4 MiB

config/door_masks/driver-side.json
+55
View File
@@ -0,0 +1,55 @@
{
"version": "x-anylabeling",
"flags": {},
"shapes": [
{
"label": "door",
"points": [
[
1426,
0
],
[
1257,
489
],
[
1195,
615
],
[
1155,
636
],
[
1108,
813
],
[
1106,
1097
],
[
1097,
1529
],
[
1917,
1535
],
[
1915,
9
]
],
"group_id": null,
"shape_type": "polygon",
"flags": {}
}
],
"imagePath": "driver-side1_reference.jpg",
"imageData": null,
"imageHeight": 1536,
"imageWidth": 1920,
"constantDoorMask": true
}
config/door_masks/driver-side1_reference.jpg
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 1.2 MiB

config/door_masks/driver-side_reference.jpg
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 980 KiB

include/bounded_queue.hpp
-6
View File
@@ -18,12 +18,6 @@ public:
if(closed_)return false;
items_.push_back(std::move(item));changed_.notify_all();return true;
}
bool pushLatest(T item) {
std::lock_guard<std::mutex> lock(mutex_);
if(closed_)return false;
if(items_.size()>=capacity_)items_.pop_front();
items_.push_back(std::move(item));changed_.notify_all();return true;
}
std::optional<T> pop() {
std::unique_lock<std::mutex> lock(mutex_);
changed_.wait(lock,[&]{return closed_||!items_.empty();});
include/brightness_gate.hpp
+33
View File
@@ -0,0 +1,33 @@
#pragma once
#include <opencv2/core.hpp>
#include <string>
namespace cms {
struct BrightnessGateConfig {
bool enabled{true};
double roi_x1{0.02}, roi_y1{0.05}, roi_x2{0.88}, roi_y2{0.98};
double dark_pixel_luma{40.0};
double enter_median_luma{18.0}, enter_dark_ratio{0.85}, enter_dark_tile_ratio{0.75};
double exit_median_luma{24.0}, exit_dark_ratio{0.80}, exit_dark_tile_ratio{0.70};
int startup_frames{5}, enter_frames{15}, exit_frames{15};
};
struct BrightnessDecision {
std::string state{"INITIALIZING"}, reason{"initializing"}, transition;
bool allow_downstream{};
double median_luma{}, p75_luma{}, mean_luma{}, dark_pixel_luma{40.0}, dark_ratio{}, dark_tile_ratio{};
int dark_streak{}, bright_streak{};
int enter_frames{}, exit_frames{};
};
class BrightnessGate {
public:
explicit BrightnessGate(BrightnessGateConfig config = {});
BrightnessDecision update(cv::Mat const& bgr);
private:
BrightnessGateConfig config_;
std::string state_{"INITIALIZING"};
int startup_count_{}, startup_dark_votes_{}, dark_streak_{}, bright_streak_{};
};
}
include/camera.hpp
+3 -1
View File
@@ -4,16 +4,18 @@
#include <memory>
#include <optional>
#include <string>
#include <nlohmann/json.hpp>
namespace cms {
struct CameraConfig {
std::string device{"/dev/video5"};
std::string video;
int width{1920},height{1536},fps{30},buffers{4},timeout_ms{2000};
};
struct CapturedFrame {
CaptureMetadata metadata;
std::shared_ptr<const BgrImage> image;
// Present only for offline replay; keeps original timestamps and context.
std::shared_ptr<const nlohmann::json> replay_record;
};
// The capture thread exclusively owns read(); requestStop() is thread safe.
class Camera {
include/camera_pipeline.hpp
+8 -6
View File
@@ -1,5 +1,6 @@
#pragma once
#include "camera.hpp"
#include "brightness_gate.hpp"
#include "frame_result.hpp"
#include "output_writer.hpp"
#include <filesystem>
@@ -7,17 +8,16 @@
namespace cms {
struct PipelineConfig {
static constexpr char const* session_variant="inference-only";
CameraConfig camera;
CanConfig can;
OutputOptions output;
std::filesystem::path engines,door_mask,door_zone_config;
bool can_enabled{true};
std::string can_side{"left"},active_door{"can"};
double can_timeout_ms{1000};
std::uint64_t max_frames{};
std::filesystem::path results_path,pose_records_path;
std::filesystem::path record_session,replay_session;
std::filesystem::path engines,door_mask;
double detector_confidence{0.30};
int max_people{3};
double warning_relative{0.30},release_relative{0.45};
BrightnessGateConfig brightness_gate;
std::size_t queue_capacity{2};
bool door_mask_calibration_verified{false};
};
@@ -39,6 +39,8 @@ public:
CameraPipeline& operator=(CameraPipeline const&)=delete;
void start();
FrameResultPtr latest() const;
CanSnapshot canStatus() const;
bool sendCan(CanFrame const& frame);
bool running() const noexcept;
void requestStop() noexcept;
void join();
include/can_bus.hpp
+37
View File
@@ -0,0 +1,37 @@
#pragma once
#include "can_types.hpp"
#include <memory>
#include <string_view>
namespace cms {
// One lifecycle per instance; only one owner per device index in a process.
// start() must run after StopSignals so the receiver inherits its signal mask.
// Missing hardware is reported by snapshot(), and does not stop inference.
class CanBus {
public:
explicit CanBus(CanConfig config);
~CanBus();
CanBus(CanBus const&)=delete;
CanBus& operator=(CanBus const&)=delete;
void start();
void requestStop() noexcept;
void join() noexcept;
CanSnapshot snapshot() const;
// Thread-safe on-demand data-frame send; invalid frames throw, SDK failure
// returns false. Never generates periodic or inference-triggered messages.
bool send(CanFrame const& frame);
// Send one standard 8-byte 0x1F6 frame on CH1. Both nibbles of
// data[0] contain 0 for "clear", "watch" and "unavailable", 1 for every
// other level: 0x00 or 0x11. Bytes 1-7 are zero. Returns {false, CH1 result}.
std::array<bool,2> sendRisk(std::string_view riskLevel);
// Nonblocking live-inference path. Attempts the initial binary risk once,
// then only encoded 0/1 changes. Latest pending state wins while busy;
// returning to the in-flight state cancels a redundant pending transition.
// True means accepted (including unchanged), not delivered. No periodic
// retransmission or retry of a failed state without a new binary change.
bool queueRisk(std::string_view riskLevel);
private:
struct Impl;
std::unique_ptr<Impl> impl_;
};
}
include/can_comm.h
-54
View File
@@ -1,54 +0,0 @@
// can_comm.h - USBCANFD CAN/CANFD 收发封装
//
// 整合自 vision-anti-pinch_bak/main.cpp 的 CAN 模块, 提供:
// can_start() - 打开设备, 初始化并启动双通道接收线程
// can_stop() - 停止接收并关闭设备
// can_send() - 发送一帧 CAN (8 字节)
// canfd_send() - 发送一帧 CANFD (64 字节)
//
// 依赖 zcan.h (include/) 和 libusbcanfd.so + libusb-1.0.so (lib/)
#pragma once
#include <cstdint>
#include <string>
namespace CanComm {
// 初始化并启动 CAN 收发(双通道, 仲裁域 500kbps / 数据域 2000kbps)。
// 返回 true 成功; false 失败(主流程可继续仅取图)。
bool can_start();
// 停止 CAN 接收线程并关闭设备。
void can_stop();
// 注册报文解析回调。收到每帧 CAN/CANFD 数据帧时调用。
// time: 时间戳(us)
// msg_id: CAN-ID
// data: 数据指针(回调内有效, 不要保存)
// len: 数据长度
// 传 nullptr 取消注册。
using MsgCallback = void (*)(int64_t time, uint32_t msg_id, const uint8_t *data, uint32_t len);
void set_msg_callback(MsgCallback cb);
// 发送一帧 CAN2.0 数据帧(8 字节)。
// chn_idx: 通道号 0/1
// id: CAN-ID
// data: 8 字节数据(不足补 0, 超出截断)
// 返回实际发送帧数(0 失败)。
uint32_t can_send(int chn_idx, uint32_t id, const uint8_t data[8]);
// 发送一帧 CANFD 数据帧(64 字节, 扩展帧+加速)。
// chn_idx: 通道号 0/1
// id: CAN-ID
// data: 64 字节数据(不足补 0, 超出截断)
// 返回实际发送帧数(0 失败)。
uint32_t canfd_send(int chn_idx, uint32_t id, const uint8_t data[64]);
// 是否已打开并运行
bool is_open();
// 累计接收帧数(线程安全)
int receive_count();
} // namespace CanComm
include/can_state.hpp
-22
View File
@@ -1,22 +0,0 @@
#pragma once
#include <array>
#include <string>
namespace cms {
struct CanSnapshot {
std::array<int,4> positions{{-1,-1,-1,-1}}; // LF, RF, LR, RR
double age_ms{-1};
bool valid{false};
std::string source{"hardware"},side{"left"};
};
class CanInput {
public:
explicit CanInput(bool enabled);
~CanInput();
CanInput(CanInput const&)=delete;
CanInput& operator=(CanInput const&)=delete;
CanSnapshot snapshot(std::string const& side,double timeoutMs) const;
private:
bool opened_{};
};
std::string activeDoor(CanSnapshot const& state);
}
include/can_types.hpp
+39
View File
@@ -0,0 +1,39 @@
#pragma once
#include <array>
#include <cstdint>
#include <string>
#include <vector>
namespace cms {
inline constexpr unsigned canReceiveChannel=0;
inline constexpr unsigned canTransmitChannel=1;
inline constexpr unsigned canHardwareChannelCount=2;
inline constexpr unsigned liveCanChannelCount=1; // Only CH0 contributes live receive data.
struct CanConfig {
bool enabled{true};
int device_index{0};
int timeout_ms{1000};
bool termination{true};
};
struct CanDoorSignals {
// Raw unsigned bytes: left front, right front, left rear, right rear.
// No scale/unit is specified in the reference protocol.
std::array<int,4> positions{{-1,-1,-1,-1}};
bool valid{},fd{},extended{};
std::uint64_t received{},received_monotonic_ns{};
std::uint32_t device_timestamp_us{};
double age_ms{-1};
};
struct CanSnapshot {
std::string source{"live"},status{"DISABLED"};
// Keep two slots for existing recorded sessions; live CH1 stays unset.
std::array<CanDoorSignals,2> channels;
std::uint64_t received{},transmitted{},transmit_errors{},receive_errors{};
};
struct CanFrame {
int channel{};
std::uint32_t id{};
bool fd{},extended{},bitrate_switch{};
std::vector<std::uint8_t> data;
};
}
include/door_mask_annotator.hpp
+22
View File
@@ -0,0 +1,22 @@
#pragma once
#include <opencv2/core.hpp>
#include <cstdint>
#include <filesystem>
#include <optional>
#include <string>
namespace cms {
struct DoorMaskAnnotationOptions {
std::filesystem::path output_json;
std::filesystem::path reference_image;
bool overwrite{};
std::optional<std::string> source_video;
std::optional<std::int64_t> source_frame_index;
};
// Returns false when the user cancels without writing any annotation files.
bool annotateDoorMask(cv::Mat const& image, DoorMaskAnnotationOptions const& options);
}
include/door_zone.hpp
-22
View File
@@ -1,22 +0,0 @@
#pragma once
#include "door_risk.hpp"
#include <deque>
#include <unordered_map>
namespace cms {
class DoorZoneAssigner {
public:
explicit DoorZoneAssigner(std::filesystem::path const& configuration);
Json update(int frame, Json const& record, cv::Size size);
private:
std::string classify(double probability) const;
Json features_;
double frontMin_{},rearMax_{},minimumHeight_{};
int minimumPoseFeatures_{},historyFrames_{},staleFrames_{};
std::unordered_map<int,std::deque<double>> history_;
std::unordered_map<int,int> lastSeen_;
};
struct DoorDecisions { Json doors; Json selected; };
DoorDecisions buildDoorRisks(Json const& raw, Json const& assignments,
std::string const& activeDoor, bool canValid);
}
include/frame_result.hpp
+3 -15
View File
@@ -1,8 +1,8 @@
#pragma once
// Public downstream contract. Owns its data; no GPU, JSON or network handles.
#include "image.hpp"
#include "can_state.hpp"
#include <map>
#include "can_types.hpp"
#include "brightness_gate.hpp"
#include <array>
#include <cstdint>
#include <memory>
@@ -24,7 +24,6 @@ struct RegionRisk {
std::string risk_level;
std::string decision;
std::string source;
std::string door_zone;
int track_id{};
std::string side;
std::optional<std::string> state;
@@ -34,8 +33,6 @@ struct RegionRisk {
std::optional<double> region_scale_px, distance_relative;
};
struct RiskResult {
std::string risk_type;
std::optional<std::string> door_id;
std::string status;
std::optional<double> risk_score;
std::string risk_level;
@@ -67,21 +64,11 @@ struct HandRegion {
struct StageTiming {
double detector_ms{}, pose_ms{}, fusion_risk_ms{}, capture_to_result_ms{};
};
struct DoorAssignment {
int track_id{}, person_index{}, pose_feature_count{}, history_frames{};
std::string door_zone, raw_door_zone, evidence_source;
double front_probability{}, smoothed_front_probability{}, confidence{};
std::map<std::string,double> features, feature_frontness;
};
struct FrameResult {
int frame_index{};
std::vector<int> track_ids;
std::vector<std::array<double,4>> boxes;
RiskResult risk;
RiskResult ungated_risk;
std::map<std::string,RiskResult> door_risks;
std::vector<DoorAssignment> door_assignments;
std::string active_door;
CanSnapshot can;
CaptureMetadata capture;
StageTiming timing;
@@ -91,6 +78,7 @@ struct FrameResult {
std::shared_ptr<const BgrImage> image;
std::shared_ptr<const BinaryMask> door_mask_pixels;
bool door_mask_calibration_verified{};
BrightnessDecision brightness;
};
using FrameResultPtr = std::shared_ptr<const FrameResult>;
}
include/inference_session.hpp
+38
View File
@@ -0,0 +1,38 @@
#pragma once
#include "camera.hpp"
#include "frame_result.hpp"
#include <nlohmann/json.hpp>
#include <filesystem>
#include <fstream>
#include <opencv2/videoio.hpp>
namespace cms {
struct PipelineConfig;
// H.264/MP4 plus a matching frame index and per-frame inference records.
// finish() flushes the encoder and marks the session replayable.
class SessionWriter {
public:
explicit SessionWriter(PipelineConfig const& config);
void write(FrameResult const& result, nlohmann::json const& stages,
nlohmann::json const* source = nullptr);
void finish();
private:
std::filesystem::path directory_;
std::ofstream index_;
cv::VideoWriter video_;
int width_{},height_{};
std::uint64_t count_{};
bool finished_{};
};
class SessionReader {
public:
explicit SessionReader(PipelineConfig& config);
std::optional<CapturedFrame> read();
private:
std::filesystem::path directory_;
std::ifstream index_;
cv::VideoCapture video_;
int width_{},height_{};
std::uint64_t count_{},lastFrame_{},expectedCount_{};
};
}
include/result_conversion.hpp
+26 -23
View File
@@ -2,30 +2,33 @@
#include "frame_result.hpp"
#include "door_risk.hpp"
namespace cms {
template<class T> inline std::optional<T> optionalField(Json const& j,char const* key) {
auto it=j.find(key);return it==j.end()||it->is_null()?std::nullopt:std::optional<T>(it->get<T>());
template<class T> inline std::optional<T> optionalField(Json const& value,char const* key) {
auto it=value.find(key);return it==value.end()||it->is_null()?std::nullopt:std::optional<T>(it->get<T>());
}
inline RiskResult parseRisk(Json const& j) {
RiskResult r;r.status=j.at("status");r.risk_score=optionalField<double>(j,"risk_score");r.risk_level=j.at("risk_level");r.decision=j.at("decision");
r.risk_type=j.value("risk_type",std::string());r.door_id=optionalField<std::string>(j,"door_id");
r.source=optionalField<std::string>(j,"source");r.track_id=optionalField<int>(j,"track_id");r.side=optionalField<std::string>(j,"side");
if(j.contains("door_mask")) {auto const& d=j.at("door_mask");r.door_mask={d.at("status"),d.at("annotation_index"),optionalField<std::string>(d,"path"),d.at("format"),d.at("area_px")};}
r.warning_distance_relative=j.value("warning_distance_relative",.30);r.release_distance_relative=j.value("release_distance_relative",.45);
for(auto const& x:j.at("regions")) {
RegionRisk v;v.status=x.at("status");v.risk_score=optionalField<double>(x,"risk_score");v.risk_level=x.at("risk_level");v.decision=x.at("decision");v.source=x.at("source");
v.track_id=x.at("track_id");v.side=x.at("side");v.state=optionalField<std::string>(x,"state");v.door_zone=x.value("door_zone",std::string());
v.region_area_px=optionalField<int>(x,"region_area_px");v.door_overlap_px=optionalField<int>(x,"door_overlap_px");
v.region_overlap_ratio=optionalField<double>(x,"region_overlap_ratio");v.min_distance_px=optionalField<double>(x,"min_distance_px");
v.region_scale_px=optionalField<double>(x,"region_scale_px");v.distance_relative=optionalField<double>(x,"distance_relative");r.regions.push_back(std::move(v));
inline FrameResult makeFrameResult(Json const& record,Json const& risk,
CaptureMetadata capture) {
FrameResult out;
out.frame_index=record.at("frame_index").get<int>();
out.track_ids=record.at("track_ids").get<std::vector<int>>();
out.boxes=record.at("boxes").get<std::vector<std::array<double,4>>>();
out.capture=capture;
auto& r=out.risk;
r.status=risk.at("status");r.risk_score=optionalField<double>(risk,"risk_score");
r.risk_level=risk.at("risk_level");r.decision=risk.at("decision");
r.source=optionalField<std::string>(risk,"source");r.track_id=optionalField<int>(risk,"track_id");r.side=optionalField<std::string>(risk,"side");
auto const& d=risk.at("door_mask");
r.door_mask={d.at("status"),d.at("annotation_index"),optionalField<std::string>(d,"path"),d.at("format"),d.at("area_px")};
r.warning_distance_relative=risk.at("warning_distance_relative");r.release_distance_relative=risk.at("release_distance_relative");
for(auto const& j:risk.at("regions")) {
RegionRisk v;
v.status=j.at("status");v.risk_score=optionalField<double>(j,"risk_score");
v.risk_level=j.at("risk_level");v.decision=j.at("decision");v.source=j.at("source");
v.track_id=j.at("track_id");v.side=j.at("side");v.state=optionalField<std::string>(j,"state");
v.region_area_px=optionalField<int>(j,"region_area_px");v.door_overlap_px=optionalField<int>(j,"door_overlap_px");
v.region_overlap_ratio=optionalField<double>(j,"region_overlap_ratio");v.min_distance_px=optionalField<double>(j,"min_distance_px");
v.region_scale_px=optionalField<double>(j,"region_scale_px");v.distance_relative=optionalField<double>(j,"distance_relative");
r.regions.push_back(std::move(v));
}
return r;
}
inline DoorAssignment parseAssignment(Json const& j) {
DoorAssignment a;a.track_id=j.at("track_id");a.person_index=j.at("person_index");a.door_zone=j.at("door_zone");a.raw_door_zone=j.at("raw_door_zone");
a.front_probability=j.at("front_probability");a.smoothed_front_probability=j.at("smoothed_front_probability");a.confidence=j.at("confidence");a.evidence_source=j.at("evidence_source");
a.pose_feature_count=j.at("pose_feature_count");a.history_frames=j.at("history_frames");a.features=j.at("features").get<std::map<std::string,double>>();a.feature_frontness=j.at("feature_frontness").get<std::map<std::string,double>>();return a;
}
inline FrameResult makeFrameResult(Json const& record,Json const& raw,CaptureMetadata capture) {
FrameResult r;r.frame_index=record.at("frame_index");r.track_ids=record.at("track_ids").get<std::vector<int>>();r.boxes=record.at("boxes").get<std::vector<std::array<double,4>>>();r.capture=capture;r.ungated_risk=parseRisk(raw);return r;
return out;
}
}
include/serialization.hpp
+2
View File
@@ -2,5 +2,7 @@
#include "frame_result.hpp"
#include "door_risk.hpp"
namespace cms {
Json serializeCan(CanSnapshot const& can);
CanSnapshot deserializeCan(Json const& value);
Json serializeResult(FrameResult const& result);
}
include/v4l2_capture.h
-93
View File
@@ -1,93 +0,0 @@
#ifndef V4L2_CAPTURE_H
#define V4L2_CAPTURE_H
#include <cstdint>
#include <stddef.h>
#include <string>
#include <vector>
// V4L2 流式(MMAP)取图封装
// 参考 quick_bring_up_64g.sh 中 v4l2src 的行为:
// - 打开 /dev/videoN
// - 协商 UYVY 像素格式与分辨率
// - 设置相机控制(sensor_mode / trig_pin 等, 等价于 v4l2-ctl -c ...)
// - MMAP 方式申请内核缓冲区并流式采集
class V4l2Capture {
public:
// 单帧采集结果
struct Frame {
void *data = nullptr; // 指向 MMAP 缓冲区, 释放前有效
size_t size = 0; // 实际帧字节数(bytesused)
uint32_t width = 0;
uint32_t height = 0;
uint32_t pixelformat = 0; // V4L2_PIX_FMT_* (如 V4L2_PIX_FMT_UYVY)
uint32_t bytes_per_line = 0;
uint32_t sequence = 0; // 驱动序号
uint64_t timestamp_us = 0; // 单调时钟时间戳(us)
int index = -1; // 内部缓冲区索引
};
V4l2Capture() = default;
~V4l2Capture();
V4l2Capture(const V4l2Capture &) = delete;
V4l2Capture &operator=(const V4l2Capture &) = delete;
// 打开设备节点, 例如 /dev/video5
bool open(const std::string &device);
// 关闭设备并释放所有资源
void close();
bool isOpen() const { return fd_ >= 0; }
bool needsReconnect() const { return reconnect_needed_; }
// 协商像素格式与分辨率. pixelformat 使用 V4L2_PIX_FMT_* 四字符码
bool setFormat(uint32_t width, uint32_t height, uint32_t pixelformat);
// 设置帧率(通过 VIDIOC_S_PARM). fps 分子/分母均为 fps/1
bool setFps(uint32_t fps);
// 设置 V4L2 控制, 等价 v4l2-ctl -c <name>=<value>. id 为 V4L2_CID_* 之四字符码
// 例如 sensor_mode=0x009a2008, trig_pin=0x009a2084
bool setControl(uint32_t id, int32_t value);
// 申请 MMAP 缓冲区, 默认 4 个
bool initMmap(uint32_t buffer_count = 4);
// 开始 / 停止采集
bool startStreaming();
bool stopStreaming();
// 阻塞取一帧, 超时 timeout_ms 毫秒. 成功返回 true, 需配对调用 releaseFrame()
bool getFrame(Frame &frame, int timeout_ms = 1000);
// 归还缓冲区给驱动队列, 必须在下次 getFrame 前调用
void releaseFrame(const Frame &frame);
uint32_t width() const { return width_; }
uint32_t height() const { return height_; }
uint32_t pixelformat() const { return pixelformat_; }
private:
struct MmapBuffer {
void *start = nullptr;
size_t length = 0;
bool queued = false; // 是否在驱动队列中
};
// 归还指定索引的缓冲区 (内部使用)
void releaseFrameByIndex(int idx);
int fd_ = -1;
uint32_t width_ = 0;
uint32_t height_ = 0;
uint32_t pixelformat_ = 0;
uint32_t bytes_per_line_ = 0;
bool streaming_ = false;
bool reconnect_needed_ = false;
std::vector<MmapBuffer> buffers_;
int last_dequeued_ = -1; // 当前已出队但未归还的缓冲区索引
};
#endif // V4L2_CAPTURE_H
include/visualizer.hpp
+4 -2
View File
@@ -5,7 +5,9 @@
namespace cms {
class CameraPipeline;
// Pure drawing from the immutable result. Never re-runs inference.
cv::Mat renderResult(FrameResult const& result);
cv::Mat renderResult(FrameResult const& result,CanSnapshot const* liveCan=nullptr,bool drawText=true);
cv::Mat renderVisualization(FrameResult const& result,CanSnapshot const& liveCan);
void renderCanOverlay(cv::Mat& image,CanSnapshot const& can);
// Optional observer: closing the window returns without stopping the pipeline.
void runLocalVisualization(CameraPipeline const& pipeline);
void runLocalVisualization(CameraPipeline const& pipeline, bool keepFinalFrame = false);
}
orin/README.md
+52 -12
View File
@@ -1,28 +1,48 @@
# Orin 纯 C++ 运行包
# Orin 可直接运行目录
相机每次等待默认 2 秒，连续 6 次超时才退出（约 12 秒）；有效帧恢复后重新计数。Ctrl+C / SIGTERM 主动停止不需要等待 6 次超时。
适用 AGX Orin / JetPack 5.1.2 (R35.4.1)、CUDA 11.4、TensorRT 8.5.x、OpenCV 4.5.4。不依赖 Python。
本目录包含实际在 Jetson AGX Orin 上编译的 aarch64 程序和 TensorRT FP16 引擎。
目标环境：L4T R35.4.1 / JetPack 5.1.2，CUDA 11.4，TensorRT 8.5.2，OpenCV 4.5.4。
使用系统安装的 CUDA、TensorRT、OpenCV 共享库；不适用于任意 JetPack 版本或 x86 主机。
```bash
cd orin
sha256sum -c SHA256SUMS
./scripts/initialize_camera.sh # 开机后无 /dev/video5 时执行，需要 sudo
./run.sh # exec build/camera_risk，默认无可视化
./run.sh --visualize # 可选 Orin 本地图形窗口
# 直接执行 ELF：
./build/camera_risk --config config/camera.json
./scripts/initialize_camera.sh
./run.sh
```
只运行一个摄像头进程。Ctrl+C / SIGTERM 停止；关闭可视化窗口不停止推理。无图形桌面时继续无窗口运行。
实时推理、录制和回灌默认开启可视化，显示门区、检测框、手臂/手部关键点及风险信息。使用 `./run.sh --no-visualize` 关闭；`--visualize` 仍可显式开启。需要板端桌面 DISPLAY / WAYLAND_DISPLAY，没有显示环境时自动跳过窗口并继续推理。
默认持续推理直到 Ctrl+C / SIGTERM；正常退出没有统计输出。
预览窗口关闭后推理继续。结果通过 C++ CameraPipeline::latest() 获取，不通过端口或结果文件传递。
CAN 0x391 data[0..3] 为左前、右前、左后、右后。默认左侧，`--can-side right` 切换右侧。缺失或超时报文输出 CAN_UNKNOWN，风险分数为空。
程序为 build/camera_risk，配置为 config/camera.json，引擎为 engines/*.engine。
./run.sh 从所在目录解析路径，也可以在其他工作目录通过绝对路径调用。
`--no-can --active-door both` 模拟门状态；`--duration 30` 限时运行；`--save-results FILE.jsonl` 显式保存结果，默认不写文件。完整参数见 `./run.sh --help`。
bin/drivers 中 GMSL 模块对应内核 5.10.120-tegra。初始化脚本检查内核版本；加载模块需要 root 或现有 sudo 权限。
门区保留当前室内联调设置，door.calibration_verified=false。
最新结果保存在进程内的 `shared_ptr<const cms::FrameResult>`；C++ 下游调用 `pipeline.latest()` 读取。接口头文件位于仓库 include/，不是 Python 变量或跨进程共享内存。
## Door mask 标注工具
模型、驱动和编译产物绑定目标 GPU、TensorRT 与内核环境。标定文件需匹配实际机位，默认 calibration_verified=false。系统 CUDA/TensorRT/OpenCV 库由 JetPack 提供。
停止正在占用摄像头的推理程序后运行：
```bash
./annotate_door_mask.sh --camera --output config/door_masks/driver-side.json
```
左键添加多边形顶点，右键或 `U` 撤销，`R` 清空，`S` 或 Enter 保存，Esc 或 `Q` 取消。工具输出 x-anylabeling JSON 及同目录参考 JPEG。
已有图像和视频模式不会打开摄像头：
```bash
./annotate_door_mask.sh --image /path/to/frame.jpg --output config/door_masks/driver-side.json
./annotate_door_mask.sh --video /path/to/video.mp4 --output config/door_masks/driver-side.json
./annotate_door_mask.sh --video /path/to/video.mp4 --frame 244 --output config/door_masks/driver-side.json
```
视频未指定 `--frame` 时先通过滑块或 `A/D`、`J/L` 选择参考帧；帧号从 0 开始。
## 可选保存（默认全部关闭）
@@ -41,3 +61,23 @@ CAN 0x391 data[0..3] 为左前、右前、左后、右后。默认左侧，`--ca
同一帧三类文件同名（扩展名不同），文件名包含运行标识和帧号，多次运行不覆盖。JSON 的 `images` 提供相对于 `output/` 的图片路径，未选择保存的图片为 null。源码入口写入工程 `output/`；独立包入口写入 `orin/output/`。默认不创建目录、不保存文件。
保存对象是完成推理的帧；惰性分支暂停时不新增文件。开启保存会增加编码和磁盘 I/O 开销。写入失败时报告错误并停止，不静默丢失文件。
## H.264 推理过程录制与回灌
```bash
./run.sh --record-session output/sessions/drive-001
# Ctrl+C 正常停止，完成 MP4 封装后再回灌
./run.sh --replay-session output/sessions/drive-001 --save-results
```
录制目录必须不存在。会话保存一份 H.264 视频、逐帧过程 JSON、推理配置与门区副本；回灌解码视频并重新执行 TensorRT 推理，不打开摄像头，结束后自动退出。使用当前配置指向的引擎；有损压缩会引起推理结果差异。需要 OpenCV FFmpeg 的 H.264 编码支持。强制退出或断电导致未封装完成的会话不可回灌；正常 Ctrl+C / SIGTERM 会完成封装。详细格式与限制见工程 README。
## CAN 状态与发送接口
实时运行默认从 USBCANFD CH0 接收、从 CH1 发送（两个通道均为 500 kbps / 2 Mbps）。预览显示 `0x391` 的 LF/RF/LR/RR 四门实际位置反馈原始值，仅显示 CH0，并标记新鲜度和收发计数。`--no-can` 可关闭；设备缺失不停止推理。`config/camera.json` 的 `can.termination` 同时控制两个通道的终端电阻，默认沿用参考工程的开启设置。
`lib/` 包含 SDK 与 libusb 及其 SONAME 链接。回灌使用录制的 CAN 快照，不访问总线；旧会话显示 NOT_RECORDED。按需发送通过 C++ `CameraPipeline::sendCan()`，参数与完整示例见工程 README。
首个实时推理风险状态发送一次，此后仅在二值风险变化时，由后台线程在 CH1 发送一帧标准 CAN `0x1F6`，长度 8 字节：首字节高、低四位同时填写相同风险值，`clear`、`watch` 和 `unavailable` 时各为 0，`warning`、`danger` 及未知状态各为 1，即首字节为 `0x00` 或 `0x11`，后七字节清零。CH0 仅接收，CH1 仅发送。发送等待不阻塞推理，最多保留一个待发结果，忙时由最新结果替换；发送失败计入 TX 错误。仅在报文编码 0 与 1 之间变化时触发发送，相同编码的等级切换不重发。状态不变及发送失败时均不自动重试，等待新的二值变化。等待期间回到正在发送的状态会取消冗余待发报文。`--no-can` 和视频回灌禁用发送。停止时清空待发结果，等待正在执行的 SDK 调用结束后释放设备。
USB-CAN 首次接入如出现权限不足，运行 `./scripts/enable_can_access.sh`（需要 sudo，运行用户属于 plugdev），仅为 `3068:0009` 适配器安装设备访问规则。
orin/SHA256SUMS
+14 -10
View File
@@ -1,15 +1,19 @@
836d9b85249866642f7f3ca7cfe4eec092f29fd3b44bd9fdbe1c2ba3ca3d68b0 ./README.md
9ba768d559fd3b64e833b50c62c70f2adfd6c169620aea5fba32cfb1b4c5b0e9 ./annotations/constant_door_mask.json
3544fb1c7b1b27b39db27d8f849d5e8652f5d09a950a59c5dd0c7703086b4f6b ./annotations/door_zone_multifeature.json
6e54ad1474a8a00a93bb5e1ffe9362eb31beab41a5b9c4b2e17baa5051932e27 ./annotate_door_mask.sh
6d5e874fd27fcfd75d303d7e35d9672028af71041f52dcc82a1e83676faefa65 ./bin/drivers/max9295.ko
8a78d04ef8be44e9b62a5e4ee315d3d6d679e34aedff02a234bbf4d40e222933 ./bin/drivers/max9296.ko
3da628587477cc35d5e3f8aee86e718e2ddf16808e893e00ba6e164b2307e843 ./bin/drivers/sgx-yuv-gmsl2.ko
a2616e193ef9f006b49b4bb4ac43ae3ffc4cee1f1edf6d58ed6f7f9e5f933ad1 ./build/camera_risk
718c43eba7c9a86178b272404b14e201445a3998dae81cd32b840f83018ba2b5 ./config/camera.json
cc34c1e0281c27b380a9a90bd58cfa65fefa322612be0de42a7faa86e15ef39e ./build/camera_risk
12ebf1d21a967cc4d7620636352fc5779a697a0fb9feaa8bdb339927292a3517 ./build/door_mask_tool
e1ac783278a1dc4ebeb15f48072a865a51dc44bc31dcb42b4a8aa96b8bdc067e ./config/camera.json
7808f4ae27de49a02fe51f8e6b60ae8454ff775ea3e98fa0a7c633ee85027fb4 ./config/door_mask.json
ebcd09e8c2e35008adbb0c1d222e580b17e0df1c8093df3481ba3374cd9a8174 ./engines/rtmw_l_384.engine
f6a65b926ac1144484f4aa93cd26d7997d32d62310ab93fe867f2bd4412222e9 ./engines/yolo11s.engine
8dae8c557deadbe6bd7241c0dcd7025cb007077ee39a79b115e953a2b22cd9c1 ./lib/libusb-1.0.so
8dae8c557deadbe6bd7241c0dcd7025cb007077ee39a79b115e953a2b22cd9c1 ./lib/libusb-1.0.so.0
ab8b7f7249d5fb73d1d33d14fa7532a522088ffb6c25f37ba334daf4c6b4d313 ./lib/libusbcanfd.so
ebcd09e8c2e35008adbb0c1d222e580b17e0df1c8093df3481ba3374cd9a8174 ./models/engines/rtmw_l_384.engine
f6a65b926ac1144484f4aa93cd26d7997d32d62310ab93fe867f2bd4412222e9 ./models/engines/yolo11s.engine
4e3bd58314a9d70b6b243f14c0e26ffd1b1c6dcf8bcbf4c44629f1767f72a5ec ./run.sh
11b5fbc3c17c0610ec0514a0a26cf5d92588d15c0e50e7086d9db5a5c0306e99 ./scripts/initialize_camera.sh
bce5a22d49633454ed2c6bc92b608edf3b718af2424c283ed8f56ba93987200a ./scripts/initialize_camera_impl.sh
ab8b7f7249d5fb73d1d33d14fa7532a522088ffb6c25f37ba334daf4c6b4d313 ./lib/libusbcanfd.so.1.0.12
73e6be03ec983fe6c14b0923c69cad7ec816ed4260257e6aecaf7fe341686cfc ./README.md
7f42b0e7f9396e0d82313a892fc62c153261dab1206a206a466bce6376f58a36 ./run.sh
41ebe2dcf57674232ca137d024e99447b467049cb1c63a284daae274296cb78c ./scripts/99-usbcanfd.rules
920102fc5e91bde9b78f6f7c5cb2ff8761a0c39b63c7a28b3347d5de5fe7c60a ./scripts/enable_can_access.sh
54e1d63e3aa72ad9b056a224d55968d4947ca6703a8d1c405570de5707a7e7ef ./scripts/initialize_camera.sh
orin/annotate_door_mask.sh
Executable
+4
View File
@@ -0,0 +1,4 @@
#!/usr/bin/env bash
set -euo pipefail
root="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" && pwd)"
exec "$root/build/door_mask_tool" --config "$root/config/camera.json" "$@"
orin/annotations/constant_door_mask.json
-77
View File
@@ -1,77 +0,0 @@
{
"version": "x-anylabeling",
"flags": {},
"shapes": [
{
"label": "door",
"points": [
[
212.86,
145.919
],
[
378.891,
851.857
],
[
468.293,
996.446
],
[
536.408,
1294.131
],
[
553.437,
1404.7
],
[
561.951,
1532.279
],
[
51.086,
1532.279
],
[
8.514,
1515.268
],
[
0.0,
1468.489
],
[
8.514,
26.845
],
[
25.543,
5.582
],
[
131.973,
-2.924
],
[
191.574,
1.329
],
[
208.603,
145.919
]
],
"group_id": null,
"shape_type": "polygon",
"flags": {}
}
],
"imagePath": "constant_door_mask_reference.jpg",
"imageData": null,
"imageHeight": 1536,
"imageWidth": 1920,
"sourceVideo": "D:\\huang.xuewei\\CMS\\video\\20260921104502\\segment_009.mp4",
"sourceFrameIndex": 244,
"constantDoorMask": true
}
orin/annotations/door_zone_multifeature.json
-47
View File
@@ -1,47 +0,0 @@
{
"version": "v9.10-research-calibration-1",
"description": "Temporary-camera calibration from seg_01 and seg_08; recalibrate for the production camera.",
"front_probability_min": 0.72,
"rear_probability_max": 0.28,
"minimum_person_height": 0.4,
"minimum_pose_features": 4,
"history_frames": 5,
"stale_track_frames": 120,
"features": {
"shoulder_center_x": {
"rear": 0.53,
"front": 0.64,
"weight": 1.2
},
"shoulder_center_y": {
"rear": 0.54,
"front": 0.46,
"weight": 1.3
},
"shoulder_width": {
"rear": 0.14,
"front": 0.2,
"weight": 1.4
},
"torso_height": {
"rear": 0.35,
"front": 0.47,
"weight": 1.5
},
"hip_center_y": {
"rear": 0.89,
"front": 0.93,
"weight": 0.6
},
"box_center_x": {
"rear": 0.48,
"front": 0.58,
"weight": 0.35
},
"box_height": {
"rear": 0.7,
"front": 0.82,
"weight": 0.45
}
}
}
orin/build/camera_risk
BIN
View File
Binary file not shown.
orin/build/door_mask_tool
Executable
BIN
View File
Binary file not shown.
orin/config/camera.json
+40 -4
View File
@@ -1,6 +1,42 @@
{
"camera": {"device":"/dev/video5","width":1920,"height":1536,"fps":30,"buffers":4,"timeout_ms":2000},
"inference": {"engine_directory":"../models/engines","detector_confidence":0.30,"max_people":3,"queue_capacity":2},
"door": {"mask":"../annotations/constant_door_mask.json","zone_config":"../annotations/door_zone_multifeature.json","warning_relative":0.30,"release_relative":0.45,"calibration_verified":false},
"can": {"enabled":true,"side":"left","timeout_ms":1000}
"camera": {
"device": "/dev/video5",
"width": 1920,
"height": 1536,
"fps": 30,
"buffers": 4,
"timeout_ms": 2000
},
"inference": {
"engine_directory": "../engines",
"detector_confidence": 0.3,
"max_people": 3,
"queue_capacity": 2
},
"door": {
"mask": "door_mask.json",
"warning_relative": 0.3,
"release_relative": 0.45,
"calibration_verified": false
},
"brightness_gate": {
"enabled": true,
"roi": [0.02, 0.05, 0.88, 0.98],
"dark_pixel_luma": 40.0,
"enter_median_luma": 18.0,
"enter_dark_ratio": 0.85,
"enter_dark_tile_ratio": 0.75,
"exit_median_luma": 24.0,
"exit_dark_ratio": 0.80,
"exit_dark_tile_ratio": 0.70,
"startup_frames": 5,
"enter_frames": 15,
"exit_frames": 15
},
"can": {
"enabled": true,
"device_index": 0,
"timeout_ms": 1000,
"termination": true
}
}
orin/config/door_mask.json
+55
View File
@@ -0,0 +1,55 @@
{
"version": "x-anylabeling",
"flags": {},
"shapes": [
{
"label": "door",
"points": [
[
1426,
0
],
[
1257,
489
],
[
1195,
615
],
[
1155,
636
],
[
1108,
813
],
[
1106,
1097
],
[
1097,
1529
],
[
1917,
1535
],
[
1915,
9
]
],
"group_id": null,
"shape_type": "polygon",
"flags": {}
}
],
"imagePath": "driver-side1_reference.jpg",
"imageData": null,
"imageHeight": 1536,
"imageWidth": 1920,
"constantDoorMask": true
}
orin/models/engines/rtmw_l_384.engine → orin/engines/rtmw_l_384.engine
View File
orin/models/engines/yolo11s.engine → orin/engines/yolo11s.engine
View File
orin/run.sh
+1 -2
View File
@@ -1,5 +1,4 @@
#!/usr/bin/env bash
set -euo pipefail
root="$(cd "$(dirname "$0")" && pwd)"
export LD_LIBRARY_PATH="$root/lib${LD_LIBRARY_PATH:+:$LD_LIBRARY_PATH}"
root="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" && pwd)"
exec "$root/build/camera_risk" --config "$root/config/camera.json" "$@"
orin/scripts/99-usbcanfd.rules
+2
View File
@@ -0,0 +1,2 @@
# USBCANFD adapter used by the reference project. Grant the plugdev group access.
SUBSYSTEM=="usb", ATTR{idVendor}=="3068", ATTR{idProduct}=="0009", MODE="0660", GROUP="plugdev", TAG+="uaccess"
orin/scripts/enable_can_access.sh
Executable
+10
View File
@@ -0,0 +1,10 @@
#!/usr/bin/env bash
set -euo pipefail
root="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" && pwd)"
root_command() { if [[ $EUID -eq 0 ]]; then "$@"; else sudo -n "$@"; fi; }
getent group plugdev >/dev/null || { echo 'Create a plugdev group and add the runtime user before running this script.' >&2; exit 1; }
root_command install -m 0644 "$root/99-usbcanfd.rules" /etc/udev/rules.d/99-usbcanfd.rules
root_command udevadm control --reload-rules
root_command udevadm trigger --subsystem-match=usb --attr-match=idVendor=3068 --attr-match=idProduct=0009 --action=add
root_command udevadm settle
printf '%s\n' 'USBCANFD 3068:0009 access enabled for plugdev. Newly added group members must log in again.'
orin/scripts/initialize_camera.sh
+19 -3
View File
@@ -1,5 +1,21 @@
#!/usr/bin/env bash
set -euo pipefail
root="$(cd "$(dirname "$0")/.." && pwd)"
export DRIVER_DIR="$root/bin/drivers"
exec "$root/scripts/initialize_camera_impl.sh" "$@"
root="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")/.." && pwd)"
device=/dev/video5
loaded() { test -d "/sys/module/$1"; }
root_command() { if [[ $EUID -eq 0 ]]; then "$@"; else sudo -n "$@"; fi; }
if [[ -c "$device" ]] && loaded max9295 && loaded max9296 && loaded sgx_yuv_gmsl2; then
echo "Camera already initialized: $device"
exit 0
fi
for driver in max9295 max9296 sgx-yuv-gmsl2; do
file="$root/bin/drivers/$driver.ko"
[[ -f "$file" ]] || { echo "Missing driver: $file" >&2; exit 1; }
[[ "$(modinfo -F vermagic "$file")" == "$(uname -r) "* ]] || { echo "Kernel mismatch: $file" >&2; exit 1; }
done
loaded max9295 || root_command insmod "$root/bin/drivers/max9295.ko"
loaded max9296 || root_command insmod "$root/bin/drivers/max9296.ko"
loaded sgx_yuv_gmsl2 || root_command insmod "$root/bin/drivers/sgx-yuv-gmsl2.ko" gmsl_mode=2,2,2,2
for _ in {1..20}; do [[ -c "$device" ]] && break; sleep 0.25; done
[[ -c "$device" ]] || { echo "$device was not created; verify camera/DTB" >&2; exit 1; }
v4l2-ctl -d "$device" -c sensor_mode=1,trig_pin=0xffff0007
orin/scripts/initialize_camera_impl.sh
-27
View File
@@ -1,27 +0,0 @@
#!/usr/bin/env bash
# Initialize the camera using the packaged Orin drivers.
set -euo pipefail
root="$(cd "$(dirname "$0")/.." && pwd)"
driver_dir="${DRIVER_DIR:-$root/orin/bin/drivers}"
device="${1:-/dev/video5}"
kernel="$(uname -r)"
for driver in max9295 max9296 sgx-yuv-gmsl2; do
module="${driver//-/_}"
[[ -d "/sys/module/$module" ]] && continue
file="$driver_dir/$driver.ko"
[[ -f "$file" ]] || { echo "Missing driver: $file" >&2; exit 1; }
vermagic="$(modinfo -F vermagic "$file")"
if [[ -n "$vermagic" && "${vermagic%% *}" != "$kernel" ]]; then
echo "Kernel mismatch: $file ($vermagic), running $kernel" >&2; exit 1
fi
if [[ -z "$vermagic" ]]; then
echo "No vermagic metadata: $file; using normal insmod kernel validation."
fi
options=()
[[ "$driver" == sgx-yuv-gmsl2 ]] && options=(gmsl_mode=2,2,2,2)
sudo -n insmod "$file" "${options[@]}"
done
for _ in {1..20}; do [[ -c "$device" ]] && break; sleep .25; done
[[ -c "$device" ]] || { echo "Camera not created: $device" >&2; exit 1; }
v4l2-ctl -d "$device" -c sensor_mode=1,trig_pin=0xffff0007
echo "Camera ready: $device"
run_camera.sh
+1 -2
View File
@@ -1,5 +1,4 @@
#!/usr/bin/env bash
set -euo pipefail
root="$(cd "$(dirname "$0")" && pwd)"
export LD_LIBRARY_PATH="$root/orin/lib${LD_LIBRARY_PATH:+:$LD_LIBRARY_PATH}"
root="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" && pwd)"
exec "$root/build/camera_risk" --config "$root/config/camera.json" "$@"
scripts/build.sh
+2 -2
View File
@@ -1,7 +1,7 @@
#!/usr/bin/env bash
set -euo pipefail
root="$(cd "$(dirname "$0")/.." && pwd)"
root="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")/.." && pwd)"
cmake_bin="${CMAKE_COMMAND:-cmake}"
if [[ -x "$HOME/miniconda3/envs/yolo/bin/cmake" ]]; then cmake_bin="${CMAKE_COMMAND:-$HOME/miniconda3/envs/yolo/bin/cmake}"; fi
"$cmake_bin" -S "$root" -B "$root/build" -DCMAKE_BUILD_TYPE=Release -DCMAKE_CUDA_COMPILER="${CUDACXX:-/usr/local/cuda/bin/nvcc}"
"$cmake_bin" -S "$root" -B "$root/build" -DCMAKE_BUILD_TYPE=Release -DCMAKE_CUDA_COMPILER="${CUDACXX:-/usr/local/cuda/bin/nvcc}" "$@"
"$cmake_bin" --build "$root/build" -j4
scripts/enable_can_access.sh
Executable
+4
View File
@@ -0,0 +1,4 @@
#!/usr/bin/env bash
set -euo pipefail
root="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")/.." && pwd)"
exec "$root/orin/scripts/enable_can_access.sh" "$@"
scripts/initialize_camera.sh
+2 -25
View File
@@ -1,27 +1,4 @@
#!/usr/bin/env bash
# Initialize the camera using the packaged Orin drivers.
set -euo pipefail
root="$(cd "$(dirname "$0")/.." && pwd)"
driver_dir="${DRIVER_DIR:-$root/orin/bin/drivers}"
device="${1:-/dev/video5}"
kernel="$(uname -r)"
for driver in max9295 max9296 sgx-yuv-gmsl2; do
module="${driver//-/_}"
[[ -d "/sys/module/$module" ]] && continue
file="$driver_dir/$driver.ko"
[[ -f "$file" ]] || { echo "Missing driver: $file" >&2; exit 1; }
vermagic="$(modinfo -F vermagic "$file")"
if [[ -n "$vermagic" && "${vermagic%% *}" != "$kernel" ]]; then
echo "Kernel mismatch: $file ($vermagic), running $kernel" >&2; exit 1
fi
if [[ -z "$vermagic" ]]; then
echo "No vermagic metadata: $file; using normal insmod kernel validation."
fi
options=()
[[ "$driver" == sgx-yuv-gmsl2 ]] && options=(gmsl_mode=2,2,2,2)
sudo -n insmod "$file" "${options[@]}"
done
for _ in {1..20}; do [[ -c "$device" ]] && break; sleep .25; done
[[ -c "$device" ]] || { echo "Camera not created: $device" >&2; exit 1; }
v4l2-ctl -d "$device" -c sensor_mode=1,trig_pin=0xffff0007
echo "Camera ready: $device"
root="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")/.." && pwd)"
exec "$root/orin/scripts/initialize_camera.sh" "$@"
scripts/package_orin.sh
+8 -12
View File
@@ -1,18 +1,14 @@
#!/usr/bin/env bash
# Assemble the C++ runtime; no Python interpreter is used for packaging or execution.
set -euo pipefail
root="$(cd "$(dirname "$0")/.." && pwd)"
package="$root/orin"
[[ -x "$root/build/camera_risk" ]] || { echo 'Compile on Orin with scripts/build.sh first.' >&2; exit 1; }
mkdir -p "$package/build" "$package/config" "$package/annotations" "$package/scripts"
cp -p "$root/build/camera_risk" "$package/build/"
cp -p "$root/annotations/"*.json "$package/annotations/"
# Source and standalone configurations differ only in the engine directory.
sed 's|../orin/models/engines|../models/engines|' "$root/config/camera.json" > "$package/config/camera.json"
cp -p "$root/scripts/initialize_camera.sh" "$package/scripts/initialize_camera_impl.sh"
[[ -x "$root/build/camera_risk" && -x "$root/build/door_mask_tool" ]] || { echo 'Build on Orin first.' >&2; exit 1; }
cp -p "$root/build/camera_risk" "$root/orin/build/"
cp -p "$root/build/door_mask_tool" "$root/orin/build/"
cp -p "$root/annotate_door_mask.sh" "$root/orin/"
cp -p "$root/config/door_mask.json" "$root/orin/config/"
sed 's|../orin/engines|../engines|' "$root/config/camera.json" > "$root/orin/config/camera.json"
(
cd "$package"
find . -type f ! -path './output/*' ! -path './outputs/*' ! -path './logs/*' ! -name SHA256SUMS -print0 |
cd "$root/orin"
find . \( -type f -o -type l \) ! -path './output/*' ! -path './outputs/*' ! -path './logs/*' ! -name SHA256SUMS -print0 |
sort -z | xargs -0 sha256sum > SHA256SUMS
)
echo "C++ runtime ready: $package/build/camera_risk"
src/brightness_gate.cpp
+79
View File
@@ -0,0 +1,79 @@
#include "brightness_gate.hpp"
#include <opencv2/imgproc.hpp>
#include <algorithm>
#include <cmath>
#include <stdexcept>
#include <vector>
namespace cms {
namespace {
double percentile(std::vector<unsigned char> const& values, double fraction) {
double position=fraction*static_cast<double>(values.size()-1);
auto lower=static_cast<std::size_t>(std::floor(position));
auto upper=static_cast<std::size_t>(std::ceil(position));
std::vector<unsigned char> ordered=values;
std::nth_element(ordered.begin(),ordered.begin()+upper,ordered.end());
double high=ordered[upper];
if(lower==upper)return high;
auto low=*std::max_element(ordered.begin(),ordered.begin()+upper);
return low+(high-low)*(position-lower);
}
}
BrightnessGate::BrightnessGate(BrightnessGateConfig config):config_(config) {
if(config_.startup_frames<1||config_.enter_frames<1||config_.exit_frames<1||
config_.roi_x1<0||config_.roi_y1<0||config_.roi_x2>1||config_.roi_y2>1||
config_.roi_x1>=config_.roi_x2||config_.roi_y1>=config_.roi_y2)
throw std::invalid_argument("Invalid brightness gate configuration");
}
BrightnessDecision BrightnessGate::update(cv::Mat const& bgr) {
if(bgr.empty()||bgr.type()!=CV_8UC3)throw std::invalid_argument("Brightness gate requires a non-empty BGR frame");
int x1=static_cast<int>(std::nearbyint(config_.roi_x1*bgr.cols));
int y1=static_cast<int>(std::nearbyint(config_.roi_y1*bgr.rows));
int x2=static_cast<int>(std::nearbyint(config_.roi_x2*bgr.cols));
int y2=static_cast<int>(std::nearbyint(config_.roi_y2*bgr.rows));
cv::Mat resized,luma;
cv::resize(bgr(cv::Rect(x1,y1,x2-x1,y2-y1)),resized,{160,120},0,0,cv::INTER_AREA);
cv::cvtColor(resized,luma,cv::COLOR_BGR2YCrCb);
std::vector<unsigned char> values;values.reserve(luma.total());
std::size_t dark=0;double sum=0;
for(int y=0;y<luma.rows;++y)for(int x=0;x<luma.cols;++x) {
auto value=luma.at<cv::Vec3b>(y,x)[0];values.push_back(value);sum+=value;
if(value<config_.dark_pixel_luma)++dark;
}
BrightnessDecision decision;
decision.enter_frames=config_.enter_frames;decision.exit_frames=config_.exit_frames;
decision.dark_pixel_luma=config_.dark_pixel_luma;
decision.median_luma=percentile(values,0.50);
decision.p75_luma=percentile(values,0.75);
decision.mean_luma=sum/values.size();decision.dark_ratio=static_cast<double>(dark)/values.size();
int dark_tiles=0;
for(int ty=0;ty<4;++ty)for(int tx=0;tx<4;++tx) {
std::vector<unsigned char> tile;tile.reserve(30*40);
for(int y=ty*30;y<(ty+1)*30;++y)for(int x=tx*40;x<(tx+1)*40;++x)tile.push_back(luma.at<cv::Vec3b>(y,x)[0]);
if(percentile(tile,0.50)<config_.dark_pixel_luma)++dark_tiles;
}
decision.dark_tile_ratio=static_cast<double>(dark_tiles)/16;
bool is_dark=decision.median_luma<=config_.enter_median_luma&&decision.dark_ratio>=config_.enter_dark_ratio&&decision.dark_tile_ratio>=config_.enter_dark_tile_ratio;
bool is_bright=decision.median_luma>=config_.exit_median_luma&&decision.dark_ratio<=config_.exit_dark_ratio&&decision.dark_tile_ratio<=config_.exit_dark_tile_ratio;
auto previous=state_;
if(!config_.enabled)state_="NORMAL";
else if(state_=="INITIALIZING") {
++startup_count_;startup_dark_votes_+=is_dark;
if(startup_count_>=config_.startup_frames)
state_=startup_dark_votes_>=((config_.startup_frames+1)/2)?"LOW_LIGHT":"NORMAL";
} else if(state_=="NORMAL") {
dark_streak_=is_dark?dark_streak_+1:0;bright_streak_=0;
if(dark_streak_>=config_.enter_frames){state_="LOW_LIGHT";dark_streak_=0;}
} else {
bright_streak_=is_bright?bright_streak_+1:0;dark_streak_=0;
if(bright_streak_>=config_.exit_frames){state_="NORMAL";bright_streak_=0;}
}
decision.state=state_;decision.allow_downstream=state_=="NORMAL";
decision.dark_streak=dark_streak_;decision.bright_streak=bright_streak_;
if(previous!=state_)decision.transition=previous+"->"+state_;
if(state_=="INITIALIZING")decision.reason="initializing";
else if(state_=="LOW_LIGHT")decision.reason=is_bright&&bright_streak_?"recovery_pending":"low_light";
else decision.reason=is_dark&&dark_streak_?"low_light_pending":"sufficient_light";
return decision;
}
}
src/camera.cpp
+160 -60
View File
@@ -1,90 +1,190 @@
#include "camera.hpp"
#include "v4l2_capture.h"
#include "uyvy_converter.hpp"
#include <linux/videodev2.h>
#include <opencv2/videoio.hpp>
#include <chrono>
#include <sys/eventfd.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <poll.h>
#include <fcntl.h>
#include <unistd.h>
#include <time.h>
#include <cerrno>
#include <algorithm>
#include <cstring>
#include <limits>
#include <stdexcept>
#include <thread>
namespace cms {
std::uint64_t monotonicNs(){return std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::steady_clock::now().time_since_epoch()).count();}
std::uint64_t monotonicNs() {
timespec time{};
if(clock_gettime(CLOCK_MONOTONIC,&time)!=0)throw std::runtime_error("clock_gettime failed");
return std::uint64_t(time.tv_sec)*1000000000ULL+time.tv_nsec;
}
namespace {
void checkedIoctl(int fd,unsigned long request,void* argument,char const* operation) {
int status;
do {status=ioctl(fd,request,argument);} while(status<0&&errno==EINTR);
if(status<0)throw std::runtime_error(std::string(operation)+": "+std::strerror(errno));
}
}
struct Camera::Impl {
struct Buffer {void* data{};std::size_t size{};};
CameraConfig config;
V4l2Capture capture;
cv::VideoCapture video;
std::unique_ptr<UyvyConverter> converter;
std::atomic<bool> stopped{false};
int fd{-1},stopFd{-1};bool streaming{};
std::uint32_t stride{},lastSequence{};bool haveSequence{};
std::uint64_t index{};
std::uint32_t previousSequence{};
bool haveSequence{};
std::uint32_t consecutiveErrors{};
std::atomic<bool> stopped{false};
std::vector<Buffer> buffers;
std::unique_ptr<UyvyConverter> converter;
explicit Impl(CameraConfig c):config(std::move(c)) {
if(!config.video.empty()) {
if(!video.open(config.video))throw std::runtime_error("Cannot open video: "+config.video);
if(int(video.get(cv::CAP_PROP_FRAME_WIDTH))!=config.width||int(video.get(cv::CAP_PROP_FRAME_HEIGHT))!=config.height)throw std::runtime_error("Video dimensions must match camera configuration");
} else {
if(config.width<=0||config.width>8192||config.height<=0||config.height>8192||config.width%2||config.fps<=0||config.fps>240||config.buffers<2||config.buffers>32||config.timeout_ms<1)
throw std::invalid_argument("Invalid camera configuration");
try {
stopFd=eventfd(0,EFD_CLOEXEC|EFD_NONBLOCK);
if(stopFd<0)throw std::runtime_error("Cannot create camera stop event");
openCapture();
}
} catch(...) {release();throw;}
}
void openCapture() {
try {
if(!capture.open(config.device)||!capture.setFormat(config.width,config.height,V4L2_PIX_FMT_UYVY))throw std::runtime_error("Camera open/format failed");
if(capture.width()!=unsigned(config.width)||capture.height()!=unsigned(config.height))throw std::runtime_error("Unexpected negotiated camera dimensions");
capture.setFps(config.fps);
if(!capture.initMmap(config.buffers)||!capture.startStreaming())throw std::runtime_error("Camera streaming failed");
fd=open(config.device.c_str(),O_RDWR|O_NONBLOCK|O_CLOEXEC);
if(fd<0)throw std::runtime_error("Cannot open "+config.device+": "+std::strerror(errno));
v4l2_capability capability{};checkedIoctl(fd,VIDIOC_QUERYCAP,&capability,"VIDIOC_QUERYCAP");
auto caps=(capability.capabilities&V4L2_CAP_DEVICE_CAPS)?capability.device_caps:capability.capabilities;
if(!(caps&V4L2_CAP_VIDEO_CAPTURE)||!(caps&V4L2_CAP_STREAMING))throw std::runtime_error("Camera does not support single-plane streaming capture");
v4l2_format format{};format.type=V4L2_BUF_TYPE_VIDEO_CAPTURE;
format.fmt.pix.width=config.width;format.fmt.pix.height=config.height;
format.fmt.pix.pixelformat=V4L2_PIX_FMT_UYVY;format.fmt.pix.field=V4L2_FIELD_NONE;
checkedIoctl(fd,VIDIOC_S_FMT,&format,"VIDIOC_S_FMT");
if(format.fmt.pix.pixelformat!=V4L2_PIX_FMT_UYVY||format.fmt.pix.width!=static_cast<unsigned>(config.width)||format.fmt.pix.height!=static_cast<unsigned>(config.height))
throw std::runtime_error("Camera rejected the configured UYVY resolution; refusing mismatched mask coordinates");
stride=format.fmt.pix.bytesperline;
if(stride<static_cast<unsigned>(config.width*2))throw std::runtime_error("Invalid camera row stride");
v4l2_streamparm params{};params.type=V4L2_BUF_TYPE_VIDEO_CAPTURE;
if(ioctl(fd,VIDIOC_G_PARM,&params)==0&&(params.parm.capture.capability&V4L2_CAP_TIMEPERFRAME)) {
params.parm.capture.timeperframe.numerator=1;params.parm.capture.timeperframe.denominator=config.fps;
checkedIoctl(fd,VIDIOC_S_PARM,&params,"VIDIOC_S_PARM");
auto const& interval=params.parm.capture.timeperframe;
if(!interval.numerator||interval.denominator!=static_cast<unsigned>(config.fps)*interval.numerator)
throw std::runtime_error("Camera rejected requested frame rate");
}
v4l2_requestbuffers request{};request.count=config.buffers;
request.type=V4L2_BUF_TYPE_VIDEO_CAPTURE;request.memory=V4L2_MEMORY_MMAP;
checkedIoctl(fd,VIDIOC_REQBUFS,&request,"VIDIOC_REQBUFS");
if(request.count<2)throw std::runtime_error("Insufficient V4L2 buffers");
buffers.resize(request.count);
for(unsigned i=0;i<request.count;++i) {
v4l2_buffer buffer{};buffer.type=request.type;buffer.memory=request.memory;buffer.index=i;
checkedIoctl(fd,VIDIOC_QUERYBUF,&buffer,"VIDIOC_QUERYBUF");
if(buffer.length<std::size_t(stride)*config.height)throw std::runtime_error("Camera buffer smaller than negotiated frame");
buffers[i].size=buffer.length;
auto mapping=mmap(nullptr,buffer.length,PROT_READ|PROT_WRITE,MAP_SHARED,fd,buffer.m.offset);
if(mapping==MAP_FAILED)throw std::runtime_error("Camera mmap failed");
buffers[i].data=mapping;checkedIoctl(fd,VIDIOC_QBUF,&buffer,"VIDIOC_QBUF");
}
if(!converter)converter=std::make_unique<UyvyConverter>(config.width,config.height);
}catch(...){capture.close();throw;}
auto type=V4L2_BUF_TYPE_VIDEO_CAPTURE;checkedIoctl(fd,VIDIOC_STREAMON,&type,"VIDIOC_STREAMON");streaming=true;
} catch(...) {releaseCapture();throw;}
}
std::optional<CapturedFrame> read() {
if(stopped)return {};
if(video.isOpened()) {
cv::Mat bgr;if(!video.read(bgr))return {};
auto image=std::make_shared<BgrImage>();image->width=bgr.cols;image->height=bgr.rows;image->stride=bgr.cols*3;
image->pixels.resize(std::size_t(image->stride)*image->height);
for(int y=0;y<bgr.rows;++y)std::memcpy(image->pixels.data()+y*image->stride,bgr.ptr(y),image->stride);
CaptureMetadata m{};m.frame_index=index;m.driver_sequence=index++;m.capture_monotonic_ns=m.received_monotonic_ns=monotonicNs();return CapturedFrame{m,image};
}
auto started=monotonicNs();unsigned misses=0,failedWindows=0;
bool reconnect=false;
~Impl(){release();}
void releaseCapture() noexcept {
if(streaming){auto type=V4L2_BUF_TYPE_VIDEO_CAPTURE;ioctl(fd,VIDIOC_STREAMOFF,&type);streaming=false;}
for(auto const& b:buffers)if(b.data)munmap(b.data,b.size);
buffers.clear();
if(fd>=0){close(fd);fd=-1;}
}
void release() noexcept {
releaseCapture();
if(stopFd>=0){close(stopFd);stopFd=-1;}
}
void stop() noexcept {
stopped=true;std::uint64_t one=1;
if(stopFd>=0){auto ignored=write(stopFd,&one,sizeof(one));(void)ignored;}
}
std::optional<CapturedFrame> readFrame() {
auto windowStart=monotonicNs();unsigned failedWindows=0;
auto windowNs=std::uint64_t(config.timeout_ms)*1000000;
std::string lastError="no camera frame";
while(!stopped) {
auto expired=[&](bool timedOut=false) {
auto now=monotonicNs();
if((now-started)*1e-6>=config.timeout_ms) {
if(timedOut||now-windowStart>=windowNs) {
if(++failedWindows>=6)
throw std::runtime_error("Camera capture failed 6 consecutive times: "+config.device+": "+lastError);
started=now;
windowStart=now;
}
if(reconnect) {
try {openCapture();reconnect=false;haveSequence=false;}
catch(std::exception const& e){lastError=e.what();std::this_thread::sleep_for(std::chrono::milliseconds(20));continue;}
};
auto pause=[&] {
pollfd stopEvent{stopFd,POLLIN,0};
// Back off transient driver errors while retaining immediate cancellation.
poll(&stopEvent,1,20);
};
auto reconnect=[&] {
releaseCapture();
if(haveSequence){++index;haveSequence=false;}
};
while(!stopped.load()) {
expired();
if(fd<0) {
try {openCapture();}
catch(std::exception const& e){lastError=e.what();pause();continue;}
}
V4l2Capture::Frame f;
if(!capture.getFrame(f,100)) {
if(stopped)return {};
++misses;
if(capture.needsReconnect()){lastError="camera device unavailable";capture.close();reconnect=true;}
// Avoid spinning when the driver immediately reports an error.
std::this_thread::sleep_for(std::chrono::milliseconds(1));
continue;
if(stopped.load())return {};
auto elapsed=monotonicNs()-windowStart;
auto remaining=elapsed<windowNs?windowNs-elapsed:1;
int waitMs=static_cast<int>((remaining+999999)/1000000);
pollfd descriptors[2]={{fd,POLLIN,0},{stopFd,POLLIN,0}};
int ready=poll(descriptors,2,waitMs);
if(ready<0&&errno==EINTR)continue;
if(stopped.load()||(descriptors[1].revents&POLLIN))return {};
if(ready==0){expired(true);continue;}
if(ready<0||(descriptors[0].revents&(POLLERR|POLLHUP|POLLNVAL))) {
lastError="camera disconnected or poll failed";reconnect();pause();continue;
}
auto stride=f.bytes_per_line?f.bytes_per_line:unsigned(config.width*2);
if(f.size<std::size_t(stride)*config.height) {
lastError="incomplete camera frame";++misses;capture.releaseFrame(f);
std::this_thread::sleep_for(std::chrono::milliseconds(1));continue;
v4l2_buffer buffer{};buffer.type=V4L2_BUF_TYPE_VIDEO_CAPTURE;buffer.memory=V4L2_MEMORY_MMAP;
int status=ioctl(fd,VIDIOC_DQBUF,&buffer);
if(status<0&&(errno==EAGAIN||errno==EINTR))continue;
if(status<0) {
lastError="VIDIOC_DQBUF: "+std::string(std::strerror(errno));
// EIO can be a transient corrupt frame; a lost device needs reopening.
if(errno!=EIO)reconnect();
pause();continue;
}
CaptureMetadata m{};m.frame_index=index++;m.driver_sequence=f.sequence;
m.received_monotonic_ns=m.capture_monotonic_ns=monotonicNs();m.driver_timestamp_ns=f.timestamp_us*1000;m.discarded_error_frames=misses;
if(haveSequence){auto delta=std::uint32_t(f.sequence-previousSequence);if(delta>0&&delta<0x80000000U)m.skipped_driver_frames=delta-1;}
previousSequence=f.sequence;haveSequence=true;
try {
auto image=converter->convert(f.data,stride);capture.releaseFrame(f);return CapturedFrame{m,std::move(image)};
}catch(...){capture.releaseFrame(f);throw;}
if(buffer.index>=buffers.size()){lastError="invalid V4L2 buffer index";reconnect();pause();continue;}
bool invalid=(buffer.flags&V4L2_BUF_FLAG_ERROR)||buffer.bytesused<std::size_t(stride)*config.height||buffer.bytesused>buffers[buffer.index].size;
if(invalid) {
++consecutiveErrors;lastError="corrupt or incomplete camera frame";
if(ioctl(fd,VIDIOC_QBUF,&buffer)<0)reconnect();
pause();continue;
}
CapturedFrame frame;
auto& m=frame.metadata;
m.discarded_error_frames=consecutiveErrors;
m.driver_sequence=buffer.sequence;m.received_monotonic_ns=monotonicNs();
m.driver_timestamp_ns=std::uint64_t(buffer.timestamp.tv_sec)*1000000000ULL+std::uint64_t(buffer.timestamp.tv_usec)*1000;
m.driver_timestamp_monotonic=(buffer.flags&V4L2_BUF_FLAG_TIMESTAMP_MASK)==V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
m.capture_monotonic_ns=m.received_monotonic_ns;
if(haveSequence) {
auto delta=static_cast<std::uint32_t>(buffer.sequence-lastSequence);
if(delta==0||delta>0x7fffffffU) {
lastError="camera sequence reset or repeated frame";
reconnect();pause();continue;
}
m.skipped_driver_frames=delta-1;index+=delta;
}
m.frame_index=index;
// GPU failures remain fatal; only capture failures consume the retry budget.
try {frame.image=converter->convert(buffers[buffer.index].data,stride);}
catch(...) {ioctl(fd,VIDIOC_QBUF,&buffer);throw;}
if(ioctl(fd,VIDIOC_QBUF,&buffer)<0){lastError="VIDIOC_QBUF failed";reconnect();pause();continue;}
lastSequence=buffer.sequence;haveSequence=true;consecutiveErrors=0;
return frame; // The next read starts a fresh six-window retry budget.
}
return {};
}
};
Camera::Camera(CameraConfig c):impl_(std::make_unique<Impl>(std::move(c))){}
Camera::Camera(CameraConfig config):impl_(std::make_unique<Impl>(std::move(config))){}
Camera::~Camera()=default;
std::optional<CapturedFrame> Camera::read(){return impl_->read();}
void Camera::requestStop()noexcept{impl_->stopped=true;}
std::optional<CapturedFrame> Camera::read(){return impl_->readFrame();}
void Camera::requestStop() noexcept {impl_->stop();}
}
src/camera_pipeline.cpp
+78 -47
View File
@@ -4,11 +4,10 @@
#include "detector.hpp"
#include "door_risk.hpp"
#include "records.hpp"
#include "result_conversion.hpp"
#include "door_zone.hpp"
#include "can_state.hpp"
#include "inference_session.hpp"
#include "can_bus.hpp"
#include "serialization.hpp"
#include <fstream>
#include "result_conversion.hpp"
#include <opencv2/core.hpp>
#include <atomic>
#include <cmath>
@@ -49,21 +48,21 @@ void attachPoses(FrameResult& result,fangjia::PoseFrameResult const& fused) {
}
}
struct CameraPipeline::Impl {
struct Detected {CapturedFrame frame;std::vector<fangjia::Box> boxes;double elapsed_ms{};};
struct Detected {CapturedFrame frame;std::vector<fangjia::Box> boxes;BrightnessDecision brightness;double elapsed_ms{};};
PipelineConfig config;
BoundedQueue<CapturedFrame> captures;
BoundedQueue<Detected> detections;
LatestResult results;
std::unique_ptr<OutputWriter> outputWriter;
std::unique_ptr<SessionWriter> sessionWriter;
std::unique_ptr<SessionReader> sessionReader;
fangjia::TrtLogger logger;
std::unique_ptr<Detector> detector;
std::unique_ptr<fangjia::RtmwEngine> pose;
std::unique_ptr<DoorRisk> risk;
std::unique_ptr<DoorZoneAssigner> zone;
std::unique_ptr<CanInput> can;
std::ofstream output, poseOutput;
std::shared_ptr<const BinaryMask> doorPixels;
std::unique_ptr<Camera> camera;
std::unique_ptr<CanBus> can;
std::thread captureThread,detectorThread,inferenceThread;
mutable std::mutex stateMutex;
std::exception_ptr failure;
@@ -73,10 +72,9 @@ struct CameraPipeline::Impl {
double totalLatency{};
explicit Impl(PipelineConfig c):config(std::move(c)),captures(config.queue_capacity),detections(config.queue_capacity) {
if(config.can_side!="left"&&config.can_side!="right")throw std::invalid_argument("CAN side must be left or right");
if(config.active_door!="can"&&config.active_door!="front"&&config.active_door!="rear"&&config.active_door!="both"&&config.active_door!="none")throw std::invalid_argument("Invalid active door");
if(!config.can_enabled&&config.active_door=="can")throw std::invalid_argument("Disabled CAN requires an explicit simulated active door");
if(config.can_timeout_ms<=0)throw std::invalid_argument("CAN timeout must be positive");
if(!config.replay_session.empty())sessionReader=std::make_unique<SessionReader>(config);
auto canConfig=config.can;if(sessionReader)canConfig.enabled=false;
can=std::make_unique<CanBus>(canConfig);
if(config.queue_capacity>8)throw std::invalid_argument("Queue capacity must be between 1 and 8");
if(config.max_people<1||config.max_people>16||!std::isfinite(config.detector_confidence)||config.detector_confidence<=0||config.detector_confidence>=1)
throw std::invalid_argument("Invalid detector configuration");
@@ -87,9 +85,11 @@ struct CameraPipeline::Impl {
if(captureThread.joinable())captureThread.join();
if(detectorThread.joinable())detectorThread.join();
if(inferenceThread.joinable())inferenceThread.join();
can->join();
}
void stop() noexcept {
stopped=true;active=false;
can->requestStop();
// Camera stays alive until all threads have joined. start()/destruction
// are owner-thread operations; requestStop() only interrupts existing work.
{std::lock_guard<std::mutex> lock(stateMutex);if(camera)camera->requestStop();}
@@ -105,7 +105,9 @@ struct CameraPipeline::Impl {
void start() {
{std::lock_guard<std::mutex> lock(stateMutex);if(started||stopped)throw std::logic_error("CameraPipeline can only be started once");started=true;}
try {
can->start();
outputWriter=std::make_unique<OutputWriter>(config.output);
sessionWriter=std::make_unique<SessionWriter>(config);
detector=std::make_unique<Detector>(config.engines/"yolo11s.engine",logger,config.detector_confidence,config.max_people);
pose=std::make_unique<fangjia::RtmwEngine>(config.engines/"rtmw_l_384.engine",logger,config.camera.width,config.camera.height,config.max_people);
DoorOptions options;options.constant=config.door_mask;options.warning=config.warning_relative;options.release=config.release_relative;
@@ -115,18 +117,10 @@ struct CameraPipeline::Impl {
auto const& mask=risk->mask();pixels->pixels.resize(std::size_t(mask.cols)*mask.rows);
for(int y=0;y<mask.rows;++y)std::memcpy(pixels->pixels.data()+std::size_t(y)*mask.cols,mask.ptr(y),mask.cols);
doorPixels=std::move(pixels);
zone=std::make_unique<DoorZoneAssigner>(config.door_zone_config);
auto openOutput=[](std::ofstream& out,std::filesystem::path const& path) {
if(path.empty())return;
if(!path.parent_path().empty())std::filesystem::create_directories(path.parent_path());
out.exceptions(std::ios::failbit|std::ios::badbit);out.open(path);
};
openOutput(output,config.results_path);openOutput(poseOutput,config.pose_records_path);
Stream warmup;
pose->warmup(warmup.value);
detector->infer(cv::Mat(config.camera.height,config.camera.width,CV_8UC3,cv::Scalar()),warmup.value);
{std::lock_guard<std::mutex> lock(stateMutex);camera=std::make_unique<Camera>(config.camera);if(stopped)camera->requestStop();}
can=std::make_unique<CanInput>(config.can_enabled);
{std::lock_guard<std::mutex> lock(stateMutex);if(!sessionReader)camera=std::make_unique<Camera>(config.camera);if(stopped&&camera)camera->requestStop();}
active=!stopped.load();
captureThread=std::thread([this]{try{captureLoop();}catch(...){fail();}});
detectorThread=std::thread([this]{try{detectorLoop();}catch(...){fail();}});
@@ -135,32 +129,59 @@ struct CameraPipeline::Impl {
}
void captureLoop() {
while(!stopped) {
auto frame=camera->read();if(!frame)break;
auto frame=sessionReader?sessionReader->read():camera->read();if(!frame)break;
gaps+=frame->metadata.skipped_driver_frames;
errorFrames+=frame->metadata.discarded_error_frames;
bool queued=config.camera.video.empty()?captures.pushLatest(std::move(*frame)):captures.push(std::move(*frame));
if(!queued)break;
if(!captures.push(std::move(*frame)))break;
++captured;
}
captures.close();
}
void detectorLoop() {
Stream stream;
Stream stream;BrightnessGate gate(config.brightness_gate);
while(!stopped) {
auto frame=captures.pop();if(!frame)break;
auto startedAt=monotonicNs();auto boxes=detector->infer(imageView(*frame->image),stream.value);
Detected item{std::move(*frame),std::move(boxes),elapsedMs(startedAt)};
bool queued=config.camera.video.empty()?detections.pushLatest(std::move(item)):detections.push(std::move(item));
if(!queued)break;
auto startedAt=monotonicNs();auto brightness=gate.update(imageView(*frame->image));
std::vector<fangjia::Box> boxes;
if(brightness.allow_downstream)boxes=detector->infer(imageView(*frame->image),stream.value);
if(!detections.push({std::move(*frame),std::move(boxes),std::move(brightness),elapsedMs(startedAt)}))break;
}
detections.close();
}
void inferenceLoop() {
Stream stream;fangjia::BoxTracker boxesTracker;fangjia::PoseFusionTracker fusion;std::uint64_t processed=0;
Stream stream;fangjia::BoxTracker boxesTracker;fangjia::PoseFusionTracker fusion;
while(!stopped) {
auto detected=detections.pop();if(!detected)break;
if(processed>static_cast<std::uint64_t>(std::numeric_limits<int>::max()))throw std::runtime_error("Frame index limit reached; restart pipeline");
auto index=static_cast<int>(processed++);
if(detected->frame.metadata.frame_index>static_cast<std::uint64_t>(std::numeric_limits<int>::max()))throw std::runtime_error("Frame index limit reached; restart pipeline");
if(!detected->brightness.allow_downstream) {
boxesTracker=fangjia::BoxTracker{};
fusion=fangjia::PoseFusionTracker{};
int index=static_cast<int>(detected->frame.metadata.frame_index);
auto emptyFusion=fusion.update({}, {}, index);
auto record=makeRecord(index,{}, {},emptyFusion,!config.record_session.empty(),false);
auto qualityStatus=detected->brightness.state;
Json invalidRisk={{"status",qualityStatus},{"risk_score",nullptr},{"risk_level","unavailable"},
{"decision","BRIGHTNESS_GATE"},{"source","brightness_gate"},{"track_id",nullptr},{"side",nullptr},
{"door_mask",{{"status","not_evaluated"},{"annotation_index",index},{"path",nullptr},{"format","none"},{"area_px",0}}},
{"warning_distance_relative",config.warning_relative},{"release_distance_relative",config.release_relative},{"regions",Json::array()}};
auto result=std::make_shared<FrameResult>(makeFrameResult(record,invalidRisk,detected->frame.metadata));
result->brightness=detected->brightness;
result->image=std::move(detected->frame.image);result->door_mask_pixels=doorPixels;
result->door_mask_calibration_verified=config.door_mask_calibration_verified;
if(detected->frame.replay_record) {
auto const& recorded=detected->frame.replay_record->at("result");
if(recorded.contains("can"))result->can=deserializeCan(recorded.at("can"));
else result->can.status="NOT_RECORDED";
result->can.source="replay";
} else result->can=can->snapshot();
result->timing={detected->elapsed_ms,0,0,elapsedMs(result->capture.capture_monotonic_ns)};
FrameResultPtr immutable=result;results.publish(std::move(immutable));
{std::lock_guard<std::mutex> lock(stateMutex);totalLatency+=result->timing.capture_to_result_ms;++published;}
if(!config.record_session.empty())sessionWriter->write(*result,record,detected->frame.replay_record.get());
outputWriter->write(*result);
continue;
}
auto index=static_cast<int>(detected->frame.metadata.frame_index);
auto& boxes=detected->boxes;
auto frame=imageView(*detected->frame.image);
boxesTracker.update(boxes,index,frame.cols,frame.rows);
@@ -174,17 +195,19 @@ struct CameraPipeline::Impl {
}
auto poseElapsed=elapsedMs(poseStart);auto fusionStart=monotonicNs();
auto fused=fusion.update(raw,isolated,index);
auto record=makeRecord(index,raw,isolated,fused,poseOutput.is_open(),true);
auto record=makeRecord(index,raw,isolated,fused,!config.record_session.empty(),false);
auto riskData=risk->evaluate(index,record);
auto result=std::make_shared<FrameResult>(makeFrameResult(record,riskData,detected->frame.metadata));
auto assignments=zone->update(index,record,frame.size());
result->can=can->snapshot(config.can_side,config.can_timeout_ms);
result->active_door=activeDoor(result->can);
if(config.active_door!="can") {result->active_door=config.active_door;result->can.source="simulation";result->can.valid=true;}
auto decisions=buildDoorRisks(riskData,assignments,result->active_door,result->can.valid);
result->risk=parseRisk(decisions.selected);
for(auto const& entry:decisions.doors.items())result->door_risks[entry.key()]=parseRisk(entry.value());
for(auto const& a:assignments)result->door_assignments.push_back(parseAssignment(a));
result->brightness=detected->brightness;
if(detected->frame.replay_record) {
auto const& recorded=detected->frame.replay_record->at("result");
if(recorded.contains("can"))result->can=deserializeCan(recorded.at("can"));
else result->can.status="NOT_RECORDED";
result->can.source="replay";
}else {
if(!sessionReader&&config.can.enabled&&result->brightness.allow_downstream)can->queueRisk(result->risk.risk_level);
result->can=can->snapshot();
}
attachPoses(*result,fused);
result->image=std::move(detected->frame.image);result->door_mask_pixels=doorPixels;
result->door_mask_calibration_verified=config.door_mask_calibration_verified;
@@ -192,13 +215,10 @@ struct CameraPipeline::Impl {
FrameResultPtr immutable=result;
results.publish(std::move(immutable));
{std::lock_guard<std::mutex> lock(stateMutex);totalLatency+=result->timing.capture_to_result_ms;++published;}
if(!config.record_session.empty())sessionWriter->write(*result,record,detected->frame.replay_record.get());
outputWriter->write(*result);
if(output.is_open())output<<serializeResult(*result).dump()<<'\n';
if(poseOutput.is_open())poseOutput<<record.dump()<<'\n';
if(config.max_frames&&processed>=config.max_frames){stop();break;}
}
if(output.is_open())output.flush();
if(poseOutput.is_open())poseOutput.flush();
sessionWriter->finish();
active=false;
}
};
@@ -206,9 +226,20 @@ CameraPipeline::CameraPipeline(PipelineConfig config):impl_(std::make_unique<Imp
CameraPipeline::~CameraPipeline()=default;
void CameraPipeline::start(){impl_->start();}
FrameResultPtr CameraPipeline::latest()const{return impl_->results.load();}
CanSnapshot CameraPipeline::canStatus()const {
if(impl_->sessionReader) {
auto result=impl_->results.load();if(result)return result->can;
CanSnapshot s;s.source="replay";s.status="WAITING";return s;
}
return impl_->can->snapshot();
}
bool CameraPipeline::sendCan(CanFrame const& frame) {
if(impl_->sessionReader)throw std::logic_error("CAN transmission is disabled during replay");
return impl_->can->send(frame);
}
bool CameraPipeline::running()const noexcept{return impl_->active.load();}
void CameraPipeline::requestStop()noexcept{impl_->stop();}
void CameraPipeline::join(){impl_->joinWorkers();{std::lock_guard<std::mutex> lock(impl_->stateMutex);impl_->camera.reset();impl_->can.reset();}impl_->rethrowFailure();}
void CameraPipeline::join(){impl_->joinWorkers();{std::lock_guard<std::mutex> lock(impl_->stateMutex);impl_->camera.reset();}impl_->rethrowFailure();}
PipelineStats CameraPipeline::stats()const {
std::lock_guard<std::mutex> lock(impl_->stateMutex);auto count=impl_->published.load();
return {impl_->captured.load(),count,impl_->gaps.load(),impl_->errorFrames.load(),count?impl_->totalLatency/count:0};
src/can_bus.cpp
+225
View File
@@ -0,0 +1,225 @@
#include "can_bus.hpp"
#include "zcan.h"
#include <algorithm>
#include <atomic>
#include <chrono>
#include <condition_variable>
#include <iostream>
#include <mutex>
#include <optional>
#include <set>
#include <stdexcept>
#include <thread>
namespace cms {
namespace {
constexpr unsigned deviceType=33,receiveCapacity=128;
std::mutex ownersMutex;
std::set<int> owners;
std::uint64_t nowNs() {
return std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::steady_clock::now().time_since_epoch()).count();
}
bool assertedRisk(std::string_view level) {
return level!="clear"&&level!="watch"&&level!="unavailable";
}
void validate(CanFrame const& f) {
if(f.channel!=static_cast<int>(canTransmitChannel)||f.id>(f.extended?0x1fffffffu:0x7ffu)||(!f.fd&&f.bitrate_switch))
throw std::invalid_argument("Invalid CAN channel, identifier or BRS flag");
auto size=f.data.size();
if((!f.fd&&size>8)||(f.fd&&!(size<=8||size==12||size==16||size==20||size==24||size==32||size==48||size==64)))
throw std::invalid_argument("Invalid CAN/CANFD payload length");
}
}
struct CanBus::Impl {
CanConfig config;
mutable std::mutex stateMutex;
std::mutex lifecycleMutex,waitMutex,riskMutex;
std::condition_variable wake,riskWake;
std::optional<bool> pendingRisk,lastQueuedRisk;
CanSnapshot state;
std::atomic<bool> stopped{false};
bool started{},opened{},owned{};
std::array<bool,2> initialized{};
std::thread receiver,riskSender;
explicit Impl(CanConfig c):config(c) {
if(c.device_index<0||c.timeout_ms<=0)throw std::invalid_argument("Invalid CAN configuration");
state.status=c.enabled?"STARTING":"DISABLED";
}
~Impl(){requestStop();join();}
void setStatus(char const* value) {std::lock_guard<std::mutex> lock(stateMutex);state.status=value;}
void releaseDevice() noexcept {
if(opened) {
for(unsigned i=0;i<canHardwareChannelCount;++i)if(initialized[i])VCI_ResetCAN(deviceType,config.device_index,i);
VCI_CloseDevice(deviceType,config.device_index);
opened=false;initialized.fill(false);
}
if(owned){std::lock_guard<std::mutex> lock(ownersMutex);owners.erase(config.device_index);owned=false;}
}
void start() {
std::lock_guard<std::mutex> lock(lifecycleMutex);
if(started)throw std::logic_error("CAN bus can only be started once");
started=true;
if(!config.enabled||stopped)return;
{
std::lock_guard<std::mutex> ownerLock(ownersMutex);
if(!owners.insert(config.device_index).second)throw std::logic_error("CAN device already owned by another pipeline");
owned=true;
}
try {
if(VCI_OpenDevice(deviceType,config.device_index,0)!=1)throw std::runtime_error("VCI_OpenDevice failed");
opened=true;
// Reference USBCANFD: 60 MHz clock, arbitration 500 kbps, data 2 Mbps.
for(unsigned i=0;i<canHardwareChannelCount;++i) {
ZCAN_INIT init{};init.clk=60000000;init.mode=0;
init.aset.tseg1=14;init.aset.tseg2=3;init.aset.sjw=2;init.aset.brp=5;
init.dset.tseg1=10;init.dset.tseg2=2;init.dset.sjw=2;init.dset.brp=1;
if(VCI_InitCAN(deviceType,config.device_index,i,&init)!=1)throw std::runtime_error("VCI_InitCAN failed");
initialized[i]=true;
U32 termination=config.termination?1:0;
if(VCI_SetReference(deviceType,config.device_index,i,CMD_CAN_TRES,&termination)!=1)
throw std::runtime_error("CAN termination configuration failed");
}
U32 merge=1;
if(VCI_SetReference(deviceType,config.device_index,canReceiveChannel,ZCAN_CMD_SET_CHNL_RECV_MERGE,&merge)!=1)
throw std::runtime_error("CAN merged receive configuration failed");
for(unsigned i=0;i<canHardwareChannelCount;++i)if(VCI_StartCAN(deviceType,config.device_index,i)!=1)
throw std::runtime_error("VCI_StartCAN failed");
setStatus("OPEN");
// Exactly one reader; merged CH1 input is deliberately ignored.
receiver=std::thread([this]{receiveLoop();});
riskSender=std::thread([this]{riskLoop();});
std::cout<<"CAN device "<<config.device_index<<": CH0 receive, CH1 transmit, 500 kbps / 2 Mbps"<<std::endl;
}catch(std::exception const& e) {
requestStop();
if(receiver.joinable())receiver.join();
releaseDevice();setStatus("UNAVAILABLE");
std::cerr<<"CAN unavailable; inference continues: "<<e.what()<<std::endl;
}
}
void requestStop() noexcept {
stopped=true;wake.notify_all();
{std::lock_guard<std::mutex> lock(riskMutex);pendingRisk.reset();}
riskWake.notify_all();
}
void join() noexcept {
requestStop();
// A sender may be inside send() holding lifecycleMutex. Join it before
// taking that mutex or closing the device.
if(riskSender.joinable())riskSender.join();
std::lock_guard<std::mutex> lock(lifecycleMutex);
if(receiver.joinable())receiver.join();
releaseDevice();
if(config.enabled&&started)setStatus("STOPPED");
}
void receiveFrame(ZCAN_MSG_HDR const& header,std::uint8_t const* data,bool fd,unsigned channel) {
std::lock_guard<std::mutex> lock(stateMutex);
// Ignore remote, error and transmit-echo frames; they are not input.
if(header.inf.sdf||header.inf.err||header.inf.tx)return;
if(channel>=liveCanChannelCount)return;
if(channel>1||header.len>(fd?64:8)||header.id>(header.inf.sef?0x1fffffffu:0x7ffu)) {
++state.receive_errors;return;
}
++state.received;
if(header.id!=0x391)return;
if(header.len<4){++state.receive_errors;return;}
auto& s=state.channels[channel];
for(unsigned i=0;i<4;++i)s.positions[i]=data[i];
++s.received;s.received_monotonic_ns=nowNs();s.device_timestamp_us=header.ts;
s.fd=fd;s.extended=header.inf.sef;
}
void receiveLoop() noexcept {
try {
std::array<ZCANDataObj,receiveCapacity> frames{};
while(!stopped) {
auto count=VCI_ReceiveData(deviceType,config.device_index,canReceiveChannel,frames.data(),receiveCapacity,100);
if(count>receiveCapacity) {
{std::lock_guard<std::mutex> lock(stateMutex);++state.receive_errors;state.status="ERROR";}
std::cerr<<"CAN receive failed; inference continues without live CAN"<<std::endl;
break;
}
for(unsigned i=0;i<count&&!stopped;++i) {
auto const& item=frames[i];
if(item.chnl>=liveCanChannelCount)continue;
if(item.dataType==ZCAN_DT_ZCAN_CAN_DATA)receiveFrame(item.data.zcanCANData.hdr,item.data.zcanCANData.dat,false,item.chnl);
else if(item.dataType==ZCAN_DT_ZCAN_CANFD_DATA)receiveFrame(item.data.zcanCANFDData.hdr,item.data.zcanCANFDData.dat,true,item.chnl);
else if(item.dataType==ZCAN_DT_ZCAN_ERROR_DATA){std::lock_guard<std::mutex> lock(stateMutex);++state.receive_errors;}
}
if(!count){std::unique_lock<std::mutex> lock(waitMutex);wake.wait_for(lock,std::chrono::milliseconds(5),[this]{return stopped.load();});}
}
}catch(...) {setStatus("ERROR");}
}
CanSnapshot snapshot() const {
std::lock_guard<std::mutex> lock(stateMutex);auto result=state;auto now=nowNs();
for(auto& c:result.channels) {
c.age_ms=c.received?(now-c.received_monotonic_ns)*1e-6:-1;
c.valid=state.status=="OPEN"&&!stopped&&c.received&&c.age_ms<=config.timeout_ms;
}
return result;
}
bool queueRisk(std::string_view level) {
std::lock_guard<std::mutex> lock(riskMutex);
{std::lock_guard<std::mutex> stateLock(stateMutex);
if(stopped||state.status!="OPEN")return false;
}
bool risk=assertedRisk(level);
if(lastQueuedRisk&&*lastQueuedRisk==risk)return true;
lastQueuedRisk=risk;pendingRisk=risk;riskWake.notify_one();return true;
}
void riskLoop() noexcept {
try {
CanFrame frame;frame.id=0x1f6;frame.data.assign(8,0);
std::optional<bool> lastAttemptedRisk;
while(!stopped) {
{
std::unique_lock<std::mutex> lock(riskMutex);
riskWake.wait(lock,[this]{return stopped||pendingRisk.has_value();});
if(stopped)break;
bool risk=*pendingRisk;pendingRisk.reset();
// Coalesce A -> B -> A while A is being sent. Repeated
// inference results and failed sends do not trigger retries.
if(lastAttemptedRisk&&*lastAttemptedRisk==risk)continue;
lastAttemptedRisk=risk;
frame.data[0]=risk?0x11:0x00;
}
if(!stopped){frame.channel=canTransmitChannel;send(frame);}
}
}catch(...) {setStatus("ERROR");}
}
bool send(CanFrame const& f) {
validate(f);
std::lock_guard<std::mutex> lock(lifecycleMutex);
bool ready;
{std::lock_guard<std::mutex> stateLock(stateMutex);ready=state.status=="OPEN";}
unsigned sent=0;
if(opened&&!stopped&&ready) {
ZCAN_MSG_HDR header{};header.id=f.id;header.chn=f.channel;header.len=f.data.size();
header.inf.fmt=f.fd?1:0;header.inf.sef=f.extended;header.inf.brs=f.bitrate_switch;
if(f.fd) {
ZCAN_FD_MSG message{};message.hdr=header;std::copy(f.data.begin(),f.data.end(),message.dat);
sent=VCI_TransmitFD(deviceType,config.device_index,f.channel,&message,1);
}else {
ZCAN_20_MSG message{};message.hdr=header;std::copy(f.data.begin(),f.data.end(),message.dat);
sent=VCI_Transmit(deviceType,config.device_index,f.channel,&message,1);
}
}
std::lock_guard<std::mutex> stateLock(stateMutex);
if(sent==1)++state.transmitted;else ++state.transmit_errors;
return sent==1;
}
};
CanBus::CanBus(CanConfig c):impl_(std::make_unique<Impl>(c)){}
CanBus::~CanBus()=default;
void CanBus::start(){impl_->start();}
void CanBus::requestStop()noexcept{impl_->requestStop();}
void CanBus::join()noexcept{impl_->join();}
CanSnapshot CanBus::snapshot()const{return impl_->snapshot();}
bool CanBus::send(CanFrame const& f){return impl_->send(f);}
bool CanBus::queueRisk(std::string_view level){return impl_->queueRisk(level);}
std::array<bool,2> CanBus::sendRisk(std::string_view riskLevel) {
CanFrame frame;frame.id=0x1f6;frame.data.assign(8,0);
frame.data[0]=assertedRisk(riskLevel)?0x11:0x00;
std::array<bool,2> sent{};
frame.channel=canTransmitChannel;sent[canTransmitChannel]=send(frame);
return sent;
}
}
src/can_comm.cpp
-341
View File
@@ -1,341 +0,0 @@
// can_comm.cpp - USBCANFD CAN/CANFD 收发封装实现
//
// 代码迁移自 vision-anti-pinch_bak/main.cpp 的 CAN 模块部分。
#include "can_comm.h"
#include <cstdio>
#include <cstring>
#include <pthread.h>
#include <atomic>
#include <unistd.h>
#include "zcan.h"
/* =====================================================================
* 类型/宏/全局变量
* ===================================================================== */
typedef uint32_t UINT;
typedef uint32_t U32;
#define msleep(ms) usleep((ms) * 1000)
#define USBCANFD 33
#define MAX_CHANNELS 2 // 最大通道数量
#define RX_WAIT_TIME 100
#define RX_BUFF_SIZE 1000
// 接收线程上下文
typedef struct
{
int dev_type; // 设备类型
int dev_idx; // 设备索引
int chn_idx; // 通道号
int total; // 接收总数
std::atomic<int> stop{0}; // 线程结束标志
} THREAD_CTX;
/* CAN 运行标志, 由信号处理函数(Ctrl+C)或外部置位 */
static volatile bool g_can_running = true;
static std::atomic<int> g_receive_count{0};
/* ===== 仅供 can_start/can_stop 共享的文件内静态变量 ===== */
static int s_DevType = USBCANFD;
static int s_DevIdx = 0;
static THREAD_CTX s_rx_ctx[MAX_CHANNELS];
static pthread_t s_rx_threads[MAX_CHANNELS];
static bool s_thread_started[MAX_CHANNELS]{};
static bool s_can_opened = false;
namespace CanComm {
bool is_open() { return s_can_opened; }
int receive_count() { return g_receive_count.load(); }
/* ===== 报文解析回调 (由 main.cpp 通过 set_msg_callback 注册) ===== */
static MsgCallback s_msg_cb = nullptr;
void set_msg_callback(MsgCallback cb) { s_msg_cb = cb; }
} // namespace CanComm
/* =====================================================================
* 帧构建与收发
* ===================================================================== */
namespace CanComm {
// 构建 CAN 帧
static void construct_can_frame(ZCAN_20_MSG *can_msg, UINT id, int chn_idx, int pad)
{
memset(can_msg, 0, sizeof(ZCAN_20_MSG));
can_msg->hdr.inf.txm = 0; // 0-正常发送
can_msg->hdr.inf.fmt = 0; // 0-CAN
can_msg->hdr.inf.sdf = 0; // 0-数据帧, 1-远程帧
can_msg->hdr.inf.sef = 0; // 0-标准帧, 1-扩展帧
can_msg->hdr.id = id; // ID
can_msg->hdr.chn = chn_idx; // 通道
can_msg->hdr.len = 8; // 数据长度
if (pad > 0)
{
can_msg->hdr.pad = pad; // 发送后延迟 pad ms
can_msg->hdr.inf.qsend = 1; // 队列发送帧,仅判断首帧
can_msg->hdr.inf.qsend_100us = 0; // 队列发送单位,0-ms,1-100us
}
for (int i = 0; i < can_msg->hdr.len; i++)
can_msg->dat[i] = i;
}
// 构建 CANFD 帧
static void construct_canfd_frame(ZCAN_FD_MSG *canfd_msg, UINT id, int chn_idx, int pad)
{
memset(canfd_msg, 0, sizeof(ZCAN_FD_MSG));
canfd_msg->hdr.inf.txm = 0; // 0-正常发送
canfd_msg->hdr.inf.fmt = 1; // 0-CAN, 1-CANFD
canfd_msg->hdr.inf.sdf = 0; // 0-数据帧 CANFD只有数据帧!
canfd_msg->hdr.inf.sef = 1; // 0-标准帧, 1-扩展帧
canfd_msg->hdr.inf.brs = 1; // canfd 加速
canfd_msg->hdr.id = id; // ID
canfd_msg->hdr.chn = chn_idx; // 通道
canfd_msg->hdr.len = 64; // 数据长度
if (pad > 0)
{
canfd_msg->hdr.pad = 100; // 发送后延迟100ms
canfd_msg->hdr.inf.qsend = 1; // 队列发送帧,仅判断首帧
canfd_msg->hdr.inf.qsend_100us = 0; // 队列发送单位,0-ms,1-100us
}
for (int i = 0; i < canfd_msg->hdr.len; i++)
canfd_msg->dat[i] = i;
}
// 分发报文到 main.cpp 注册的解析回调
static inline void dispatch_msg(uint32_t ts, uint32_t msg_id, const uint8_t *data, uint32_t len)
{
if (s_msg_cb)
s_msg_cb(ts, msg_id, data, len);
}
// 接收线程
static void *rx_thread(void *data)
{
THREAD_CTX *ctx = (THREAD_CTX *)data;
int DevType = ctx->dev_type;
int DevIdx = ctx->dev_idx;
int chn_idx = ctx->chn_idx;
ZCAN_20_MSG can_data[RX_BUFF_SIZE];
ZCAN_FD_MSG canfd_data[RX_BUFF_SIZE];
while (!ctx->stop)
{
memset(can_data, 0, sizeof(can_data));
memset(canfd_data, 0, sizeof(canfd_data));
int rcount = VCI_Receive(DevType, DevIdx, chn_idx, can_data, RX_BUFF_SIZE, RX_WAIT_TIME); // CAN
for (int i = 0; i < rcount; ++i)
{
if (can_data[i].hdr.inf.sdf == 0)
{ // 数据帧
dispatch_msg(can_data->hdr.ts, can_data[i].hdr.id, can_data[i].dat, can_data[i].hdr.len);
}
}
ctx->total += rcount;
rcount = VCI_ReceiveFD(DevType, DevIdx, chn_idx, canfd_data, RX_BUFF_SIZE, RX_WAIT_TIME); // CANFD
for (int i = 0; i < rcount; ++i)
{
dispatch_msg(canfd_data->hdr.ts, canfd_data[i].hdr.id, canfd_data[i].dat, canfd_data[i].hdr.len);
}
ctx->total += rcount;
msleep(10);
}
g_receive_count = ctx->total;
printf("chn: %d receive %d\n", chn_idx, ctx->total);
pthread_exit(0);
}
// 合并接收线程
static void *rx_merge_thread(void *data)
{
THREAD_CTX *ctx = (THREAD_CTX *)data;
int DevType = ctx->dev_type;
int DevIdx = ctx->dev_idx;
ZCANDataObj obj_data[RX_BUFF_SIZE];
while (!ctx->stop)
{
memset(obj_data, 0, sizeof(obj_data));
int rcount = VCI_ReceiveData(DevType, DevIdx, 0, obj_data, RX_BUFF_SIZE, RX_WAIT_TIME); // 第三个参数固定为0就好
for (int i = 0; i < rcount; ++i)
{
if (obj_data[i].dataType == ZCAN_DT_ZCAN_CAN_DATA)
{ // CAN
ZCAN_20_MSG *can_data = &obj_data[i].data.zcanCANData;
if (can_data->hdr.inf.sdf == 0)
{ // 数据帧
dispatch_msg(can_data->hdr.ts, can_data->hdr.id, can_data->dat, can_data->hdr.len);
}
}
else if (obj_data[i].dataType == ZCAN_DT_ZCAN_CANFD_DATA)
{ // CANFD
ZCAN_FD_MSG *canfd_data = &obj_data[i].data.zcanCANFDData;
dispatch_msg(canfd_data->hdr.ts, canfd_data->hdr.id, canfd_data->dat, canfd_data->hdr.len);
}
}
ctx->total += rcount;
msleep(10);
}
g_receive_count = ctx->total;
printf("merge receive %d\n", ctx->total);
pthread_exit(0);
}
/*
* 初始化并启动 CAN 收发。
* 返回 true 成功; false 失败(主流程可继续仅取图)。
*/
bool can_start()
{
g_can_running = true;
int IsMerge = 1; // 是否合并接收
// 打开设备
if (!VCI_OpenDevice(s_DevType, s_DevIdx, 0))
{
printf("Open device fail\n");
return false;
}
s_can_opened = true;
printf("Open device success\n");
// 初始化,启动通道
for (int i = 0; i < MAX_CHANNELS; i++)
{
ZCAN_INIT init{}; // 波特率结构体,数据根据zcanpro的波特率计算器得出
init.clk = 60000000; // clock: 60M(V1.01) 80M(V1.03即以上)
init.mode = 0; // 0-正常
init.aset.tseg1 = 14; // 仲裁域 500kbps
init.aset.tseg2 = 3;
init.aset.sjw = 2;
init.aset.smp = 0;
init.aset.brp = 5;
init.dset.tseg1 = 10; // 数据域 2000kbps
init.dset.tseg2 = 2;
init.dset.sjw = 2;
init.dset.smp = 0;
init.dset.brp = 1;
if (!VCI_InitCAN(s_DevType, s_DevIdx, i, &init)) // 初始化通道
{
printf("InitCAN(%d) fail\n", i);
can_stop();return false;
}
printf("InitCAN(%d) success\n", i);
U32 on = 1;
if (!VCI_SetReference(s_DevType, s_DevIdx, i, CMD_CAN_TRES, &on)) // 终端电阻
{
printf("CMD_CAN_TRES fail\n");
}
if (i == 0)
{
if (!VCI_SetReference(s_DevType, s_DevIdx, i, ZCAN_CMD_SET_CHNL_RECV_MERGE, &IsMerge)) // 合并接收
{
printf("ZCAN_CMD_SET_CHNL_RECV_MERGE fail\n");
}
}
if (!VCI_StartCAN(s_DevType, s_DevIdx, i)) // 启动通道
{
printf("StartCAN(%d) fail\n", i);
can_stop();return false;
}
printf("StartCAN(%d) success\n", i);
s_rx_ctx[i].dev_type = s_DevType;
s_rx_ctx[i].dev_idx = s_DevIdx;
s_rx_ctx[i].chn_idx = i;
s_rx_ctx[i].total = 0;
s_rx_ctx[i].stop = 0;
if (IsMerge && i == 0)
{
s_thread_started[i]=pthread_create(&s_rx_threads[i], NULL, rx_merge_thread, &s_rx_ctx[i])==0; // 合并接收线程
}
else
{
s_thread_started[i]=pthread_create(&s_rx_threads[i], NULL, rx_thread, &s_rx_ctx[i])==0; // 创建接收线程
}
if(!s_thread_started[i]){can_stop();return false;}
}
return true;
}
/*
* 停止 CAN 接收并关闭设备。
*/
void can_stop()
{
if (!s_can_opened)
return;
g_can_running = false;
for (int i = 0; i < MAX_CHANNELS; i++)
{
s_rx_ctx[i].stop = 1;
if(s_thread_started[i]){pthread_join(s_rx_threads[i], NULL);s_thread_started[i]=false;} // 等待接收线程退出
if (!VCI_ResetCAN(s_DevType, s_DevIdx, i)) // 复位通道
printf("ResetCAN(%d) fail\n", i);
else
printf("ResetCAN(%d) success!\n", i);
}
if (!VCI_CloseDevice(s_DevType, s_DevIdx))
printf("CloseDevice fail\n");
else
printf("CloseDevice success\n");
s_can_opened = false;
}
uint32_t can_send(int chn_idx, uint32_t id, const uint8_t data[8])
{
if (!s_can_opened)
return 0;
ZCAN_20_MSG msg;
construct_can_frame(&msg, id, chn_idx, 0);
for (int i = 0; i < 8; ++i)
msg.dat[i] = data[i];
return VCI_Transmit(s_DevType, s_DevIdx, chn_idx, &msg, 1);
}
uint32_t canfd_send(int chn_idx, uint32_t id, const uint8_t data[64])
{
if (!s_can_opened)
return 0;
ZCAN_FD_MSG msg;
construct_canfd_frame(&msg, id, chn_idx, 0);
for (int i = 0; i < 64; ++i)
msg.dat[i] = data[i];
return VCI_TransmitFD(s_DevType, s_DevIdx, chn_idx, &msg, 1);
}
} // namespace CanComm
src/can_state.cpp
-39
View File
@@ -1,39 +0,0 @@
#include "can_state.hpp"
#include "can_comm.h"
#include "camera.hpp"
#include <mutex>
#include <stdexcept>
namespace cms {
namespace {
std::mutex mutex;
std::array<int,4> positions{{-1,-1,-1,-1}};
std::uint64_t timestamp{};
bool owned{};
void received(int64_t,uint32_t id,uint8_t const* data,uint32_t size) {
if(id!=0x391||size<4)return;
std::lock_guard<std::mutex> lock(mutex);
for(int i=0;i<4;++i)positions[i]=data[i];
timestamp=monotonicNs();
}
}
CanInput::CanInput(bool enabled) {
{std::lock_guard<std::mutex> lock(mutex);if(owned)throw std::logic_error("Only one CAN input may run per process");owned=true;positions.fill(-1);timestamp=0;}
if(enabled){CanComm::set_msg_callback(received);opened_=CanComm::can_start();}
}
CanInput::~CanInput() {
if(opened_)CanComm::can_stop();
CanComm::set_msg_callback(nullptr);
std::lock_guard<std::mutex> lock(mutex);owned=false;
}
CanSnapshot CanInput::snapshot(std::string const& side,double timeoutMs) const {
std::lock_guard<std::mutex> lock(mutex);CanSnapshot s;s.positions=positions;s.side=side;
s.age_ms=timestamp?(monotonicNs()-timestamp)*1e-6:-1;
int offset=side=="left"?0:1;
s.valid=s.age_ms>=0&&s.age_ms<=timeoutMs&&s.positions[offset]>=0&&s.positions[offset+2]>=0;return s;
}
std::string activeDoor(CanSnapshot const& s) {
if(!s.valid)return "none";
int i=s.side=="left"?0:1;bool front=s.positions[i]==0,rear=s.positions[i+2]==0;
return front&&rear?"both":front?"front":rear?"rear":"none";
}
}
src/config.cpp
+18 -3
View File
@@ -19,10 +19,25 @@ PipelineConfig loadConfig(std::filesystem::path const& path) {
auto const& door=j.at("door");config.door_mask=resolve(door.at("mask"));
config.warning_relative=door.at("warning_relative");config.release_relative=door.at("release_relative");
config.door_mask_calibration_verified=door.at("calibration_verified");
config.door_zone_config=resolve(door.at("zone_config"));
if(j.contains("brightness_gate")) {
auto const& gate=j.at("brightness_gate");auto& c=config.brightness_gate;
c.enabled=gate.value("enabled",c.enabled);
if(gate.contains("roi")){auto const& roi=gate.at("roi");if(!roi.is_array()||roi.size()!=4)throw std::runtime_error("brightness_gate.roi must contain four normalized coordinates");c.roi_x1=roi[0];c.roi_y1=roi[1];c.roi_x2=roi[2];c.roi_y2=roi[3];}
c.dark_pixel_luma=gate.value("dark_pixel_luma",c.dark_pixel_luma);
c.enter_median_luma=gate.value("enter_median_luma",c.enter_median_luma);
c.enter_dark_ratio=gate.value("enter_dark_ratio",c.enter_dark_ratio);
c.enter_dark_tile_ratio=gate.value("enter_dark_tile_ratio",c.enter_dark_tile_ratio);
c.exit_median_luma=gate.value("exit_median_luma",c.exit_median_luma);
c.exit_dark_ratio=gate.value("exit_dark_ratio",c.exit_dark_ratio);
c.exit_dark_tile_ratio=gate.value("exit_dark_tile_ratio",c.exit_dark_tile_ratio);
c.startup_frames=gate.value("startup_frames",c.startup_frames);
c.enter_frames=gate.value("enter_frames",c.enter_frames);
c.exit_frames=gate.value("exit_frames",c.exit_frames);
}
if(j.contains("can")) {
auto const& can=j.at("can");config.can_enabled=can.value("enabled",true);
config.can_side=can.value("side",std::string("left"));config.can_timeout_ms=can.value("timeout_ms",1000.0);
auto const& can=j.at("can");config.can.enabled=can.value("enabled",true);
config.can.device_index=can.value("device_index",0);config.can.timeout_ms=can.value("timeout_ms",1000);
config.can.termination=can.value("termination",true);
}
return config;
}
src/door_mask_annotator.cpp
+137
View File
@@ -0,0 +1,137 @@
#include "door_mask_annotator.hpp"
#include <nlohmann/json.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/highgui/highgui_c.h>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <algorithm>
#include <cmath>
#include <fstream>
#include <stdexcept>
#include <string>
#include <vector>
namespace cms {
namespace {
using Json = nlohmann::ordered_json;
constexpr char windowName[]="Door Mask Annotator";
constexpr int maxDisplayWidth=1280;
constexpr int maxDisplayHeight=720;
constexpr int footerHeight=72;
struct EditorState {
cv::Mat image;
std::vector<cv::Point> points;
int displayWidth{};
int displayHeight{};
std::string message="Left click: add point Right click/U: undo R: reset S/Enter: save Esc/Q: cancel";
cv::Point toOriginal(int x,int y) const {
auto ox=static_cast<int>(std::lround(double(x)*(image.cols-1)/std::max(displayWidth-1,1)));
auto oy=static_cast<int>(std::lround(double(y)*(image.rows-1)/std::max(displayHeight-1,1)));
return {std::clamp(ox,0,image.cols-1),std::clamp(oy,0,image.rows-1)};
}
cv::Point toDisplay(cv::Point const& p) const {
return {static_cast<int>(std::lround(double(p.x)*(displayWidth-1)/std::max(image.cols-1,1))),
static_cast<int>(std::lround(double(p.y)*(displayHeight-1)/std::max(image.rows-1,1)))};
}
};
void onMouse(int event,int x,int y,int,void* opaque) {
auto& state=*static_cast<EditorState*>(opaque);
if(x<0||y<0||x>=state.displayWidth||y>=state.displayHeight)return;
if(event==cv::EVENT_LBUTTONDOWN) {
state.points.push_back(state.toOriginal(x,y));
state.message="Point added. Add at least 3 points, then press S or Enter.";
} else if(event==cv::EVENT_RBUTTONDOWN&&!state.points.empty()) {
state.points.pop_back();state.message="Last point removed.";
}
}
cv::Mat render(EditorState const& state) {
cv::Mat scaled;
cv::resize(state.image,scaled,{state.displayWidth,state.displayHeight},0,0,cv::INTER_AREA);
std::vector<cv::Point> displayed;
displayed.reserve(state.points.size());
for(auto const& point:state.points)displayed.push_back(state.toDisplay(point));
if(displayed.size()>=3) {
auto overlay=scaled.clone();
cv::fillPoly(overlay,std::vector<std::vector<cv::Point>>{displayed},cv::Scalar(30,180,255));
cv::addWeighted(overlay,0.25,scaled,0.75,0,scaled);
cv::polylines(scaled,std::vector<std::vector<cv::Point>>{displayed},true,cv::Scalar(20,230,255),2,cv::LINE_AA);
} else if(displayed.size()>=2) {
cv::polylines(scaled,std::vector<std::vector<cv::Point>>{displayed},false,cv::Scalar(20,230,255),2,cv::LINE_AA);
}
for(std::size_t i=0;i<displayed.size();++i) {
cv::circle(scaled,displayed[i],5,cv::Scalar(20,20,255),-1,cv::LINE_AA);
cv::putText(scaled,std::to_string(i+1),displayed[i]+cv::Point(7,-7),cv::FONT_HERSHEY_SIMPLEX,.45,cv::Scalar(255,255,255),1,cv::LINE_AA);
}
cv::Mat canvas(state.displayHeight+footerHeight,state.displayWidth,CV_8UC3,cv::Scalar(24,27,32));
scaled.copyTo(canvas(cv::Rect(0,0,state.displayWidth,state.displayHeight)));
cv::putText(canvas,"Door polygon points: "+std::to_string(state.points.size()),{12,state.displayHeight+25},cv::FONT_HERSHEY_SIMPLEX,.62,cv::Scalar(235,235,235),1,cv::LINE_AA);
cv::putText(canvas,state.message,{12,state.displayHeight+54},cv::FONT_HERSHEY_SIMPLEX,.48,cv::Scalar(180,220,255),1,cv::LINE_AA);
return canvas;
}
bool usablePolygon(std::vector<cv::Point> const& points) {
return points.size()>=3&&std::abs(cv::contourArea(points))>=10.0;
}
void writeFiles(EditorState const& state,DoorMaskAnnotationOptions const& options) {
auto const output=std::filesystem::absolute(options.output_json);
auto const reference=std::filesystem::absolute(options.reference_image);
if(output.extension()!=".json")throw std::invalid_argument("Door mask output must use the .json extension");
if(!options.overwrite&&(std::filesystem::exists(output)||std::filesystem::exists(reference)))
throw std::runtime_error("Output already exists; use --force to replace it: "+output.string());
if(!output.parent_path().empty())std::filesystem::create_directories(output.parent_path());
if(!reference.parent_path().empty())std::filesystem::create_directories(reference.parent_path());
auto imageTmp=reference;imageTmp.replace_filename(reference.stem().string()+".tmp"+reference.extension().string());
if(!cv::imwrite(imageTmp.string(),state.image,{cv::IMWRITE_JPEG_QUALITY,95}))
throw std::runtime_error("Cannot write reference image: "+reference.string());
std::error_code error;
if(options.overwrite)std::filesystem::remove(reference,error);
std::filesystem::rename(imageTmp,reference);
Json shape={{"label","door"},{"points",Json::array()},{"group_id",nullptr},{"shape_type","polygon"},{"flags",Json::object()}};
for(auto const& point:state.points)shape["points"].push_back({point.x,point.y});
Json document={{"version","x-anylabeling"},{"flags",Json::object()},{"shapes",Json::array({shape})},
{"imagePath",reference.filename().string()},{"imageData",nullptr},{"imageHeight",state.image.rows},{"imageWidth",state.image.cols},
{"constantDoorMask",true}};
if(options.source_video)document["sourceVideo"]=*options.source_video;
if(options.source_frame_index)document["sourceFrameIndex"]=*options.source_frame_index;
auto text=document.dump(2)+"\n";
auto jsonTmp=output;jsonTmp+=".tmp";
std::ofstream stream;stream.exceptions(std::ios::failbit|std::ios::badbit);
stream.open(jsonTmp,std::ios::binary|std::ios::trunc);stream.write(text.data(),static_cast<std::streamsize>(text.size()));stream.close();
if(options.overwrite)std::filesystem::remove(output,error);
std::filesystem::rename(jsonTmp,output);
}
}
bool annotateDoorMask(cv::Mat const& image,DoorMaskAnnotationOptions const& options) {
if(image.empty()||image.type()!=CV_8UC3)throw std::invalid_argument("Door mask annotation requires a non-empty BGR image");
if(options.output_json.empty()||options.reference_image.empty())throw std::invalid_argument("Door mask output paths cannot be empty");
auto scale=std::min({1.0,double(maxDisplayWidth)/image.cols,double(maxDisplayHeight)/image.rows});
EditorState state{image.clone(),{},std::max(1,static_cast<int>(std::lround(image.cols*scale))),
std::max(1,static_cast<int>(std::lround(image.rows*scale)))};
cv::namedWindow(windowName,cv::WINDOW_AUTOSIZE);
cv::setMouseCallback(windowName,onMouse,&state);
try {
while(true) {
cv::imshow(windowName,render(state));
auto raw=cv::waitKeyEx(20),key=raw&0xff;
if(cvGetWindowHandle(windowName)==nullptr) {cv::destroyWindow(windowName);return false;}
if(key==27||key=='q'||key=='Q') {cv::destroyWindow(windowName);return false;}
if(key=='r'||key=='R') {state.points.clear();state.message="Polygon cleared.";continue;}
if((key=='u'||key=='U'||key==8||key==127)&&!state.points.empty()) {state.points.pop_back();state.message="Last point removed.";continue;}
if(key=='s'||key=='S'||key==10||key==13) {
if(!usablePolygon(state.points)) {state.message="A valid door polygon needs at least 3 non-collinear points.";continue;}
writeFiles(state,options);cv::destroyWindow(windowName);return true;
}
}
} catch(...) {cv::destroyWindow(windowName);throw;}
}
}
src/door_mask_tool.cpp
+160
View File
@@ -0,0 +1,160 @@
#include "camera.hpp"
#include "config.hpp"
#include "door_mask_annotator.hpp"
#include <opencv2/highgui.hpp>
#include <opencv2/highgui/highgui_c.h>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/videoio.hpp>
#include <algorithm>
#include <cmath>
#include <cstdint>
#include <cstdlib>
#include <filesystem>
#include <iostream>
#include <limits>
#include <optional>
#include <stdexcept>
#include <string>
namespace {
constexpr char videoWindowName[]="Select Door Mask Reference Frame";
cv::Mat captureReference(std::filesystem::path const& configPath) {
auto config=cms::loadConfig(configPath);
cms::Camera camera(config.camera);
std::optional<cms::CapturedFrame> frame;
// Let exposure and the camera link settle while retaining the newest frame.
for(int i=0;i<5;++i)frame=camera.read();
camera.requestStop();
if(!frame||!frame->image)throw std::runtime_error("Camera stopped before an annotation frame was captured");
auto const& image=*frame->image;
return cv::Mat(image.height,image.width,CV_8UC3,const_cast<std::uint8_t*>(image.pixels.data()),image.stride).clone();
}
cv::Mat readVideoFrame(cv::VideoCapture& video,int index) {
if(!video.set(cv::CAP_PROP_POS_FRAMES,index))throw std::runtime_error("Cannot seek to video frame "+std::to_string(index));
cv::Mat frame;
if(!video.read(frame)||frame.empty())throw std::runtime_error("Cannot decode video frame "+std::to_string(index));
return frame;
}
std::optional<std::pair<cv::Mat,std::int64_t>> selectVideoFrame(std::filesystem::path const& path,std::optional<std::int64_t> requested) {
cv::VideoCapture video(path.string(),cv::CAP_FFMPEG);
if(!video.isOpened())throw std::runtime_error("Cannot open annotation video: "+path.string());
auto countValue=video.get(cv::CAP_PROP_FRAME_COUNT);
if(!std::isfinite(countValue)||countValue<1||countValue>std::numeric_limits<int>::max())
throw std::runtime_error("Annotation video does not report a usable frame count: "+path.string());
auto count=static_cast<int>(std::llround(countValue));
if(requested) {
if(*requested<0||*requested>=count)throw std::out_of_range("--frame is outside the video: "+std::to_string(*requested));
return std::pair<cv::Mat,std::int64_t>{readVideoFrame(video,static_cast<int>(*requested)),*requested};
}
if(count==1)return std::pair<cv::Mat,std::int64_t>{readVideoFrame(video,0),0};
int position=0,current=-1;
cv::Mat selected;
cv::namedWindow(videoWindowName,cv::WINDOW_AUTOSIZE);
cv::createTrackbar("Frame",videoWindowName,&position,count-1,[](int,void*){},nullptr);
try {
while(true) {
if(position!=current) {selected=readVideoFrame(video,position);current=position;}
auto scale=std::min({1.0,1280.0/selected.cols,680.0/selected.rows});
auto width=std::max(1,static_cast<int>(std::lround(selected.cols*scale)));
auto height=std::max(1,static_cast<int>(std::lround(selected.rows*scale)));
cv::Mat preview,canvas(height+58,width,CV_8UC3,cv::Scalar(24,27,32));
cv::resize(selected,preview,{width,height},0,0,cv::INTER_AREA);
preview.copyTo(canvas(cv::Rect(0,0,width,height)));
auto timeMs=video.get(cv::CAP_PROP_POS_MSEC);
cv::putText(canvas,"Frame "+std::to_string(position)+" / "+std::to_string(count-1)+" "+
std::to_string(static_cast<std::int64_t>(std::llround(timeMs)))+" ms",{12,height+23},cv::FONT_HERSHEY_SIMPLEX,.6,cv::Scalar(235,235,235),1,cv::LINE_AA);
cv::putText(canvas,"Trackbar or A/D: previous/next J/L: -/+30 S/Enter: select Esc/Q: cancel",
{12,height+47},cv::FONT_HERSHEY_SIMPLEX,.45,cv::Scalar(180,220,255),1,cv::LINE_AA);
cv::imshow(videoWindowName,canvas);
auto raw=cv::waitKeyEx(20),key=raw&0xff,next=position;
if(cvGetWindowHandle(videoWindowName)==nullptr) {cv::destroyWindow(videoWindowName);return std::nullopt;}
if(key==27||key=='q'||key=='Q') {cv::destroyWindow(videoWindowName);return std::nullopt;}
if(key=='a'||key=='A'||raw==65361||raw==2424832)next=position-1;
else if(key=='d'||key=='D'||raw==65363||raw==2555904)next=position+1;
else if(key=='j'||key=='J')next=position-30;
else if(key=='l'||key=='L')next=position+30;
else if(key=='s'||key=='S'||key==10||key==13) {
auto result=std::pair<cv::Mat,std::int64_t>{selected.clone(),position};
cv::destroyWindow(videoWindowName);return result;
}
next=std::clamp(next,0,count-1);
if(next!=position) {position=next;cv::setTrackbarPos("Frame",videoWindowName,position);}
}
} catch(...) {cv::destroyWindow(videoWindowName);throw;}
}
void printHelp() {
std::cout<<
"door_mask_tool --output MASK.json [--camera | --image IMAGE | --video VIDEO [--frame N]] [--config CAMERA.json] [--force]\n"
"\n"
"With no input option, --camera is used. Image and video modes never open the camera.\n"
"Video mode opens a frame selector unless --frame selects a zero-based frame directly.\n"
"The reference JPEG is stored beside MASK.json. Existing files are preserved unless --force is used.\n"
"\n"
"Mouse/keys: left click adds a point; right click or U undoes; R resets;\n"
"S or Enter saves; Esc or Q cancels without writing files.\n";
}
}
int main(int argc,char** argv)try {
auto root=std::filesystem::canonical("/proc/self/exe").parent_path().parent_path();
std::filesystem::path configPath=root/"config/camera.json",imagePath,videoPath,outputPath;
std::optional<std::int64_t> requestedFrame;
bool force=false,cameraInput=false;
for(int i=1;i<argc;++i) {
std::string argument=argv[i];
auto value=[&](char const* name) {if(++i>=argc)throw std::invalid_argument(std::string(name)+" needs a path");return std::filesystem::path(argv[i]);};
if(argument=="--output")outputPath=value("--output");
else if(argument=="--camera")cameraInput=true;
else if(argument=="--image")imagePath=value("--image");
else if(argument=="--video")videoPath=value("--video");
else if(argument=="--frame") {
if(++i>=argc)throw std::invalid_argument("--frame needs a zero-based frame number");
std::size_t consumed{};auto text=std::string(argv[i]);auto parsed=std::stoll(text,&consumed);
if(consumed!=text.size()||parsed<0)throw std::invalid_argument("--frame must be a non-negative integer");
requestedFrame=parsed;
}
else if(argument=="--config")configPath=value("--config");
else if(argument=="--force")force=true;
else if(argument=="--help"||argument=="-h") {printHelp();return 0;}
else throw std::invalid_argument("Unknown option: "+argument);
}
if(outputPath.empty())throw std::invalid_argument("--output MASK.json is required");
auto inputs=int(cameraInput)+int(!imagePath.empty())+int(!videoPath.empty());
if(inputs>1)throw std::invalid_argument("Choose exactly one input: --camera, --image, or --video");
if(inputs==0)cameraInput=true;
if(requestedFrame&&videoPath.empty())throw std::invalid_argument("--frame can only be used with --video");
if(outputPath.extension()!=".json")throw std::invalid_argument("Door mask output must use the .json extension");
auto reference=outputPath.parent_path()/(outputPath.stem().string()+"_reference.jpg");
if(!force&&(std::filesystem::exists(outputPath)||std::filesystem::exists(reference)))
throw std::runtime_error("Output already exists; use --force to replace it: "+outputPath.string());
auto display=std::getenv("DISPLAY"),wayland=std::getenv("WAYLAND_DISPLAY");
if((!display||!*display)&&(!wayland||!*wayland))throw std::runtime_error("No graphical display is available for annotation");
cv::Mat image;std::optional<std::int64_t> selectedFrame;
if(cameraInput) {
std::cout<<"Capturing annotation frame using "<<std::filesystem::absolute(configPath)<<std::endl;
image=captureReference(configPath);
} else if(!imagePath.empty()) {
image=cv::imread(imagePath.string(),cv::IMREAD_COLOR);
if(image.empty())throw std::runtime_error("Cannot read annotation image: "+imagePath.string());
} else {
auto selected=selectVideoFrame(videoPath,requestedFrame);
if(!selected) {std::cout<<"Video frame selection cancelled; no files written.\n";return 2;}
image=std::move(selected->first);selectedFrame=selected->second;
}
cms::DoorMaskAnnotationOptions options;options.output_json=outputPath;options.reference_image=reference;options.overwrite=force;
if(!videoPath.empty()) {options.source_video=std::filesystem::absolute(videoPath).string();options.source_frame_index=selectedFrame;}
if(!cms::annotateDoorMask(image,options)) {std::cout<<"Annotation cancelled; no files written.\n";return 2;}
std::cout<<"Door mask saved: "<<std::filesystem::absolute(outputPath)<<"\n"
<<"Reference image saved: "<<std::filesystem::absolute(reference)<<std::endl;
return 0;
} catch(std::exception const& e) {
std::cerr<<"Door mask tool failed: "<<e.what()<<std::endl;return 1;
}
src/door_zone.cpp
-94
View File
@@ -1,94 +0,0 @@
#include "door_zone.hpp"
#include <algorithm>
#include <cmath>
#include <fstream>
#include <map>
namespace cms {
namespace {
double median(std::deque<double> const& values) {
std::vector<double> sorted(values.begin(),values.end());
std::sort(sorted.begin(),sorted.end());auto n=sorted.size();
return n%2?sorted[n/2]:(sorted[n/2-1]+sorted[n/2])*0.5;
}
std::string upper(std::string value) { for(auto& c:value)c=char(std::toupper(static_cast<unsigned char>(c)));return value; }
Json features(Json const& record,std::size_t position,cv::Size size,int& poseCount) {
auto const& b=record.at("boxes").at(position);
double w=std::max(size.width,1),h=std::max(size.height,1);
Json result={{"box_center_x",(b[0].get<double>()+b[2].get<double>())*.5/w},{"box_height",(b[3].get<double>()-b[1].get<double>())/h}};
poseCount=0;
if(!record.contains("temporal_selected_keypoints")||position>=record["temporal_selected_keypoints"].size())return result;
auto const& points=record["temporal_selected_keypoints"][position];
for(int k:{5,6,11,12})if(k>=int(points.size())||points[k].size()<2||!std::isfinite(points[k][0].get<double>())||!std::isfinite(points[k][1].get<double>()))return result;
double sx=(points[5][0].get<double>()+points[6][0].get<double>())*.5;
double sy=(points[5][1].get<double>()+points[6][1].get<double>())*.5;
double hy=(points[11][1].get<double>()+points[12][1].get<double>())*.5;
result["shoulder_center_x"]=sx/w;result["shoulder_center_y"]=sy/h;
result["shoulder_width"]=std::hypot(points[5][0].get<double>()-points[6][0].get<double>(),points[5][1].get<double>()-points[6][1].get<double>())/w;
result["torso_height"]=std::abs(hy-sy)/h;result["hip_center_y"]=hy/h;poseCount=5;return result;
}
Json baseRisk(std::string const& door,std::string const& level,Json score,std::string const& status,std::string const& decision,Json regions=Json::array()) {
return {{"door_id",door},{"status",status},{"risk_score",score},{"risk_level",level},{"decision",decision},{"risk_type",upper(door)+"_"+upper(level)},{"source",nullptr},{"track_id",nullptr},{"side",nullptr},{"regions",regions}};
}
}
DoorZoneAssigner::DoorZoneAssigner(std::filesystem::path const& configuration) {
std::ifstream file(configuration);if(!file)throw std::runtime_error("Cannot read door-zone configuration: "+configuration.string());
auto j=Json::parse(file);features_=j.at("features");
frontMin_=j.value("front_probability_min",.72);rearMax_=j.value("rear_probability_max",.28);
minimumHeight_=j.value("minimum_person_height",.4);minimumPoseFeatures_=j.value("minimum_pose_features",4);
historyFrames_=j.value("history_frames",5);staleFrames_=j.value("stale_track_frames",120);
if(historyFrames_<1||staleFrames_<0||rearMax_<0||frontMin_>1||rearMax_>=frontMin_)throw std::invalid_argument("Invalid door-zone configuration");
}
std::string DoorZoneAssigner::classify(double p) const {return p>=frontMin_?"front":p<=rearMax_?"rear":"transition";}
Json DoorZoneAssigner::update(int frame,Json const& record,cv::Size size) {
Json assignments=Json::array();auto const& boxes=record.at("boxes");auto ids=record.value("track_ids",Json::array());
for(std::size_t i=0;i<boxes.size();++i) {
int id=i<ids.size()?ids[i].get<int>():int(i)+1,count=0;
auto f=features(record,i,size,count);double weighted=0,total=0;Json contributions=Json::object();
for(auto const& entry:features_.items())if(f.contains(entry.key())) {
auto const& c=entry.value();double rear=c.at("rear"),front=c.at("front"),weight=c.at("weight");
double score=front==rear?.5:std::clamp((f[entry.key()].get<double>()-rear)/(front-rear),0.0,1.0);
contributions[entry.key()]=rounded(score,6);weighted+=score*weight;total+=weight;
}
double probability=total?weighted/total:.5;std::string rawZone,zone;
if(f["box_height"].get<double>()<minimumHeight_)rawZone=zone="unassigned";
else if(count<minimumPoseFeatures_)rawZone=zone="transition";
else {rawZone=classify(probability);auto& values=history_[id];values.push_back(probability);while(int(values.size())>historyFrames_)values.pop_front();zone=classify(median(values));}
auto it=history_.find(id);double smoothed=it==history_.end()||it->second.empty()?probability:median(it->second);
lastSeen_[id]=frame;for(auto& value:f.items())value.value()=rounded(value.value().get<double>(),6);
assignments.push_back({{"track_id",id},{"person_index",i},{"door_zone",zone},{"raw_door_zone",rawZone},{"front_probability",rounded(probability,6)},{"smoothed_front_probability",rounded(smoothed,6)},{"confidence",rounded(std::abs(smoothed-.5)*2,6)},{"evidence_source","pose_primary_bbox_secondary"},{"pose_feature_count",count},{"features",f},{"feature_frontness",contributions},{"history_frames",it==history_.end()?0:it->second.size()}});
}
for(auto it=lastSeen_.begin();it!=lastSeen_.end();)if(frame-it->second>staleFrames_){history_.erase(it->first);it=lastSeen_.erase(it);}else ++it;
return assignments;
}
DoorDecisions buildDoorRisks(Json const& raw,Json const& assignments,std::string const& active,bool canValid) {
std::map<int,std::string> zones;for(auto const& a:assignments)zones[a.at("track_id").get<int>()]=a.at("door_zone");
Json regions=Json::array();for(auto region:raw.value("regions",Json::array())){int id=region.value("track_id",-1);region["door_zone"]=zones.count(id)?zones[id]:"unknown";regions.push_back(std::move(region));}
DoorDecisions result{Json::object(),Json::object()};
for(std::string door:{"front","rear"}) {
if(active!="both"&&active!=door){result.doors[door]=baseRisk(door,"inactive",nullptr,"inactive","DOOR_CAN_NOT_ACTIVE");continue;}
Json exact=Json::array(),transition=Json::array(),nearest=nullptr;
for(auto const& r:regions){if(r["door_zone"]==door){exact.push_back(r);if(!r.at("risk_score").is_null()&&(nearest.is_null()||r["risk_score"].get<double>()>nearest["risk_score"].get<double>()))nearest=r;}if(r["door_zone"]=="transition")transition.push_back(r);}
if(!nearest.is_null()&&(transition.empty()||(nearest["risk_level"]!="clear"&&nearest["risk_level"]!="watch"))) {
auto level=nearest["risk_level"].get<std::string>();auto r=baseRisk(door,level,nearest["risk_score"],"calculated",nearest["decision"],exact);
for(auto key:{"source","track_id","side"}){r[key]=nearest[key];}
result.doors[door]=std::move(r);
} else if(!transition.empty())result.doors[door]=baseRisk(door,"unknown",nullptr,"door_zone_ambiguous","EVIDENCE_UNAVAILABLE_TRANSITION_ZONE",transition);
else result.doors[door]=baseRisk(door,"clear",0.0,"calculated","CLEAR_NO_PERSON_ASSIGNED_TO_ACTIVE_DOOR");
}
if(active=="front"||active=="rear")result.selected=result.doors[active];
else if(active=="none") {
result.selected=baseRisk("none","inactive",nullptr,"inactive","NO_DOOR_CAN_ACTIVE");result.selected["door_id"]=nullptr;result.selected["risk_type"]="NO_ACTIVE_DOOR";
} else {
std::map<std::string,int> severity{{"danger",4},{"warning",3},{"watch",2},{"unknown",1},{"clear",0}};
result.selected=result.doors["front"];
if(severity[result.doors["rear"]["risk_level"]]>severity[result.selected["risk_level"]])result.selected=result.doors["rear"];
}
for(auto key:{"door_mask","warning_distance_relative","release_distance_relative"})result.selected[key]=raw.at(key);
if(!canValid) {
for(auto& entry:result.doors.items()){auto& r=entry.value();r["risk_type"]=upper(entry.key())+"_UNKNOWN";r["risk_level"]="unknown";r["risk_score"]=nullptr;r["decision"]="CAN_UNAVAILABLE_OR_STALE";}
result.selected["risk_type"]="CAN_UNKNOWN";result.selected["risk_level"]="unknown";result.selected["risk_score"]=nullptr;result.selected["decision"]="CAN_UNAVAILABLE_OR_STALE";
}
return result;
}
}
src/inference_session.cpp
+172
View File
@@ -0,0 +1,172 @@
#include "inference_session.hpp"
#include "camera_pipeline.hpp"
#include "serialization.hpp"
#include <cstring>
#include <iomanip>
#include <iostream>
#include <limits>
#include <sstream>
namespace cms {
namespace {
using Json = nlohmann::json;
Json readJson(std::filesystem::path const& path) {
std::ifstream input(path);
if(!input)throw std::runtime_error("Cannot read session file: "+path.string());
return Json::parse(input);
}
void atomicWrite(std::filesystem::path const& path,char const* data,std::size_t size) {
auto tmp=path;tmp+=".tmp";
std::ofstream out;out.exceptions(std::ios::failbit|std::ios::badbit);
out.open(tmp,std::ios::binary|std::ios::trunc);
out.write(data,static_cast<std::streamsize>(size));out.close();
std::filesystem::rename(tmp,path);
}
void writeJson(std::filesystem::path const& path,Json const& value) {
auto text=value.dump(2)+"\n";atomicWrite(path,text.data(),text.size());
}
std::string frameName(std::uint64_t index) {
std::ostringstream name;name<<std::setw(12)<<std::setfill('0')<<index;return name.str();
}
Json captureJson(CaptureMetadata const& c) {
return {{"frame_index",c.frame_index},{"driver_sequence",c.driver_sequence},
{"driver_timestamp_ns",c.driver_timestamp_ns},{"received_monotonic_ns",c.received_monotonic_ns},
{"capture_monotonic_ns",c.capture_monotonic_ns},{"driver_timestamp_monotonic",c.driver_timestamp_monotonic},
{"skipped_driver_frames",c.skipped_driver_frames},{"discarded_error_frames",c.discarded_error_frames}};
}
}
SessionWriter::SessionWriter(PipelineConfig const& c):directory_(c.record_session) {
if(directory_.empty())return;
width_=c.camera.width;height_=c.camera.height;
if(width_<=0||height_<=0||width_>16384||height_>16384||width_%2||height_%2||c.camera.fps<=0)
throw std::invalid_argument("H.264 recording requires positive even dimensions and positive FPS");
// Exclusive creation: never append to or overwrite another session.
if(!directory_.parent_path().empty())std::filesystem::create_directories(directory_.parent_path());
if(!std::filesystem::create_directory(directory_))throw std::runtime_error("Recording directory already exists: "+directory_.string());
std::filesystem::create_directory(directory_/"frames");
Json config={{"width",c.camera.width},{"height",c.camera.height},{"fps",c.camera.fps},
{"detector_confidence",c.detector_confidence},{"max_people",c.max_people},
{"warning_relative",c.warning_relative},{"release_relative",c.release_relative},
{"brightness_gate",{{"enabled",c.brightness_gate.enabled},{"roi",{c.brightness_gate.roi_x1,c.brightness_gate.roi_y1,c.brightness_gate.roi_x2,c.brightness_gate.roi_y2}},
{"dark_pixel_luma",c.brightness_gate.dark_pixel_luma},{"enter_median_luma",c.brightness_gate.enter_median_luma},
{"enter_dark_ratio",c.brightness_gate.enter_dark_ratio},{"enter_dark_tile_ratio",c.brightness_gate.enter_dark_tile_ratio},
{"exit_median_luma",c.brightness_gate.exit_median_luma},{"exit_dark_ratio",c.brightness_gate.exit_dark_ratio},
{"exit_dark_tile_ratio",c.brightness_gate.exit_dark_tile_ratio},{"startup_frames",c.brightness_gate.startup_frames},
{"enter_frames",c.brightness_gate.enter_frames},{"exit_frames",c.brightness_gate.exit_frames}}},
{"calibration_verified",c.door_mask_calibration_verified},
{"can",{{"enabled",c.can.enabled},{"device_index",c.can.device_index},{"timeout_ms",c.can.timeout_ms},{"termination",c.can.termination}}}};
Json models=Json::object();
for(auto name:{"yolo11s.engine","rtmw_l_384.engine"})
models[name]={{"path",(c.engines/name).string()},{"bytes",std::filesystem::file_size(c.engines/name)}};
writeJson(directory_/"door_mask.json",readJson(c.door_mask));
if(!video_.open((directory_/"frames.pending.mp4").string(),cv::CAP_FFMPEG,
cv::VideoWriter::fourcc('a','v','c','1'),c.camera.fps,cv::Size(width_,height_)))
throw std::runtime_error("Cannot open H.264 encoder; OpenCV FFmpeg with an H.264 encoder is required");
writeJson(directory_/"session.json",{{"schema_version",2},{"format","cms-inference-session"},
{"video",{{"file","frames.mp4"},{"codec","h264"},{"lossy",true}}},{"variant",PipelineConfig::session_variant},{"config",config},{"models",models},{"source_session",c.replay_session.string()}});
index_.exceptions(std::ios::failbit|std::ios::badbit);index_.open(directory_/"frames.txt");
std::cout<<"Recording inference session: "<<directory_<<std::endl;
}
void SessionWriter::write(FrameResult const& r,Json const& stages,Json const* source) {
if(directory_.empty())return;
if(finished_)throw std::logic_error("Session is already finalized");
if(!r.image)throw std::runtime_error("Session frame has no image");
auto const& im=*r.image;
if(im.width!=width_||im.height!=height_||im.stride<im.width*3||im.pixels.size()<std::size_t(im.stride)*im.height)
throw std::runtime_error("Invalid session image");
auto stem=frameName(count_);
cv::Mat view(im.height,im.width,CV_8UC3,const_cast<std::uint8_t*>(im.pixels.data()),im.stride);
video_.write(view);
Json record={{"video_frame_index",count_},{"frame_index",r.frame_index},{"capture",captureJson(r.capture)},
{"stages",stages},{"result",serializeResult(r)}};
// Preserve original acquisition timestamps across replay -> record cycles.
if(source)record["source_capture"]=source->contains("source_capture")?source->at("source_capture"):source->at("capture");
writeJson(directory_/"frames"/(stem+".json"),record);
index_<<stem<<'\n';index_.flush();++count_;
}
void SessionWriter::finish() {
if(directory_.empty()||finished_)return;
video_.release();index_.close();
// MP4 stores the encoded packet count. Check it after flushing delayed
// frames; OpenCV's write() itself cannot report encoder/disk errors.
if(count_) {
cv::VideoCapture check((directory_/"frames.pending.mp4").string(),cv::CAP_FFMPEG);
if(!check.isOpened()||check.get(cv::CAP_PROP_FRAME_COUNT)!=double(count_))
throw std::runtime_error("H.264 recording is incomplete; session not finalized");
}
std::filesystem::rename(directory_/"frames.pending.mp4",directory_/"frames.mp4");
writeJson(directory_/"complete.json",{{"frame_count",count_},{"video_bytes",std::filesystem::file_size(directory_/"frames.mp4")}});
finished_=true;
std::cout<<"Inference session finalized: "<<count_<<" H.264 frames"<<std::endl;
}
SessionReader::SessionReader(PipelineConfig& c):directory_(c.replay_session) {
auto manifest=readJson(directory_/"session.json");
if(manifest.at("schema_version")!=2||manifest.at("format")!="cms-inference-session")
throw std::runtime_error("Unsupported inference session format");
if(manifest.at("variant")!=PipelineConfig::session_variant)throw std::runtime_error("Session belongs to a different pipeline variant");
auto completion=readJson(directory_/"complete.json");
expectedCount_=completion.at("frame_count").get<std::uint64_t>();
if(completion.at("video_bytes")!=std::filesystem::file_size(directory_/"frames.mp4"))
throw std::runtime_error("Session video size does not match finalized recording");
auto const& j=manifest.at("config");
width_=j.at("width");height_=j.at("height");
if(width_<=0||height_<=0||width_>16384||height_>16384||width_%2||height_%2)throw std::runtime_error("Invalid session dimensions");
c.camera.width=width_;c.camera.height=height_;c.camera.fps=j.at("fps");
c.detector_confidence=j.at("detector_confidence");c.max_people=j.at("max_people");
c.warning_relative=j.at("warning_relative");c.release_relative=j.at("release_relative");
if(j.contains("brightness_gate")) {
auto const& gate=j.at("brightness_gate");auto& cgate=c.brightness_gate;
cgate.enabled=gate.value("enabled",cgate.enabled);
if(gate.contains("roi")){auto const& roi=gate.at("roi");cgate.roi_x1=roi[0];cgate.roi_y1=roi[1];cgate.roi_x2=roi[2];cgate.roi_y2=roi[3];}
cgate.dark_pixel_luma=gate.value("dark_pixel_luma",cgate.dark_pixel_luma);
cgate.enter_median_luma=gate.value("enter_median_luma",cgate.enter_median_luma);
cgate.enter_dark_ratio=gate.value("enter_dark_ratio",cgate.enter_dark_ratio);
cgate.enter_dark_tile_ratio=gate.value("enter_dark_tile_ratio",cgate.enter_dark_tile_ratio);
cgate.exit_median_luma=gate.value("exit_median_luma",cgate.exit_median_luma);
cgate.exit_dark_ratio=gate.value("exit_dark_ratio",cgate.exit_dark_ratio);
cgate.exit_dark_tile_ratio=gate.value("exit_dark_tile_ratio",cgate.exit_dark_tile_ratio);
cgate.startup_frames=gate.value("startup_frames",cgate.startup_frames);
cgate.enter_frames=gate.value("enter_frames",cgate.enter_frames);
cgate.exit_frames=gate.value("exit_frames",cgate.exit_frames);
}
c.door_mask_calibration_verified=j.at("calibration_verified");
c.door_mask=directory_/"door_mask.json";
index_.open(directory_/"frames.txt");
if(!index_)throw std::runtime_error("Cannot open session frame index");
std::cout<<"Replaying inference session: "<<directory_<<" (recorded inference settings, current engines)"<<std::endl;
}
std::optional<CapturedFrame> SessionReader::read() {
std::string stem;
if(!std::getline(index_,stem)) {
if(!index_.eof()||count_!=expectedCount_)throw std::runtime_error("Truncated session index");
return {};
}
// No path strings from the archive are used as filenames. Check sequence,
// including its newline, so corruption cannot silently skip a frame.
if(index_.eof()||count_>=expectedCount_||stem!=frameName(count_))throw std::runtime_error("Invalid or truncated session frame index");
auto record=std::make_shared<Json>(readJson(directory_/"frames"/(stem+".json")));
if(record->at("video_frame_index")!=count_)throw std::runtime_error("Video/record frame index mismatch");
auto frameIndex=record->at("frame_index").get<std::int64_t>();
if(frameIndex<0||frameIndex>std::numeric_limits<int>::max()||(count_&&std::uint64_t(frameIndex)<=lastFrame_))
throw std::runtime_error("Session frame indices must increase and fit int");
// Opening FFmpeg can create decoder threads. Do this in the capture
// worker, after StopSignals has installed the inherited signal mask.
if(!video_.isOpened()&&(!video_.open((directory_/"frames.mp4").string(),cv::CAP_FFMPEG)||
video_.get(cv::CAP_PROP_FRAME_COUNT)!=double(expectedCount_)))
throw std::runtime_error("Cannot open complete session video");
cv::Mat im;
if(!video_.read(im)||im.empty()||im.cols!=width_||im.rows!=height_||im.type()!=CV_8UC3)throw std::runtime_error("Missing or malformed session video frame: "+stem);
auto image=std::make_shared<BgrImage>();image->width=width_;image->height=height_;image->stride=width_*3;
image->pixels.resize(std::size_t(image->stride)*height_);
for(int y=0;y<height_;++y)std::memcpy(image->pixels.data()+std::size_t(y)*image->stride,im.ptr(y),image->stride);
auto const& c=record->at("capture");CaptureMetadata metadata;
metadata.frame_index=frameIndex;metadata.driver_sequence=c.at("driver_sequence");
metadata.driver_timestamp_ns=c.at("driver_timestamp_ns");metadata.driver_timestamp_monotonic=c.at("driver_timestamp_monotonic");
metadata.skipped_driver_frames=c.at("skipped_driver_frames");metadata.discarded_error_frames=c.at("discarded_error_frames");
// Old monotonic clocks cannot measure current replay latency.
metadata.received_monotonic_ns=metadata.capture_monotonic_ns=monotonicNs();
lastFrame_=frameIndex;++count_;
return CapturedFrame{metadata,std::move(image),std::move(record)};
}
}
src/main.cpp
+44 -61
View File
@@ -2,73 +2,56 @@
#include "config.hpp"
#include "stop_signals.hpp"
#include "visualizer.hpp"
#include <chrono>
#include <algorithm>
#include <cmath>
#include <cstdlib>
#include <functional>
#include <iostream>
#include <thread>
#include <map>
int main(int argc,char** argv)try {
auto root=std::filesystem::canonical("/proc/self/exe").parent_path().parent_path();
auto configPath=root/"config/camera.json";
for(int i=1;i<argc;++i)if(std::string(argv[i])=="--config") {
if(++i>=argc)throw std::invalid_argument("--config needs a path");configPath=argv[i];
cms::OutputOptions output;output.directory=root/"output";
bool visualize=true,disableCan=false;
std::filesystem::path recordSession,replaySession;
std::map<std::string,std::function<void(int&)>> arguments{
{"--record-session",[&](int&i){if(++i>=argc)throw std::invalid_argument("--record-session needs a directory");recordSession=argv[i];}},
{"--replay-session",[&](int&i){if(++i>=argc)throw std::invalid_argument("--replay-session needs a directory");replaySession=argv[i];}},
{"--save-original",[&](int&){output.original=true;}},
{"--save-annotated",[&](int&){output.annotated=true;}},
{"--save-results",[&](int&){output.results=true;}},
{"--no-can",[&](int&){disableCan=true;}},
{"--visualize",[&](int&){visualize=true;}},
{"--no-visualize",[&](int&){visualize=false;}},
{"--config",[&](int&i){if(++i>=argc)throw std::invalid_argument("--config needs a file");configPath=argv[i];}},
{"--help",[](int&){std::cout<<"camera_risk [--record-session NEW_DIR] [--replay-session DIR] [--save-original] [--save-annotated] [--save-results] [--config FILE] [--no-can] [--visualize|--no-visualize]\nLive CAN/CANFD reception on CH0 and change-triggered 0x1F6 binary risk transmission on CH1 are enabled by default.\nRisk: clear/watch/unavailable=0, warning/danger/unknown=1; send initial state once, then only binary changes. No automatic retries. --no-can disables reception and transmission.\nReplay uses recorded CAN only and never transmits.\nVisualization is enabled by default for live inference, recording and replay.\nContinuous camera inference; replay runs to EOF without a camera.\nH.264 sessions require a new recording directory. Stop with Ctrl+C or SIGTERM to finalize video.\n";std::exit(0);}}
};
for(int i=1;i<argc;++i){auto handler=arguments.find(argv[i]);if(handler==arguments.end())throw std::invalid_argument("Unknown option: "+std::string(argv[i]));handler->second(i);}
auto config=cms::loadConfig(configPath);config.output=output;config.record_session=recordSession;config.replay_session=replaySession;
if(disableCan)config.can.enabled=false;
cms::CameraPipeline pipeline(config);
cms::StopSignals signals(pipeline);
pipeline.start();
/*
* 下游使用示例：在下游自己的线程/调用位置读取最新结果。
* 首帧尚未完成时 result 为空；读取不会影响取图和推理。
*
* auto result = pipeline.latest();
* if (result) {
* const auto& risk = result->risk;
* const auto& people = result->persons;
* // result->frame_index / track_ids / boxes
* // result->image / door_mask_pixels / capture / timing
* // people[i].selected / smoothed / temporal：各 133 个关键点
* }
*/
// Optional observer. Closing/failing the window never requests inference stop.
if(visualize) {
auto display=std::getenv("DISPLAY");auto wayland=std::getenv("WAYLAND_DISPLAY");
if((!display||!*display)&&(!wayland||!*wayland))std::cerr<<"No display session; inference continues without visualization.\n";
else try {cms::runLocalVisualization(pipeline,!replaySession.empty());}
catch(std::exception const& e){std::cerr<<"Visualization unavailable; inference continues: "<<e.what()<<std::endl;}
}
for(int i=1;i<argc;++i)if(std::string(argv[i])=="--help"||std::string(argv[i])=="-h") {
std::cout<<"camera_risk [--config FILE] [--visualize] [--active-door can|front|rear|both|none]\n"
<<" [--can-side left|right] [--no-can] [--duration SECONDS] [--max-frames N]\n"
<<" [--video FILE] [--save-original] [--save-annotated] [--save-results [FILE.jsonl]] [--save-pose-records FILE.jsonl]\n"
<<"Pure C++ inference; the latest immutable FrameResult is kept in memory.\n";return 0;
}
auto config=cms::loadConfig(configPath);config.output.directory=root/"output";bool visualize=false;double duration=0;
auto value=[&](int& i){if(++i>=argc)throw std::invalid_argument("Missing option value");return std::string(argv[i]);};
for(int i=1;i<argc;++i) {
std::string arg=argv[i];
if(arg=="--config")value(i);
else if(arg=="--visualize")visualize=true;
else if(arg=="--no-can")config.can_enabled=false;
else if(arg=="--active-door")config.active_door=value(i);
else if(arg=="--can-side")config.can_side=value(i);
else if(arg=="--video")config.camera.video=value(i);
else if(arg=="--save-original")config.output.original=true;
else if(arg=="--save-annotated")config.output.annotated=true;
else if(arg=="--save-results") {
if(i+1<argc&&std::string(argv[i+1]).rfind("--",0)!=0)config.results_path=value(i); // Existing explicit JSONL path.
else config.output.results=true;
}
else if(arg=="--save-pose-records")config.pose_records_path=value(i);
else if(arg=="--max-frames"){auto n=std::stoll(value(i));if(n<0)throw std::invalid_argument("Negative frame count");config.max_frames=n;}
else if(arg=="--duration"){duration=std::stod(value(i));if(!std::isfinite(duration)||duration<0)throw std::invalid_argument("Invalid duration");}
else throw std::invalid_argument("Unknown option: "+arg);
}
cv::setNumThreads(2);
cms::CameraPipeline pipeline(config);cms::StopSignals signals(pipeline);pipeline.start();
auto started=std::chrono::steady_clock::now();std::atomic<bool> monitorStop{false};
std::thread monitor([&] {
auto report=started;
while(!monitorStop&&pipeline.running()) {
auto now=std::chrono::steady_clock::now();double elapsed=std::chrono::duration<double>(now-started).count();
if(duration>0&&elapsed>=duration){pipeline.requestStop();break;}
if(std::chrono::duration<double>(now-report).count()>=5) {
auto s=pipeline.stats();auto r=pipeline.latest();
std::cout<<"frames="<<s.published<<" fps="<<s.published/elapsed<<" risk="<<(r?r->risk.risk_type:"STARTING")<<std::endl;report=now;
}
std::this_thread::sleep_for(std::chrono::milliseconds(50));
}
});
try {
if(visualize) {
if(!std::getenv("DISPLAY")&&!std::getenv("WAYLAND_DISPLAY"))std::cerr<<"No display session; inference continues without visualization.\n";
else try{cms::runLocalVisualization(pipeline);}catch(std::exception const& e){std::cerr<<"Visualization unavailable; inference continues: "<<e.what()<<std::endl;}
}
// Downstream C++ code reads this same variable from its own thread:
// auto result = pipeline.latest(); // shared_ptr<const cms::FrameResult>
// if(result) { use(result->risk, result->door_risks, result->persons); }
pipeline.join();
}catch(...){monitorStop=true;pipeline.requestStop();monitor.join();throw;}
monitorStop=true;monitor.join();auto stats=pipeline.stats();
auto seconds=std::chrono::duration<double>(std::chrono::steady_clock::now()-started).count();
std::cout<<"frames="<<stats.published<<" elapsed_seconds="<<seconds<<" fps="<<stats.published/std::max(seconds,.001)<<" capture_to_result_ms="<<stats.mean_capture_to_result_ms<<std::endl;
pipeline.join();
return 0;
}catch(std::exception const& e){std::cerr<<"Camera pipeline failed: "<<e.what()<<std::endl;return 1;}
src/result_renderer.cpp
+94 -11
View File
@@ -22,7 +22,26 @@ void segment(cv::Mat& image,cv::Point a,cv::Point b,cv::Scalar c,int thickness,b
for(double x=0;x<length;x+=14)cv::line(image,cv::Point2d(a)+d*x,cv::Point2d(a)+d*std::min(x+8,length),c,thickness,cv::LINE_AA);
}
}
cv::Mat renderResult(FrameResult const& r) {
void renderCanOverlay(cv::Mat& canvas,CanSnapshot const& can) {
cv::rectangle(canvas,{0,96},{canvas.cols,190},{20,24,30},-1);
std::ostringstream header;
header<<"CAN 0x391 ACTUAL POSITION | "<<can.source<<" "<<can.status<<" | RX "<<can.received
<<" TX "<<can.transmitted<<" | errors RX "<<can.receive_errors<<" TX "<<can.transmit_errors;
cv::putText(canvas,header.str(),{24,119},cv::FONT_HERSHEY_SIMPLEX,.50,{235,235,235},1,cv::LINE_AA);
constexpr char const* labels[]={"LF","RF","LR","RR"};
auto channels=can.source=="replay"?can.channels.size():liveCanChannelCount;
for(std::size_t channel=0;channel<channels;++channel) {
auto const& s=can.channels[channel];std::ostringstream line;
line<<"CH"<<channel<<" "<<(s.valid?"LIVE":s.received?"STALE":"WAIT")<<" ";
for(std::size_t i=0;i<4;++i) {
line<<labels[i]<<": ";if(s.positions[i]<0)line<<"--";else line<<s.positions[i];line<<" ";
}
if(s.received)line<<"age "<<std::fixed<<std::setprecision(0)<<s.age_ms<<" ms "<<(s.fd?"CANFD":"CAN")<<" #"<<s.received;
auto color=s.valid?cv::Scalar(80,220,90):cv::Scalar(0,190,255);
cv::putText(canvas,line.str(),{24,146+int(channel)*24},cv::FONT_HERSHEY_SIMPLEX,.52,color,1,cv::LINE_AA);
}
}
cv::Mat renderResult(FrameResult const& r,CanSnapshot const* liveCan,bool drawText) {
if(!r.image)throw std::invalid_argument("Result has no frame image");
auto const& image=*r.image;
if(image.width<=0||image.height<=0||image.stride<image.width*3||image.pixels.size()<std::size_t(image.stride)*image.height)
@@ -42,10 +61,10 @@ cv::Mat renderResult(FrameResult const& r) {
for(auto const& person:r.persons) {
auto const& b=person.box;
cv::rectangle(canvas,{int(b[0]),int(b[1])},{int(b[2]),int(b[3])},{0,220,255},2);
std::ostringstream text;text<<"ID "<<person.track_id<<" "<<std::fixed<<std::setprecision(2)<<person.confidence;
cv::putText(canvas,text.str(),{int(b[0])+4,std::max(180,int(b[1])+25)},cv::FONT_HERSHEY_SIMPLEX,.8,{0,220,255},2,cv::LINE_AA);
for(auto const& a:r.door_assignments)if(a.track_id==person.track_id)
cv::putText(canvas,a.door_zone,{int(b[0])+4,std::max(155,int(b[1])-8)},cv::FONT_HERSHEY_SIMPLEX,.7,{0,220,255},2,cv::LINE_AA);
if(drawText) {
std::ostringstream text;text<<"ID "<<person.track_id<<" "<<std::fixed<<std::setprecision(2)<<person.confidence;
cv::putText(canvas,text.str(),{int(b[0])+4,std::max(180,int(b[1])+25)},cv::FONT_HERSHEY_SIMPLEX,.8,{0,220,255},2,cv::LINE_AA);
}
for(int s=0;s<2;++s) {
auto const& state=person.hands[s];auto const& k=person.selected;
cv::Scalar arm=s==0?cv::Scalar(40,40,255):cv::Scalar(30,230,30);
@@ -66,17 +85,81 @@ cv::Mat renderResult(FrameResult const& r) {
auto c=color(risk==r.risk.regions.end()?"unavailable":risk->risk_level);
cv::polylines(canvas,std::vector<std::vector<cv::Point>>{hull},true,c,2,cv::LINE_AA);
}
for(auto const& region:r.risk.regions)if(region.source=="YOLO_BODY_BOX_FALLBACK") {
if(drawText)for(auto const& region:r.risk.regions)if(region.source=="YOLO_BODY_BOX_FALLBACK") {
auto it=std::find(r.track_ids.begin(),r.track_ids.end(),region.track_id);
if(it!=r.track_ids.end()){auto const& b=r.boxes.at(it-r.track_ids.begin());cv::putText(canvas,"BODY FALLBACK",{int(b[0])+4,int(b[1])+50},cv::FONT_HERSHEY_SIMPLEX,.65,{0,160,255},2,cv::LINE_AA);}
}
cv::rectangle(canvas,{0,0},{canvas.cols,148},{20,24,30},-1);
std::ostringstream headline;headline<<r.risk.risk_type<<" ";if(r.risk.risk_score)headline<<std::fixed<<std::setprecision(3)<<*r.risk.risk_score;else headline<<"N/A";headline<<" "<<r.risk.risk_level;
cv::putText(canvas,headline.str(),{24,40},cv::FONT_HERSHEY_SIMPLEX,1.0,color(r.risk.risk_level),2,cv::LINE_AA);
if(drawText) {
cv::rectangle(canvas,{0,0},{canvas.cols,94},{20,24,30},-1);
std::ostringstream headline;headline<<"RISK ";if(r.risk.risk_score)headline<<std::fixed<<std::setprecision(3)<<*r.risk.risk_score;else headline<<"N/A";headline<<" "<<r.risk.risk_level;
cv::putText(canvas,headline.str(),{24,30},cv::FONT_HERSHEY_SIMPLEX,.72,color(r.risk.risk_level),1,cv::LINE_AA);
std::ostringstream status;status<<"camera frame "<<r.frame_index<<" | seq "<<r.capture.driver_sequence<<" | latency "<<std::fixed<<std::setprecision(1)<<r.timing.capture_to_result_ms<<" ms";
cv::putText(canvas,status.str(),{24,82},cv::FONT_HERSHEY_SIMPLEX,.72,{235,235,235},2,cv::LINE_AA);
cv::putText(canvas,status.str(),{24,57},cv::FONT_HERSHEY_SIMPLEX,.52,{235,235,235},1,cv::LINE_AA);
std::string calibration=r.door_mask_calibration_verified?"door calibration verified":"door calibration NOT verified (bring-up)";
cv::putText(canvas,calibration+" | CAN "+r.can.source+" | q/Esc: close preview",{24,123},cv::FONT_HERSHEY_SIMPLEX,.70,{0,190,255},2,cv::LINE_AA);
cv::putText(canvas,calibration+" | q/Esc: close preview",{24,83},cv::FONT_HERSHEY_SIMPLEX,.50,{0,190,255},1,cv::LINE_AA);
renderCanOverlay(canvas,liveCan?*liveCan:r.can);
auto const& gate=r.brightness;
cv::Scalar gateColor=gate.state=="NORMAL"?cv::Scalar(80,220,90):cv::Scalar(0,190,255);
cv::rectangle(canvas,{0,191},{canvas.cols,279},{20,24,30},-1);
std::ostringstream metrics;
metrics<<"BRIGHTNESS "<<gate.state<<" | median "<<std::fixed<<std::setprecision(1)<<gate.median_luma
<<" | p75 "<<gate.p75_luma<<" | dark<"<<std::setprecision(0)<<gate.dark_pixel_luma
<<" "<<std::setprecision(1)<<gate.dark_ratio*100.0<<"%"
<<" | dark tiles "<<gate.dark_tile_ratio*100.0<<"%";
cv::putText(canvas,metrics.str(),{24,220},cv::FONT_HERSHEY_SIMPLEX,.58,gateColor,1,cv::LINE_AA);
std::ostringstream detail;
detail<<gate.reason<<" | enter "<<gate.dark_streak<<"/"<<gate.enter_frames
<<" | recovery "<<gate.bright_streak<<"/"<<gate.exit_frames;
if(!gate.transition.empty())detail<<" | "<<gate.transition;
if(!gate.allow_downstream)detail<<" | RISK INVALID";
cv::putText(canvas,detail.str(),{24,250},cv::FONT_HERSHEY_SIMPLEX,.50,{225,225,225},1,cv::LINE_AA);
}
return canvas;
}
cv::Mat renderVisualization(FrameResult const& r,CanSnapshot const& can) {
constexpr int width=1000,headerHeight=150,imageHeight=650,canvasHeight=800;
cv::Mat output(canvasHeight,width,CV_8UC3,cv::Scalar(20,24,30));
cv::rectangle(output,{0,0},{width,headerHeight},{20,24,30},-1);
std::ostringstream headline;headline<<"RISK ";
if(r.risk.risk_score)headline<<std::fixed<<std::setprecision(3)<<*r.risk.risk_score;else headline<<"N/A";
headline<<" "<<r.risk.risk_level<<" | frame "<<r.frame_index<<" | latency "
<<std::fixed<<std::setprecision(1)<<r.timing.capture_to_result_ms<<" ms";
cv::putText(output,headline.str(),{14,24},cv::FONT_HERSHEY_SIMPLEX,.55,color(r.risk.risk_level),1,cv::LINE_AA);
std::string calibration=r.door_mask_calibration_verified?"door calibration verified":"door calibration NOT verified (bring-up)";
cv::putText(output,calibration,{14,45},cv::FONT_HERSHEY_SIMPLEX,.40,{0,190,255},1,cv::LINE_AA);
std::ostringstream canStatus;canStatus<<"CAN "<<can.status<<" | RX "<<can.received<<" TX "<<can.transmitted
<<" | errors RX "<<can.receive_errors<<" TX "<<can.transmit_errors;
cv::putText(output,canStatus.str(),{14,67},cv::FONT_HERSHEY_SIMPLEX,.40,{220,220,220},1,cv::LINE_AA);
constexpr char const* labels[]={"LF","RF","LR","RR"};
auto channels=can.source=="replay"?can.channels.size():liveCanChannelCount;
std::ostringstream positions;
for(std::size_t ch=0;ch<channels&&ch<2;++ch) {
if(ch)positions<<" | ";
positions<<"CH"<<ch<<" ";
for(std::size_t i=0;i<4;++i){positions<<labels[i]<<":";if(can.channels[ch].positions[i]<0)positions<<"--";else positions<<can.channels[ch].positions[i];if(i<3)positions<<" ";}
}
cv::putText(output,positions.str(),{14,88},cv::FONT_HERSHEY_SIMPLEX,.39,{180,210,180},1,cv::LINE_AA);
auto const& gate=r.brightness;
cv::Scalar gateColor=gate.state=="NORMAL"?cv::Scalar(80,220,90):cv::Scalar(0,190,255);
std::ostringstream metrics;
metrics<<"BRIGHTNESS "<<gate.state<<" | median "<<std::fixed<<std::setprecision(1)<<gate.median_luma
<<" | p75 "<<gate.p75_luma<<" | dark<"<<std::setprecision(0)<<gate.dark_pixel_luma
<<" "<<std::setprecision(1)<<gate.dark_ratio*100.0<<"% | dark tiles "<<gate.dark_tile_ratio*100.0<<"%";
cv::putText(output,metrics.str(),{14,112},cv::FONT_HERSHEY_SIMPLEX,.43,gateColor,1,cv::LINE_AA);
std::ostringstream detail;
detail<<gate.reason<<" | enter "<<gate.dark_streak<<"/"<<gate.enter_frames
<<" | recovery "<<gate.bright_streak<<"/"<<gate.exit_frames;
if(!gate.transition.empty())detail<<" | "<<gate.transition;
if(!gate.allow_downstream)detail<<" | RISK INVALID";
cv::putText(output,detail.str(),{14,136},cv::FONT_HERSHEY_SIMPLEX,.40,{225,225,225},1,cv::LINE_AA);
cv::Mat frame=renderResult(r,&can,false),scaled;
double scale=std::min(double(width)/frame.cols,double(imageHeight)/frame.rows);
cv::Size size(std::max(1,int(std::lround(frame.cols*scale))),std::max(1,int(std::lround(frame.rows*scale))));
cv::resize(frame,scaled,size,0,0,cv::INTER_AREA);
cv::Rect destination((width-size.width)/2,headerHeight+(imageHeight-size.height)/2,size.width,size.height);
scaled.copyTo(output(destination));
return output;
}
}
src/serialization.cpp
+37 -6
View File
@@ -4,7 +4,6 @@ namespace {
template<class T> Json optional(std::optional<T> const& x){return x?Json(*x):Json(nullptr);}
Json riskJson(RiskResult const& r) {
Json out={{"status",r.status},{"risk_score",optional(r.risk_score)},{"risk_level",r.risk_level},{"decision",r.decision},{"source",optional(r.source)},{"track_id",optional(r.track_id)},{"side",optional(r.side)},{"regions",Json::array()}};
if(!r.risk_type.empty()){out["risk_type"]=r.risk_type;out["door_id"]=optional(r.door_id);}
if(!r.door_mask.status.empty()) {
auto const& d=r.door_mask;out["door_mask"]={{"status",d.status},{"annotation_index",d.annotation_index},{"path",optional(d.path)},{"format",d.format},{"area_px",d.area_px}};
out["warning_distance_relative"]=r.warning_distance_relative;out["release_distance_relative"]=r.release_distance_relative;
@@ -12,7 +11,6 @@ Json riskJson(RiskResult const& r) {
for(auto const& v:r.regions) {
Json j={{"status",v.status},{"risk_score",optional(v.risk_score)},{"risk_level",v.risk_level},{"decision",v.decision},{"source",v.source},{"track_id",v.track_id},{"side",v.side}};
if(v.state)j["state"]=*v.state;
if(!v.door_zone.empty())j["door_zone"]=v.door_zone;
if(v.region_area_px) {
j["region_area_px"]=optional(v.region_area_px);j["door_overlap_px"]=optional(v.door_overlap_px);j["region_overlap_ratio"]=optional(v.region_overlap_ratio);
j["min_distance_px"]=optional(v.min_distance_px);j["region_scale_px"]=optional(v.region_scale_px);j["distance_relative"]=optional(v.distance_relative);
@@ -22,16 +20,37 @@ Json riskJson(RiskResult const& r) {
return out;
}
}
Json serializeCan(CanSnapshot const& s) {
Json out={{"source",s.source},{"status",s.status},{"received",s.received},{"transmitted",s.transmitted},
{"transmit_errors",s.transmit_errors},{"receive_errors",s.receive_errors},{"channels",Json::array()}};
for(auto const& c:s.channels)out["channels"].push_back({{"positions",c.positions},{"valid",c.valid},
{"received",c.received},{"received_monotonic_ns",c.received_monotonic_ns},{"device_timestamp_us",c.device_timestamp_us},
{"age_ms",c.age_ms},{"fd",c.fd},{"extended",c.extended}});
return out;
}
CanSnapshot deserializeCan(Json const& j) {
CanSnapshot s;s.source=j.at("source");s.status=j.at("status");
s.received=j.at("received");s.transmitted=j.at("transmitted");
s.transmit_errors=j.at("transmit_errors");s.receive_errors=j.at("receive_errors");
auto const& channels=j.at("channels");
if(!channels.is_array()||channels.size()!=2)throw std::runtime_error("Invalid recorded CAN channels");
for(std::size_t i=0;i<2;++i) {
auto const& v=channels[i];auto& c=s.channels[i];
if(!v.at("positions").is_array()||v.at("positions").size()!=4)throw std::runtime_error("Invalid recorded CAN positions");
c.positions=v.at("positions").get<std::array<int,4>>();
for(auto raw:c.positions)if(raw < -1||raw>255)throw std::runtime_error("Recorded CAN position out of range");
c.valid=v.at("valid");c.received=v.at("received");c.received_monotonic_ns=v.at("received_monotonic_ns");
c.device_timestamp_us=v.at("device_timestamp_us");c.age_ms=v.at("age_ms");c.fd=v.at("fd");c.extended=v.at("extended");
}
return s;
}
Json serializeResult(FrameResult const& r) {
Json out={{"frame_index",r.frame_index},{"track_ids",r.track_ids},{"boxes",r.boxes},{"skeletons",Json::array()},{"active_door",r.active_door},{"door_assignments",Json::array()},{"door_risks",Json::object()},{"risk",riskJson(r.risk)},{"ungated_risk",riskJson(r.ungated_risk)}};
Json out={{"frame_index",r.frame_index},{"track_ids",r.track_ids},{"boxes",r.boxes},{"skeletons",Json::array()},{"risk",riskJson(r.risk)}};
for(std::size_t i=0;i<r.persons.size();++i) {
auto const& p=r.persons[i];Json points=Json::array(),metadata=Json::object();for(auto const& k:p.selected)points.push_back({rounded(k.x,3),rounded(k.y,3)});
for(int s=0;s<2;++s){auto const& h=p.hands[s];metadata[s==0?"left":"right"]={{"limb_state",h.limb_consensus?"consensus":"inferred"},{"hand_source",h.source},{"hand_state",h.state},{"distance_eligible",h.distance_eligible},{"alarm_eligible",h.alarm_eligible}};}
out["skeletons"].push_back({{"track_id",p.track_id},{"person_index",i},{"layout","coco_wholebody_133"},{"keypoints",points},{"metadata",metadata}});
}
for(auto const& a:r.door_assignments)out["door_assignments"].push_back({{"track_id",a.track_id},{"person_index",a.person_index},{"door_zone",a.door_zone},{"raw_door_zone",a.raw_door_zone},{"front_probability",a.front_probability},{"smoothed_front_probability",a.smoothed_front_probability},{"confidence",a.confidence},{"evidence_source",a.evidence_source},{"pose_feature_count",a.pose_feature_count},{"history_frames",a.history_frames},{"features",a.features},{"feature_frontness",a.feature_frontness}});
for(auto const& entry:r.door_risks)out["door_risks"][entry.first]=riskJson(entry.second);
out["can"]={{"positions",r.can.positions},{"age_ms",r.can.age_ms},{"valid",r.can.valid},{"source",r.can.source},{"side",r.can.side}};
for(std::size_t i=0;i<r.persons.size();++i) {
auto const& p=r.persons[i];auto& skeleton=out["skeletons"][i];
skeleton["confidence"]=p.confidence;skeleton["keypoint_scores"]=Json::array();
@@ -43,6 +62,18 @@ Json serializeResult(FrameResult const& r) {
skeleton["temporal_keypoints"].push_back({b.x,b.y,b.score});
}
}
out["can"]=serializeCan(r.can);
out["vision_enabled"]=r.brightness.allow_downstream;
out["image_quality"]=r.brightness.state;
out["risk_valid"]=r.brightness.allow_downstream;
out["brightness"]={{"state",r.brightness.state},{"reason",r.brightness.reason},
{"transition",r.brightness.transition.empty()?Json(nullptr):Json(r.brightness.transition)},
{"allow_downstream",r.brightness.allow_downstream},{"median_luma",r.brightness.median_luma},
{"p75_luma",r.brightness.p75_luma},{"mean_luma",r.brightness.mean_luma},
{"dark_pixel_luma",r.brightness.dark_pixel_luma},
{"dark_ratio",r.brightness.dark_ratio},{"dark_tile_ratio",r.brightness.dark_tile_ratio},
{"dark_streak",r.brightness.dark_streak},{"bright_streak",r.brightness.bright_streak},
{"enter_frames",r.brightness.enter_frames},{"exit_frames",r.brightness.exit_frames}};
out["timing"]={{"detector_ms",r.timing.detector_ms},{"pose_ms",r.timing.pose_ms},{"fusion_risk_ms",r.timing.fusion_risk_ms},{"capture_to_result_ms",r.timing.capture_to_result_ms}};
out["capture"]={{"sequence",r.capture.driver_sequence},{"capture_monotonic_ms",r.capture.capture_monotonic_ns*1e-6},{"driver_timestamp_ms",r.capture.driver_timestamp_ns*1e-6}};
out["capture_to_result_ms"]=rounded(r.timing.capture_to_result_ms,3);out["calibration_verified"]=r.door_mask_calibration_verified;return out;
src/v4l2_capture.cpp
-394
View File
@@ -1,394 +0,0 @@
#include "v4l2_capture.h"
#include <fcntl.h>
#include <unistd.h>
#include <errno.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <linux/videodev2.h>
#include <cstring>
#include <cstdio>
#include <algorithm>
// 四字符码构造宏 (使用 v4l2_fourcc 避免重复定义)
// 注意: 这里使用videodev2.h已有的v4l2_fourcc宏
namespace {
// 将 v4l2_capability 的 driver[16] / card[32] 转成 C 字符串 (v4l2_capability 使用 __u8)
inline std::string to_cstr(const __u8 *arr, size_t len) {
std::string s;
s.reserve(len);
for (size_t i = 0; i < len && arr[i] != '\0'; ++i) {
char c = static_cast<char>(arr[i]);
if (c >= 32 && c < 127) s += c; // 只保留可打印 ASCII
}
// 去除末尾空格
while (!s.empty() && s.back() == ' ') s.pop_back();
return s;
}
// 打印 v4l2 格式调试信息
void dumpFormat(struct v4l2_format &fmt, const char *msg) {
printf("%s: width=%u height=%u pixfmt=0x%08x '%c%c%c%c' bpl=%u size=%u\n",
msg,
fmt.fmt.pix.width,
fmt.fmt.pix.height,
fmt.fmt.pix.pixelformat,
(fmt.fmt.pix.pixelformat) & 0xFF,
(fmt.fmt.pix.pixelformat >> 8) & 0xFF,
(fmt.fmt.pix.pixelformat >> 16) & 0xFF,
(fmt.fmt.pix.pixelformat >> 24) & 0xFF,
fmt.fmt.pix.bytesperline,
fmt.fmt.pix.sizeimage);
}
} // namespace
V4l2Capture::~V4l2Capture() {
close();
}
bool V4l2Capture::open(const std::string &device) {
reconnect_needed_ = false;
if (fd_ >= 0) {
return false;
}
fd_ = ::open(device.c_str(), O_RDWR | O_NONBLOCK, 0);
if (fd_ < 0) {
printf("V4l2Capture: open %s failed: %s\n", device.c_str(), strerror(errno));
return false;
}
// 检查设备能力
struct v4l2_capability cap;
if (ioctl(fd_, VIDIOC_QUERYCAP, &cap) < 0) {
printf("V4l2Capture: VIDIOC_QUERYCAP failed: %s\n", strerror(errno));
::close(fd_);
fd_ = -1;
return false;
}
printf("V4l2Capture: opened %s, driver='%s' card='%s', caps=0x%x\n",
device.c_str(),
to_cstr(cap.driver, sizeof(cap.driver)).c_str(),
to_cstr(cap.card, sizeof(cap.card)).c_str(),
cap.capabilities);
// 必须支持 V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING
if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
printf("V4l2Capture: not a video capture device\n");
::close(fd_);
fd_ = -1;
return false;
}
if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
printf("V4l2Capture: does not support streaming I/O\n");
::close(fd_);
fd_ = -1;
return false;
}
return true;
}
void V4l2Capture::close() {
if (streaming_) {
stopStreaming();
}
// MMAP mappings keep the old V4L2 queue alive even after closing its fd.
// Release them before reopening, otherwise VIDIOC_S_FMT returns EBUSY.
for (auto const& buffer : buffers_) {
if (buffer.start && buffer.start != MAP_FAILED) {
munmap(buffer.start, buffer.length);
}
}
buffers_.clear();
if (fd_ >= 0) {
::close(fd_);
fd_ = -1;
}
streaming_ = false;
last_dequeued_ = -1;
}
bool V4l2Capture::setFormat(uint32_t width, uint32_t height, uint32_t pixelformat) {
if (fd_ < 0) return false;
struct v4l2_format fmt;
std::memset(&fmt, 0, sizeof(fmt));
fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
fmt.fmt.pix.width = width;
fmt.fmt.pix.height = height;
fmt.fmt.pix.pixelformat = pixelformat;
fmt.fmt.pix.field = V4L2_FIELD_NONE; // 逐行扫描
if (ioctl(fd_, VIDIOC_S_FMT, &fmt) < 0) {
printf("V4l2Capture: VIDIOC_S_FMT failed: %s\n", strerror(errno));
return false;
}
dumpFormat(fmt, "V4l2Capture: setFormat");
width_ = fmt.fmt.pix.width;
height_ = fmt.fmt.pix.height;
pixelformat_ = fmt.fmt.pix.pixelformat;
bytes_per_line_ = fmt.fmt.pix.bytesperline;
// 如果驱动不按请求设置, 给出警告
if (width_ != width || height_ != height) {
printf("V4l2Capture: warning: requested %ux%u but got %ux%u\n",
width, height, width_, height_);
}
if (pixelformat_ != pixelformat) {
char req[5] = {0}, got[5] = {0};
req[0] = pixelformat & 0xFF; req[1] = (pixelformat>>8)&0xFF;
req[2] = (pixelformat>>16)&0xFF; req[3] = (pixelformat>>24)&0xFF;
got[0] = pixelformat_ & 0xFF; got[1] = (pixelformat_>>8)&0xFF;
got[2] = (pixelformat_>>16)&0xFF; got[3] = (pixelformat_>>24)&0xFF;
printf("V4l2Capture: warning: requested '%s' but got '%s'\n", req, got);
}
return true;
}
bool V4l2Capture::setFps(uint32_t fps) {
if (fd_ < 0) return false;
struct v4l2_streamparm parm;
std::memset(&parm, 0, sizeof(parm));
parm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
if (ioctl(fd_, VIDIOC_G_PARM, &parm) < 0) {
printf("V4l2Capture: VIDIOC_G_PARM failed: %s\n", strerror(errno));
return false;
}
// 尝试设置 FPS. 需要 V4L2_CAP_TIMEPERFRAME 支持
if (parm.parm.capture.capability & V4L2_CAP_TIMEPERFRAME) {
parm.parm.capture.timeperframe.numerator = 1;
parm.parm.capture.timeperframe.denominator = fps;
if (ioctl(fd_, VIDIOC_S_PARM, &parm) < 0) {
printf("V4l2Capture: VIDIOC_S_PARM fps=%u failed: %s\n", fps, strerror(errno));
return false;
}
printf("V4l2Capture: set fps=%u (actual %u/%u)\n",
fps,
parm.parm.capture.timeperframe.denominator,
parm.parm.capture.timeperframe.numerator);
} else {
printf("V4l2Capture: device does not support V4L2_CAP_TIMEPERFRAME\n");
}
return true;
}
bool V4l2Capture::setControl(uint32_t id, int32_t value) {
if (fd_ < 0) return false;
struct v4l2_control ctrl;
std::memset(&ctrl, 0, sizeof(ctrl));
ctrl.id = id;
ctrl.value = value;
if (ioctl(fd_, VIDIOC_S_CTRL, &ctrl) < 0) {
// 有些控制可能是只读的或不存在, 打印警告但不失败
// 继续尝试 VIDIOC_TRY_CTRL 看是否可用
struct v4l2_ext_controls ext_ctrls = {};
struct v4l2_ext_control ext_ctrl = {};
ext_ctrl.id = id;
ext_ctrl.value = value;
ext_ctrls.count = 1;
ext_ctrls.controls = &ext_ctrl;
if (ioctl(fd_, VIDIOC_TRY_EXT_CTRLS, &ext_ctrls) < 0) {
printf("V4l2Capture: VIDIOC_S_CTRL id=0x%08x value=%d failed: %s (ignored)\n",
id, value, strerror(errno));
return false;
} else {
printf("V4l2Capture: VIDIOC_S_CTRL id=0x%08x value=%d not supported (ignored)\n", id, value);
return false;
}
}
printf("V4l2Capture: set control id=0x%08x value=%d\n", id, value);
return true;
}
bool V4l2Capture::initMmap(uint32_t buffer_count) {
if (fd_ < 0) return false;
// 请求 MMAP 缓冲区
struct v4l2_requestbuffers req;
std::memset(&req, 0, sizeof(req));
req.count = buffer_count;
req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
req.memory = V4L2_MEMORY_MMAP;
if (ioctl(fd_, VIDIOC_REQBUFS, &req) < 0) {
printf("V4l2Capture: VIDIOC_REQBUFS failed: %s\n", strerror(errno));
return false;
}
if (req.count < buffer_count) {
printf("V4l2Capture: only %u buffers available (requested %u)\n", req.count, buffer_count);
buffer_count = req.count;
}
printf("V4l2Capture: allocated %u MMAP buffers\n", (unsigned)req.count);
buffers_.resize(buffer_count);
for (uint32_t i = 0; i < buffer_count; ++i) {
struct v4l2_buffer buf;
std::memset(&buf, 0, sizeof(buf));
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP;
buf.index = i;
if (ioctl(fd_, VIDIOC_QUERYBUF, &buf) < 0) {
printf("V4l2Capture: VIDIOC_QUERYBUF %u failed: %s\n", i, strerror(errno));
return false;
}
buffers_[i].length = buf.length;
buffers_[i].start = mmap(nullptr, buf.length,
PROT_READ | PROT_WRITE, MAP_SHARED,
fd_, buf.m.offset);
if (buffers_[i].start == MAP_FAILED) {
printf("V4l2Capture: mmap buffer %u failed: %s\n", i, strerror(errno));
return false;
}
buffers_[i].queued = false;
printf("V4l2Capture: buffer %u: addr=%p len=%zu\n",
i, buffers_[i].start, buffers_[i].length);
}
return true;
}
bool V4l2Capture::startStreaming() {
if (fd_ < 0 || streaming_) return false;
// 把所有缓冲区入队
for (uint32_t i = 0; i < buffers_.size(); ++i) {
struct v4l2_buffer buf;
std::memset(&buf, 0, sizeof(buf));
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP;
buf.index = i;
buf.bytesused = 0; // 驱动会填充
if (ioctl(fd_, VIDIOC_QBUF, &buf) < 0) {
printf("V4l2Capture: VIDIOC_QBUF %u failed: %s\n", i, strerror(errno));
return false;
}
buffers_[i].queued = true;
}
// 启动流
int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
if (ioctl(fd_, VIDIOC_STREAMON, &type) < 0) {
printf("V4l2Capture: VIDIOC_STREAMON failed: %s\n", strerror(errno));
return false;
}
streaming_ = true;
printf("V4l2Capture: streaming started\n");
return true;
}
bool V4l2Capture::stopStreaming() {
if (fd_ < 0 || !streaming_) return false;
int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
if (ioctl(fd_, VIDIOC_STREAMOFF, &type) < 0) {
printf("V4l2Capture: VIDIOC_STREAMOFF failed: %s\n", strerror(errno));
return false;
}
streaming_ = false;
last_dequeued_ = -1;
printf("V4l2Capture: streaming stopped\n");
return true;
}
bool V4l2Capture::getFrame(Frame &frame, int timeout_ms) {
if (fd_ < 0 || !streaming_) { reconnect_needed_ = true; return false; }
reconnect_needed_ = false;
fd_set fds;
FD_ZERO(&fds);
FD_SET(fd_, &fds);
struct timeval tv;
tv.tv_sec = timeout_ms / 1000;
tv.tv_usec = (timeout_ms % 1000) * 1000;
int r = select(fd_ + 1, &fds, nullptr, nullptr, &tv);
if (r < 0) {
if (errno == EINTR) return getFrame(frame, timeout_ms); // 重试
reconnect_needed_ = true;
printf("V4l2Capture: select failed: %s\n", strerror(errno));
return false;
}
if (r == 0) {
// 超时
return false;
}
struct v4l2_buffer buf;
std::memset(&buf, 0, sizeof(buf));
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP;
if (ioctl(fd_, VIDIOC_DQBUF, &buf) < 0) {
if (errno == EAGAIN || errno == EIO) {
return false; // 再试
}
reconnect_needed_ = true;
printf("V4l2Capture: VIDIOC_DQBUF failed: %s\n", strerror(errno));
return false;
}
// 如果之前有未归还的缓冲区, 先归还
if (last_dequeued_ >= 0) {
releaseFrameByIndex(last_dequeued_);
}
uint32_t idx = buf.index;
if (idx >= buffers_.size()) {
reconnect_needed_ = true;
printf("V4l2Capture: invalid buffer index %u\n", idx);
return false;
}
last_dequeued_ = idx;
if ((buf.flags & V4L2_BUF_FLAG_ERROR) || buf.bytesused > buffers_[idx].length) {
releaseFrameByIndex(idx);
last_dequeued_ = -1;
return false;
}
frame.data = buffers_[idx].start;
frame.size = buf.bytesused;
frame.width = width_;
frame.height = height_;
frame.pixelformat = pixelformat_;
frame.bytes_per_line = bytes_per_line_;
frame.sequence = buf.sequence;
frame.timestamp_us = (uint64_t)buf.timestamp.tv_sec * 1000000ULL + buf.timestamp.tv_usec;
frame.index = idx;
return true;
}
void V4l2Capture::releaseFrame(const Frame &frame) {
if (frame.index >= 0) {
releaseFrameByIndex(frame.index);
last_dequeued_ = -1;
}
}
void V4l2Capture::releaseFrameByIndex(int idx) {
if (idx < 0 || (uint32_t)idx >= buffers_.size()) return;
struct v4l2_buffer buf;
std::memset(&buf, 0, sizeof(buf));
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP;
buf.index = idx;
if (ioctl(fd_, VIDIOC_QBUF, &buf) < 0) {
printf("V4l2Capture: VIDIOC_QBUF %d failed: %s\n", idx, strerror(errno));
}
buffers_[idx].queued = true;
}
src/visualizer.cpp
+19 -3
View File
@@ -3,17 +3,33 @@
#include <opencv2/highgui.hpp>
#include <opencv2/highgui/highgui_c.h>
#include <opencv2/imgproc.hpp>
#include <chrono>
namespace cms {
namespace { constexpr char windowName[]="CMS Camera Risk"; }
void runLocalVisualization(CameraPipeline const& pipeline) {
void runLocalVisualization(CameraPipeline const& pipeline,bool keepFinalFrame) {
struct Window {
Window(){cv::namedWindow(windowName,cv::WINDOW_NORMAL);cv::resizeWindow(windowName,1000,800);}
~Window(){try{if(cvGetWindowHandle(windowName))cv::destroyWindow(windowName);}catch(...) {}}
} window;
FrameResultPtr shown;
while(pipeline.running()) {
auto nextRefresh=std::chrono::steady_clock::now();
for(;;) {
bool running=pipeline.running();
if(!running&&!keepFinalFrame)break;
auto result=pipeline.latest();
if(result&&result!=shown){cv::imshow(windowName,renderResult(*result));shown=std::move(result);}
if(!running&&!result)break;
auto now=std::chrono::steady_clock::now();
if(result!=shown||now>=nextRefresh) {
auto can=pipeline.canStatus();cv::Mat image;
if(result)image=renderVisualization(*result,can);
else {
image=cv::Mat(800,1000,CV_8UC3,cv::Scalar(20,24,30));
cv::putText(image,"Waiting for inference...",{24,64},cv::FONT_HERSHEY_SIMPLEX,.8,{235,235,235},2);
cv::putText(image,can.status,{24,120},cv::FONT_HERSHEY_SIMPLEX,.6,{235,235,235},1);
}
cv::imshow(windowName,image);shown=std::move(result);
nextRefresh=now+std::chrono::milliseconds(100);
}
int key=cv::waitKey(16)&0xff;
if(key==27||key=='q'||key=='Q'||cvGetWindowHandle(windowName)==nullptr)return;
}
tests/ORIN_391_REPORT.md
+33
View File
@@ -0,0 +1,33 @@
# 0x391 实际位置反馈与 CH0 单通道修复（2026-09-28）
用户要求显示四门实际位置反馈，并明确不使用 CH1。本次保留 `0x391` 第 0/1/2/3 字节到 LF/RF/LR/RR 的无符号原始值映射，不改为门状态、不引入未经确认的百分比或角度换算。
实时模式仅初始化、启动、接收和发送 CH0；拒绝向 CH1 调用发送 API，忽略合并接收中的 CH1 数据，画面仅显示 CH0，并标明 ACTUAL POSITION。历史会话的双通道快照结构与回灌显示保持兼容，`sendRisk()` 返回值第二槽固定为 false。
## 实机现象与修复结果
用户第一次确认左前门打开后，原始接收工具取得 CH0、CANFD、64 字节的 `0x391`，前四字节为 `1F 00 00 00`，即左前门反馈 31。由此确认实际位置字段映射有真实报文依据。
修复前，真实摄像头/TensorRT 与原双通道风险发送同时运行时，接收计数长时间停顿；反馈年龄最高观察到约 5.84 秒。CH0 发送成功、CH1 发送失败，接收在发送等待期间停顿。仅将发送移到后台线程不能消除适配器发送等待对接收的影响。
最终仅 CH0 版本在同一硬件上运行：10 秒采样内 RX 持续增长，TX 到 299、TX 错误为 0，各采样点保持 LIVE。用户再次确认左前门打开后，最终版同一进程显示实际位置 `26,0,0,0`，末次采样更新为 `25,0,0,0`，0x391 计数从 4 增至 33。显示层由相同 `renderResult()` 绘制，已检查 [CAN 画面裁剪](orin_391_overlay.png)，可见 LF 26，CH1 未显示。
详细结果与运行二进制校验见 [实机结果](orin_391_results.json)。原始日志为 `/tmp/vap_391_open.log`，对照采样为 `/tmp/vap_391_after.log`，采样源码为 `/tmp/vap_391_pipeline.cpp`。
## 本机模拟、板端故障注入、硬件覆盖
- 本机模拟：`tests/run_can.sh`（UBSan、模拟 SDK）通过。检查 CH1 不初始化/配置/启动/重置/发送、CH1 输入不覆盖 CH0、实际位置更新、CAN/CANFD、过期恢复、单通道风险报文、发送失败与恢复、后台最新值覆盖、停止清理。
- 本机模拟：`tests/run_session.sh` 通过，保持已有会话 CAN 快照兼容；构建与 `git diff --check` 通过。
- 板端故障注入：未执行。本次未修改共享摄像头采集、停止或恢复逻辑。
- 真实硬件：Orin 上使用既有 `/dev/video5`、TensorRT 引擎、USBCANFD-200U 与配置；用户操作左前门，验证实际位置进入推理期间的实时显示。采样程序调用画面绘制，未运行 GUI 事件循环；正式程序另按原命令 `./run_camera.sh` 启动验证。
- 未覆盖：其余三扇门逐一动作、长期运行、CH0 断开/缺失 ACK、独立总线分析仪验证风险报文。接收错误计数仍增长（画面记录 RX errors 5801），本次未确定该总线错误的原因，不能宣称总线无错误。
## 部署与版本
最终运行 `/proc/<pid>/exe`、`build/camera_risk` 和 `orin/build/camera_risk` SHA256 一致：
`418637a35f6ad05441578db1333e15714262a9c0ee088f10c2dd84923471cd0f`
独立包可执行文件已更新，`orin/SHA256SUMS` 已刷新并完整校验。正式程序使用原命令 `./run_camera.sh`，SIGTERM 后约 0.868 秒退出、退出码 0。测试前未发现常驻 camera_risk，测试后没有保留临时采样进程；没有修改原启动参数。
硬件验证时，本机即 Orin 工作区，检出 `inference-only`，HEAD 为 `515a9f32d8c0ebf83503d4d240ffe917dc09950c`；本次联网读取 Git 远端 `inference-only` 为 `0192fccbf06cb928eac098b3dfad32abf0fc370d`。上述硬件验证在未提交工作区进行。经用户要求，最终修复、后台风险发送、部署产物及验证记录一并纳入本报告所在提交；Git 远端尚未推送同步。以上二进制哈希标识已验证的本地部署产物。
tests/ORIN_CAN_REPORT.md
+32
View File
@@ -0,0 +1,32 @@
# CAN 收发与 0x391 显示实现记录（2026-09-24）
范围：仅当前 `inference-only` 分支。用户明确当前并非真实 CAN 环境，接收实测暂不验证；真实 CAN 接收和发送均未验收。
## 已完成的模拟和离线检查
- `tests/run_can.sh`：UBSan + 模拟 SDK，无硬件访问。覆盖四个连续字节独立解析、双通道隔离、CAN/CANFD 数据帧、过期与恢复、无关 ID / 短报文不刷新状态、按需发送编码、非法 ID/通道/长度/BRS、发送失败、接收错误、缺失适配器、部分初始化失败清理、设备单一所有权和停止后拒绝发送。
- `tests/run_session.sh`：人工图像 H.264 往返与过程记录，新增 CAN 快照保存/读取检查，四门值、有效状态与年龄保持不变。
- 静态可视化检查：人工构造 CH0 LIVE、CH1 STALE 快照，四门不同值均可见，显示收发计数和报文年龄，未读取任何 CAN 设备。临时图 `/tmp/vap_can_overlay.png`。
- 源码构建通过，SDK 和 libusb 从独立包加载，配置和 `--no-can` 接入完成。
以上均不能代替真实 CAN 总线验证。
## 已执行的 Orin 摄像头 / TensorRT 检查
在用户要求暂停 CAN 实测前执行过 `tests/orin_session_test.py`，测试目录 `/tmp/vap-can-hardware-20260924/`。当时 USB 节点为 `root:root 0664`，SDK 打开失败，状态为 UNAVAILABLE，实际接收数为 0。
在这一缺失 CAN 的回退条件下，真实 `/dev/video5` 与 TensorRT 完成 13 帧 H.264 录制、13 帧完整回灌，CAN UNAVAILABLE 快照在回灌中保持一致并标记 `source=replay`。13 帧检测到人员；跟踪 ID 一致 13/13，风险决策一致 8/13。有损视频产生姿态/风险差异，不能把视频回灌解释为算法输出逐位相同。SIGTERM 退出 1.317 秒，回灌 SIGINT 退出 0.916 秒。完整原始结果在 `orin_can_results.json`。
此项仅证明真实摄像头/TensorRT、无 CAN 回退、状态录制/回灌及停止，不证明 CAN 收发。
## 设备访问准备与延期项目
已读取 USB 描述确认 `3068:0009` 为 ZLG USBCANFD-200U。经批准安装仅针对该 VID/PID 的 udev 规则，将设备设为 `root:plugdev 0660`；orin 原已属于 plugdev。规则及安装脚本均纳入独立包。权限修复后的接收测试被用户取消，未启动；后续没有再打开适配器接收或发送报文。
未验证实际总线 `0x391` 布局/物理单位、CAN/CANFD 真实发送或 ACK、双通道接线、波特率/终端电阻与实车匹配、硬件断开重连、长时间运行。没有新增板端故障注入或物理拔插操作。发送仅提供按需 C++ API，未定义或自动发送任何业务控制报文。
## 部署校验
本次程序 SHA256：`e8788d22fd6db0e3b9edc95a02322a51ca7951e1df81fea10f34db7da3143461`。
权限修复前的板端测试核对过 `/proc/<pid>/exe` 与源码构建文件一致。更新独立包时核对源码构建与 `orin/build/camera_risk` 哈希一致，并刷新 `orin/SHA256SUMS`（包含 SDK 库及 SONAME 链接）。用户要求暂不验证接收后，仅做文件和依赖校验，不再启动 CAN 测试进程。测试前没有常驻推理进程，没有修改既有启动参数。
tests/ORIN_CH1_TX_REPORT.md
+25
View File
@@ -0,0 +1,25 @@
# CAN CH1 发送切换（2026-09-29）
实时 CAN 改为 CH0 只接收、CH1 只发送。CH0 与 CH1 都使用 60 MHz 时钟、仲裁域 500 kbps、数据域 2 Mbps，并随设备生命周期一同初始化、启动和复位；合并接收仍由 CH0 的单一接收线程执行，收到的 CH1 数据被忽略。自动风险报文和按需 `sendCan()` 均只允许 CH1，CH0 发送参数会抛出 `std::invalid_argument`。
风险协议未改变：标准 CAN `0x1F6`、DLC 8，首字节 `clear=0x00`，其余状态 `0x11`，后七字节清零；首次状态发送一次，之后仅在二值状态变化时发送。摄像头采集、停止恢复、0x391 解析和实时画面只显示 CH0 的行为未改动。
## 本机模拟回归
`tests/run_can.sh` 通过（模拟 SDK + UBSan）。测试桩强制要求 CH0/CH1 均完成初始化、终端电阻配置、启动和复位，所有经典 CAN/CANFD 发送调用必须使用 CH1；覆盖 CH0 接收及 CH1 输入忽略、按需发送校验、风险变化触发、发送失败、阻塞合并和停止清理。`tests/run_session.sh` 通过，会话兼容性未受影响；完整 CMake 构建和 `git diff --check` 通过。
## Orin 真实硬件验证
使用当前 Orin 的 `/dev/video5`、TensorRT 引擎、USBCANFD-200U 和原 `config/camera.json`，以 `./run_camera.sh --no-visualize --save-results` 运行 12 秒。SDK 识别到双通道适配器并打印 `CH0 receive, CH1 transmit`；产生 260 个结果 JSON，末次快照为 CAN `OPEN`、CH1 成功发送 1 帧、发送错误 0，CH0 的 `0x391` 计数为 11，证明摄像头/TensorRT 推理、CH0 接收和 CH1 SDK 发送可在同一进程继续运行。
本次未进行板端故障注入或物理拔插，没有人为触发风险 0/1 变化，也未使用独立总线分析仪或 CH1 接收节点核验线上 `0x1F6` 的电气载荷与 ACK。因此“CH1 线上报文已被对端接收”仍未覆盖，不能由 SDK 成功计数替代。测试前无常驻 `camera_risk`，测试结束恢复为无常驻进程；原配置和启动脚本未修改。
详细计数与版本见 [实机结果](orin_ch1_tx_results.json)。
## 部署核对
源码构建 `build/camera_risk` 与独立包 `orin/build/camera_risk` SHA256 一致：
`feca0b7f476171f45489c22f11a51ba1fe0dabe2d9a17bfcdc50110d1afdb276`
独立包已运行 `sha256sum -c orin/SHA256SUMS` 并全部通过。当前检出分支为 `inference-only`，HEAD `f87e5aad40478824da16eb42c76b40383de3a9a1`；修改位于未提交工作区。联网只读核对远端 `inference-only` 为 `0192fccbf06cb928eac098b3dfad32abf0fc370d`，本次未创建提交或推送远端。
tests/ORIN_FPS_REPORT.md
+41
View File
@@ -0,0 +1,41 @@
# 实时摄像头 FPS 修复（2026-09-28）
> 以下为后台双通道发送与 FPS 修复阶段的历史验证记录，通道、二进制哈希与 Git 状态均指当时版本。最终 CH0 单通道实现和实机结果见 [0x391 修复记录](ORIN_391_REPORT.md)。
用户通过默认 `./run_camera.sh` 运行实时摄像头时卡顿。原因已在宿主 Orin 复现：`inferenceLoop()` 同步发送 CH0、CH1 的 `0x1F6`，SDK 发送失败路径两通道合计等待约 3 秒，使结果发布和后续推理一起停顿。
## 真实硬件对照
设备：Jetson AGX Orin、`/dev/video5` 1920×1536 30 FPS、现有 YOLO/RTMW TensorRT 引擎、ZLG USBCANFD-200U。保留配置、模型、CAN 波特率和终端电阻设置；未调整功耗或时钟。
使用链接相应版本 `camera_risk_core` 的临时采样程序，各运行约 15 秒，按实际 published 结果增量计算 FPS，并调用 `renderResult()` 绘制。该指标为真实推理输出吞吐，非相机配置 FPS；采样程序不调用 GUI 的 imshow/waitKey，不录制、不保存图片。
| 路径 | 稳定输出 | 风险融合及发送阶段 | 结果延迟 | CAN 状态 |
|---|---|---|---|---|
| 修复前，同步双通道 CAN | 约 0.33 FPS | 3016–3019 ms/帧 | 随积压升至约 12 秒 | OPEN，TX=0，发送错误递增 |
| 修复前，仅对照关闭 CAN | 29.93–30.00 FPS | 约 5–6.6 ms/帧 | 约 42–46 ms | DISABLED |
| 修复后，后台双通道 CAN | 29.97–30.00 FPS | 约 6–6.9 ms/帧 | 约 46–50 ms | OPEN，TX=0，发送错误递增 |
首秒包含启动过渡，后两组约 27 FPS，不纳入稳定区间。画面绘制约 19 ms/帧。另做真实 TensorRT 视频回灌参考采样约 43–48 FPS，不能代替实时摄像头验证。
## 修改与模拟回归
实时推理改用 `queueRisk()`，不等待 SDK。独立发送线程处理一对双通道帧，最多保留一个待发风险，新的覆盖尚未发送的旧结果；没有无限队列或定时旧值重发。`0x1F6`、DLC 8、`clear→0x00`/其他→`0x11` 和后七字节清零的协议保持不变。按需 `sendCan()` 仍是同步接口。
停止时清空待发风险，唤醒后台线程，等待正在执行的 SDK 调用返回，再释放设备；不持有 SDK 生命周期锁等待发送线程，避免死锁。CAN 快照反映已完成的发送，不保证包括当前推理帧。
`tests/run_can.sh`（模拟 SDK + UBSan）通过，新增阻塞发送期间 1000 次提交、最新结果替换、双通道、失败和停止清理覆盖；构建及 `git diff --check` 通过。未进行板端故障注入。
## 正式程序和文件核对
经宿主进程检查确认测试前没有 `camera_risk`，最后按原命令 `./run_camera.sh` 启动正式程序，运行约 12 秒后 SIGTERM，退出码 0、停止约 1.267 秒。未改启动参数；测试后确认没有遗留进程。记录位于 `/tmp/vap-fps-default-verification.json` 和 `/tmp/vap-fps-default-run.log`；临时采样源码为 `/tmp/vap_fps_probe.cpp`。
实际 `/proc/<pid>/exe`、`build/camera_risk`、`orin/build/camera_risk` SHA256 均为：
`a84680513aa61fb2af353f97e6369bbef80c9f12521ecd88454e72fcecab447f`
独立包 SHA256SUMS 已刷新并完整校验。修改留在本地工作区，未提交/推送；版本引用详情见 [风险发送记录](RISK_CAN_REPORT.md)。
## 未覆盖项
物理 CAN 发送依然失败，本次只确认该等待已与推理解耦，未确认 ACK、线缆、接收端或总线配置的故障原因。未使用独立总线分析仪验收线上载荷，也未测试物理断连重连或长期耐久。当前场景约 30 FPS 不代表任意人数/场景都能达到同样吞吐。
tests/ORIN_RISK_CHANGE_REPORT.md
+27
View File
@@ -0,0 +1,27 @@
# CAN 风险变化触发发送（2026-09-28）
实时自动发送仍仅使用 CH0、标准 CAN `0x1F6`、8 字节；首字节 `clear=0x00`，其余状态 `0x11`，后七字节清零。首个风险状态尝试发送一次，之后仅在报文的二值风险变化时发送。`watch/warning/danger/unavailable` 间切换不重复发送。
后台最多保留最新待发状态。发送 A 期间若状态 A→B→A，取消冗余的待发 A；短暂中间状态可能被合并。发送失败计入错误，不随后续相同状态重试，也无周期重发；发生新的二值变化才触发后续发送。显式按需 `sendCan()` / `sendRisk()` 保持调用即尝试发送的接口语义。摄像头采集、停止恢复和 0x391 解析未改动。
## 本机模拟回归
`tests/run_can.sh` 通过（模拟 SDK + UBSan）。新增首次 clear、首次非 clear、1000 次相同状态去重、不同非 clear 等级去重、0/1 往返、失败后相同状态不重试、新变化恢复发送、发送阻塞期间回到原状态不重复发帧，以及后续 clear 不丢失。原有收发、CH0 限制、过期恢复、最新状态合并和停止回归通过。构建与 `git diff --check` 通过。
## 真实硬件验证
使用 Orin 现有摄像头、TensorRT 引擎、USBCANFD 和原配置。12 秒产生 357 个推理结果，观测风险始终为 `unavailable`（二值 1），CH0 成功发送 1 帧，发送错误为 0；0x391 接收计数持续增长至 15。证明稳定风险不会随每个推理结果重复发送，推理与位置反馈接收持续工作。
本次未进行板端故障注入或物理拔插，未人为向实车注入伪造风险状态。实机场景没有发生风险 0/1 切换，变化载荷和失败路径由本机模拟回归覆盖；尚未以独立总线分析仪核验线上报文，未覆盖长期运行或接收端重启后状态重新同步。不能把模拟切换测试当作真实场景风险切换验证。
采样源码 `/tmp/vap_risk_change_probe.cpp`，原始日志 `/tmp/vap_risk_change_probe.log`。详细计数与版本见 [实机结果](orin_risk_change_results.json)。
## 部署核对
另按原命令 `./run_camera.sh` 启动正式程序；SIGTERM 后约 0.919 秒退出，退出码 0。测试前无常驻 camera_risk，测试结束恢复无常驻进程状态，未修改启动参数。
运行中 `/proc/<pid>/exe`、源码构建 `build/camera_risk`、独立包 `orin/build/camera_risk` SHA256 一致：
`5fa01c08baf73b1e044288b5e85f753c10f10d7cc9559ff25952bc4618f01899`
独立包已更新并刷新 `orin/SHA256SUMS`。硬件验证时本机即 Orin，检出 `inference-only`，HEAD 为 `f87e5aad40478824da16eb42c76b40383de3a9a1`，当时修复位于未提交工作区。经用户要求，最终修改和验证记录纳入本报告所在本地提交。验证时联网核对远端 `inference-only` 为 `0192fccbf06cb928eac098b3dfad32abf0fc370d`，未推送本次修改。
tests/ORIN_SESSION_REPORT.md
+45
View File
@@ -0,0 +1,45 @@
# H.264 推理过程录制与回灌验证（2026-09-24）
范围：按本次要求，仅修改 `inference-only`。测试直接在当前 `orin-desktop` 的 Jetson Orin 上执行；真实 `/dev/video5`，1920×1536 UYVY，L4T R35.4.1，CUDA 11.4，TensorRT，OpenCV 4.5.4 / FFmpeg H.264。未使用 SSH。
## 本机模拟与编解码回归
- `tests/run_session.sh`：实际 H.264 编码/解码，使用人工图像及模拟结果，不加载 TensorRT。覆盖有步长的 BGR 输入、视频帧顺序、帧号间隔、配置恢复、原始时间戳保留、编码器刷新、正常 EOF、空会话、拒绝覆盖、缺失 JSON、非法/截断索引、不完整会话和奇数尺寸。启用 UBSan。
- `tests/run_camera_timeout.sh`：模拟 V4L2 采集；通过超时重试、有效帧后重新计数、第六次超时失败与主动停止回归。此项不是本次真实硬件故障恢复的证明。
## 真实摄像头与 TensorRT
脚本：`tests/orin_session_test.py`。完整数据见 `orin_session_results.json`。
| 项目 | 结果 |
|---|---|
| 摄像头录制 | 13 个完成推理的帧，H.264 / MP4，1920×1536 |
| 视频体积 | 303,989 字节；同帧数未压缩 BGR 为 115,015,680 字节，约缩小 378 倍，仅代表本次静态场景 |
| 完整回灌 | 13/13 帧，顺序与原帧号一致，原始采集元数据保留，EOF 正常退出 |
| 结果检查 | 13/13 跟踪 ID 与风险决策一致；本次无检测到的人员 |
| 录制期间 SIGTERM | 1.417 秒完整退出，视频封装成功 |
| 回灌期间 SIGINT | 0.865 秒完整退出，保存 2 帧后停止，视频封装成功 |
| 实际运行程序核对 | `/proc/<pid>/exe` 与源码构建二进制 SHA256 相同 |
实机首次测试发现：若在 `CameraPipeline` 构造期间打开视频解码器，FFmpeg 工作线程可能在 `StopSignals` 安装前创建，SIGINT 可直接终止进程。已将解码器打开操作移至继承信号掩码的采集线程，修复后重跑上述实机测试全部通过。
测试前没有 `camera_risk` 进程，未更改既有启动参数；测试脚本仅管理自己启动的进程，测试后不留下常驻进程。原始视频、逐帧记录与日志在 `/tmp/vap-video-session-hardware-20260924-fixed/`，不提交摄像头图像。
## 板端故障注入与范围限制
本次未做板端 V4L2 故障注入或物理拔线操作；共享相机恢复实现保持原样，其既有硬件报告见 `ORIN_TEST_REPORT.md`，不能视作本次新增功能的实测覆盖。当前实机画面没有检测到人员，RTMW 完成初始化/预热，但未覆盖有人员的逐帧姿态推理、遮挡隔离推理或风险变化；报告中的关键点差异为 0 不代表有人员场景的精度结论。
未覆盖 GUI 预览、GPU 故障、磁盘写满、断电/强杀后的媒体恢复、长时间录制与 NVENC/NVDEC 硬件编解码。当前为软件 H.264 编解码；强杀留下的未完成会话按设计拒绝回灌。
## 经测试的程序 SHA256
`0f178057529eb8be44bb9a639504dfe57a1e353a84d2351fd907304f6d951c11`
独立包 `orin/build/camera_risk` 已更新，`orin/SHA256SUMS` 全部校验通过。使用独立包实际运行并完整回灌 13 帧，再次核对 `/proc/<pid>/exe` 与上述哈希一致；部署检查日志位于 `/tmp/vap-video-session-deployment-20260924/`。测试结束恢复原来的无常驻进程状态。
## 默认开启可视化补充验证（2026-09-24）
实时推理、录制和回灌默认显示全部现有叠加；新增 `--no-visualize`，无显示环境自动跳过预览。已在 Orin 的真实桌面上通过 X11 窗口列表确认默认创建 `CMS Camera Risk` 窗口，并使用更新后的独立包完成真实摄像头 13 帧录制、13 帧完整回灌、录制 SIGTERM（1.267 秒）及回灌 SIGINT（0.916 秒）检查。另验证 `--no-visualize` 和清除 DISPLAY / WAYLAND_DISPLAY 后均能正常完成 TensorRT 回灌。
新二进制 SHA256：`cd9711d71c6d04f40adf9434f893abe1744e9e2013f1459ddf736a80dcbbfd64`。源码构建文件、独立包文件及 `/proc/<pid>/exe` 一致；`orin/SHA256SUMS` 已刷新。完整数据在 `orin_session_results.json` 的 `default_visualization` 中，日志位于 `/tmp/vap-default-preview-20260924/`。此次未重复模拟采集和板端故障注入；仍未覆盖有人员时的关键点显示、窗口按钮/键盘关闭操作。测试前后均无常驻推理进程。
tests/RISK_CAN_REPORT.md
+27
View File
@@ -0,0 +1,27 @@
# 0x1F6 二值风险发送检查（2026-09-28）
> 以下为双通道风险发送阶段的历史验证记录，通道、二进制哈希与 Git 状态均指当时版本。最终 CH0 单通道实现和实机结果见 [0x391 修复记录](ORIN_391_REPORT.md)。
实现位于当前 `inference-only` 工作区：每个新的实时推理结果提交给后台 CAN 发送线程，在 CH0、CH1 各尝试发送一帧标准 CAN 数据报文，ID `0x1F6`、DLC 8。首字节 bit0–3 和 bit4–7 同时填写相同风险值，`clear` 时各为 0，其他状态（含无人或证据不足时的 `unavailable`）各为 1，即首字节为 `0x00` 或 `0x11`，后七字节清零。最多保留一个待发结果，忙时由最新结果替换，不保证每个推理帧都上总线。回灌和禁用 CAN 时不自动发送，没有独立周期或旧结果重发。
## 本机模拟与构建
- `./tests/run_can.sh` 通过（UBSan、模拟 ZLG SDK）：覆盖 clear/watch/warning/danger/unavailable/未知/空状态、高低四位相同赋值、恢复 clear 后两者同时清零、标准帧标志、ID、DLC、完整载荷、每结果双通道各一帧、任一通道失败不影响另一通道尝试、收发计数、停止和禁用后不访问发送 API。
- 后台发送补充回归通过：模拟 SDK 阻塞时可连续提交 1000 个新风险并读取快照；释放阻塞后仅发送正在处理及最新待发的两对报文，没有旧结果积压；停止时丢弃待发结果，等待 SDK 返回后释放设备。单通道失败不抑制另一通道发送。
- `./tests/run_session.sh` 通过：人工图像的 H.264 编解码、录制/回灌顺序、CAN 快照保存、停止收尾与损坏数据检查。这不是摄像头/TensorRT 验证，也不证明实际推理循环的总线发送。
- `cmake --build build -j2` 通过；`git diff --check` 通过。
- `--help` 显示新的发送规则并返回 0；SDK 初始化同时报告 NvRmMem/Nvmap 设备不可用，不能据此声称 GPU 推理可运行。
## 板端故障注入与真实硬件操作
后续经授权在宿主 Orin 上完成真实摄像头、TensorRT 和 USB-CAN 性能对照。原同步发送路径约 0.33 FPS，SDK 双通道等待约 3 秒且均失败；改为后台发送后约 30 FPS，CAN 仍报告发送失败。详见 [FPS 修复记录](ORIN_FPS_REPORT.md)。未进行板端故障注入或物理拔插。未修改启动配置和参数，测试前后均无常驻程序。
未覆盖：真实总线成功发送与 ACK、双通道线上载荷、回灌时物理总线无发送、断连重连、长期运行。模拟通过及 FPS 恢复不代表这些项目已验收。
## 文件同步与版本状态
已更新工作区独立包 `orin/build/camera_risk`，刷新 `orin/SHA256SUMS`，完整清单校验通过；源码构建文件与独立包二进制 SHA256 相同：
`a84680513aa61fb2af353f97e6369bbef80c9f12521ecd88454e72fcecab447f`
本次改动未提交、未推送，未进行 Git 发布。当前本地 HEAD 为 `515a9f32d8c0ebf83503d4d240ffe917dc09950c`；本地保存的 `origin/inference-only` 引用为 `0192fccbf06cb928eac098b3dfad32abf0fc370d`，未联网核对远端最新值。未修改共享采集、相机停止或恢复逻辑；其他两分支没有本次修改的 `src/can_bus.cpp`，未同步。已按原命令 `./run_camera.sh` 启动正式程序，核对 `/proc/<pid>/exe`、构建文件和独立包哈希一致，SIGTERM 约 1.267 秒正常退出，恢复测试前无常驻进程状态。
tests/can_bus_test.cpp
+249
View File
@@ -0,0 +1,249 @@
#include "can_bus.hpp"
#include "zcan.h"
#include <atomic>
#include <chrono>
#include <condition_variable>
#include <deque>
#include <iostream>
#include <mutex>
#include <stdexcept>
#include <thread>
#include <limits>
namespace {
using namespace std::chrono_literals;
std::mutex mutex;
std::condition_variable changed;
std::deque<ZCANDataObj> incoming;
std::atomic<int> opens{0},closes{0},initializations{0},starts{0},resets{0},terminationCalls{0},mergeCalls{0},txCalls{0},rxCalls{0};
std::atomic<bool> unavailable{false},initFailure{false},receiveFailure{false},txFailure{false};
ZCAN_20_MSG sentCan{};ZCAN_FD_MSG sentFd{};
std::vector<ZCAN_20_MSG> sentCanFrames;
int failedTxChannel=-1;
std::mutex txMutex;
std::condition_variable txWake;
std::atomic<bool> blockTx{false},txBlocked{false};
void check(bool ok,char const* message){if(!ok)throw std::runtime_error(message);}
template<class F> void waitFor(F f) {
auto deadline=std::chrono::steady_clock::now()+2s;
while(!f()){check(std::chrono::steady_clock::now()<deadline,"Asynchronous CAN condition timed out");std::this_thread::sleep_for(1ms);}
}
template<class F> void rejects(F f){bool failed=false;try{f();}catch(std::exception const&){failed=true;}check(failed,"Expected rejection");}
void reset() {
opens=closes=initializations=starts=resets=terminationCalls=mergeCalls=txCalls=rxCalls=0;
unavailable=initFailure=receiveFailure=txFailure=false;
sentCanFrames.clear();failedTxChannel=-1;
blockTx=false;txBlocked=false;
std::lock_guard<std::mutex> lock(mutex);incoming.clear();
}
void inject(unsigned channel,bool fd,std::uint32_t id,std::initializer_list<std::uint8_t> bytes,unsigned stamp=123) {
ZCANDataObj obj{};obj.chnl=channel;obj.dataType=fd?ZCAN_DT_ZCAN_CANFD_DATA:ZCAN_DT_ZCAN_CAN_DATA;
auto& hdr=fd?obj.data.zcanCANFDData.hdr:obj.data.zcanCANData.hdr;
hdr.id=id;hdr.chn=channel;hdr.len=bytes.size();hdr.ts=stamp;
auto* data=fd?obj.data.zcanCANFDData.dat:obj.data.zcanCANData.dat;
std::copy(bytes.begin(),bytes.end(),data);
{std::lock_guard<std::mutex> lock(mutex);incoming.push_back(obj);}changed.notify_all();
}
}
extern "C" {
U32 VCI_OpenDevice(U32 type,U32,U32){check(type==33,"Device type changed");++opens;return unavailable?0:1;}
U32 VCI_CloseDevice(U32,U32){++closes;return 1;}
U32 VCI_InitCAN(U32,U32,U32 port,ZCAN_INIT* init){
check(port<2,"Unexpected CAN channel initialized");++initializations;
check(init->clk==60000000&&init->mode==0&&init->aset.brp==5&&init->dset.brp==1,"Reference timing not retained");
return initFailure?0:1;
}
U32 VCI_SetReference(U32,U32,U32 port,U32 ref,void* data){
check(port<2,"Unexpected CAN channel configured");
if(ref==CMD_CAN_TRES){check(*static_cast<U32*>(data)==1,"Invalid termination configuration");++terminationCalls;}
else if(ref==ZCAN_CMD_SET_CHNL_RECV_MERGE){check(port==0&&*static_cast<U32*>(data)==1,"Invalid merged receive configuration");++mergeCalls;}
else check(false,"Unexpected CAN reference configuration");
return 1;
}
U32 VCI_StartCAN(U32,U32,U32 port){check(port<2,"Unexpected CAN channel started");++starts;return 1;}
U32 VCI_ResetCAN(U32,U32,U32 port){check(port<2,"Unexpected CAN channel reset");++resets;return 1;}
U32 VCI_ReceiveData(U32,U32,U32 port,ZCANDataObj* out,U32 capacity,U32){
check(port==0,"Multiple merged receive readers");++rxCalls;
if(receiveFailure)return std::numeric_limits<U32>::max();
std::unique_lock<std::mutex> lock(mutex);
changed.wait_for(lock,10ms,[]{return !incoming.empty();});
U32 count=0;while(!incoming.empty()&&count<capacity){out[count++]=incoming.front();incoming.pop_front();}return count;
}
U32 VCI_Transmit(U32,U32,U32 port,ZCAN_20_MSG* message,U32 count){
check(port==1&&count==1&&port==message->hdr.chn,"CAN send did not use CH1");++txCalls;sentCan=*message;
{
std::unique_lock<std::mutex> lock(txMutex);sentCanFrames.push_back(*message);
if(blockTx) {
txBlocked=true;txWake.notify_all();
if(!txWake.wait_for(lock,2s,[]{return !blockTx.load();}))throw std::runtime_error("Test send gate timed out");
}
}
return txFailure||int(port)==failedTxChannel?0:1;
}
U32 VCI_TransmitFD(U32,U32,U32 port,ZCAN_FD_MSG* message,U32 count){
check(port==1&&count==1&&port==message->hdr.chn,"CANFD send did not use CH1");++txCalls;sentFd=*message;return txFailure?0:1;
}
}
int main()try {
using namespace cms;
reset();{
CanConfig c;c.enabled=false;CanBus bus(c);bus.start();
check(bus.snapshot().status=="DISABLED"&&!bus.send({1,0x123,false,false,false,{1,2}}),"Disabled CAN used hardware");
}check(opens==0,"Disabled CAN opened device");
reset();unavailable=true;{
CanBus bus({});bus.start();check(bus.snapshot().status=="UNAVAILABLE","Missing adapter not reported");
}check(closes==0,"Closed unopened device");
reset();initFailure=true;{
CanBus bus({});bus.start();check(bus.snapshot().status=="UNAVAILABLE","Partial failure not reported");
}check(closes==1&&resets==0,"Partial initialization leaked device");
reset();{
CanConfig c;c.timeout_ms=150;CanBus bus(c);bus.start();
check(initializations==2&&starts==2&&terminationCalls==2&&mergeCalls==1,
"CH0/CH1 initialization or merged receive configuration is wrong");
CanBus duplicate(c);rejects([&]{duplicate.start();});
inject(0,false,0x391,{1,37,128,255},123);
waitFor([&]{return bus.snapshot().received==1;});
check(bus.snapshot().channels[0].positions==std::array<int,4>{1,37,128,255},"Four byte positions overlap");
inject(1,true,0x391,{99,99,99,99},789);
inject(0,true,0x391,{31,25,76,100},456);
waitFor([&]{return bus.snapshot().received==2;});
auto s=bus.snapshot();
check(s.channels[0].positions==std::array<int,4>{31,25,76,100},"Actual positions not refreshed");
check(s.channels[1].received==0&&!s.channels[1].valid&&s.channels[1].positions==std::array<int,4>{-1,-1,-1,-1},"CH1 input accepted");
check(s.channels[0].valid&&s.channels[0].fd&&s.channels[0].device_timestamp_us==456,"Freshness/frame metadata wrong");
auto timestamp=s.channels[0].received_monotonic_ns;
inject(0,false,0x390,{9,9,9,9});inject(0,false,0x391,{99,99,99});
waitFor([&]{return bus.snapshot().received==4;});
s=bus.snapshot();check(s.channels[0].received_monotonic_ns==timestamp&&s.receive_errors==1,"Invalid input refreshed signal");
waitFor([&]{return !bus.snapshot().channels[0].valid;});
inject(0,false,0x391,{0,1,2,3});waitFor([&]{return bus.snapshot().channels[0].valid;});
check(bus.send({1,0x321,false,false,false,{1,2,3}}),"CAN send failed");
check(sentCan.hdr.len==3&&!sentCan.hdr.inf.sef&&sentCan.dat[2]==3,"CAN encoding incorrect");
CanFrame fd{1,0x12345,true,true,true,std::vector<std::uint8_t>(64,0xa5)};
check(bus.send(fd)&&sentFd.hdr.len==64&&sentFd.hdr.inf.brs&&sentFd.hdr.inf.sef&&sentFd.dat[63]==0xa5,"CANFD encoding incorrect");
fd.data.resize(9);rejects([&]{bus.send(fd);});
rejects([&]{bus.send({0,0x391,false,false,false,{}});});
rejects([&]{bus.send({1,0x800,false,false,false,{}});});
rejects([&]{bus.send({1,0x391,false,false,true,{}});});
txFailure=true;check(!bus.send({1,0,false,false,false,{}}),"SDK send failure ignored");
check(bus.snapshot().transmitted==2&&bus.snapshot().transmit_errors==1,"TX counters wrong");
receiveFailure=true;waitFor([&]{return bus.snapshot().status=="ERROR";});
check(!bus.snapshot().channels[0].valid&&!bus.send({1,0,false,false,false,{}}),"Receive error left bus live");
bus.requestStop();bus.join();check(closes==1&&resets==2,"Shutdown did not release CH0 and CH1");
check(!bus.send({1,0,false,false,false,{}}),"Transmission after stop");
}check(closes==1,"Device closed twice");
reset();{
CanBus bus({});bus.start();
for(auto level:{"clear","watch","warning","danger","unavailable","unknown","","clear"}) {
auto sent=bus.sendRisk(level);
check(sent==std::array<bool,2>{false,true},"Risk must use CH1 only");
}
check(sentCanFrames.size()==8,"Risk must send exactly once per channel per result");
for(std::size_t i=0;i<sentCanFrames.size();++i) {
auto const& message=sentCanFrames[i];
check(message.hdr.id==0x1f6&&message.hdr.len==8&&message.hdr.chn==1,
"Risk ID, DLC or channel incorrect");
check(!message.hdr.inf.fmt&&!message.hdr.inf.sef&&!message.hdr.inf.brs&&!message.hdr.inf.sdf,
"Risk must use a standard classic CAN data frame");
check(message.dat[0]==(i==0||i==1||i==4||i==7?0x00:0x11),
"Risk must encode clear, watch and unavailable as zero in both nibbles");
for(int byte=1;byte<8;++byte)check(message.dat[byte]==0,"Risk reserved bits not zero");
}
failedTxChannel=1;
check(bus.sendRisk("danger")==std::array<bool,2>{false,false},"CH1 failure reported success");
failedTxChannel=-1;
check(bus.sendRisk("clear")==std::array<bool,2>{false,true},"CH1 did not recover after send failure");
check(bus.snapshot().transmitted==9&&bus.snapshot().transmit_errors==1,"Risk TX counters wrong");
auto calls=txCalls.load();bus.requestStop();
check(bus.sendRisk("danger")==std::array<bool,2>{false,false}&&txCalls==calls,"Risk sent after stop");
}
reset();{
CanConfig c;c.enabled=false;CanBus bus(c);bus.start();
check(bus.sendRisk("danger")==std::array<bool,2>{false,false}&&txCalls==0&&opens==0,
"Disabled risk send accessed hardware");
check(!bus.queueRisk("danger"),"Disabled bus accepted asynchronous risk");
}
reset();{
CanBus bus({});check(!bus.queueRisk("danger"),"Unstarted bus accepted risk");bus.start();
// A blocked hardware SDK call must not stall inference submission or snapshots.
blockTx=true;check(bus.queueRisk("danger"),"Risk queue rejected ready bus");
waitFor([]{return txBlocked.load();});
auto start=std::chrono::steady_clock::now();
for(int i=0;i<1000;++i)check(bus.queueRisk(i%2?"clear":"warning"),"Risk queue stalled/rejected update");
check(bus.snapshot().transmitted==0,"Blocked SDK falsely reported delivery");
check(std::chrono::steady_clock::now()-start<250ms,"Risk submission waited for the SDK");
{std::lock_guard<std::mutex> lock(txMutex);blockTx=false;}txWake.notify_all();
waitFor([&]{return bus.snapshot().transmitted==2;});
{
std::lock_guard<std::mutex> lock(txMutex);
check(sentCanFrames.size()==2,"Stale pending results accumulated");
for(int i=0;i<2;++i)check(sentCanFrames[i].hdr.chn==1&&
sentCanFrames[i].dat[0]==(i==0?0x11:0x00),"Async risk or latest result incorrect");
}
failedTxChannel=1;
check(bus.queueRisk("danger"),"Ready queue rejected changed risk");
waitFor([&]{auto s=bus.snapshot();return s.transmitted==2&&s.transmit_errors==1;});
failedTxChannel=-1;txBlocked=false;blockTx=true;
check(bus.queueRisk("clear"),"Ready queue rejected clear risk");waitFor([]{return txBlocked.load();});
check(bus.queueRisk("danger"),"Queue rejected pending stop fixture");
auto calls=txCalls.load();bus.requestStop();
check(!bus.queueRisk("danger"),"Stopped bus accepted risk");
{std::lock_guard<std::mutex> lock(txMutex);blockTx=false;}txWake.notify_all();
bus.join();check(txCalls==calls,"Stop sent a pending result");
check(closes==1&&resets==2,"Async shutdown leaked hardware");
}
reset();{
CanBus bus({});bus.start();
check(bus.queueRisk("clear"),"Initial clear rejected");
waitFor([&]{return bus.snapshot().transmitted==1;});
for(int i=0;i<1000;++i)check(bus.queueRisk("clear"),"Unchanged risk rejected");
std::this_thread::sleep_for(30ms);
check(txCalls==1,"Repeated clear sent redundant frames");
check(bus.queueRisk("watch"),"Watch risk rejected");
std::this_thread::sleep_for(30ms);
check(txCalls==1,"Watch resent clear's zero value");
check(bus.queueRisk("warning"),"Risk change rejected");
waitFor([&]{return bus.snapshot().transmitted==2;});
for(auto level:{"warning","danger","unknown",""})
check(bus.queueRisk(level),"Equivalent binary risk rejected");
std::this_thread::sleep_for(30ms);
check(txCalls==2,"Equivalent non-clear levels resent binary risk");
check(bus.queueRisk("unavailable"),"Unavailable transition rejected");
waitFor([&]{return bus.snapshot().transmitted==3;});
check(sentCanFrames.back().dat[0]==0,"Unavailable risk must send zero");
check(bus.queueRisk("clear")&&bus.queueRisk("watch"),"Zero-risk transition rejected");
std::this_thread::sleep_for(30ms);
check(bus.snapshot().transmitted==3,"Clear/watch resent unavailable's zero value");
failedTxChannel=1;
check(bus.queueRisk("danger"),"Failed-send fixture rejected");
waitFor([&]{return bus.snapshot().transmit_errors==1;});
for(int i=0;i<1000;++i)check(bus.queueRisk("warning"),"Unchanged failed state rejected");
std::this_thread::sleep_for(30ms);
check(txCalls==4,"Unchanged failed state retried automatically");
failedTxChannel=-1;
check(bus.queueRisk("clear"),"Recovery transition rejected");
waitFor([&]{return bus.snapshot().transmitted==4;});
check(bus.queueRisk("danger"),"Second danger transition rejected");
waitFor([&]{return bus.snapshot().transmitted==5;});
bus.join();
check(sentCanFrames.size()==6,"Unexpected change-triggered frame count");
for(std::size_t i=0;i<sentCanFrames.size();++i)
check(sentCanFrames[i].dat[0]==(i%2?0x11:0),"Risk transition payload incorrect");
}
reset();{
CanBus bus({});bus.start();blockTx=true;
check(bus.queueRisk("danger"),"Initial non-clear rejected");
waitFor([]{return txBlocked.load();});
check(bus.queueRisk("clear")&&bus.queueRisk("warning"),"Busy transitions rejected");
{std::lock_guard<std::mutex> lock(txMutex);blockTx=false;}txWake.notify_all();
waitFor([&]{return bus.snapshot().transmitted==1;});
std::this_thread::sleep_for(30ms);
check(txCalls==1,"Returning to in-flight risk sent redundant frame");
check(bus.queueRisk("clear"),"Later clear change lost");
waitFor([&]{return bus.snapshot().transmitted==2;});
bus.join();
check(sentCanFrames.size()==2&&sentCanFrames[0].dat[0]==0x11&&sentCanFrames[1].dat[0]==0,
"Latest-state coalescing lost final clear");
}
std::cout<<"PASS CAN/CANFD CH0 reception, CH1 transmission, expiry/recovery, TX validation, change-triggered binary risk 0x1F6, errors and lifecycle cleanup\n";
}catch(std::exception const& e){std::cerr<<e.what()<<'\n';return 1;}
tests/orin_391_overlay.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 47 KiB

tests/orin_391_results.json
+58
View File
@@ -0,0 +1,58 @@
{
"running_sha256": "418637a35f6ad05441578db1333e15714262a9c0ee088f10c2dd84923471cd0f",
"build_sha256": "418637a35f6ad05441578db1333e15714262a9c0ee088f10c2dd84923471cd0f",
"stop_seconds": 0.8675570619998325,
"exit_code": 0,
"command": "./run_camera.sh",
"first_sample": {
"seconds": 0.25,
"rx": 8833,
"tx": 5,
"tx_errors": 0,
"valid": true,
"received_391": 4,
"age_ms": 147.083,
"positions": [
26,
0,
0,
0
]
},
"last_sample": {
"seconds": 10.0,
"rx": 35669,
"tx": 299,
"tx_errors": 0,
"valid": true,
"received_391": 33,
"age_ms": 84.5888,
"positions": [
25,
0,
0,
0
]
},
"sample_count": 40,
"max_observed_age_ms": 913.11,
"observed_positions": [
[
25,
0,
0,
0
],
[
26,
0,
0,
0
]
],
"checks": "real camera/TensorRT/CH0/renderer; not CAN fault injection",
"local_head": "515a9f32d8c0ebf83503d4d240ffe917dc09950c",
"remote_inference_only": "0192fccbf06cb928eac098b3dfad32abf0fc370d",
"source_state": "uncommitted workspace at hardware verification",
"package_sha256": "418637a35f6ad05441578db1333e15714262a9c0ee088f10c2dd84923471cd0f"
}
tests/orin_can_results.json
+25
View File
@@ -0,0 +1,25 @@
{
"platform": "orin-desktop",
"binary": "/home/orin/vision-anti-pinch-0923/build/camera_risk",
"binary_sha256": "e8788d22fd6db0e3b9edc95a02322a51ca7951e1df81fea10f34db7da3143461",
"running_binary_sha256": "e8788d22fd6db0e3b9edc95a02322a51ca7951e1df81fea10f34db7da3143461",
"recorded_frames": 13,
"replayed_frames": 13,
"frames_with_people": 13,
"max_raw_pose_error_px": 938.180541992,
"matching_track_frames": 13,
"matching_decision_frames": 8,
"video_bytes": 322966,
"uncompressed_bgr_bytes": 115015680,
"sigterm_seconds": 1.3167834819996642,
"replay_sigint_seconds": 0.9158224279999558,
"interrupted_frames": 2,
"can_statuses": [
"UNAVAILABLE"
],
"can_received": 0,
"can_0x391_by_channel": [
0,
0
]
}
tests/orin_ch1_tx_results.json
+23
View File
@@ -0,0 +1,23 @@
{
"command": "./run_camera.sh --no-visualize --save-results",
"duration_seconds": 12,
"published_results": 260,
"observed_risk_levels": [
"unavailable"
],
"can_status": "OPEN",
"transmit_channel": 1,
"transmitted": 1,
"transmit_errors": 0,
"receive_channel": 0,
"received_391": 11,
"received_total": 31811,
"receive_errors": 4965,
"running_sha256": "feca0b7f476171f45489c22f11a51ba1fe0dabe2d9a17bfcdc50110d1afdb276",
"build_sha256": "feca0b7f476171f45489c22f11a51ba1fe0dabe2d9a17bfcdc50110d1afdb276",
"package_sha256": "feca0b7f476171f45489c22f11a51ba1fe0dabe2d9a17bfcdc50110d1afdb276",
"local_branch": "inference-only",
"local_head": "f87e5aad40478824da16eb42c76b40383de3a9a1",
"remote_inference_only": "0192fccbf06cb928eac098b3dfad32abf0fc370d",
"source_state": "uncommitted workspace at hardware verification"
}
tests/orin_risk_change_results.json
+21
View File
@@ -0,0 +1,21 @@
{
"running_sha256": "5fa01c08baf73b1e044288b5e85f753c10f10d7cc9559ff25952bc4618f01899",
"build_sha256": "5fa01c08baf73b1e044288b5e85f753c10f10d7cc9559ff25952bc4618f01899",
"stop_seconds": 0.9191952890005268,
"exit_code": 0,
"command": "./run_camera.sh",
"published_results": 357,
"observed_results": 357,
"observed_risk_levels": [
"unavailable"
],
"observed_binary_states": 1,
"transmitted": 1,
"transmit_errors": 0,
"received_391": 15,
"duration_seconds": 12,
"local_head": "f87e5aad40478824da16eb42c76b40383de3a9a1",
"remote_inference_only": "0192fccbf06cb928eac098b3dfad32abf0fc370d",
"source_state": "uncommitted workspace at hardware verification",
"package_sha256": "5fa01c08baf73b1e044288b5e85f753c10f10d7cc9559ff25952bc4618f01899"
}
tests/orin_session_results.json
+43
View File
@@ -0,0 +1,43 @@
{
"platform": "orin-desktop",
"binary": "/home/orin/vision-anti-pinch-0923/build/camera_risk",
"binary_sha256": "0f178057529eb8be44bb9a639504dfe57a1e353a84d2351fd907304f6d951c11",
"running_binary_sha256": "0f178057529eb8be44bb9a639504dfe57a1e353a84d2351fd907304f6d951c11",
"recorded_frames": 13,
"replayed_frames": 13,
"frames_with_people": 0,
"max_raw_pose_error_px": 0.0,
"matching_track_frames": 13,
"matching_decision_frames": 13,
"video_bytes": 303989,
"uncompressed_bgr_bytes": 115015680,
"sigterm_seconds": 1.4174426249992393,
"replay_sigint_seconds": 0.8654193540005508,
"interrupted_frames": 2,
"standalone_deployment": {
"binary": "/home/orin/vision-anti-pinch-0923/orin/build/camera_risk",
"binary_sha256": "0f178057529eb8be44bb9a639504dfe57a1e353a84d2351fd907304f6d951c11",
"running_binary_sha256": "0f178057529eb8be44bb9a639504dfe57a1e353a84d2351fd907304f6d951c11",
"replayed_frames": 13
},
"default_visualization": {
"platform": "orin-desktop",
"binary": "/home/orin/vision-anti-pinch-0923/orin/build/camera_risk",
"binary_sha256": "cd9711d71c6d04f40adf9434f893abe1744e9e2013f1459ddf736a80dcbbfd64",
"running_binary_sha256": "cd9711d71c6d04f40adf9434f893abe1744e9e2013f1459ddf736a80dcbbfd64",
"recorded_frames": 13,
"replayed_frames": 13,
"frames_with_people": 0,
"max_raw_pose_error_px": 0.0,
"matching_track_frames": 13,
"matching_decision_frames": 13,
"video_bytes": 317961,
"uncompressed_bgr_bytes": 115015680,
"sigterm_seconds": 1.266686278000634,
"replay_sigint_seconds": 0.9155338489999849,
"interrupted_frames": 2,
"default_window_observed": true,
"no_visualize_replay_passed": true,
"headless_replay_passed": true
}
}
tests/orin_session_test.py
+140
View File
@@ -0,0 +1,140 @@
#!/usr/bin/env python3
"""Real camera + TensorRT record/replay check. Runs only explicitly on Orin.
Records to a new test directory; never stops an existing deployment process.
"""
import argparse
import hashlib
import json
import os
from pathlib import Path
import signal
import subprocess
import time
def rows(session):
index = session / 'frames.txt'
return index.read_text().splitlines() if index.exists() else []
def wait_frames(process, session, count, timeout=90):
deadline = time.monotonic() + timeout
while len(rows(session)) < count:
if process.poll() is not None:
raise RuntimeError(f'Inference exited early: {process.returncode}')
if time.monotonic() > deadline:
raise RuntimeError('Timed out awaiting real inference frames')
time.sleep(.05)
def stop(process, signum=signal.SIGTERM):
start = time.monotonic()
process.send_signal(signum)
code = process.wait(timeout=15)
if code:
raise RuntimeError(f'Nonzero stop exit: {code}')
return time.monotonic() - start
def run_record(command, session, log, count, signum=signal.SIGTERM):
with log.open('w') as output:
process = subprocess.Popen(command + ['--record-session', str(session)], stdout=output, stderr=subprocess.STDOUT)
try:
wait_frames(process, session, count)
return stop(process, signum)
finally:
if process.poll() is None:
process.terminate()
process.wait(timeout=15)
def load(session):
return [json.loads((session / 'frames' / f'{stem}.json').read_text()) for stem in rows(session)]
def main():
parser = argparse.ArgumentParser()
parser.add_argument('--binary', type=Path, required=True)
parser.add_argument('--config', type=Path, required=True)
parser.add_argument('--output', type=Path, required=True)
parser.add_argument('--replay-input', type=Path, help='Verify deployment with an existing session; no camera access')
args = parser.parse_args()
args.output.mkdir(parents=True, exist_ok=False)
binary = args.binary.resolve()
config = args.config.resolve()
command = [str(binary), '--config', str(config)]
# Refuse camera contention rather than alter an existing deployment.
if subprocess.run(['pgrep', '-x', 'camera_risk'], stdout=subprocess.DEVNULL).returncode == 0:
raise RuntimeError('An existing camera_risk process is running; preserve its launch parameters first')
if args.replay_input:
replayed = args.output / 'replayed'
with (args.output/'replay.log').open('w') as log:
process = subprocess.Popen(command + ['--replay-session', str(args.replay_input.resolve()), '--record-session', str(replayed)], stdout=log, stderr=subprocess.STDOUT)
try:
running_hash = hashlib.sha256(Path(f'/proc/{process.pid}/exe').read_bytes()).hexdigest()
assert process.wait(timeout=120) == 0, 'Deployment replay failed'
finally:
if process.poll() is None:
process.terminate()
process.wait(timeout=15)
original, restored = load(args.replay_input), load(replayed)
assert [r['frame_index'] for r in original] == [r['frame_index'] for r in restored]
result = {'binary': str(binary), 'binary_sha256': hashlib.sha256(binary.read_bytes()).hexdigest(),
'running_binary_sha256': running_hash, 'replayed_frames': len(restored)}
assert result['binary_sha256'] == running_hash
(args.output/'report.json').write_text(json.dumps(result, indent=2)+'\n')
print(json.dumps(result, indent=2))
return
recorded, replayed = args.output / 'recorded', args.output / 'replayed'
term_seconds = run_record(command, recorded, args.output / 'record.log', 12)
original = load(recorded)
with (args.output / 'replay.log').open('w') as log:
process = subprocess.Popen(command + ['--replay-session', str(recorded), '--record-session', str(replayed)], stdout=log, stderr=subprocess.STDOUT)
try:
# Verify the running executable, not just the file on disk.
running_hash = hashlib.sha256(Path(f'/proc/{process.pid}/exe').read_bytes()).hexdigest()
if process.wait(timeout=120):
raise RuntimeError('Replay failed')
finally:
if process.poll() is None:
process.terminate()
process.wait(timeout=15)
restored = load(replayed)
assert len(original) == len(restored) >= 12, 'EOF dropped frames'
max_pose_error = 0.0
matching_tracks = matching_decisions = 0
for before, after, stem in zip(original, restored, rows(recorded)):
assert before['frame_index'] == after['frame_index']
assert before['capture'] == after['source_capture']
if 'can' in before['result']:
expected_can = dict(before['result']['can'], source='replay')
assert expected_can == after['result']['can'], 'Recorded CAN state changed during replay'
assert before['video_frame_index'] == after['video_frame_index']
matching_tracks += before['result']['track_ids'] == after['result']['track_ids']
matching_decisions += before['result']['risk']['decision'] == after['result']['risk']['decision']
for stage in ('raw_keypoints', 'isolated_keypoints'):
for a, b in zip(before['stages'][stage], after['stages'][stage]):
for ka, kb in zip(a, b):
max_pose_error = max(max_pose_error, *(abs(x-y) for x, y in zip(ka, kb)))
# H.264 is lossy: report algorithm differences, do not require bitwise equality.
probe = json.loads(subprocess.check_output(['ffprobe', '-v', 'error', '-select_streams', 'v:0', '-show_entries', 'stream=codec_name,nb_frames,width,height', '-of', 'json', str(recorded/'frames.mp4')]))['streams'][0]
assert probe['codec_name'] == 'h264' and int(probe['nb_frames']) == len(original)
# Exercise cancellation while replay has queued input, and reuse afterwards.
interrupt_seconds = run_record(command + ['--replay-session', str(recorded)], args.output/'interrupted', args.output/'interrupt.log', 1, signal.SIGINT)
interrupted_count = json.loads((args.output/'interrupted/complete.json').read_text())['frame_count']
assert 0 < interrupted_count < len(original), 'Cancellation did not stop before EOF'
result = {'platform': os.uname().nodename, 'binary': str(binary), 'binary_sha256': hashlib.sha256(binary.read_bytes()).hexdigest(),
'running_binary_sha256': running_hash, 'recorded_frames': len(original), 'replayed_frames': len(restored),
'frames_with_people': sum(bool(row['result']['track_ids']) for row in original),
'max_raw_pose_error_px': max_pose_error, 'matching_track_frames': matching_tracks, 'matching_decision_frames': matching_decisions,
'video_bytes': (recorded/'frames.mp4').stat().st_size, 'uncompressed_bgr_bytes': len(original)*int(probe['width'])*int(probe['height'])*3, 'sigterm_seconds': term_seconds, 'replay_sigint_seconds': interrupt_seconds, 'interrupted_frames': interrupted_count,
'can_statuses': sorted({r['result'].get('can', {}).get('status', 'NOT_RECORDED') for r in original}),
'can_received': max(r['result'].get('can', {}).get('received', 0) for r in original),
'can_0x391_by_channel': [max((r['result'].get('can', {}).get('channels', [{}, {}])[ch].get('received', 0) for r in original)) for ch in range(2)]}
assert result['binary_sha256'] == running_hash
(args.output/'report.json').write_text(json.dumps(result, indent=2)+'\n')
print(json.dumps(result, indent=2))
if __name__ == '__main__':
main()
tests/run_camera_timeout.sh
+1 -1
View File
@@ -3,7 +3,7 @@ set -euo pipefail
root="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")/.." && pwd)"
test_dir="$(mktemp -d)"
trap 'rm -rf "$test_dir"' EXIT
flags=(-std=c++17 -O1 -g -Wall -Wextra -Wpedantic -pthread -fsanitize=undefined -fno-sanitize-recover=all -I"$root/include")
flags=(-std=c++17 -O1 -g -Wall -Wextra -Wpedantic -pthread -fsanitize=undefined -fno-sanitize-recover=all -I"$root/include" -I"$root/third_party")
links=()
if [[ -f "$root/include/v4l2_capture.h" ]]; then
flags+=(-DTEST_V4L2_CAPTURE -I"$root/tests/stubs")
tests/run_can.sh
Executable
+9
View File
@@ -0,0 +1,9 @@
#!/usr/bin/env bash
set -euo pipefail
root="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")/.." && pwd)"
test_dir="$(mktemp -d)"
trap 'rm -rf "$test_dir"' EXIT
"${CXX:-g++}" -std=c++17 -O1 -g -Wall -Wextra -Wpedantic -pthread \
-fsanitize=undefined -fno-sanitize-recover=all -I"$root/include" -isystem "$root/third_party/zlg" \
"$root/src/can_bus.cpp" "$root/tests/can_bus_test.cpp" -o "$test_dir/can_bus_test"
"$test_dir/can_bus_test"
tests/run_session.sh
Executable
+11
View File
@@ -0,0 +1,11 @@
#!/usr/bin/env bash
set -euo pipefail
root="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")/.." && pwd)"
test_dir="$(mktemp -d)"
trap 'rm -rf "$test_dir"' EXIT
read -ra opencv_flags <<< "$(pkg-config --cflags --libs opencv4)"
"${CXX:-g++}" -std=c++17 -O1 -g -Wall -Wextra -Wpedantic -fsanitize=undefined -fno-sanitize-recover=all \
-I"$root/include" -I"$root/third_party" "$root/tests/session_test.cpp" \
"$root/src/inference_session.cpp" "$root/src/serialization.cpp" "$root/src/door_risk.cpp" \
"${opencv_flags[@]}" -o "$test_dir/session_test"
"$test_dir/session_test" "$test_dir"
tests/run_v4l2_reopen.sh
-7
View File
@@ -1,7 +0,0 @@
#!/usr/bin/env bash
set -euo pipefail
root="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")/.." && pwd)"
test_dir="$(mktemp -d)"
trap 'rm -rf "$test_dir"' EXIT
"${CXX:-g++}" -std=c++17 -O1 -g -fsanitize=undefined -fno-sanitize-recover=all -I"$root/include" "$root/src/v4l2_capture.cpp" "$root/tests/v4l2_reopen_test.cpp" -Wl,--wrap=open -Wl,--wrap=close -Wl,--wrap=ioctl -Wl,--wrap=mmap -Wl,--wrap=munmap -o "$test_dir/v4l2_reopen_test"
"$test_dir/v4l2_reopen_test"
tests/session_test.cpp
+71
View File
@@ -0,0 +1,71 @@
#include "inference_session.hpp"
#include "camera_pipeline.hpp"
#include "serialization.hpp"
#include <chrono>
#include <cmath>
#include <iostream>
#include <stdexcept>
namespace cms {
std::uint64_t monotonicNs() {
return std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::steady_clock::now().time_since_epoch()).count();
}
}
using namespace cms;
void check(bool condition,char const* message){if(!condition)throw std::runtime_error(message);}
template<class F> void rejects(F f){bool failed=false;try{f();}catch(std::exception const&){failed=true;}check(failed,"Expected rejection");}
int main(int argc,char** argv)try {
if(argc!=2)return 2;
std::filesystem::path root=argv[1];
PipelineConfig c;c.record_session=root/"recorded";c.engines=root;c.door_mask=root/"mask.json";
c.camera.width=64;c.camera.height=48;c.detector_confidence=.42;
std::ofstream(c.door_mask)<<"{}";
for(auto name:{"yolo11s.engine","rtmw_l_384.engine"})std::ofstream(root/name)<<"fixture";
auto im=std::make_shared<BgrImage>();im->width=64;im->height=48;im->stride=200;
im->pixels.resize(9600);
for(int y=0;y<48;++y)for(int x=0;x<192;++x)im->pixels[y*200+x]=32+x/3+y%4;
FrameResult r;r.image=im;r.frame_index=3;r.capture.frame_index=3;r.capture.driver_sequence=42;
r.can.status="OPEN";r.can.received=7;r.can.channels[0].positions={0,25,100,255};
r.can.channels[0].valid=true;r.can.channels[0].received=7;r.can.channels[0].age_ms=12.5;
r.capture.capture_monotonic_ns=123;r.capture.received_monotonic_ns=124;
{
SessionWriter writer(c);writer.write(r,{{"raw_keypoints",{1,2,3}}});
r.frame_index=9;r.capture.frame_index=9;
for(auto& pixel:im->pixels)pixel+=80;
writer.write(r,{{"raw_keypoints",{4,5,6}}});
writer.finish();writer.finish();
rejects([&]{writer.write(r,{});});
}
rejects([&]{SessionWriter duplicate(c);});
PipelineConfig replay;replay.replay_session=c.record_session;
SessionReader reader(replay);check(replay.camera.width==64&&replay.detector_confidence==.42,"Settings not restored");
auto first=reader.read();check(first&&first->metadata.frame_index==3,"First index mismatch");
auto can=deserializeCan(first->replay_record->at("result").at("can"));
check(can.channels[0].positions==r.can.channels[0].positions&&can.channels[0].valid&&can.channels[0].age_ms==12.5,"Recorded CAN snapshot changed");
check(first->metadata.driver_sequence==42&&first->metadata.capture_monotonic_ns!=123,"Capture metadata mismatch");
check(first->replay_record->at("capture").at("capture_monotonic_ns")==123,"Original clock lost");
for(int y=0;y<48;++y)for(int x=0;x<192;++x)check(std::abs(int(first->image->pixels[y*192+x])-(32+x/3+y%4))<12,"Decoded first frame mismatch");
auto second=reader.read();check(second&&second->metadata.frame_index==9,"Gap/order lost");
check(second->image->pixels[0]>first->image->pixels[0]+60,"Video frame order lost");
check(!reader.read(),"EOF not drained");
// Unindexed temporary metadata must not become input.
std::ofstream(c.record_session/"frames/000000000002.json.tmp")<<"incomplete";
SessionReader prefix(replay);check(prefix.read()&&prefix.read()&&!prefix.read(),"Uncommitted data replayed");
std::filesystem::remove(c.record_session/"frames/000000000001.json");
SessionReader missing(replay);missing.read();rejects([&]{missing.read();});
std::ofstream(c.record_session/"frames.txt",std::ios::trunc)<<"../bad\n";
SessionReader invalid(replay);rejects([&]{invalid.read();});
std::ofstream(c.record_session/"frames.txt",std::ios::trunc)<<"000000000000";
SessionReader truncated(replay);rejects([&]{truncated.read();});
std::ofstream(c.record_session/"frames.txt",std::ios::trunc)<<"000000000000\n";
SessionReader shortIndex(replay);shortIndex.read();rejects([&]{shortIndex.read();});
std::filesystem::remove(c.record_session/"complete.json");
rejects([&]{SessionReader incomplete(replay);});
c.record_session=root/"empty";
{SessionWriter empty(c);empty.finish();}
replay.replay_session=c.record_session;
SessionReader empty(replay);check(!empty.read(),"Empty recording has frames");
c.camera.width=63;c.record_session=root/"odd";
rejects([&]{SessionWriter odd(c);});
std::cout<<"H.264 encode/decode, settings, order, EOF, stop finalization and corruption checks passed\n";
}catch(std::exception const& e){std::cerr<<e.what()<<'\n';return 1;}
tests/v4l2_reopen_test.cpp
-29
View File
@@ -1,29 +0,0 @@
#include "v4l2_capture.h"
#include <linux/videodev2.h>
#include <sys/mman.h>
#include <cstdarg>
#include <cerrno>
#include <cstdlib>
#include <stdexcept>
#include <iostream>
static int mappings=0;static bool failSecondMap=false;
extern "C" {
int __wrap_open(char const*,int,...){return 50;}
int __wrap_close(int){return 0;}
void* __wrap_mmap(void*,size_t n,int,int,int,off_t){if(failSecondMap&&mappings==1){errno=ENOMEM;return MAP_FAILED;}++mappings;return std::malloc(n);}
int __wrap_munmap(void* p,size_t){if(!p||p==MAP_FAILED||mappings<1)std::abort();std::free(p);--mappings;return 0;}
int __wrap_ioctl(int,unsigned long req,...){
va_list args;va_start(args,req);void* p=va_arg(args,void*);va_end(args);
if(req==VIDIOC_QUERYCAP){*static_cast<v4l2_capability*>(p)={};static_cast<v4l2_capability*>(p)->capabilities=V4L2_CAP_VIDEO_CAPTURE|V4L2_CAP_STREAMING;return 0;}
if(req==VIDIOC_S_FMT){if(mappings){errno=EBUSY;return -1;}static_cast<v4l2_format*>(p)->fmt.pix.bytesperline=4;return 0;}
if(req==VIDIOC_QUERYBUF){static_cast<v4l2_buffer*>(p)->length=8;return 0;}
return 0;
}
}
static void check(bool b){if(!b)throw std::runtime_error("V4L2 reopen regression");}
int main()try{
V4l2Capture camera;
for(int i=0;i<3;++i){check(camera.open("/mock"));check(camera.setFormat(2,2,V4L2_PIX_FMT_UYVY));check(camera.initMmap(2));check(camera.startStreaming());camera.close();check(mappings==0);}
check(camera.open("/mock"));check(camera.setFormat(2,2,V4L2_PIX_FMT_UYVY));failSecondMap=true;check(!camera.initMmap(2));camera.close();check(mappings==0);
std::cout<<"PASS repeated reopen and partial mmap failure release every mapping\n";return 0;
}catch(std::exception const& e){std::cerr<<e.what()<<'\n';return 1;}
third_party/nlohmann/LICENSE.MIT
Vendored
+1 -1
View File
@@ -1,4 +1,4 @@
MIT License
MIT License
Copyright (c) 2013-2022 Niels Lohmann
third_party/zlg/README.md
Vendored
+5
View File
@@ -0,0 +1,5 @@
# USBCANFD SDK
`zcan.h` and the runtime libraries in `orin/lib/` were copied unchanged from the user-specified reference project `/home/orin/vision-anti-pinch` on 2026-09-24. The runtime libraries are aarch64 `libusbcanfd.so` (SONAME `libusbcanfd.so.1.0.12`) and `libusb-1.0.so` (SONAME `libusb-1.0.so.0`). Symlinks retain these SONAMEs in the standalone package.
`src/can_bus.cpp` uses the reference device type and 500 kbps / 2 Mbps initialization settings. It implements one merged receive worker, per-channel state, explicit data-frame transmission, and lifecycle cleanup instead of importing the reference global callback/receive-thread implementation.
include/zcan.h → third_party/zlg/zcan.h
Vendored
View File