Compare commits
6 Commits
0192fccbf0
...
use-can
| Author | SHA1 | Date | |
|---|---|---|---|
 zmv
|
a35e4a1754 | ||
|
zmv
|
713bfd3672 | ||
 xhc
|
d24de544cb | ||
|
zmv
|
e1e7fbe2fb | ||
|
zmv
|
0f609612a6 | ||
|
zmv
|
9aadf9cbda |
+4
-5
@@ -1,5 +1,4 @@
|
||||
*.sh text eol=lf
|
||||
*.engine -text
|
||||
*.ko -text
|
||||
/orin/** -text
|
||||
/config/door_mask.json -text
|
||||
*.engine binary
|
||||
*.onnx binary
|
||||
*.so binary
|
||||
*.ko binary
|
||||
|
||||
+23
-4
@@ -1,4 +1,23 @@
|
||||
/build/
|
||||
/engines/
|
||||
/logs/
|
||||
*.tmp
|
||||
# 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
|
||||
|
||||
# Optional per-frame output, including standalone Orin runs
|
||||
output/
|
||||
|
||||
@@ -0,0 +1,7 @@
|
||||
# 工程交付约定
|
||||
|
||||
- 共享采集、停止与恢复逻辑的修改，应同步本地 `inference-only`、`use-can`、`lazy-inference` 三个分支，并完成 Git 远端与 Orin 部署同步。
|
||||
- 本机模拟测试仅作为回归检查，不得用它代替 Orin 上的真实摄像头和 TensorRT 验证。报告须区分本机模拟、板端故障注入、真实硬件操作，明确未覆盖项目。
|
||||
- 短暂取流故障的重点是期限内恢复后在同一进程继续输出新的推理结果，并重新计数；不能只验证超时后退出。
|
||||
- 发布前核对本地提交、Git 远端、Orin 检出版本及实际运行二进制的哈希。更新 Orin 可执行文件后同时刷新独立包 SHA256SUMS。
|
||||
- 更新测试期间保留原启动参数，测试结束后恢复运行部署版本。不要把旧进程仍在运行当作新版本已部署。
|
||||
+9
-8
@@ -1,23 +1,24 @@
|
||||
cmake_minimum_required(VERSION 3.24)
|
||||
if(NOT DEFINED CMAKE_CUDA_ARCHITECTURES)
|
||||
set(CMAKE_CUDA_ARCHITECTURES native)
|
||||
endif()
|
||||
project(camera_risk LANGUAGES CXX CUDA)
|
||||
project(vision_anti_pinch 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)
|
||||
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 x86_64-linux-gnu REQUIRED)
|
||||
find_path(TENSORRT_INCLUDE_DIR NvInfer.h PATH_SUFFIXES aarch64-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/door_risk.cpp src/camera.cpp src/camera_pipeline.cpp src/config.cpp)
|
||||
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)
|
||||
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)
|
||||
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_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_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")
|
||||
|
||||
@@ -0,0 +1,9 @@
|
||||
.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
|
||||
@@ -1,73 +1,119 @@
|
||||
# Vision Anti-Pinch
|
||||
# Vision Anti-Pinch — use-can
|
||||
|
||||
C++ / TensorRT 实时摄像头风险推理，算法来自最新 run_risk.py。
|
||||
持续取图、推理并更新内存中的 FrameResult；可视化可选。
|
||||
相机默认每 **2000 ms** 无有效帧记一次失败，连续 **6 次**才停止流水线（纯超时约 **12 秒**）；成功取得有效帧后重新计数。每个等待周期内仍以 100 ms 间隔检查停止请求，CAN 接收不受重试影响。取流错误或坏帧也计入重试期限；设备失效时尝试重开相机，恢复后继续推理。单次等待时间由 `config/camera.json` 的 `camera.timeout_ms` 配置。
|
||||
|
||||
## Orin 直接运行
|
||||
## 编译与运行
|
||||
|
||||
适用环境：Jetson AGX Orin、JetPack 5.1.2 / L4T R35.4.1、CUDA 11.4、TensorRT 8.5.2、OpenCV 4.5.4。
|
||||
仓库已包含该板端编译的程序和重新构建的 TensorRT 引擎。
|
||||
目标环境：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+ 和相应开发库。
|
||||
|
||||
```bash
|
||||
git clone https://git.kendeji.fun/zmv/vision-anti-pinch.git
|
||||
cd vision-anti-pinch/orin
|
||||
./scripts/initialize_camera.sh # 开机后初始化 /dev/video5 摄像头链路
|
||||
./run.sh # 持续推理，结果只保存在内存
|
||||
./scripts/build.sh # 或 make
|
||||
./scripts/initialize_camera.sh # 开机后摄像头尚未初始化时执行
|
||||
./run_camera.sh # 执行 build/camera_risk
|
||||
./run_camera.sh --visualize # 可选 OpenCV 本地窗口
|
||||
```
|
||||
|
||||
可选板端本地可视化：
|
||||
预编译 Orin 包：
|
||||
|
||||
```bash
|
||||
./run.sh --visualize
|
||||
cd orin
|
||||
./scripts/initialize_camera.sh
|
||||
./run.sh # 脚本直接 exec C++ ELF 程序
|
||||
# 也可以直接执行：
|
||||
./build/camera_risk --config config/camera.json
|
||||
```
|
||||
|
||||
Ctrl+C / SIGTERM 停止程序。
|
||||
关闭预览窗口或按 q/Esc 只关闭预览，推理继续。
|
||||
默认无窗口、不保存文件；Ctrl+C / SIGTERM 停止。`--visualize` 使用 Orin 的图形桌面，关闭窗口仅停止显示、推理继续。没有桌面环境时保持无窗口推理。
|
||||
|
||||
摄像头为 /dev/video5，1920×1536 UYVY，30 FPS。
|
||||
当前 door mask 是指定的 constant_door_mask.json 副本，标定状态为 false；装车后应更换与实际画面匹配的门区并更新标定状态。
|
||||
## 流水线与结果变量
|
||||
|
||||
## 下游获取结果
|
||||
```
|
||||
V4L2 MMAP 采集 / NPP UYVY 转换（线程 1）
|
||||
→ YOLO TensorRT 检测（线程 2）
|
||||
→ RTMW TensorRT / 隔离推理 / C++ 时序融合（线程 3）
|
||||
→ 前后门多特征归属 / 因果中值 / mask 风险 / CAN 过滤
|
||||
→ shared_ptr<const FrameResult> 最新结果变量
|
||||
```
|
||||
|
||||
main.cpp 注释中也包含以下示例。下游通过自己的调用位置或线程读取：
|
||||
摄像头使用有界队列并丢弃积压旧帧；可视化独立读取最新结果。
|
||||
下游直接链接 `camera_risk_core`，在自己的线程中读取：
|
||||
|
||||
```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& people = result->persons;
|
||||
// result->frame_index / track_ids / boxes
|
||||
// result->image / door_mask_pixels / capture / timing
|
||||
// people[i].selected / smoothed / temporal：各 133 个关键点
|
||||
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>）
|
||||
}
|
||||
// 退出时：
|
||||
pipeline.requestStop();
|
||||
pipeline.join();
|
||||
```
|
||||
|
||||
latest() 返回 shared_ptr<const FrameResult>；首帧尚未完成时为空。
|
||||
公共定义位于 include/camera_pipeline.hpp 和 include/frame_result.hpp。
|
||||
`latest()` 返回拥有数据的只读快照，读者互不消费结果，旧引用不会被新帧修改。慢读者可以跳过中间帧；异常通过 `join()` 传播。停止后的快照只代表最后结果，持续控制应检查运行状态和时间戳。
|
||||
公共结构定义见 `include/frame_result.hpp`，接口见 `include/camera_pipeline.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
|
||||
./scripts/build.sh
|
||||
./scripts/initialize_camera.sh
|
||||
./run_camera.sh
|
||||
./run_camera.sh --help
|
||||
./run_camera.sh --duration 30
|
||||
./run_camera.sh --save-results /path/results.jsonl
|
||||
```
|
||||
|
||||
源码构建要求 CMake 3.24+、CUDA/NPP、TensorRT 和 OpenCV 开发依赖。
|
||||
构建脚本会优先使用 Orin 上已有的 ~/miniconda3/envs/yolo/bin/cmake。
|
||||
源码配置直接使用 orin/engines/ 中的板端引擎。该引擎仅适用于上述 Orin 环境；仓库不包含 ONNX 或模型转换脚本。
|
||||
独立库目标为 camera_risk_core；可视化在 camera_risk_visualizer。
|
||||
显式保存时每帧写一行 JSON，核心字段保留 frame_index、track_ids、boxes、skeletons、door_assignments、door_risks、risk、ungated_risk、can；文件由用户指定路径管理。`--video` 可使用同一 C++ 流程处理视频文件，尺寸需匹配配置。
|
||||
|
||||
## 目录
|
||||
## 目录与打包
|
||||
|
||||
- src/、include/：采集、推理、结果变量及可选绘制。
|
||||
- config/：源码构建程序的配置和固定门区。
|
||||
- scripts/：构建和摄像头初始化脚本。
|
||||
- orin/：已编译程序、TensorRT 引擎、配置、当前 GMSL 驱动及启动脚本。
|
||||
- third_party/：nlohmann JSON 及其许可证。
|
||||
- `src/`：C++ / CUDA 源文件及 main.cpp。
|
||||
- `include/`：头文件及下游数据结构。
|
||||
- `third_party/nlohmann/`：C++ JSON 依赖及许可证。
|
||||
- `config/`、`annotations/`：运行配置与标定。
|
||||
- `scripts/`：编译、摄像头初始化、打包。
|
||||
- `orin/`：可单独复制的 ELF 程序、模型、驱动、CAN 库和配置。
|
||||
|
||||
NPP 用于 UYVY 转换、YOLO 缩放填充、RTMW 仿射裁剪；CUDA 用于通道转换与归一化。
|
||||
风险几何计算仍包含 CPU/OpenCV 操作；算法内部的临时 JSON 对象只在内存中使用。
|
||||
修改源码后，在 Orin 重新编译并执行 `./scripts/package_orin.sh`，或执行 `make package`。运行、打包均不使用 Python。打包后可在 orin 目录运行 `sha256sum -c SHA256SUMS`。
|
||||
|
||||
当前摄像头驱动启动时偶发首帧 ERROR，应用会丢弃并计数；持续错误、采集超时或推理错误会明确报错并退出。
|
||||
现有 GMSL 驱动停止采集时可产生 videobuf2 缓冲区告警，v4l2-ctl 同样复现；需在驱动侧完成量产修复和验证。
|
||||
## 可选保存（默认全部关闭）
|
||||
|
||||
三个独立参数可以单独使用，也可以组合：
|
||||
|
||||
```bash
|
||||
./run_camera.sh --save-original --save-annotated --save-results
|
||||
# 独立包：./orin/run.sh --save-original --save-annotated --save-results
|
||||
```
|
||||
|
||||
开启任一项时，程序自动创建可执行文件所属工程的 `output/` 及三个子目录：
|
||||
|
||||
- `original/`：`--save-original` 保存未叠加的原图，JPEG 质量 95。
|
||||
- `annotated/`：`--save-annotated` 保存带框、关键点及风险文字的 JPEG，无需开启可视化窗口。
|
||||
- `results/`：`--save-results` 每个完成推理的帧保存一个 JSON，包含帧号、检测框、跟踪 ID、关键点及置信度、风险和耗时；有 CAN 的分支还包含相应状态。
|
||||
|
||||
同一帧三类文件同名（扩展名不同），文件名包含运行标识和帧号，多次运行不覆盖。JSON 的 `images` 提供相对于 `output/` 的图片路径，未选择保存的图片为 null。源码入口写入工程 `output/`；独立包入口写入 `orin/output/`。默认不创建目录、不保存文件。
|
||||
|
||||
保存对象是完成推理的帧；惰性分支暂停时不新增文件。开启保存会增加编码和磁盘 I/O 开销。写入失败时报告错误并停止，不静默丢失文件。
|
||||
|
||||
相机超时回归测试：`./tests/run_camera_timeout.sh`；加 `--wall-clock` 验证默认 6 次超时约 12 秒。测试直接编译实际相机代码，使用模拟采集和转换器，无需摄像头、GPU 或 OpenCV；不覆盖视频回放路径。
|
||||
|
||||
板端真实摄像头 / TensorRT 恢复与停止测试记录见 [Orin 测试报告](tests/ORIN_TEST_REPORT.md)。
|
||||
|
||||
@@ -0,0 +1,47 @@
|
||||
{
|
||||
"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
|
||||
}
|
||||
}
|
||||
}
|
||||
+4
-20
@@ -1,22 +1,6 @@
|
||||
{
|
||||
"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
|
||||
}
|
||||
"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}
|
||||
}
|
||||
|
||||
@@ -18,6 +18,12 @@ 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();});
|
||||
|
||||
@@ -8,6 +8,7 @@
|
||||
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 {
|
||||
|
||||
@@ -1,13 +1,20 @@
|
||||
#pragma once
|
||||
#include "camera.hpp"
|
||||
#include "frame_result.hpp"
|
||||
#include "output_writer.hpp"
|
||||
#include <filesystem>
|
||||
#include <memory>
|
||||
|
||||
namespace cms {
|
||||
struct PipelineConfig {
|
||||
CameraConfig camera;
|
||||
std::filesystem::path engines,door_mask;
|
||||
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;
|
||||
double detector_confidence{0.30};
|
||||
int max_people{3};
|
||||
double warning_relative{0.30},release_relative{0.45};
|
||||
|
||||
@@ -0,0 +1,54 @@
|
||||
// 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
|
||||
@@ -0,0 +1,22 @@
|
||||
#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);
|
||||
}
|
||||
@@ -0,0 +1,22 @@
|
||||
#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);
|
||||
}
|
||||
@@ -1,6 +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 <array>
|
||||
#include <cstdint>
|
||||
#include <memory>
|
||||
@@ -22,6 +24,7 @@ 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;
|
||||
@@ -31,6 +34,8 @@ 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;
|
||||
@@ -62,11 +67,22 @@ 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;
|
||||
std::vector<PersonResult> persons;
|
||||
|
||||
@@ -0,0 +1,20 @@
|
||||
#pragma once
|
||||
#include "frame_result.hpp"
|
||||
#include <filesystem>
|
||||
#include <string>
|
||||
namespace cms {
|
||||
struct OutputOptions {
|
||||
bool original{},annotated{},results{};
|
||||
std::filesystem::path directory;
|
||||
bool enabled() const {return original||annotated||results;}
|
||||
};
|
||||
// Called by the inference worker; one matching filename stem per completed frame.
|
||||
class OutputWriter {
|
||||
public:
|
||||
explicit OutputWriter(OutputOptions options);
|
||||
void write(FrameResult const& result);
|
||||
private:
|
||||
OutputOptions options_;
|
||||
std::string runId_;
|
||||
};
|
||||
}
|
||||
@@ -2,33 +2,30 @@
|
||||
#include "frame_result.hpp"
|
||||
#include "door_risk.hpp"
|
||||
namespace cms {
|
||||
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>());
|
||||
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>());
|
||||
}
|
||||
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));
|
||||
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));
|
||||
}
|
||||
return out;
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,6 @@
|
||||
#pragma once
|
||||
#include "frame_result.hpp"
|
||||
#include "door_risk.hpp"
|
||||
namespace cms {
|
||||
Json serializeResult(FrameResult const& result);
|
||||
}
|
||||
@@ -0,0 +1,93 @@
|
||||
#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
|
||||
+443
@@ -0,0 +1,443 @@
|
||||
#pragma once
|
||||
|
||||
#ifdef __cplusplus
|
||||
#define EXTERN_C extern "C"
|
||||
#else
|
||||
#define EXTERN_C
|
||||
#endif
|
||||
|
||||
#define ZCAN_API
|
||||
#define INVALID_HANDLE_VALUE ((HANDLE)(-1))
|
||||
|
||||
#define CMD_CAN_FILTER 0x14 // 滤波
|
||||
#define CMD_CAN_TTX 0x16 // 定时发送
|
||||
#define CMD_CAN_TTX_CTL 0x17 // 使能定时发送
|
||||
#define CMD_CAN_TRES 0x18 // CAN终端电阻
|
||||
|
||||
#define ZCAN_CMD_SET_CHNL_RECV_MERGE 0x32 // 设置合并接收 0:不合并接收;1:合并接收
|
||||
#define ZCAN_CMD_GET_CHNL_RECV_MERGE 0x33 // 获取是否开启合并接收 0:不合并接收;1:合并接收
|
||||
|
||||
#define CMD_SET_SN 0x42 // 获取SN号
|
||||
#define CMD_GET_SN 0x43 // 设置SN号
|
||||
#define CMD_CAN_TX_TIMEOUT 0x44 // 发送超时
|
||||
|
||||
#define ZCAN_CMD_GET_SEND_QUEUE_SIZE 0x100 // 获取队列大小，uint32_t
|
||||
#define ZCAN_CMD_GET_SEND_QUEUE_SPACE 0x101 // 获取队列剩余空间, uint32_t
|
||||
#define ZCAN_CMD_SET_SEND_QUEUE_CLR 0x102 // 清空发送队列,1：清空
|
||||
#define ZCAN_CMD_SET_SEND_QUEUE_EN 0x103 // 开启发送队列,1：使能
|
||||
|
||||
typedef unsigned char U8;
|
||||
typedef unsigned short U16;
|
||||
typedef unsigned int U32;
|
||||
typedef unsigned long long U64;
|
||||
|
||||
enum eZCANDataDEF
|
||||
{
|
||||
//数据类型
|
||||
ZCAN_DT_ZCAN_CAN_DATA = 1, // CAN/CANFD数据
|
||||
ZCAN_DT_ZCAN_CANFD_DATA = 2, // CAN/CANFD数据
|
||||
ZCAN_DT_ZCAN_ERROR_DATA = 3, // CAN/CANFD错误数据
|
||||
ZCAN_DT_ZCAN_LIN_DATA = 4, // LIN数据
|
||||
ZCAN_DT_ZCAN_LIN_ERROR_DATA = 5, // LIN错误数据
|
||||
};
|
||||
#pragma pack(push, 1)
|
||||
|
||||
typedef enum {
|
||||
ZCAN_TX_NORM = 0, /**< normal transmission */
|
||||
ZCAN_TX_ONCE = 1, /**< single-shot transmission */
|
||||
ZCAN_SR_NORM = 2, /**< self reception */
|
||||
ZCAN_SR_ONCE = 3, /**< single-shot transmission & self reception */
|
||||
} ZCAN_TX_MODE;
|
||||
|
||||
/** CAN filter configuration */
|
||||
typedef struct {
|
||||
U8 type; /**< 0-std_frame, 1-ext_frame */
|
||||
U8 pad[3];
|
||||
U32 sid; /**< start-id */
|
||||
U32 eid; /**< end-id */
|
||||
} ZCAN_FILTER;
|
||||
|
||||
typedef struct {
|
||||
U32 size;
|
||||
ZCAN_FILTER table[64];
|
||||
} ZCAN_FILTER_TABLE;
|
||||
|
||||
/** controller initialization */
|
||||
typedef struct {
|
||||
U32 clk; // 时钟频率
|
||||
U32 mode; /**< bit0-normal/listen_only, bit1-ISO/BOSCH */
|
||||
struct {
|
||||
U8 tseg1;
|
||||
U8 tseg2;
|
||||
U8 sjw;
|
||||
U8 smp;
|
||||
U16 brp;
|
||||
} aset;
|
||||
struct {
|
||||
U8 tseg1;
|
||||
U8 tseg2;
|
||||
U8 sjw;
|
||||
U8 smp;
|
||||
U16 brp;
|
||||
} dset;
|
||||
} ZCAN_INIT;
|
||||
|
||||
/** CAN message info */
|
||||
typedef struct {
|
||||
U32 txm : 4; /**< TX-mode, @see ZCAN_TX_MODE */
|
||||
U32 fmt : 4; /**< 0-CAN2.0, 1-CANFD */
|
||||
U32 sdf : 1; /**< 0-data_frame, 1-remote_frame */
|
||||
U32 sef : 1; /**< 0-std_frame, 1-ext_frame */
|
||||
U32 err : 1; /**< error flag */
|
||||
U32 brs : 1; /**< bit-rate switch */
|
||||
U32 est : 1; /**< error state */
|
||||
U32 tx : 1; /**< received valid, tx frame */
|
||||
U32 echo : 1; /**< tx valid, echo frame */
|
||||
U32 qsend_100us:1; /**< queue send delay unit, 1-100us, 0-ms */
|
||||
U32 qsend:1; /**< send valid, queue send frame */
|
||||
U32 pad : 15;
|
||||
} ZCAN_MSG_INF;
|
||||
|
||||
/** CAN message header */
|
||||
typedef struct {
|
||||
U32 ts; /**< timestamp */
|
||||
U32 id; /**< CAN-ID */
|
||||
ZCAN_MSG_INF inf; /**< @see ZCAN_MSG_INF */
|
||||
U16 pad; /**< send queue delay */
|
||||
U8 chn; /**< channel */
|
||||
U8 len; /**< data length */
|
||||
} ZCAN_MSG_HDR;
|
||||
|
||||
/** CAN2.0-frame */
|
||||
typedef struct {
|
||||
ZCAN_MSG_HDR hdr;
|
||||
U8 dat[8];
|
||||
} ZCAN_20_MSG;
|
||||
|
||||
/** CANFD-frame */
|
||||
typedef struct {
|
||||
ZCAN_MSG_HDR hdr;
|
||||
U8 dat[64];
|
||||
} ZCAN_FD_MSG;
|
||||
|
||||
// 错误帧
|
||||
typedef struct {
|
||||
ZCAN_MSG_HDR hdr;
|
||||
U8 dat[8];
|
||||
} ZCAN_ERR_MSG;
|
||||
|
||||
// 定时发送结构体
|
||||
typedef struct {
|
||||
U32 interval; // 定时发送周期，单位 100us
|
||||
U16 repeat; // 发送次数，0等于循环发
|
||||
U8 index; // 定时发送列表的帧索引号，也就是第几条定时发送报文
|
||||
U8 flags; // 0-此帧禁用定时发送，1-此帧使能定时发送
|
||||
ZCAN_FD_MSG msg; // CANFD帧结构体
|
||||
} ZCAN_TTX;
|
||||
|
||||
typedef struct {
|
||||
U32 size; // 实际生效的数组的长度（用来控制实际生效多少条定时发送帧结构体）
|
||||
ZCAN_TTX table[8]; // 最大设置8条
|
||||
} ZCAN_TTX_CFG;
|
||||
|
||||
// 设备信息
|
||||
typedef struct {
|
||||
U16 hwv; /**< hardware version */
|
||||
U16 fwv; /**< firmware version */
|
||||
U16 drv; /**< driver version */
|
||||
U16 api; /**< API version */
|
||||
U16 irq; /**< IRQ */
|
||||
U8 chn; /**< channels */
|
||||
U8 sn[20]; /**< serial number */
|
||||
U8 id[40]; /**< card id */
|
||||
U16 pad[4];
|
||||
} ZCAN_DEV_INF;
|
||||
|
||||
/** controller status */
|
||||
typedef struct {
|
||||
U8 IR; /**< not used(for backward compatibility) */
|
||||
U8 MOD; /**< not used */
|
||||
U8 SR; /**< not used */
|
||||
U8 ALC; /**< not used */
|
||||
U8 ECC; /**< not used */
|
||||
U8 EWL; /**< not used */
|
||||
U8 RXE; /**< RX errors */
|
||||
U8 TXE; /**< TX errors */
|
||||
U32 PAD;
|
||||
} ZCAN_STAT;
|
||||
|
||||
//lin begin
|
||||
typedef struct _VCI_LIN_INIT_CONFIG
|
||||
{
|
||||
U8 linMode; // 是否作为主机，0-从机，1-主机
|
||||
U8 chkSumMode; // 校验方式，1-经典校验 2-增强校验 3-自动(对应eZLINChkSumMode的模式)
|
||||
U16 reserved; // 保留
|
||||
U32 linBaud; // 波特率，取值1000~20000
|
||||
}ZCAN_LIN_INIT_CONFIG, *PZCAN_LIN_INIT_CONFIG;
|
||||
|
||||
typedef struct tagZCANLINData
|
||||
{
|
||||
union{
|
||||
struct {
|
||||
U8 ID : 6; // 帧ID
|
||||
U8 Parity : 2; // 帧ID校验
|
||||
}unionVal;
|
||||
U8 rawVal; // 受保护的ID原始值
|
||||
} PID; // 受保护的ID
|
||||
struct
|
||||
{
|
||||
U64 timeStamp; // 时间戳，单位微秒(us)
|
||||
U8 dataLen; // 数据长度
|
||||
U8 dir; // 传输方向，0-接收 1-发送
|
||||
U8 chkSum; // 数据校验, 部分设备不支持校验数据的获取
|
||||
U8 reserved[13]; // 保留
|
||||
U8 data[8]; // 数据
|
||||
}RxData; // 仅接收数据时有效
|
||||
U8 reserved[7]; // 保留
|
||||
}ZCANLINData;
|
||||
|
||||
typedef struct tagZCANLINErrData
|
||||
{
|
||||
U64 timeStamp; // 时间戳, 单位微秒(us)
|
||||
union {
|
||||
struct {
|
||||
U8 ID : 6; // 帧ID
|
||||
U8 Parity : 2; // 帧ID校验
|
||||
}unionVal;
|
||||
U8 rawVal; // 受保护的ID原始值
|
||||
} PID; // 受保护的ID
|
||||
U8 dataLen;
|
||||
U8 data[8];
|
||||
union
|
||||
{
|
||||
struct
|
||||
{
|
||||
U16 errStage : 4; // 错误阶段
|
||||
U16 errReason : 4; // 错误原因
|
||||
U16 reserved : 8; // 保留
|
||||
};
|
||||
U16 unionErrData;
|
||||
}errData;
|
||||
U8 dir; // 传输方向
|
||||
U8 chkSum; // 数据校验, 部分设备不支持校验数据的获取
|
||||
U8 reserved[10]; // 保留
|
||||
}ZCANLINErrData;
|
||||
|
||||
typedef struct _VCI_LIN_MSG{
|
||||
U8 chnl; // 数据通道
|
||||
U8 dataType; // 数据类型, 0-LIN数据 1-LIN错误数据
|
||||
union
|
||||
{
|
||||
ZCANLINData zcanLINData; // LIN数据
|
||||
ZCANLINErrData zcanLINErrData; // LIN错误数据
|
||||
U8 raw[46]; // RAW数据
|
||||
} data; // 实际数据, 联合体，有效成员根据 dataType 字段而定
|
||||
}ZCAN_LIN_MSG, *PZCAN_LIN_MSG;
|
||||
|
||||
enum eZLINChkSumMode
|
||||
{
|
||||
DEFAULT = 0, // 默认，启动时配置
|
||||
CLASSIC_CHKSUM, // 经典校验
|
||||
ENHANCE_CHKSUM, // 增强校验
|
||||
AUTOMATIC, // 自动，设备自动识别校验方式（仅ZCAN_SetLINSubscribe时有效）
|
||||
};
|
||||
enum
|
||||
{
|
||||
MAX_LIN_ID_COUNT = 63,
|
||||
MIN_LIN_DLC = 1,
|
||||
MAX_LIN_DLC = 8,
|
||||
AUTO_LIN_DLC = 255,
|
||||
};
|
||||
typedef struct _VCI_LIN_PUBLISH_CFG
|
||||
{
|
||||
U8 ID; // 受保护的ID（ID取值范围为0-63）
|
||||
U8 dataLen; // dataLen范围为1-8
|
||||
U8 data[8];
|
||||
U8 chkSumMode; // 校验方式，0-默认，启动时配置 1-经典校验 2-增强校验(对应eZLINChkSumMode的模式)
|
||||
U8 reserved[5]; // 保留
|
||||
}ZCAN_LIN_PUBLISH_CFG, *PZCAN_LIN_PUBLISH_CFG;
|
||||
|
||||
typedef struct _VCI_LIN_SUBSCIBE_CFG
|
||||
{
|
||||
U8 ID; // 受保护的ID（ID取值范围为0-63）
|
||||
U8 dataLen; // dataLen范围为1-8 当为255（0xff）则表示设备自动识别报文长度
|
||||
U8 chkSumMode; // 校验方式，0-默认，启动时配置 1-经典校验 2-增强校验 3-自动(对应eZLINChkSumMode的模式)
|
||||
U8 reserved[5]; // 保留
|
||||
}ZCAN_LIN_SUBSCIBE_CFG, *PZCAN_LIN_SUBSCIBE_CFG;
|
||||
|
||||
// 合并接收数据数据结构，支持CAN/CANFD/LIN/错误等不同类型数据
|
||||
typedef struct tagZCANDataObj
|
||||
{
|
||||
U8 dataType; // 数据类型, 参考eZCANDataDEF中 数据类型 部分定义
|
||||
U8 chnl; // 数据通道
|
||||
union{
|
||||
struct{
|
||||
U16 reserved : 16; // 保留
|
||||
}unionVal;
|
||||
U16 rawVal;
|
||||
}flag; // 标志信息, 暂未使用
|
||||
U8 extraData[4]; // 额外数据, 暂未使用
|
||||
union
|
||||
{
|
||||
ZCAN_20_MSG zcanCANData; // CAN数据
|
||||
ZCAN_FD_MSG zcanCANFDData; // CANFD数据
|
||||
ZCAN_ERR_MSG zcanErrData; // 错误数据
|
||||
ZCANLINData zcanLINData; // LIN数据
|
||||
ZCANLINErrData zcanLINErrData; // LIN错误数据
|
||||
U8 raw[92]; // RAW数据
|
||||
} data; // 实际数据, 联合体，有效成员根据 dataType 字段而定
|
||||
}ZCANDataObj;
|
||||
|
||||
|
||||
// UDS传输协议版本
|
||||
typedef U8 ZCAN_UDS_TRANS_VER;
|
||||
#define ZCAN_UDS_TRANS_VER_0 0 // ISO15765-2(2004版本)
|
||||
#define ZCAN_UDS_TRANS_VER_1 1 // ISO15765-2(2016版本)
|
||||
|
||||
// 帧类型
|
||||
typedef U8 ZCAN_UDS_FRAME_TYPE;
|
||||
#define ZCAN_UDS_FRAME_CAN 0 // CAN帧
|
||||
#define ZCAN_UDS_FRAME_CANFD 1 // CANFD帧
|
||||
#define ZCAN_UDS_FRAME_CANFD_BRS 2 // CANFD加速帧
|
||||
|
||||
// UDS填充模式
|
||||
#define FILL_MODE_SHORT 0 // 小于8字节填充至8字节，大于8字节时按DLC就近填充
|
||||
#define FILL_MODE_NONE 1 // 不填充
|
||||
#define FILL_MODE_MAX 2 // 填充至最大数据长度 (不建议)
|
||||
|
||||
// CAN UDS请求数据
|
||||
typedef struct _ZCAN_UDS_REQUEST
|
||||
{
|
||||
U32 req_id; // 请求事务ID，范围0~65535，本次请求的唯一标识
|
||||
U8 channel; // 设备通道索引 0~255
|
||||
ZCAN_UDS_FRAME_TYPE frame_type; // 帧类型
|
||||
U8 reserved0[2]; // 保留
|
||||
U32 src_addr; // 请求地址
|
||||
U32 dst_addr; // 响应地址
|
||||
U8 suppress_response; // 1:抑制响应
|
||||
U8 sid; // 请求服务id
|
||||
U8 reserved1[6]; // 保留
|
||||
struct {
|
||||
U32 timeout; // 响应超时时间(ms)。因PC定时器误差，建议设置不小于200ms
|
||||
U32 enhanced_timeout; // 收到消极响应错误码为0x78后的超时时间(ms)。因PC定时器误差，建议设置不小于200ms
|
||||
U8 check_any_negative_response:1; // 接收到非本次请求服务的消极响应时是否需要判定为响应错误
|
||||
U8 wait_if_suppress_response:1; // 抑制响应时是否需要等待消极响应，等待时长为响应超时时间
|
||||
U8 flag:6; // 保留
|
||||
U8 reserved0[7]; // 保留
|
||||
} session_param; // 会话层参数
|
||||
struct {
|
||||
ZCAN_UDS_TRANS_VER version; // 传输协议版本, VERSION_0, VERSION_1
|
||||
U8 max_data_len; // 单帧最大数据长度, can:8, canfd:64
|
||||
U8 local_st_min; // 本程序发送流控时用，连续帧之间的最小间隔, 0x00-0x7F(0ms~127ms), 0xF1-0xF9(100us~900us)
|
||||
U8 block_size; // 流控帧的块大小
|
||||
U8 fill_byte; // 无效字节的填充数据
|
||||
U8 ext_frame; // 0:标准帧 1:扩展帧
|
||||
U8 is_modify_ecu_st_min; // 是否忽略ECU返回流控的STmin，强制使用本程序设置的 remote_st_min
|
||||
U8 remote_st_min; // 发送多帧时用, is_ignore_ecu_st_min = 1 时有效, 0x00-0x7F(0ms~127ms), 0xF1-0xF9(100us~900us)
|
||||
U32 fc_timeout; // 接收流控超时时间(ms), 如发送首帧后需要等待回应流控帧
|
||||
U8 fill_mode; // 数据长度填充模式, FILL_MODE_SHORT, FILL_MODE_NONE, FILL_MODE_MAX
|
||||
U8 reserved0[3]; // 保留
|
||||
} trans_param; // 传输层参数
|
||||
U8 *data; // 数据数组(不包含SID)
|
||||
U32 data_len; // 数据数组的长度
|
||||
U32 reserved2; // 保留
|
||||
} ZCAN_UDS_REQUEST;
|
||||
|
||||
// UDS错误码
|
||||
typedef U8 ZCAN_UDS_ERROR;
|
||||
#define ZCAN_UDS_ERROR_OK 0 // 没错误
|
||||
#define ZCAN_UDS_ERROR_TIMEOUT 1 // 响应超时
|
||||
#define ZCAN_UDS_ERROR_TRANSPORT 2 // 发送数据失败
|
||||
#define ZCAN_UDS_ERROR_CANCEL 3 // 取消请求
|
||||
#define ZCAN_UDS_ERROR_SUPPRESS_RESPONSE 4 // 抑制响应
|
||||
#define ZCAN_UDS_ERROR_BUSY 5 // 忙碌中
|
||||
#define ZCAN_UDS_ERROR_REQ_PARAM 6 // 请求参数错误
|
||||
#define ZCAN_UDS_ERROR_OTHTER 100
|
||||
|
||||
typedef U8 ZCAN_UDS_RESPONSE_TYPE;
|
||||
#define ZCAN_UDS_RT_NEGATIVE 0 // 消极响应
|
||||
#define ZCAN_UDS_RT_POSITIVE 1 // 积极响应
|
||||
|
||||
// UDS响应数据
|
||||
typedef struct _ZCAN_UDS_RESPONSE
|
||||
{
|
||||
ZCAN_UDS_ERROR status; // 响应状态
|
||||
U8 reserved[6]; // 保留
|
||||
ZCAN_UDS_RESPONSE_TYPE type; // 响应类型
|
||||
union {
|
||||
struct {
|
||||
U8 sid; // 响应服务id
|
||||
U32 data_len; // 数据长度(不包含SID), 数据存放在接口传入的dataBuf中
|
||||
} positive;
|
||||
struct {
|
||||
U8 neg_code; // 固定为0x7F
|
||||
U8 sid; // 请求服务id
|
||||
U8 error_code; // 错误码
|
||||
} negative;
|
||||
U8 raw[8];
|
||||
};
|
||||
} ZCAN_UDS_RESPONSE;
|
||||
|
||||
// UDS控制类型
|
||||
typedef U32 ZCAN_UDS_CTRL_CODE;
|
||||
#define ZCAN_UDS_CTRL_STOP_REQ 0 // 停止UDS请求
|
||||
|
||||
// UDS控制请求
|
||||
typedef struct _ZCAN_UDS_CTRL_REQ
|
||||
{
|
||||
U32 reqID; // 请求事务ID，指明要操作哪一条请求
|
||||
ZCAN_UDS_CTRL_CODE cmd; // 控制类型
|
||||
U8 reserved[8]; // 保留
|
||||
} ZCAN_UDS_CTRL_REQ;
|
||||
|
||||
// UDS控制结果
|
||||
typedef U32 ZCAN_UDS_CTRL_RESULT;
|
||||
#define ZCAN_UDS_CTRL_RESULT_OK 0 // 成功
|
||||
#define ZCAN_UDS_CTRL_RESULT_ERR 1 // 失败
|
||||
|
||||
// UDS控制响应数据
|
||||
typedef struct _ZCAN_UDS_CTRL_RESP
|
||||
{
|
||||
ZCAN_UDS_CTRL_RESULT result; // 操作结果
|
||||
U8 reserved[8]; // 保留
|
||||
} ZCAN_UDS_CTRL_RESP;
|
||||
|
||||
#pragma pack(pop)
|
||||
|
||||
//
|
||||
EXTERN_C U32 ZCAN_API VCI_OpenDevice(U32 Type, U32 Card, U32 Reserved);
|
||||
EXTERN_C U32 ZCAN_API VCI_CloseDevice(U32 Type, U32 Card);
|
||||
EXTERN_C U32 ZCAN_API VCI_InitCAN(U32 Type, U32 Card, U32 Port, ZCAN_INIT *pInit);
|
||||
EXTERN_C U32 ZCAN_API VCI_ReadBoardInfo(U32 Type, U32 Card, ZCAN_DEV_INF *pInfo);
|
||||
EXTERN_C U32 ZCAN_API VCI_ReadErrInfo(U32 Type, U32 Card, U32 Port, ZCAN_ERR_MSG *pErr);
|
||||
EXTERN_C U32 ZCAN_API VCI_ReadCANStatus(U32 Type, U32 Card, U32 Port, ZCAN_STAT *pStat);
|
||||
EXTERN_C U32 ZCAN_API VCI_GetReference(U32 Type, U32 Card, U32 Port, U32 Ref, void *pData);
|
||||
EXTERN_C U32 ZCAN_API VCI_SetReference(U32 Type, U32 Card, U32 Port, U32 Ref, void *pData);
|
||||
EXTERN_C U32 ZCAN_API VCI_GetReceiveNum(U32 Type, U32 Card, U32 Port);
|
||||
EXTERN_C U32 ZCAN_API VCI_ClearBuffer(U32 Type, U32 Card, U32 Port);
|
||||
EXTERN_C U32 ZCAN_API VCI_StartCAN(U32 Type, U32 Card, U32 Port);
|
||||
EXTERN_C U32 ZCAN_API VCI_ResetCAN(U32 Type, U32 Card, U32 Port);
|
||||
EXTERN_C U32 ZCAN_API VCI_Transmit(U32 Type, U32 Card, U32 Port, ZCAN_20_MSG *pData, U32 Count);
|
||||
EXTERN_C U32 ZCAN_API VCI_TransmitFD(U32 Type, U32 Card, U32 Port, ZCAN_FD_MSG *pData, U32 Count);
|
||||
EXTERN_C U32 ZCAN_API VCI_Receive(U32 Type, U32 Card, U32 Port, ZCAN_20_MSG *pData, U32 Count, U32 Time);
|
||||
EXTERN_C U32 ZCAN_API VCI_ReceiveFD(U32 Type, U32 Card, U32 Port, ZCAN_FD_MSG *pData, U32 Count, U32 Time);
|
||||
EXTERN_C U32 ZCAN_API VCI_Debug(U32 Debug);
|
||||
|
||||
// LIN
|
||||
EXTERN_C U32 VCI_InitLIN(U32 Type, U32 Card, U32 LinChn, PZCAN_LIN_INIT_CONFIG pLINInitConfig);
|
||||
EXTERN_C U32 VCI_StartLIN(U32 Type, U32 Card, U32 LinChn);
|
||||
EXTERN_C U32 VCI_ResetLIN(U32 Type, U32 Card, U32 LinChn);
|
||||
EXTERN_C U32 VCI_TransmitLIN(U32 Type, U32 Card, U32 LinChn, PZCAN_LIN_MSG pSend, U32 Len);
|
||||
EXTERN_C U32 VCI_GetLINReceiveNum(U32 Type, U32 Card, U32 LinChn);
|
||||
EXTERN_C U32 VCI_ClearLINBuffer(U32 Type, U32 Card, U32 LinChn);
|
||||
EXTERN_C U32 VCI_ReceiveLIN(U32 Type, U32 Card, U32 LinChn, PZCAN_LIN_MSG pReceive, U32 Len,int WaitTime);
|
||||
EXTERN_C U32 VCI_SetLINSubscribe(U32 Type, U32 Card, U32 LinChn, PZCAN_LIN_SUBSCIBE_CFG pSend, U32 nSubscribeCount);
|
||||
EXTERN_C U32 VCI_SetLINPublish(U32 Type, U32 Card, U32 LinChn, PZCAN_LIN_PUBLISH_CFG pSend, U32 nPublishCount);
|
||||
|
||||
// 合并接收
|
||||
EXTERN_C U32 VCI_TransmitData(unsigned Type, unsigned Card, unsigned Port, ZCANDataObj *pData, unsigned Count);
|
||||
EXTERN_C U32 VCI_ReceiveData(unsigned Type, unsigned Card, unsigned Port, ZCANDataObj *pData, unsigned Count, unsigned Time);
|
||||
|
||||
// UDS
|
||||
EXTERN_C U32 VCI_UDS_Request(unsigned Type, unsigned Card, const ZCAN_UDS_REQUEST *req, ZCAN_UDS_RESPONSE *resp, U8 *dataBuf, U32 dataBufSize);
|
||||
EXTERN_C U32 VCI_UDS_Control(unsigned Type, unsigned Card, const ZCAN_UDS_CTRL_REQ *ctrl, ZCAN_UDS_CTRL_RESP *resp);
|
||||
+35
-14
@@ -1,22 +1,43 @@
|
||||
# Orin 可直接运行目录
|
||||
# Orin 纯 C++ 运行包
|
||||
|
||||
本目录包含实际在 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 主机。
|
||||
相机每次等待默认 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。
|
||||
|
||||
```bash
|
||||
cd orin
|
||||
sha256sum -c SHA256SUMS
|
||||
./scripts/initialize_camera.sh
|
||||
./run.sh
|
||||
./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
|
||||
```
|
||||
|
||||
可视化可选：`./run.sh --visualize`，需要板端本地桌面 DISPLAY。
|
||||
默认持续推理直到 Ctrl+C / SIGTERM；正常退出没有统计输出。
|
||||
预览窗口关闭后推理继续。结果通过 C++ CameraPipeline::latest() 获取，不通过端口或结果文件传递。
|
||||
只运行一个摄像头进程。Ctrl+C / SIGTERM 停止；关闭可视化窗口不停止推理。无图形桌面时继续无窗口运行。
|
||||
|
||||
程序为 build/camera_risk，配置为 config/camera.json，引擎为 engines/*.engine。
|
||||
./run.sh 从所在目录解析路径，也可以在其他工作目录通过绝对路径调用。
|
||||
CAN 0x391 data[0..3] 为左前、右前、左后、右后。默认左侧，`--can-side right` 切换右侧。缺失或超时报文输出 CAN_UNKNOWN，风险分数为空。
|
||||
|
||||
bin/drivers 中 GMSL 模块对应内核 5.10.120-tegra。初始化脚本检查内核版本；加载模块需要 root 或现有 sudo 权限。
|
||||
门区保留当前室内联调设置，door.calibration_verified=false。
|
||||
`--no-can --active-door both` 模拟门状态；`--duration 30` 限时运行；`--save-results FILE.jsonl` 显式保存结果，默认不写文件。完整参数见 `./run.sh --help`。
|
||||
|
||||
最新结果保存在进程内的 `shared_ptr<const cms::FrameResult>`；C++ 下游调用 `pipeline.latest()` 读取。接口头文件位于仓库 include/，不是 Python 变量或跨进程共享内存。
|
||||
|
||||
模型、驱动和编译产物绑定目标 GPU、TensorRT 与内核环境。标定文件需匹配实际机位，默认 calibration_verified=false。系统 CUDA/TensorRT/OpenCV 库由 JetPack 提供。
|
||||
|
||||
## 可选保存（默认全部关闭）
|
||||
|
||||
三个独立参数可以单独使用，也可以组合：
|
||||
|
||||
```bash
|
||||
./run.sh --save-original --save-annotated --save-results
|
||||
```
|
||||
|
||||
开启任一项时，程序自动创建可执行文件所属工程的 `output/` 及三个子目录：
|
||||
|
||||
- `original/`：`--save-original` 保存未叠加的原图，JPEG 质量 95。
|
||||
- `annotated/`：`--save-annotated` 保存带框、关键点及风险文字的 JPEG，无需开启可视化窗口。
|
||||
- `results/`：`--save-results` 每个完成推理的帧保存一个 JSON，包含帧号、检测框、跟踪 ID、关键点及置信度、风险和耗时；有 CAN 的分支还包含相应状态。
|
||||
|
||||
同一帧三类文件同名（扩展名不同），文件名包含运行标识和帧号，多次运行不覆盖。JSON 的 `images` 提供相对于 `output/` 的图片路径，未选择保存的图片为 null。源码入口写入工程 `output/`；独立包入口写入 `orin/output/`。默认不创建目录、不保存文件。
|
||||
|
||||
保存对象是完成推理的帧；惰性分支暂停时不新增文件。开启保存会增加编码和磁盘 I/O 开销。写入失败时报告错误并停止，不静默丢失文件。
|
||||
|
||||
+15
-11
@@ -1,11 +1,15 @@
|
||||
926a5165a4ebffe9568cf694018afb097cfb69b6e696a19939e5d364a9e3dfa5 README.md
|
||||
6d5e874fd27fcfd75d303d7e35d9672028af71041f52dcc82a1e83676faefa65 bin/drivers/max9295.ko
|
||||
8a78d04ef8be44e9b62a5e4ee315d3d6d679e34aedff02a234bbf4d40e222933 bin/drivers/max9296.ko
|
||||
3da628587477cc35d5e3f8aee86e718e2ddf16808e893e00ba6e164b2307e843 bin/drivers/sgx-yuv-gmsl2.ko
|
||||
b5102fe0a529349bb8e9700ae1d8e8ddeaa7c4279cb28e7826c5cc32dce2bb1d build/camera_risk
|
||||
f2334c43e6a48480260c770d46a3ab0edb855f6f52bc8b91d1559e69edfad043 config/camera.json
|
||||
9ba768d559fd3b64e833b50c62c70f2adfd6c169620aea5fba32cfb1b4c5b0e9 config/door_mask.json
|
||||
ebcd09e8c2e35008adbb0c1d222e580b17e0df1c8093df3481ba3374cd9a8174 engines/rtmw_l_384.engine
|
||||
f6a65b926ac1144484f4aa93cd26d7997d32d62310ab93fe867f2bd4412222e9 engines/yolo11s.engine
|
||||
7f42b0e7f9396e0d82313a892fc62c153261dab1206a206a466bce6376f58a36 run.sh
|
||||
54e1d63e3aa72ad9b056a224d55968d4947ca6703a8d1c405570de5707a7e7ef scripts/initialize_camera.sh
|
||||
836d9b85249866642f7f3ca7cfe4eec092f29fd3b44bd9fdbe1c2ba3ca3d68b0 ./README.md
|
||||
9ba768d559fd3b64e833b50c62c70f2adfd6c169620aea5fba32cfb1b4c5b0e9 ./annotations/constant_door_mask.json
|
||||
3544fb1c7b1b27b39db27d8f849d5e8652f5d09a950a59c5dd0c7703086b4f6b ./annotations/door_zone_multifeature.json
|
||||
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
|
||||
8dae8c557deadbe6bd7241c0dcd7025cb007077ee39a79b115e953a2b22cd9c1 ./lib/libusb-1.0.so
|
||||
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
|
||||
|
||||
@@ -0,0 +1,47 @@
|
||||
{
|
||||
"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
|
||||
}
|
||||
}
|
||||
}
|
||||
Binary file not shown.
+4
-20
@@ -1,22 +1,6 @@
|
||||
{
|
||||
"camera": {
|
||||
"device": "/dev/video5",
|
||||
"width": 1920,
|
||||
"height": 1536,
|
||||
"fps": 30,
|
||||
"buffers": 4,
|
||||
"timeout_ms": 2000
|
||||
},
|
||||
"inference": {
|
||||
"engine_directory": "../engines",
|
||||
"detector_confidence": 0.30,
|
||||
"max_people": 3,
|
||||
"queue_capacity": 2
|
||||
},
|
||||
"door": {
|
||||
"mask": "door_mask.json",
|
||||
"warning_relative": 0.30,
|
||||
"release_relative": 0.45,
|
||||
"calibration_verified": false
|
||||
}
|
||||
"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}
|
||||
}
|
||||
|
||||
Binary file not shown.
Symlink
+1
@@ -0,0 +1 @@
|
||||
libusb-1.0.so
|
||||
Binary file not shown.
Symlink
+1
@@ -0,0 +1 @@
|
||||
libusbcanfd.so
|
||||
+2
-1
@@ -1,4 +1,5 @@
|
||||
#!/usr/bin/env bash
|
||||
set -euo pipefail
|
||||
root="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" && pwd)"
|
||||
root="$(cd "$(dirname "$0")" && pwd)"
|
||||
export LD_LIBRARY_PATH="$root/lib${LD_LIBRARY_PATH:+:$LD_LIBRARY_PATH}"
|
||||
exec "$root/build/camera_risk" --config "$root/config/camera.json" "$@"
|
||||
|
||||
@@ -1,21 +1,5 @@
|
||||
#!/usr/bin/env bash
|
||||
set -euo pipefail
|
||||
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
|
||||
root="$(cd "$(dirname "$0")/.." && pwd)"
|
||||
export DRIVER_DIR="$root/bin/drivers"
|
||||
exec "$root/scripts/initialize_camera_impl.sh" "$@"
|
||||
|
||||
Executable
+27
@@ -0,0 +1,27 @@
|
||||
#!/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"
|
||||
+2
-1
@@ -1,4 +1,5 @@
|
||||
#!/usr/bin/env bash
|
||||
set -euo pipefail
|
||||
root="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" && pwd)"
|
||||
root="$(cd "$(dirname "$0")" && pwd)"
|
||||
export LD_LIBRARY_PATH="$root/orin/lib${LD_LIBRARY_PATH:+:$LD_LIBRARY_PATH}"
|
||||
exec "$root/build/camera_risk" --config "$root/config/camera.json" "$@"
|
||||
|
||||
+2
-2
@@ -1,7 +1,7 @@
|
||||
#!/usr/bin/env bash
|
||||
set -euo pipefail
|
||||
root="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")/.." && pwd)"
|
||||
root="$(cd "$(dirname "$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
|
||||
|
||||
@@ -1,4 +1,27 @@
|
||||
#!/usr/bin/env bash
|
||||
# Initialize the camera using the packaged Orin drivers.
|
||||
set -euo pipefail
|
||||
root="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")/.." && pwd)"
|
||||
exec "$root/orin/scripts/initialize_camera.sh" "$@"
|
||||
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"
|
||||
|
||||
Executable
+18
@@ -0,0 +1,18 @@
|
||||
#!/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"
|
||||
(
|
||||
cd "$package"
|
||||
find . -type f ! -path './output/*' ! -path './outputs/*' ! -path './logs/*' ! -name SHA256SUMS -print0 |
|
||||
sort -z | xargs -0 sha256sum > SHA256SUMS
|
||||
)
|
||||
echo "C++ runtime ready: $package/build/camera_risk"
|
||||
+68
-124
@@ -1,146 +1,90 @@
|
||||
#include "camera.hpp"
|
||||
#include "v4l2_capture.h"
|
||||
#include "uyvy_converter.hpp"
|
||||
#include <linux/videodev2.h>
|
||||
#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 <opencv2/videoio.hpp>
|
||||
#include <chrono>
|
||||
#include <cstring>
|
||||
#include <limits>
|
||||
#include <stdexcept>
|
||||
|
||||
#include <thread>
|
||||
namespace cms {
|
||||
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));
|
||||
}
|
||||
}
|
||||
std::uint64_t monotonicNs(){return std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::steady_clock::now().time_since_epoch()).count();}
|
||||
struct Camera::Impl {
|
||||
struct Buffer {void* data{};std::size_t size{};};
|
||||
CameraConfig config;
|
||||
int fd{-1},stopFd{-1};bool streaming{};
|
||||
std::uint32_t stride{},lastSequence{};bool haveSequence{};
|
||||
std::uint64_t index{};
|
||||
std::uint32_t consecutiveErrors{};
|
||||
std::atomic<bool> stopped{false};
|
||||
std::vector<Buffer> buffers;
|
||||
V4l2Capture capture;
|
||||
cv::VideoCapture video;
|
||||
std::unique_ptr<UyvyConverter> converter;
|
||||
std::atomic<bool> stopped{false};
|
||||
std::uint64_t index{};
|
||||
std::uint32_t previousSequence{};
|
||||
bool haveSequence{};
|
||||
explicit Impl(CameraConfig c):config(std::move(c)) {
|
||||
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");
|
||||
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 {
|
||||
openCapture();
|
||||
}
|
||||
}
|
||||
void openCapture() {
|
||||
try {
|
||||
stopFd=eventfd(0,EFD_CLOEXEC|EFD_NONBLOCK);
|
||||
if(stopFd<0)throw std::runtime_error("Cannot create camera stop event");
|
||||
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,¶ms)==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,¶ms,"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");
|
||||
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");
|
||||
if(!converter)converter=std::make_unique<UyvyConverter>(config.width,config.height);
|
||||
}catch(...){capture.close();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;
|
||||
std::string lastError="no camera frame";
|
||||
while(!stopped) {
|
||||
auto now=monotonicNs();
|
||||
if((now-started)*1e-6>=config.timeout_ms) {
|
||||
if(++failedWindows>=6)
|
||||
throw std::runtime_error("Camera capture failed 6 consecutive times: "+config.device+": "+lastError);
|
||||
started=now;
|
||||
}
|
||||
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(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;}
|
||||
}
|
||||
converter=std::make_unique<UyvyConverter>(config.width,config.height);
|
||||
auto type=V4L2_BUF_TYPE_VIDEO_CAPTURE;checkedIoctl(fd,VIDIOC_STREAMON,&type,"VIDIOC_STREAMON");streaming=true;
|
||||
} catch(...) {release();throw;}
|
||||
}
|
||||
~Impl(){release();}
|
||||
void release() 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;}
|
||||
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() {
|
||||
while(!stopped.load()) {
|
||||
pollfd descriptors[2]={{fd,POLLIN,0},{stopFd,POLLIN,0}};
|
||||
int ready=poll(descriptors,2,config.timeout_ms);
|
||||
if(ready<0&&errno==EINTR)continue;
|
||||
if(stopped.load()||(descriptors[1].revents&POLLIN))return {};
|
||||
if(ready==0)throw std::runtime_error("Camera capture timeout: "+config.device);
|
||||
if(ready<0||(descriptors[0].revents&(POLLERR|POLLHUP|POLLNVAL)))throw std::runtime_error("Camera disconnected or poll failed");
|
||||
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)throw std::runtime_error("VIDIOC_DQBUF: "+std::string(std::strerror(errno)));
|
||||
if(buffer.index>=buffers.size())throw std::runtime_error("Invalid V4L2 buffer index");
|
||||
// The GMSL driver can mark its startup buffer as corrupt. Requeue
|
||||
// it without conversion or inference; persistent corruption is fatal.
|
||||
if(buffer.flags&V4L2_BUF_FLAG_ERROR) {
|
||||
checkedIoctl(fd,VIDIOC_QBUF,&buffer,"VIDIOC_QBUF(error frame)");
|
||||
if(++consecutiveErrors>=static_cast<unsigned>(config.fps))
|
||||
throw std::runtime_error("Camera produced one second of consecutive error frames");
|
||||
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;
|
||||
}
|
||||
CapturedFrame frame;
|
||||
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;
|
||||
}
|
||||
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 {
|
||||
if(buffer.bytesused<std::size_t(stride)*config.height||buffer.bytesused>buffers[buffer.index].size)
|
||||
throw std::runtime_error("Invalid camera frame: flags="+std::to_string(buffer.flags)+", used="+std::to_string(buffer.bytesused)+", required="+std::to_string(std::size_t(stride)*config.height)+", allocated="+std::to_string(buffers[buffer.index].size));
|
||||
auto& m=frame.metadata;
|
||||
m.discarded_error_frames=consecutiveErrors;consecutiveErrors=0;
|
||||
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.driver_timestamp_monotonic&&m.driver_timestamp_ns>0&&m.driver_timestamp_ns<=m.received_monotonic_ns?m.driver_timestamp_ns:m.received_monotonic_ns;
|
||||
if(haveSequence) {
|
||||
auto delta=static_cast<std::uint32_t>(buffer.sequence-lastSequence);
|
||||
if(delta==0||delta>0x7fffffffU)throw std::runtime_error("Camera sequence reset or repeated frame");
|
||||
m.skipped_driver_frames=delta-1;index+=delta;
|
||||
}
|
||||
m.frame_index=index;
|
||||
lastSequence=buffer.sequence;haveSequence=true;
|
||||
// Convert before requeuing. No published result references MMAP memory.
|
||||
frame.image=converter->convert(buffers[buffer.index].data,stride);
|
||||
} catch(...) {ioctl(fd,VIDIOC_QBUF,&buffer);throw;}
|
||||
checkedIoctl(fd,VIDIOC_QBUF,&buffer,"VIDIOC_QBUF");return frame;
|
||||
auto image=converter->convert(f.data,stride);capture.releaseFrame(f);return CapturedFrame{m,std::move(image)};
|
||||
}catch(...){capture.releaseFrame(f);throw;}
|
||||
}
|
||||
return {};
|
||||
}
|
||||
};
|
||||
Camera::Camera(CameraConfig config):impl_(std::make_unique<Impl>(std::move(config))){}
|
||||
Camera::Camera(CameraConfig c):impl_(std::make_unique<Impl>(std::move(c))){}
|
||||
Camera::~Camera()=default;
|
||||
std::optional<CapturedFrame> Camera::read(){return impl_->readFrame();}
|
||||
void Camera::requestStop() noexcept {impl_->stop();}
|
||||
std::optional<CapturedFrame> Camera::read(){return impl_->read();}
|
||||
void Camera::requestStop()noexcept{impl_->stopped=true;}
|
||||
}
|
||||
|
||||
+45
-7
@@ -5,6 +5,10 @@
|
||||
#include "door_risk.hpp"
|
||||
#include "records.hpp"
|
||||
#include "result_conversion.hpp"
|
||||
#include "door_zone.hpp"
|
||||
#include "can_state.hpp"
|
||||
#include "serialization.hpp"
|
||||
#include <fstream>
|
||||
#include <opencv2/core.hpp>
|
||||
#include <atomic>
|
||||
#include <cmath>
|
||||
@@ -50,10 +54,14 @@ struct CameraPipeline::Impl {
|
||||
BoundedQueue<CapturedFrame> captures;
|
||||
BoundedQueue<Detected> detections;
|
||||
LatestResult results;
|
||||
std::unique_ptr<OutputWriter> outputWriter;
|
||||
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::thread captureThread,detectorThread,inferenceThread;
|
||||
@@ -65,6 +73,10 @@ 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.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");
|
||||
@@ -93,6 +105,7 @@ 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 {
|
||||
outputWriter=std::make_unique<OutputWriter>(config.output);
|
||||
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;
|
||||
@@ -102,10 +115,18 @@ 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);
|
||||
active=!stopped.load();
|
||||
captureThread=std::thread([this]{try{captureLoop();}catch(...){fail();}});
|
||||
detectorThread=std::thread([this]{try{detectorLoop();}catch(...){fail();}});
|
||||
@@ -117,7 +138,8 @@ struct CameraPipeline::Impl {
|
||||
auto frame=camera->read();if(!frame)break;
|
||||
gaps+=frame->metadata.skipped_driver_frames;
|
||||
errorFrames+=frame->metadata.discarded_error_frames;
|
||||
if(!captures.push(std::move(*frame)))break;
|
||||
bool queued=config.camera.video.empty()?captures.pushLatest(std::move(*frame)):captures.push(std::move(*frame));
|
||||
if(!queued)break;
|
||||
++captured;
|
||||
}
|
||||
captures.close();
|
||||
@@ -127,16 +149,18 @@ struct CameraPipeline::Impl {
|
||||
while(!stopped) {
|
||||
auto frame=captures.pop();if(!frame)break;
|
||||
auto startedAt=monotonicNs();auto boxes=detector->infer(imageView(*frame->image),stream.value);
|
||||
if(!detections.push({std::move(*frame),std::move(boxes),elapsedMs(startedAt)}))break;
|
||||
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;
|
||||
}
|
||||
detections.close();
|
||||
}
|
||||
void inferenceLoop() {
|
||||
Stream stream;fangjia::BoxTracker boxesTracker;fangjia::PoseFusionTracker fusion;
|
||||
Stream stream;fangjia::BoxTracker boxesTracker;fangjia::PoseFusionTracker fusion;std::uint64_t processed=0;
|
||||
while(!stopped) {
|
||||
auto detected=detections.pop();if(!detected)break;
|
||||
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");
|
||||
auto index=static_cast<int>(detected->frame.metadata.frame_index);
|
||||
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++);
|
||||
auto& boxes=detected->boxes;
|
||||
auto frame=imageView(*detected->frame.image);
|
||||
boxesTracker.update(boxes,index,frame.cols,frame.rows);
|
||||
@@ -150,9 +174,17 @@ struct CameraPipeline::Impl {
|
||||
}
|
||||
auto poseElapsed=elapsedMs(poseStart);auto fusionStart=monotonicNs();
|
||||
auto fused=fusion.update(raw,isolated,index);
|
||||
auto record=makeRecord(index,raw,isolated,fused,false,false);
|
||||
auto record=makeRecord(index,raw,isolated,fused,poseOutput.is_open(),true);
|
||||
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));
|
||||
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;
|
||||
@@ -160,7 +192,13 @@ 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;}
|
||||
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();
|
||||
active=false;
|
||||
}
|
||||
};
|
||||
@@ -170,7 +208,7 @@ void CameraPipeline::start(){impl_->start();}
|
||||
FrameResultPtr CameraPipeline::latest()const{return impl_->results.load();}
|
||||
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_->rethrowFailure();}
|
||||
void CameraPipeline::join(){impl_->joinWorkers();{std::lock_guard<std::mutex> lock(impl_->stateMutex);impl_->camera.reset();impl_->can.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};
|
||||
|
||||
@@ -0,0 +1,341 @@
|
||||
// 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
|
||||
@@ -0,0 +1,39 @@
|
||||
#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";
|
||||
}
|
||||
}
|
||||
@@ -19,6 +19,11 @@ 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("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);
|
||||
}
|
||||
return config;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,94 @@
|
||||
#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;
|
||||
}
|
||||
}
|
||||
+62
-31
@@ -2,42 +2,73 @@
|
||||
#include "config.hpp"
|
||||
#include "stop_signals.hpp"
|
||||
#include "visualizer.hpp"
|
||||
#include <functional>
|
||||
#include <chrono>
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <cstdlib>
|
||||
#include <iostream>
|
||||
#include <map>
|
||||
#include <thread>
|
||||
|
||||
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";
|
||||
bool visualize=false;
|
||||
std::map<std::string,std::function<void(int&)>> arguments{
|
||||
{"--visualize",[&](int&){visualize=true;}},
|
||||
{"--config",[&](int&i){if(++i>=argc)throw std::invalid_argument("--config needs a file");configPath=argv[i];}},
|
||||
{"--help",[](int&){std::cout<<"camera_risk [--config FILE] [--visualize]\nContinuous camera inference; latest FrameResult stays in memory. Stop with Ctrl+C or SIGTERM.\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);}
|
||||
|
||||
cms::CameraPipeline pipeline(cms::loadConfig(configPath));
|
||||
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) {
|
||||
try {cms::runLocalVisualization(pipeline);}
|
||||
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])=="--config") {
|
||||
if(++i>=argc)throw std::invalid_argument("--config needs a path");configPath=argv[i];
|
||||
}
|
||||
pipeline.join();
|
||||
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;
|
||||
return 0;
|
||||
}catch(std::exception const& e){std::cerr<<"Camera pipeline failed: "<<e.what()<<std::endl;return 1;}
|
||||
|
||||
@@ -0,0 +1,69 @@
|
||||
#include "output_writer.hpp"
|
||||
#include "serialization.hpp"
|
||||
#include "visualizer.hpp"
|
||||
#include <opencv2/imgcodecs.hpp>
|
||||
#include <chrono>
|
||||
#include <ctime>
|
||||
#include <fstream>
|
||||
#include <iomanip>
|
||||
#include <iostream>
|
||||
#include <sstream>
|
||||
#include <unistd.h>
|
||||
|
||||
namespace cms {
|
||||
namespace {
|
||||
void writeFile(std::filesystem::path const& path,char const* data,std::size_t size) {
|
||||
auto temporary=path;temporary+=".tmp";
|
||||
try {
|
||||
std::ofstream file;file.exceptions(std::ios::failbit|std::ios::badbit);
|
||||
file.open(temporary,std::ios::binary|std::ios::trunc);
|
||||
file.write(data,static_cast<std::streamsize>(size));file.close();
|
||||
std::filesystem::rename(temporary,path);
|
||||
} catch(...) {
|
||||
std::error_code ignored;std::filesystem::remove(temporary,ignored);
|
||||
throw std::runtime_error("Cannot save output file: "+path.string());
|
||||
}
|
||||
}
|
||||
void writeJpeg(std::filesystem::path const& path,cv::Mat const& image) {
|
||||
std::vector<unsigned char> encoded;
|
||||
if(!cv::imencode(".jpg",image,encoded,{cv::IMWRITE_JPEG_QUALITY,95}))
|
||||
throw std::runtime_error("JPEG encoding failed: "+path.string());
|
||||
writeFile(path,reinterpret_cast<char const*>(encoded.data()),encoded.size());
|
||||
}
|
||||
}
|
||||
OutputWriter::OutputWriter(OutputOptions options):options_(std::move(options)) {
|
||||
if(!options_.enabled())return;
|
||||
if(options_.directory.empty())options_.directory=std::filesystem::current_path()/"output";
|
||||
for(auto name:{"original","annotated","results"})std::filesystem::create_directories(options_.directory/name);
|
||||
// Nanosecond wall-clock + PID prevents a later run from overwriting earlier frames.
|
||||
auto now=std::chrono::system_clock::now();auto time=std::chrono::system_clock::to_time_t(now);
|
||||
std::tm utc{};gmtime_r(&time,&utc);
|
||||
auto ns=std::chrono::duration_cast<std::chrono::nanoseconds>(now.time_since_epoch()).count()%1000000000;
|
||||
std::ostringstream id;id<<std::put_time(&utc,"%Y%m%dT%H%M%S")<<'_'<<std::setw(9)<<std::setfill('0')<<ns<<"Z_"<<getpid();
|
||||
runId_=id.str();
|
||||
std::cout<<"Saving selected outputs to "<<options_.directory<<std::endl;
|
||||
}
|
||||
void OutputWriter::write(FrameResult const& result) {
|
||||
if(!options_.enabled())return;
|
||||
std::ostringstream name;name<<runId_<<"_frame_"<<std::setw(10)<<std::setfill('0')<<result.frame_index;
|
||||
auto stem=name.str();Json original=nullptr,annotated=nullptr;
|
||||
if(options_.original) {
|
||||
if(!result.image)throw std::runtime_error("Cannot save original image: frame is missing");
|
||||
auto const& image=*result.image;
|
||||
if(image.width<=0||image.height<=0||image.stride<image.width*3||image.pixels.size()<std::size_t(image.stride)*image.height)
|
||||
throw std::runtime_error("Cannot save malformed original image");
|
||||
cv::Mat view(image.height,image.width,CV_8UC3,const_cast<std::uint8_t*>(image.pixels.data()),image.stride);
|
||||
auto path=std::filesystem::path("original")/(stem+".jpg");writeJpeg(options_.directory/path,view);original=path.generic_string();
|
||||
}
|
||||
if(options_.annotated) {
|
||||
auto path=std::filesystem::path("annotated")/(stem+".jpg");
|
||||
writeJpeg(options_.directory/path,renderResult(result));annotated=path.generic_string();
|
||||
}
|
||||
if(options_.results) {
|
||||
auto document=serializeResult(result);document["schema_version"]=1;document["run_id"]=runId_;
|
||||
document["images"]={{"original",original},{"annotated",annotated}};
|
||||
auto contents=document.dump(2)+"\n";
|
||||
writeFile(options_.directory/"results"/(stem+".json"),contents.data(),contents.size());
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,82 @@
|
||||
#include "visualizer.hpp"
|
||||
#include <opencv2/imgproc.hpp>
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <iomanip>
|
||||
#include <sstream>
|
||||
#include <stdexcept>
|
||||
|
||||
namespace cms {
|
||||
namespace {
|
||||
cv::Point point(PosePoint const& p){return {int(std::nearbyint(p.x)),int(std::nearbyint(p.y))};}
|
||||
cv::Scalar color(std::string const& level) {
|
||||
if(level=="danger")return {40,40,255};
|
||||
if(level=="warning")return {0,160,255};
|
||||
if(level=="watch")return {0,240,240};
|
||||
if(level=="clear")return {80,220,90};
|
||||
return {190,190,190};
|
||||
}
|
||||
void segment(cv::Mat& image,cv::Point a,cv::Point b,cv::Scalar c,int thickness,bool dashed) {
|
||||
if(!dashed){cv::line(image,a,b,c,thickness,cv::LINE_AA);return;}
|
||||
cv::Point2d d=b-a;double length=cv::norm(d);if(length<1)return;d*=1.0/length;
|
||||
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) {
|
||||
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)
|
||||
throw std::invalid_argument("Malformed result image");
|
||||
cv::Mat view(image.height,image.width,CV_8UC3,const_cast<std::uint8_t*>(image.pixels.data()),image.stride);
|
||||
auto canvas=view.clone();
|
||||
if(r.door_mask_pixels) {
|
||||
auto const& m=*r.door_mask_pixels;
|
||||
if(m.width!=image.width||m.height!=image.height||m.stride<m.width||m.pixels.size()<std::size_t(m.stride)*m.height)
|
||||
throw std::invalid_argument("Result mask does not match image");
|
||||
cv::Mat mask(m.height,m.width,CV_8U,const_cast<std::uint8_t*>(m.pixels.data()),m.stride);
|
||||
auto tint=canvas.clone();tint.setTo(cv::Scalar(255,180,0),mask);
|
||||
cv::addWeighted(canvas,0.75,tint,0.25,0,canvas);
|
||||
std::vector<std::vector<cv::Point>> contours;cv::findContours(mask.clone(),contours,cv::RETR_EXTERNAL,cv::CHAIN_APPROX_SIMPLE);
|
||||
cv::drawContours(canvas,contours,-1,{255,180,0},2);
|
||||
}
|
||||
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);
|
||||
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);
|
||||
for(auto e:std::vector<std::pair<int,int>>{{5+s,7+s},{7+s,9+s}}) {
|
||||
segment(canvas,point(k[e.first]),point(k[e.second]),state.limb_consensus?arm:cv::Scalar(0,140,255),3,!state.limb_consensus);
|
||||
cv::circle(canvas,point(k[e.second]),4,arm,-1,cv::LINE_AA);
|
||||
}
|
||||
if(state.state=="rejected")continue;
|
||||
int start=s==0?91:112;
|
||||
auto hand=state.distance_eligible?(s==0?cv::Scalar(255,80,220):cv::Scalar(255,190,40)):cv::Scalar(0,140,255);
|
||||
for(int f=0;f<5;++f)for(int j=0;j<4;++j){int a=j==0?start:start+f*4+j;int b=start+1+f*4+j;segment(canvas,point(k[a]),point(k[b]),hand,2,!state.distance_eligible);}
|
||||
for(int j=0;j<21;++j)cv::circle(canvas,point(k[start+j]),3,hand,-1,cv::LINE_AA);
|
||||
}
|
||||
}
|
||||
for(auto const& h:r.hand_regions) {
|
||||
std::vector<cv::Point> points,hull;for(auto const& p:h.keypoints)points.push_back(point(p));cv::convexHull(points,hull);
|
||||
auto risk=std::find_if(r.risk.regions.begin(),r.risk.regions.end(),[&](auto const& item){return item.track_id==h.track_id&&item.side==h.side;});
|
||||
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") {
|
||||
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);
|
||||
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);
|
||||
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);
|
||||
return canvas;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,50 @@
|
||||
#include "serialization.hpp"
|
||||
namespace cms {
|
||||
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;
|
||||
}
|
||||
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);
|
||||
}
|
||||
out["regions"].push_back(std::move(j));
|
||||
}
|
||||
return out;
|
||||
}
|
||||
}
|
||||
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)}};
|
||||
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();
|
||||
skeleton["smoothed_keypoints"]=Json::array();skeleton["temporal_keypoints"]=Json::array();
|
||||
for(std::size_t k=0;k<p.selected.size();++k) {
|
||||
skeleton["keypoint_scores"].push_back(p.selected[k].score);
|
||||
auto const& a=p.smoothed[k];auto const& b=p.temporal[k];
|
||||
skeleton["smoothed_keypoints"].push_back({a.x,a.y,a.score});
|
||||
skeleton["temporal_keypoints"].push_back({b.x,b.y,b.score});
|
||||
}
|
||||
}
|
||||
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;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,394 @@
|
||||
#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;
|
||||
}
|
||||
+1
-77
@@ -3,84 +3,8 @@
|
||||
#include <opencv2/highgui.hpp>
|
||||
#include <opencv2/highgui/highgui_c.h>
|
||||
#include <opencv2/imgproc.hpp>
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <iomanip>
|
||||
#include <sstream>
|
||||
#include <stdexcept>
|
||||
|
||||
namespace cms {
|
||||
namespace {
|
||||
constexpr char windowName[]="CMS Camera Risk";
|
||||
cv::Point point(PosePoint const& p){return {int(std::nearbyint(p.x)),int(std::nearbyint(p.y))};}
|
||||
cv::Scalar color(std::string const& level) {
|
||||
if(level=="danger")return {40,40,255};
|
||||
if(level=="warning")return {0,160,255};
|
||||
if(level=="watch")return {0,240,240};
|
||||
if(level=="clear")return {80,220,90};
|
||||
return {190,190,190};
|
||||
}
|
||||
void segment(cv::Mat& image,cv::Point a,cv::Point b,cv::Scalar c,int thickness,bool dashed) {
|
||||
if(!dashed){cv::line(image,a,b,c,thickness,cv::LINE_AA);return;}
|
||||
cv::Point2d d=b-a;double length=cv::norm(d);if(length<1)return;d*=1.0/length;
|
||||
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) {
|
||||
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)
|
||||
throw std::invalid_argument("Malformed result image");
|
||||
cv::Mat view(image.height,image.width,CV_8UC3,const_cast<std::uint8_t*>(image.pixels.data()),image.stride);
|
||||
auto canvas=view.clone();
|
||||
if(r.door_mask_pixels) {
|
||||
auto const& m=*r.door_mask_pixels;
|
||||
if(m.width!=image.width||m.height!=image.height||m.stride<m.width||m.pixels.size()<std::size_t(m.stride)*m.height)
|
||||
throw std::invalid_argument("Result mask does not match image");
|
||||
cv::Mat mask(m.height,m.width,CV_8U,const_cast<std::uint8_t*>(m.pixels.data()),m.stride);
|
||||
auto tint=canvas.clone();tint.setTo(cv::Scalar(255,180,0),mask);
|
||||
cv::addWeighted(canvas,0.75,tint,0.25,0,canvas);
|
||||
std::vector<std::vector<cv::Point>> contours;cv::findContours(mask.clone(),contours,cv::RETR_EXTERNAL,cv::CHAIN_APPROX_SIMPLE);
|
||||
cv::drawContours(canvas,contours,-1,{255,180,0},2);
|
||||
}
|
||||
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(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);
|
||||
for(auto e:std::vector<std::pair<int,int>>{{5+s,7+s},{7+s,9+s}}) {
|
||||
segment(canvas,point(k[e.first]),point(k[e.second]),state.limb_consensus?arm:cv::Scalar(0,140,255),3,!state.limb_consensus);
|
||||
cv::circle(canvas,point(k[e.second]),4,arm,-1,cv::LINE_AA);
|
||||
}
|
||||
if(state.state=="rejected")continue;
|
||||
int start=s==0?91:112;
|
||||
auto hand=state.distance_eligible?(s==0?cv::Scalar(255,80,220):cv::Scalar(255,190,40)):cv::Scalar(0,140,255);
|
||||
for(int f=0;f<5;++f)for(int j=0;j<4;++j){int a=j==0?start:start+f*4+j;int b=start+1+f*4+j;segment(canvas,point(k[a]),point(k[b]),hand,2,!state.distance_eligible);}
|
||||
for(int j=0;j<21;++j)cv::circle(canvas,point(k[start+j]),3,hand,-1,cv::LINE_AA);
|
||||
}
|
||||
}
|
||||
for(auto const& h:r.hand_regions) {
|
||||
std::vector<cv::Point> points,hull;for(auto const& p:h.keypoints)points.push_back(point(p));cv::convexHull(points,hull);
|
||||
auto risk=std::find_if(r.risk.regions.begin(),r.risk.regions.end(),[&](auto const& item){return item.track_id==h.track_id&&item.side==h.side;});
|
||||
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") {
|
||||
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<<"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,40},cv::FONT_HERSHEY_SIMPLEX,1.0,color(r.risk.risk_level),2,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);
|
||||
std::string calibration=r.door_mask_calibration_verified?"door calibration verified":"door calibration NOT verified (bring-up)";
|
||||
cv::putText(canvas,calibration+" | q/Esc: close preview",{24,123},cv::FONT_HERSHEY_SIMPLEX,.70,{0,190,255},2,cv::LINE_AA);
|
||||
return canvas;
|
||||
}
|
||||
namespace { constexpr char windowName[]="CMS Camera Risk"; }
|
||||
void runLocalVisualization(CameraPipeline const& pipeline) {
|
||||
struct Window {
|
||||
Window(){cv::namedWindow(windowName,cv::WINDOW_NORMAL);cv::resizeWindow(windowName,1000,800);}
|
||||
|
||||
@@ -0,0 +1,31 @@
|
||||
# Orin 相机故障恢复与停止测试（2026-09-23）
|
||||
|
||||
目标：短暂取流故障在六个等待窗口内恢复时，同一进程继续推理；有效帧到达后重新计数；主动停止不等待全部窗口。
|
||||
|
||||
环境：192.168.195.100，Jetson Orin，L4T R35.4.1，CUDA 11.4.298，TensorRT 8.5.2；真实 /dev/video5，YOLO 与 RTMW TensorRT 引擎。三个分支在该板编译。
|
||||
|
||||
使用真实采集与推理进程；通过 LD_PRELOAD 在 V4L2 系统调用处注入无帧、坏帧、EIO 和 ENODEV。每次恢复测试检查同一子进程产生新的风险 JSON；错误恢复后至少新增 10 帧。Ctrl+C 通过 PTY 写入 0x03，SIGTERM 发送给测试进程。
|
||||
|
||||
| 分支 | Ctrl+C / SIGTERM 完整退出范围 | 持续无帧退出（含清理） | 恢复后再次断流退出（含清理） | 结果 |
|
||||
|---|---:|---:|---:|---|
|
||||
| lazy-inference | 801.21–942.42 ms | 13.010 s | 13.085 s | 9/9 通过 |
|
||||
| inference-only | 793.22–1012.57 ms | 13.055 s | 12.945 s | 9/9 通过 |
|
||||
| use-can | 786.06–907.59 ms | 13.066 s | 13.155 s | 9/9 通过 |
|
||||
|
||||
每个分支覆盖：断流后恢复并重新计时；坏帧恢复；EIO 恢复；ENODEV 后重新打开设备并恢复；正常推理 Ctrl+C；重试期 Ctrl+C；第六窗口 Ctrl+C；SIGTERM；持续无帧六窗口后报错。timeout/recover 场景最后的退出码 1 是预期的超限结果，不是测试失败。
|
||||
|
||||
默认逻辑等待约 12 秒，表中的完整进程退出另含 GPU/驱动资源清理时间。最初使用的 1 秒完整退出判据过于严格：清理 GPU 资源可使总耗时略超过 1 秒；最终以不超过 2 秒验收主动停止，同时记录实测值。lazy-inference/use-can 的流水线停止日志也已记录，详见 JSON。
|
||||
|
||||
实机发现并修复：use-can 关闭时遗漏 munmap，导致重新打开后 VIDIOC_S_FMT 返回 EBUSY。修复后重跑该分支全部九项。另有原生 V4L2 重开回归测试，检查连续三次重开及部分 mmap 失败后无遗留映射。
|
||||
|
||||
补充回归：在板端执行 tests/run_camera_timeout.sh 和 --wall-clock（默认六窗口约 12.00 秒）；use-can 额外执行 tests/run_v4l2_reopen.sh。它们使用模拟采集，仅为补充，不替代上述板端真实推理测试。
|
||||
|
||||
测试范围限制：未进行物理拔线、USB-CAN 报文/拔插、驱动内核永久卡死、GPU 故障或长时间耐久测试。板上未接 USB-CAN，CAN 模式处于缺失回退。视频回放路径未改动，未包含在本次相机恢复测试中。
|
||||
|
||||
完整数据：orin_hardware_results.json。故障注入器源码：orin_capture_faults.c，仅用于测试。原始运行日志位于板端 /home/orin/vision-anti-pinch-ctrlc-test-20260923/。
|
||||
|
||||
## 经测试的可执行文件 SHA256
|
||||
|
||||
- lazy-inference: `0d77d17626644fb50b663c33edbb1a09884b34b5410a1109fc56b4d3781651b1`
|
||||
- inference-only: `3a984e1e4c0fa12285967085e3359a5b7c698d744e9c2deb2480a312eb637c1c`
|
||||
- use-can: `a2616e193ef9f006b49b4bb4ac43ae3ffc4cee1f1edf6d58ed6f7f9e5f933ad1`
|
||||
@@ -0,0 +1,192 @@
|
||||
// Exercise the production Camera with simulated capture and no GPU or camera.
|
||||
#include "camera.hpp"
|
||||
#include "uyvy_converter.hpp"
|
||||
#include <linux/videodev2.h>
|
||||
#include <chrono>
|
||||
#include <cstring>
|
||||
#include <functional>
|
||||
#include <iostream>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#ifdef TEST_V4L2_CAPTURE
|
||||
#include "v4l2_capture.h"
|
||||
#else
|
||||
#include <cstdarg>
|
||||
#include <cerrno>
|
||||
#include <poll.h>
|
||||
#include <sys/mman.h>
|
||||
#include <sys/ioctl.h>
|
||||
#endif
|
||||
|
||||
namespace {
|
||||
std::string events;
|
||||
std::size_t cursor{};
|
||||
char current{};
|
||||
unsigned sequence{};
|
||||
int timeoutMs{};
|
||||
bool realWait{};
|
||||
std::function<void()> onStop;
|
||||
unsigned char pixels[8]{};
|
||||
void check(bool condition,char const* message) {
|
||||
if(!condition)throw std::runtime_error(message);
|
||||
}
|
||||
void script(std::string value) {events=std::move(value);cursor=0;}
|
||||
char next() {
|
||||
check(cursor<events.size(),"Camera requested an unexpected extra capture attempt");
|
||||
return current=events[cursor++];
|
||||
}
|
||||
void expectTimeout(cms::Camera& camera) {
|
||||
try {camera.read();} catch(std::runtime_error const& e) {
|
||||
check(std::string(e.what()).find("6 consecutive times")!=std::string::npos,"Wrong timeout failure");
|
||||
return;
|
||||
}
|
||||
throw std::runtime_error("Six consecutive timeouts did not fail");
|
||||
}
|
||||
}
|
||||
namespace cms {
|
||||
struct UyvyConverter::Impl {int width,height;};
|
||||
UyvyConverter::UyvyConverter(int w,int h):impl_(std::make_unique<Impl>(Impl{w,h})){}
|
||||
UyvyConverter::~UyvyConverter()=default;
|
||||
std::shared_ptr<const BgrImage> UyvyConverter::convert(void const*,std::size_t) {
|
||||
auto image=std::make_shared<BgrImage>();image->width=impl_->width;image->height=impl_->height;
|
||||
image->stride=image->width*3;image->pixels.resize(image->stride*image->height);return image;
|
||||
}
|
||||
}
|
||||
#ifdef TEST_V4L2_CAPTURE
|
||||
V4l2Capture::~V4l2Capture()=default;
|
||||
bool V4l2Capture::open(std::string const&){reconnect_needed_=false;return true;}
|
||||
void V4l2Capture::close(){}
|
||||
bool V4l2Capture::setFormat(uint32_t w,uint32_t h,uint32_t p){width_=w;height_=h;pixelformat_=p;return true;}
|
||||
bool V4l2Capture::setFps(uint32_t){return true;}
|
||||
bool V4l2Capture::initMmap(uint32_t){return true;}
|
||||
bool V4l2Capture::startStreaming(){return true;}
|
||||
void V4l2Capture::releaseFrame(Frame const&){}
|
||||
bool V4l2Capture::getFrame(Frame& f,int waitMs) {
|
||||
check(waitMs<=100,"Stop polling should remain at most 100 ms");
|
||||
if(next()=='s'){onStop();return false;}
|
||||
if(current=='d'){reconnect_needed_=true;return false;}
|
||||
if(current=='e'||current=='x'){return false;}
|
||||
if(current=='t') {
|
||||
// Each scripted timeout represents a full logical capture window.
|
||||
// The separate stop test exercises the short-wait cancellation path.
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(timeoutMs));return false;
|
||||
}
|
||||
check(current=='f',"Unsupported mock V4L2 event");
|
||||
f.data=pixels;f.size=sizeof(pixels);f.width=2;f.height=2;f.bytes_per_line=4;f.sequence=sequence++;
|
||||
return true;
|
||||
}
|
||||
#else
|
||||
extern "C" {
|
||||
int __real_poll(pollfd*,nfds_t,int);
|
||||
int __real_open(char const*,int,...);
|
||||
int __real_close(int);
|
||||
int __real_ioctl(int,unsigned long,...);
|
||||
void* __real_mmap(void*,size_t,int,int,int,off_t);
|
||||
int __real_munmap(void*,size_t);
|
||||
int __wrap_open(char const* path,int flags,...) {
|
||||
if(std::strcmp(path,"/mock/camera")==0)return 100000;
|
||||
return __real_open(path,flags);
|
||||
}
|
||||
int __wrap_close(int fd){return fd==100000?0:__real_close(fd);}
|
||||
void* __wrap_mmap(void* a,size_t n,int p,int f,int fd,off_t o) {
|
||||
return fd==100000?pixels:__real_mmap(a,n,p,f,fd,o);
|
||||
}
|
||||
int __wrap_munmap(void* a,size_t n){return a==pixels?0:__real_munmap(a,n);}
|
||||
int __wrap_ioctl(int fd,unsigned long request,...) {
|
||||
va_list args;va_start(args,request);void* p=va_arg(args,void*);va_end(args);
|
||||
if(fd!=100000)return __real_ioctl(fd,request,p);
|
||||
switch(request) {
|
||||
case VIDIOC_QUERYCAP:static_cast<v4l2_capability*>(p)->capabilities=V4L2_CAP_VIDEO_CAPTURE|V4L2_CAP_STREAMING;break;
|
||||
case VIDIOC_S_FMT:static_cast<v4l2_format*>(p)->fmt.pix.bytesperline=4;break;
|
||||
case VIDIOC_G_PARM:break;
|
||||
case VIDIOC_REQBUFS:static_cast<v4l2_requestbuffers*>(p)->count=2;break;
|
||||
case VIDIOC_QUERYBUF:static_cast<v4l2_buffer*>(p)->length=sizeof(pixels);break;
|
||||
case VIDIOC_DQBUF: {
|
||||
if(current=='x'){errno=EIO;return -1;}
|
||||
auto& b=*static_cast<v4l2_buffer*>(p);b.index=0;b.bytesused=sizeof(pixels);b.sequence=sequence++;
|
||||
b.flags=current=='e'?V4L2_BUF_FLAG_ERROR:0;break;
|
||||
}
|
||||
case VIDIOC_QBUF:case VIDIOC_STREAMON:case VIDIOC_STREAMOFF:break;
|
||||
default:throw std::runtime_error("Unexpected camera ioctl");
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
int __wrap_poll(pollfd* fds,nfds_t count,int waitMs) {
|
||||
if(count==1)return __real_poll(fds,count,waitMs);
|
||||
check(count==2&&waitMs>0&&waitMs<=timeoutMs,"Camera must respect the configured timeout window");
|
||||
switch(next()) {
|
||||
case 't':if(realWait)std::this_thread::sleep_for(std::chrono::milliseconds(waitMs));return 0;
|
||||
case 'i':errno=EINTR;return -1;
|
||||
case 's':onStop();return 0;
|
||||
case 'd':fds[0].revents=POLLHUP;return 1;
|
||||
case 'f':case 'e':case 'x':fds[0].revents=POLLIN;return 1;
|
||||
default:throw std::runtime_error("Unexpected poll event");
|
||||
}
|
||||
}
|
||||
int __wrap___poll_chk(pollfd* fds,nfds_t count,int waitMs,size_t size) {
|
||||
check(size>=count*sizeof(pollfd),"Invalid fortified poll buffer size");
|
||||
return __wrap_poll(fds,count,waitMs);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
int main(int argc,char** argv)try {
|
||||
cms::CameraConfig config;config.device="/mock/camera";config.width=config.height=2;config.buffers=2;
|
||||
check(config.timeout_ms==2000,"Default per-attempt timeout must be 2 seconds");
|
||||
realWait=argc>1&&std::string(argv[1])=="--wall-clock";
|
||||
if(!realWait)config.timeout_ms=100;
|
||||
timeoutMs=config.timeout_ms;
|
||||
if(realWait) {
|
||||
cms::Camera camera(config);script("tttttt");
|
||||
auto before=std::chrono::steady_clock::now();expectTimeout(camera);
|
||||
auto elapsed=std::chrono::duration<double>(std::chrono::steady_clock::now()-before).count();
|
||||
check(cursor==6&&elapsed>=12.0&&elapsed<15.0,"Default timeout should fail after six attempts, about 12 seconds");
|
||||
std::cout<<"PASS wall-clock six-timeout duration="<<elapsed<<" s\n";
|
||||
return 0;
|
||||
}
|
||||
{
|
||||
cms::Camera camera(config);
|
||||
script("tttttf");check(bool(camera.read()),"Five timeouts should recover on a valid frame");
|
||||
script("tttttf");check(bool(camera.read()),"A valid frame must reset the timeout count");
|
||||
script("tttttt");
|
||||
expectTimeout(camera);
|
||||
check(cursor==6,"Failure must occur on the sixth timeout");
|
||||
}
|
||||
{
|
||||
cms::Camera camera(config);onStop=[&]{camera.requestStop();};
|
||||
script("tts");check(!camera.read(),"Stop must cancel retries without throwing");
|
||||
check(cursor==3,"Stop should not wait for all six timeouts");onStop={};
|
||||
}
|
||||
{
|
||||
cms::Camera camera(config);onStop=[&]{
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(timeoutMs));
|
||||
camera.requestStop();
|
||||
};
|
||||
script("ttttts");check(!camera.read(),"Stop on the final timeout must take precedence over failure");
|
||||
onStop={};
|
||||
}
|
||||
#ifndef TEST_V4L2_CAPTURE
|
||||
{
|
||||
cms::Camera camera(config);script("tttttif");
|
||||
check(bool(camera.read()),"EINTR must not count as a timeout");
|
||||
script("tttttet");expectTimeout(camera);
|
||||
check(cursor==7,"A corrupt frame must not reset the timeout count");
|
||||
}
|
||||
{
|
||||
cms::Camera camera(config);script("df");
|
||||
check(bool(camera.read()),"Capture must reopen after a transient disconnection");
|
||||
}
|
||||
{
|
||||
cms::Camera camera(config);script("xxf");
|
||||
check(bool(camera.read()),"Transient EIO must recover without restarting the pipeline");
|
||||
}
|
||||
{
|
||||
auto slow=config;slow.timeout_ms=2000;timeoutMs=slow.timeout_ms;
|
||||
cms::Camera camera(slow);script(std::string(35,'e')+"f");
|
||||
check(bool(camera.read()),"More than one second's frame count of corrupt frames must remain recoverable");
|
||||
timeoutMs=config.timeout_ms;
|
||||
}
|
||||
#endif
|
||||
std::cout<<"PASS camera timeout retries, recovery reset, sixth-timeout failure and cancellation\n";
|
||||
return 0;
|
||||
}catch(std::exception const& e){std::cerr<<"FAIL: "<<e.what()<<'\n';return 1;}
|
||||
@@ -0,0 +1,39 @@
|
||||
#define _GNU_SOURCE
|
||||
#include <dlfcn.h>
|
||||
#include <poll.h>
|
||||
#include <sys/select.h>
|
||||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
#include <time.h>
|
||||
#include <stdarg.h>
|
||||
#include <errno.h>
|
||||
#include <linux/videodev2.h>
|
||||
#include <sys/ioctl.h>
|
||||
static int calls=0;
|
||||
static int camera_fd(int fd){char name[64],target[256];snprintf(name,sizeof(name),"/proc/self/fd/%d",fd);ssize_t n=readlink(name,target,sizeof(target)-1);if(n<0)return 0;target[n]=0;return strcmp(target,"/dev/video5")==0;}
|
||||
static long long now_ms(void){struct timespec t;clock_gettime(CLOCK_MONOTONIC,&t);return (long long)t.tv_sec*1000+t.tv_nsec/1000000;}
|
||||
static void marker(const char *s){ssize_t ignored=write(2,s,strlen(s));(void)ignored;}
|
||||
static int should_stall(void){
|
||||
static long long since=0;static int phase=0,resumed=0;
|
||||
if(!getenv("VAP_TEST_STALL"))return 0;
|
||||
if(phase==0){if(++calls<60)return 0;phase=1;since=now_ms();marker("TEST_STALL_ACTIVE\n");}
|
||||
if(phase==1&&getenv("VAP_TEST_RECOVER")&&now_ms()-since>=4200){phase=2;marker("TEST_CAMERA_RECOVERED\n");return 0;}
|
||||
if(phase==2){if(++resumed<30)return 0;phase=3;marker("TEST_STALL_SECOND\n");}
|
||||
return phase==1||phase==3;
|
||||
}
|
||||
int poll(struct pollfd *fds,nfds_t count,int timeout){static int(*real_poll)(struct pollfd*,nfds_t,int);if(!real_poll)real_poll=dlsym(RTLD_NEXT,"poll");for(nfds_t i=0;i<count;++i)if(!getenv("VAP_TEST_KIND")&&camera_fd(fds[i].fd)&&should_stall()){int saved=fds[i].fd;fds[i].fd=-1;int rc=real_poll(fds,count,timeout);fds[i].fd=saved;return rc;}return real_poll(fds,count,timeout);}
|
||||
int __poll_chk(struct pollfd *fds,nfds_t count,int timeout,size_t size){(void)size;return poll(fds,count,timeout);}
|
||||
int select(int count,fd_set *readfds,fd_set *writefds,fd_set *exceptfds,struct timeval *timeout){static int(*real_select)(int,fd_set*,fd_set*,fd_set*,struct timeval*);if(!real_select)real_select=dlsym(RTLD_NEXT,"select");if(readfds)for(int fd=0;fd<count;++fd)if(!getenv("VAP_TEST_KIND")&&FD_ISSET(fd,readfds)&&camera_fd(fd)&&should_stall()){FD_CLR(fd,readfds);break;}return real_select(count,readfds,writefds,exceptfds,timeout);}
|
||||
|
||||
int ioctl(int fd,unsigned long request,...){
|
||||
static int(*real_ioctl)(int,unsigned long,...);if(!real_ioctl)real_ioctl=dlsym(RTLD_NEXT,"ioctl");
|
||||
va_list args;va_start(args,request);void *arg=va_arg(args,void*);va_end(args);
|
||||
const char *kind=getenv("VAP_TEST_KIND");
|
||||
if(kind&&request==VIDIOC_DQBUF&&camera_fd(fd)&&should_stall()){
|
||||
if(strcmp(kind,"bad_frames")==0){int rc=real_ioctl(fd,request,arg);if(rc==0)((struct v4l2_buffer*)arg)->flags|=V4L2_BUF_FLAG_ERROR;return rc;}
|
||||
errno=strcmp(kind,"device_error")==0?ENODEV:EIO;return -1;
|
||||
}
|
||||
return real_ioctl(fd,request,arg);
|
||||
}
|
||||
@@ -0,0 +1,253 @@
|
||||
[
|
||||
{
|
||||
"branch": "lazy-inference",
|
||||
"mode": "recover",
|
||||
"exit_code": 1,
|
||||
"inference_json_frames": 88,
|
||||
"gpu_models": "YOLO + RTMW TensorRT engines",
|
||||
"six_timeout_exit_seconds": 13.085,
|
||||
"recovered_real_capture": true
|
||||
},
|
||||
{
|
||||
"branch": "lazy-inference",
|
||||
"mode": "bad_frames_recover",
|
||||
"exit_code": 0,
|
||||
"inference_json_frames": 67,
|
||||
"gpu_models": "YOLO + RTMW TensorRT engines",
|
||||
"signal_exit_ms": 846.3,
|
||||
"recovered_real_capture": true,
|
||||
"frames_after_recovery": 10,
|
||||
"pipeline_stop_ms": 132.56
|
||||
},
|
||||
{
|
||||
"branch": "lazy-inference",
|
||||
"mode": "io_error_recover",
|
||||
"exit_code": 0,
|
||||
"inference_json_frames": 67,
|
||||
"gpu_models": "YOLO + RTMW TensorRT engines",
|
||||
"signal_exit_ms": 836.09,
|
||||
"recovered_real_capture": true,
|
||||
"frames_after_recovery": 10,
|
||||
"pipeline_stop_ms": 106.06
|
||||
},
|
||||
{
|
||||
"branch": "lazy-inference",
|
||||
"mode": "device_error_recover",
|
||||
"exit_code": 0,
|
||||
"inference_json_frames": 68,
|
||||
"gpu_models": "YOLO + RTMW TensorRT engines",
|
||||
"signal_exit_ms": 830.0,
|
||||
"recovered_real_capture": true,
|
||||
"frames_after_recovery": 10,
|
||||
"pipeline_stop_ms": 133.41
|
||||
},
|
||||
{
|
||||
"branch": "lazy-inference",
|
||||
"mode": "live",
|
||||
"exit_code": 0,
|
||||
"inference_json_frames": 3,
|
||||
"gpu_models": "YOLO + RTMW TensorRT engines",
|
||||
"signal_exit_ms": 833.16,
|
||||
"pipeline_stop_ms": 139.5
|
||||
},
|
||||
{
|
||||
"branch": "lazy-inference",
|
||||
"mode": "retry",
|
||||
"exit_code": 0,
|
||||
"inference_json_frames": 58,
|
||||
"gpu_models": "YOLO + RTMW TensorRT engines",
|
||||
"signal_exit_ms": 942.42,
|
||||
"pipeline_stop_ms": 201.21
|
||||
},
|
||||
{
|
||||
"branch": "lazy-inference",
|
||||
"mode": "sixth_wait",
|
||||
"exit_code": 0,
|
||||
"inference_json_frames": 58,
|
||||
"gpu_models": "YOLO + RTMW TensorRT engines",
|
||||
"signal_exit_ms": 904.29,
|
||||
"pipeline_stop_ms": 194.16
|
||||
},
|
||||
{
|
||||
"branch": "lazy-inference",
|
||||
"mode": "sigterm",
|
||||
"exit_code": 0,
|
||||
"inference_json_frames": 3,
|
||||
"gpu_models": "YOLO + RTMW TensorRT engines",
|
||||
"signal_exit_ms": 801.21,
|
||||
"pipeline_stop_ms": 123.09
|
||||
},
|
||||
{
|
||||
"branch": "lazy-inference",
|
||||
"mode": "timeout",
|
||||
"exit_code": 1,
|
||||
"inference_json_frames": 58,
|
||||
"gpu_models": "YOLO + RTMW TensorRT engines",
|
||||
"six_timeout_exit_seconds": 13.01
|
||||
},
|
||||
{
|
||||
"branch": "inference-only",
|
||||
"mode": "recover",
|
||||
"exit_code": 1,
|
||||
"inference_json_frames": 88,
|
||||
"gpu_models": "YOLO + RTMW TensorRT engines",
|
||||
"six_timeout_exit_seconds": 12.945,
|
||||
"recovered_real_capture": true
|
||||
},
|
||||
{
|
||||
"branch": "inference-only",
|
||||
"mode": "bad_frames_recover",
|
||||
"exit_code": 0,
|
||||
"inference_json_frames": 68,
|
||||
"gpu_models": "YOLO + RTMW TensorRT engines",
|
||||
"signal_exit_ms": 836.67,
|
||||
"recovered_real_capture": true,
|
||||
"frames_after_recovery": 10
|
||||
},
|
||||
{
|
||||
"branch": "inference-only",
|
||||
"mode": "io_error_recover",
|
||||
"exit_code": 0,
|
||||
"inference_json_frames": 68,
|
||||
"gpu_models": "YOLO + RTMW TensorRT engines",
|
||||
"signal_exit_ms": 827.43,
|
||||
"recovered_real_capture": true,
|
||||
"frames_after_recovery": 10
|
||||
},
|
||||
{
|
||||
"branch": "inference-only",
|
||||
"mode": "device_error_recover",
|
||||
"exit_code": 0,
|
||||
"inference_json_frames": 68,
|
||||
"gpu_models": "YOLO + RTMW TensorRT engines",
|
||||
"signal_exit_ms": 853.08,
|
||||
"recovered_real_capture": true,
|
||||
"frames_after_recovery": 10
|
||||
},
|
||||
{
|
||||
"branch": "inference-only",
|
||||
"mode": "live",
|
||||
"exit_code": 0,
|
||||
"inference_json_frames": 3,
|
||||
"gpu_models": "YOLO + RTMW TensorRT engines",
|
||||
"signal_exit_ms": 804.87
|
||||
},
|
||||
{
|
||||
"branch": "inference-only",
|
||||
"mode": "retry",
|
||||
"exit_code": 0,
|
||||
"inference_json_frames": 58,
|
||||
"gpu_models": "YOLO + RTMW TensorRT engines",
|
||||
"signal_exit_ms": 913.42
|
||||
},
|
||||
{
|
||||
"branch": "inference-only",
|
||||
"mode": "sixth_wait",
|
||||
"exit_code": 0,
|
||||
"inference_json_frames": 58,
|
||||
"gpu_models": "YOLO + RTMW TensorRT engines",
|
||||
"signal_exit_ms": 1012.57
|
||||
},
|
||||
{
|
||||
"branch": "inference-only",
|
||||
"mode": "sigterm",
|
||||
"exit_code": 0,
|
||||
"inference_json_frames": 3,
|
||||
"gpu_models": "YOLO + RTMW TensorRT engines",
|
||||
"signal_exit_ms": 793.22
|
||||
},
|
||||
{
|
||||
"branch": "inference-only",
|
||||
"mode": "timeout",
|
||||
"exit_code": 1,
|
||||
"inference_json_frames": 58,
|
||||
"gpu_models": "YOLO + RTMW TensorRT engines",
|
||||
"six_timeout_exit_seconds": 13.055
|
||||
},
|
||||
{
|
||||
"branch": "use-can",
|
||||
"mode": "recover",
|
||||
"exit_code": 1,
|
||||
"inference_json_frames": 88,
|
||||
"gpu_models": "YOLO + RTMW TensorRT engines",
|
||||
"six_timeout_exit_seconds": 13.155,
|
||||
"recovered_real_capture": true
|
||||
},
|
||||
{
|
||||
"branch": "use-can",
|
||||
"mode": "bad_frames_recover",
|
||||
"exit_code": 0,
|
||||
"inference_json_frames": 67,
|
||||
"gpu_models": "YOLO + RTMW TensorRT engines",
|
||||
"signal_exit_ms": 815.49,
|
||||
"recovered_real_capture": true,
|
||||
"frames_after_recovery": 10,
|
||||
"pipeline_stop_ms": 122.36
|
||||
},
|
||||
{
|
||||
"branch": "use-can",
|
||||
"mode": "io_error_recover",
|
||||
"exit_code": 0,
|
||||
"inference_json_frames": 68,
|
||||
"gpu_models": "YOLO + RTMW TensorRT engines",
|
||||
"signal_exit_ms": 836.05,
|
||||
"recovered_real_capture": true,
|
||||
"frames_after_recovery": 10,
|
||||
"pipeline_stop_ms": 130.73
|
||||
},
|
||||
{
|
||||
"branch": "use-can",
|
||||
"mode": "device_error_recover",
|
||||
"exit_code": 0,
|
||||
"inference_json_frames": 68,
|
||||
"gpu_models": "YOLO + RTMW TensorRT engines",
|
||||
"signal_exit_ms": 814.68,
|
||||
"recovered_real_capture": true,
|
||||
"frames_after_recovery": 10,
|
||||
"pipeline_stop_ms": 123.99
|
||||
},
|
||||
{
|
||||
"branch": "use-can",
|
||||
"mode": "live",
|
||||
"exit_code": 0,
|
||||
"inference_json_frames": 3,
|
||||
"gpu_models": "YOLO + RTMW TensorRT engines",
|
||||
"signal_exit_ms": 786.06,
|
||||
"pipeline_stop_ms": 108.96
|
||||
},
|
||||
{
|
||||
"branch": "use-can",
|
||||
"mode": "retry",
|
||||
"exit_code": 0,
|
||||
"inference_json_frames": 58,
|
||||
"gpu_models": "YOLO + RTMW TensorRT engines",
|
||||
"signal_exit_ms": 862.45,
|
||||
"pipeline_stop_ms": 167.55
|
||||
},
|
||||
{
|
||||
"branch": "use-can",
|
||||
"mode": "sixth_wait",
|
||||
"exit_code": 0,
|
||||
"inference_json_frames": 58,
|
||||
"gpu_models": "YOLO + RTMW TensorRT engines",
|
||||
"signal_exit_ms": 907.59,
|
||||
"pipeline_stop_ms": 197.07
|
||||
},
|
||||
{
|
||||
"branch": "use-can",
|
||||
"mode": "sigterm",
|
||||
"exit_code": 0,
|
||||
"inference_json_frames": 3,
|
||||
"gpu_models": "YOLO + RTMW TensorRT engines",
|
||||
"signal_exit_ms": 827.61,
|
||||
"pipeline_stop_ms": 130.51
|
||||
},
|
||||
{
|
||||
"branch": "use-can",
|
||||
"mode": "timeout",
|
||||
"exit_code": 1,
|
||||
"inference_json_frames": 58,
|
||||
"gpu_models": "YOLO + RTMW TensorRT engines",
|
||||
"six_timeout_exit_seconds": 13.066
|
||||
}
|
||||
]
|
||||
Executable
+14
@@ -0,0 +1,14 @@
|
||||
#!/usr/bin/env bash
|
||||
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")
|
||||
links=()
|
||||
if [[ -f "$root/include/v4l2_capture.h" ]]; then
|
||||
flags+=(-DTEST_V4L2_CAPTURE -I"$root/tests/stubs")
|
||||
else
|
||||
for fn in open close ioctl mmap munmap poll __poll_chk; do links+=("-Wl,--wrap=$fn"); done
|
||||
fi
|
||||
"${CXX:-g++}" "${flags[@]}" "${CAMERA_SOURCE:-$root/src/camera.cpp}" "$root/tests/camera_timeout_test.cpp" "${links[@]}" -o "$test_dir/camera_timeout_test"
|
||||
"$test_dir/camera_timeout_test" "$@"
|
||||
Executable
+7
@@ -0,0 +1,7 @@
|
||||
#!/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"
|
||||
@@ -0,0 +1,19 @@
|
||||
#pragma once
|
||||
// Camera timeout tests exercise live capture only. Stub the unused replay API
|
||||
// so the use-can Camera can be tested without OpenCV or a video decoder.
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
namespace cv {
|
||||
constexpr int CAP_PROP_FRAME_WIDTH=3,CAP_PROP_FRAME_HEIGHT=4;
|
||||
struct Mat {
|
||||
int cols{},rows{};
|
||||
unsigned char const* ptr(int)const {throw std::logic_error("Video replay is outside this test");}
|
||||
};
|
||||
class VideoCapture {
|
||||
public:
|
||||
bool isOpened()const{return false;}
|
||||
bool open(std::string const&){throw std::logic_error("Video replay is outside this test");}
|
||||
double get(int)const {throw std::logic_error("Video replay is outside this test");}
|
||||
bool read(Mat&){throw std::logic_error("Video replay is outside this test");}
|
||||
};
|
||||
}
|
||||
@@ -0,0 +1,29 @@
|
||||
#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;}
|
||||
Vendored
+1
-1
@@ -1,4 +1,4 @@
|
||||
MIT License
|
||||
MIT License
|
||||
|
||||
Copyright (c) 2013-2022 Niels Lohmann
|
||||
|
||||
|
||||
Reference in New Issue
Block a user