diff --git a/CMakeLists.txt b/CMakeLists.txt index 1a5dede..151691d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.24) if(NOT DEFINED CMAKE_CUDA_ARCHITECTURES) - set(CMAKE_CUDA_ARCHITECTURES native) + set(CMAKE_CUDA_ARCHITECTURES 87) endif() project(camera_risk LANGUAGES CXX CUDA) set(CMAKE_CXX_STANDARD 17) @@ -13,11 +13,14 @@ find_path(TENSORRT_INCLUDE_DIR NvInfer.h PATH_SUFFIXES aarch64-linux-gnu x86_64- 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/door_risk.cpp src/camera.cpp src/camera_pipeline.cpp src/config.cpp src/lazy_gate.cpp src/can_input.cpp src/can_comm.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 $<$:-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") diff --git a/README.md b/README.md index d1a7025..495f486 100644 --- a/README.md +++ b/README.md @@ -1,73 +1,76 @@ -# Vision Anti-Pinch +# Vision Anti-Pinch — lazy-inference -C++ / TensorRT 实时摄像头风险推理，算法来自最新 run_risk.py。 -持续取图、推理并更新内存中的 FrameResult；可视化可选。 +从 `inference-only` 分支派生的纯 C++ 惰性推理工程。保留该分支的 YOLO / RTMW / 时序融合 / mask 风险流程，在执行推理前加入 CAN 门状态控制。模型启动时加载、预热一次，暂停期间保持 TensorRT 引擎、上下文及显存，不反复加载。 -## Orin 直接运行 +## 触发规则 -适用环境：Jetson AGX Orin、JetPack 5.1.2 / L4T R35.4.1、CUDA 11.4、TensorRT 8.5.2、OpenCV 4.5.4。 -仓库已包含该板端编译的程序和重新构建的 TensorRT 引擎。 +接收 CAN / CAN FD `0x391`，`data[0..3]` 按百分比解释，顺序为左前、右前、左后、右后。当前仅使用左前和左后，任一满足即可检测。 -```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 # 持续推理，结果只保存在内存 -``` +| 状态 | 推理行为 | +|---|---| +| 左前或左后开度 **<15%**，且正在增加（开门） | 检测并计算风险 | +| 左前或左后开度 **<15%**，且正在减小（关门） | 检测并计算风险 | +| 恰好 15%、大于 15%，或两门均静止 | 暂停模型推理 | +| 从未收到有效报文、报文超时或左门开度值无效 | 开启检测 | +| CAN 恢复且不满足运动条件 | 停止检测 | -可选板端本地可视化： +开度不变的重复报文不会立即抹掉运动方向。默认最后一次开度变化后 **300 ms** 仍沿用方向，超过该时间判定静止，避免整数百分比报文导致频繁启停。到达 0% 后也保留这段检测时间。首次报文及超时后的首次报文只建立基线，不将断线前后的差值当作运动。 -```bash -./run.sh --visualize -``` +CAN 默认 **1000 ms** 超时。上述参数可在 `config/camera.json` 的 `lazy_inference` 中调整；15% 使用严格小于。USB CAN 缺失时后台持续重试，恢复后自动接收。当前不发送执行器指令。 -Ctrl+C / SIGTERM 停止程序。 -关闭预览窗口或按 q/Esc 只关闭预览，推理继续。 +## 编译和运行 -摄像头为 /dev/video5，1920×1536 UYVY，30 FPS。 -当前 door mask 是指定的 constant_door_mask.json 副本，标定状态为 false；装车后应更换与实际画面匹配的门区并更新标定状态。 - -## 下游获取结果 - -main.cpp 注释中也包含以下示例。下游通过自己的调用位置或线程读取： - -```cpp -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 个关键点 -} -``` - -latest() 返回 shared_ptr；首帧尚未完成时为空。 -公共定义位于 include/camera_pipeline.hpp 和 include/frame_result.hpp。 - -## 从源码构建 +目标设备：Orin / JetPack 5.1.2 / CUDA 11.4 / TensorRT 8.5 / OpenCV 4.5，构建需 CMake 3.24+。 ```bash ./scripts/build.sh -./scripts/initialize_camera.sh +./scripts/initialize_camera.sh # 摄像头尚未初始化时 ./run_camera.sh +./run_camera.sh --visualize # 可选本地 OpenCV 窗口 +./run_camera.sh --duration 30 # 运行 30 秒后正常退出 ``` -源码构建要求 CMake 3.24+、CUDA/NPP、TensorRT 和 OpenCV 开发依赖。 -构建脚本会优先使用 Orin 上已有的 ~/miniconda3/envs/yolo/bin/cmake。 -源码配置直接使用 orin/engines/ 中的板端引擎。该引擎仅适用于上述 Orin 环境；仓库不包含 ONNX 或模型转换脚本。 -独立库目标为 camera_risk_core；可视化在 camera_risk_visualizer。 +Orin 独立包可单独复制运行： + +```bash +cd orin +./scripts/initialize_camera.sh +./run.sh +``` + +`orin/build/camera_risk` 是 ARM64 C++ 可执行程序。运行不依赖 Python。修改源码后，在 Orin 编译并运行 `./scripts/package_orin.sh` 更新独立包。`cd orin && sha256sum -c SHA256SUMS` 检查文件完整性。 + +## 流水线和下游结果 + +摄像头持续采集、CAN 持续接收。启用时运行采集、YOLO、RTMW/融合/风险三阶段流水线；暂停时继续取帧以保持画面新鲜，但不执行 YOLO、RTMW 或风险计算。暂停前已提交的 GPU 工作可以完成，其结果会被丢弃。状态切换会清空待处理帧，恢复时重置跟踪与时序融合，不使用暂停前的历史人物状态。 + +```cpp +cms::CameraPipeline pipeline(cms::loadConfig("config/camera.json")); +pipeline.start(); +// 下游线程中： +auto state = pipeline.inferenceStatus(); +// state.models_ready / enabled / can_valid / reason +// state.positions[0]：左前；state.positions[2]：左后 +// state.direction[0..1]：左前/左后，-1 关门，0 静止，+1 开门 + +auto result = pipeline.latest(); // shared_ptr +if (result) { + const auto& risk = result->risk; + const auto& people = result->persons; +} +// 启动、暂停或等待恢复后的首帧时，result 为空；不能解释为风险 clear。 +// 退出：pipeline.requestStop(); pipeline.join(); +``` + +`latest()` 不返回旧运行周期的风险；调用者已持有的旧快照不会被修改，应重新读取状态和结果。`FrameResult::inference` 保存生成结果时的 CAN 状态和 generation。状态变化后的下一次读取会反映新的周期。`running()` 表示整个服务在运行，暂停推理时仍为 true。 + +默认结果仅保存在内存变量，不保存视频或逐帧文件。标准输出记录启停原因和累计推理次数；`stats()` 提供 detector_runs / pose_runs / skipped_idle 等计数。可视化暂停时显示 IDLE，不继续显示旧风险。 ## 目录 -- src/、include/：采集、推理、结果变量及可选绘制。 -- config/：源码构建程序的配置和固定门区。 -- scripts/：构建和摄像头初始化脚本。 -- orin/：已编译程序、TensorRT 引擎、配置、当前 GMSL 驱动及启动脚本。 -- third_party/：nlohmann JSON 及其许可证。 +- `src/`、`include/`：C++ / CUDA 源码及公共接口。 +- `config/`：门 mask、摄像头及惰性推理配置。 +- `orin/`：模型、可执行程序、CAN 库、相机驱动及配置。 +- `scripts/`：编译、初始化、打包；`third_party/`：JSON 依赖。 -NPP 用于 UYVY 转换、YOLO 缩放填充、RTMW 仿射裁剪；CUDA 用于通道转换与归一化。 -风险几何计算仍包含 CPU/OpenCV 操作；算法内部的临时 JSON 对象只在内存中使用。 - -当前摄像头驱动启动时偶发首帧 ERROR，应用会丢弃并计数；持续错误、采集超时或推理错误会明确报错并退出。 -现有 GMSL 驱动停止采集时可产生 videobuf2 缓冲区告警，v4l2-ctl 同样复现；需在驱动侧完成量产修复和验证。 +模型和推理算法沿用纯推理基线。当前门 mask 为研发机位配置，装车需按实际机位标定。 diff --git a/config/camera.json b/config/camera.json index 433c732..3821a7b 100644 --- a/config/camera.json +++ b/config/camera.json @@ -18,5 +18,10 @@ "warning_relative": 0.3, "release_relative": 0.45, "calibration_verified": false + }, + "lazy_inference": { + "threshold_percent": 15, + "can_timeout_ms": 1000, + "motion_timeout_ms": 300 } } diff --git a/include/bounded_queue.hpp b/include/bounded_queue.hpp index 65e979b..8004b36 100644 --- a/include/bounded_queue.hpp +++ b/include/bounded_queue.hpp @@ -18,6 +18,15 @@ public: if(closed_)return false; items_.push_back(std::move(item));changed_.notify_all();return true; } + bool pushLatest(T item) { + std::lock_guard lock(mutex_); + if(closed_)return false; + if(items_.size()==capacity_)items_.pop_front(); + items_.push_back(std::move(item));changed_.notify_all();return true; + } + void clear() { + std::lock_guard lock(mutex_);items_.clear();changed_.notify_all(); + } std::optional pop() { std::unique_lock lock(mutex_); changed_.wait(lock,[&]{return closed_||!items_.empty();}); diff --git a/include/camera_pipeline.hpp b/include/camera_pipeline.hpp index d1160f5..165a36f 100644 --- a/include/camera_pipeline.hpp +++ b/include/camera_pipeline.hpp @@ -1,12 +1,14 @@ #pragma once #include "camera.hpp" #include "frame_result.hpp" +#include "lazy_gate.hpp" #include #include namespace cms { struct PipelineConfig { CameraConfig camera; + LazyGateConfig lazy_gate; std::filesystem::path engines,door_mask; double detector_confidence{0.30}; int max_people{3}; @@ -17,9 +19,10 @@ struct PipelineConfig { struct PipelineStats { std::uint64_t captured{},published{},driver_sequence_gaps{},discarded_error_frames{}; double mean_capture_to_result_ms{}; + std::uint64_t detector_runs{},pose_runs{},skipped_idle{}; }; // One lifecycle per instance. start() warms engines before starting capture. -// Runs continuously until requestStop() or a worker failure. +// Captures and receives CAN continuously. Engines stay warm while inference is idle. // latest() reads the immutable result variable without removing or acknowledging it. // No result queue, consumer registration or frame-count termination. // Stop is safe from any thread; @@ -33,6 +36,7 @@ public: void start(); FrameResultPtr latest() const; bool running() const noexcept; + InferenceStatus inferenceStatus() const; void requestStop() noexcept; void join(); PipelineStats stats() const; diff --git a/include/can_comm.h b/include/can_comm.h new file mode 100644 index 0000000..42415e3 --- /dev/null +++ b/include/can_comm.h @@ -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 +#include + +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 diff --git a/include/can_input.hpp b/include/can_input.hpp new file mode 100644 index 0000000..b5016da --- /dev/null +++ b/include/can_input.hpp @@ -0,0 +1,17 @@ +#pragma once +#include "lazy_gate.hpp" +#include +namespace cms { +// Receives continuously, including while inference is idle; retries missing USB CAN. +class CanInput { +public: + explicit CanInput(LazyGateConfig config); + ~CanInput(); + CanInput(CanInput const&)=delete; + CanInput& operator=(CanInput const&)=delete; + InferenceStatus snapshot(); +private: + struct Impl; + std::unique_ptr impl_; +}; +} diff --git a/include/frame_result.hpp b/include/frame_result.hpp index 7999cce..bcc3a7c 100644 --- a/include/frame_result.hpp +++ b/include/frame_result.hpp @@ -1,6 +1,7 @@ #pragma once // Public downstream contract. Owns its data; no GPU, JSON or network handles. #include "image.hpp" +#include "lazy_gate.hpp" #include #include #include @@ -63,6 +64,7 @@ struct StageTiming { double detector_ms{}, pose_ms{}, fusion_risk_ms{}, capture_to_result_ms{}; }; struct FrameResult { + InferenceStatus inference; int frame_index{}; std::vector track_ids; std::vector> boxes; diff --git a/include/latest_result.hpp b/include/latest_result.hpp index d6e7623..341dbda 100644 --- a/include/latest_result.hpp +++ b/include/latest_result.hpp @@ -11,6 +11,7 @@ public: if(!result)throw std::invalid_argument("Cannot publish a null result"); std::atomic_store_explicit(&latest_,std::move(result),std::memory_order_release); } + void clear() { std::atomic_store_explicit(&latest_,FrameResultPtr{},std::memory_order_release); } FrameResultPtr load() const { return std::atomic_load_explicit(&latest_,std::memory_order_acquire); } diff --git a/include/lazy_gate.hpp b/include/lazy_gate.hpp new file mode 100644 index 0000000..963d17c --- /dev/null +++ b/include/lazy_gate.hpp @@ -0,0 +1,36 @@ +#pragma once +#include +#include +#include + +namespace cms { +struct LazyGateConfig { + int threshold_percent{15}; + int can_timeout_ms{1000}; + int motion_timeout_ms{300}; +}; +struct InferenceStatus { + bool models_ready{}; + bool enabled{true}; + bool can_valid{}; + bool left_front_trigger{},left_rear_trigger{}; + std::array positions{{-1,-1,-1,-1}}; // LF, RF, LR, RR; percent + std::array direction{}; // LF, LR: -1 closing, 0 stationary, +1 opening + std::uint64_t generation{1},sample_monotonic_ns{}; + double can_age_ms{-1}; + std::string reason{"CAN_UNAVAILABLE"}; +}; +// Pure state machine. Caller serializes access. Time is monotonic nanoseconds. +class LazyGate { +public: + explicit LazyGate(LazyGateConfig config={}); + void receive(std::uint32_t id,std::uint8_t const* data,std::size_t size,std::uint64_t now); + InferenceStatus snapshot(std::uint64_t now); +private: + LazyGateConfig config_; + bool have_sample_{}; + std::uint64_t last_sample_{}; + std::array last_change_{}; + InferenceStatus state_; +}; +} diff --git a/include/zcan.h b/include/zcan.h new file mode 100644 index 0000000..04eb2af --- /dev/null +++ b/include/zcan.h @@ -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); diff --git a/orin/README.md b/orin/README.md index ca62f85..c40305f 100644 --- a/orin/README.md +++ b/orin/README.md @@ -1,22 +1,17 @@ -# 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 主机。 +在 JetPack 5.1.2 / TensorRT 8.5 / CUDA 11.4 / OpenCV 4.5 环境执行： ```bash -cd orin -sha256sum -c SHA256SUMS -./scripts/initialize_camera.sh +./scripts/initialize_camera.sh # 摄像头未初始化时 ./run.sh +./run.sh --visualize # 可选窗口 ``` -可视化可选：`./run.sh --visualize`，需要板端本地桌面 DISPLAY。 -默认持续推理直到 Ctrl+C / SIGTERM；正常退出没有统计输出。 -预览窗口关闭后推理继续。结果通过 C++ CameraPipeline::latest() 获取，不通过端口或结果文件传递。 +`build/camera_risk` 为 ARM64 ELF 程序，无 Python 运行依赖。 -程序为 build/camera_risk，配置为 config/camera.json，引擎为 engines/*.engine。 -./run.sh 从所在目录解析路径，也可以在其他工作目录通过绝对路径调用。 +CAN 0x391 的 data[0..3] 为左前、右前、左后、右后百分比。左前或左后开度 <15% 且正在增加或减小时推理；其余有效 CAN 状态暂停。CAN 缺失或超时 1000 ms 时推理，恢复后重新判断。 -bin/drivers 中 GMSL 模块对应内核 5.10.120-tegra。初始化脚本检查内核版本；加载模块需要 root 或现有 sudo 权限。 -门区保留当前室内联调设置，door.calibration_verified=false。 +模型只加载预热一次并常驻；相机和 CAN 始终运行。重复相同开度保留运动方向 300 ms，之后视为静止。参数见 config/camera.json 的 lazy_inference。默认结果保存在 C++ 内存，不写逐帧文件；暂停时 latest() 为空。标准输出记录启停状态与累计模型运行次数。 + +文件完整性：`sha256sum -c SHA256SUMS`。 diff --git a/orin/SHA256SUMS b/orin/SHA256SUMS index 6034f9e..4d682c1 100644 --- a/orin/SHA256SUMS +++ b/orin/SHA256SUMS @@ -1,11 +1,13 @@ -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 +cc83bfe41b66445843ef56b535e267939ed7944869474ca5aed6908f981b4687 ./README.md +6d5e874fd27fcfd75d303d7e35d9672028af71041f52dcc82a1e83676faefa65 ./bin/drivers/max9295.ko +8a78d04ef8be44e9b62a5e4ee315d3d6d679e34aedff02a234bbf4d40e222933 ./bin/drivers/max9296.ko +3da628587477cc35d5e3f8aee86e718e2ddf16808e893e00ba6e164b2307e843 ./bin/drivers/sgx-yuv-gmsl2.ko +9b467b4caf53d5cc5cdb5f71687e246146c05ef72fc7d7c593681409f63eec70 ./build/camera_risk +951c514ee76e10233a84746f25a884e0a1e8cbed84df57845a5aff66af9aecbd ./config/camera.json +9ba768d559fd3b64e833b50c62c70f2adfd6c169620aea5fba32cfb1b4c5b0e9 ./config/door_mask.json +ebcd09e8c2e35008adbb0c1d222e580b17e0df1c8093df3481ba3374cd9a8174 ./engines/rtmw_l_384.engine +f6a65b926ac1144484f4aa93cd26d7997d32d62310ab93fe867f2bd4412222e9 ./engines/yolo11s.engine +8dae8c557deadbe6bd7241c0dcd7025cb007077ee39a79b115e953a2b22cd9c1 ./lib/libusb-1.0.so +ab8b7f7249d5fb73d1d33d14fa7532a522088ffb6c25f37ba334daf4c6b4d313 ./lib/libusbcanfd.so +a0b083ad029fd6430941f5aff472150c441f7d5c4b6d7f201ed1999f0f86095a ./run.sh +9122cd4ed17fb26da1cb3f9456f923508430a518eb346c5038919d4475d8d2e8 ./scripts/initialize_camera.sh diff --git a/orin/build/camera_risk b/orin/build/camera_risk index 276b22c..c57dd89 100755 Binary files a/orin/build/camera_risk and b/orin/build/camera_risk differ diff --git a/orin/config/camera.json b/orin/config/camera.json index 6cd1134..a1f0218 100644 --- a/orin/config/camera.json +++ b/orin/config/camera.json @@ -9,14 +9,19 @@ }, "inference": { "engine_directory": "../engines", - "detector_confidence": 0.30, + "detector_confidence": 0.3, "max_people": 3, "queue_capacity": 2 }, "door": { "mask": "door_mask.json", - "warning_relative": 0.30, + "warning_relative": 0.3, "release_relative": 0.45, "calibration_verified": false + }, + "lazy_inference": { + "threshold_percent": 15, + "can_timeout_ms": 1000, + "motion_timeout_ms": 300 } } diff --git a/orin/lib/libusb-1.0.so b/orin/lib/libusb-1.0.so new file mode 100644 index 0000000..0824ee0 Binary files /dev/null and b/orin/lib/libusb-1.0.so differ diff --git a/orin/lib/libusb-1.0.so.0 b/orin/lib/libusb-1.0.so.0 new file mode 120000 index 0000000..4970d7f --- /dev/null +++ b/orin/lib/libusb-1.0.so.0 @@ -0,0 +1 @@ +libusb-1.0.so \ No newline at end of file diff --git a/orin/lib/libusbcanfd.so b/orin/lib/libusbcanfd.so new file mode 100644 index 0000000..72b9370 Binary files /dev/null and b/orin/lib/libusbcanfd.so differ diff --git a/orin/lib/libusbcanfd.so.1.0.12 b/orin/lib/libusbcanfd.so.1.0.12 new file mode 120000 index 0000000..2ee2cf4 --- /dev/null +++ b/orin/lib/libusbcanfd.so.1.0.12 @@ -0,0 +1 @@ +libusbcanfd.so \ No newline at end of file diff --git a/orin/run.sh b/orin/run.sh index c153d56..23913b5 100755 --- a/orin/run.sh +++ b/orin/run.sh @@ -1,4 +1,5 @@ #!/usr/bin/env bash set -euo pipefail root="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" && pwd)" +export LD_LIBRARY_PATH="$root/lib${LD_LIBRARY_PATH:+:$LD_LIBRARY_PATH}" exec "$root/build/camera_risk" --config "$root/config/camera.json" "$@" diff --git a/orin/scripts/initialize_camera.sh b/orin/scripts/initialize_camera.sh index 5f70e1f..6c26c46 100755 --- a/orin/scripts/initialize_camera.sh +++ b/orin/scripts/initialize_camera.sh @@ -11,7 +11,9 @@ 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; } + vermagic="$(modinfo -F vermagic "$file")" + # Some supplied drivers omit this metadata; normal insmod still validates them. + [[ -z "$vermagic" || "$vermagic" == "$(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" diff --git a/run_camera.sh b/run_camera.sh index c153d56..41b9369 100755 --- a/run_camera.sh +++ b/run_camera.sh @@ -1,4 +1,5 @@ #!/usr/bin/env bash set -euo pipefail root="$(cd -- "$(dirname -- "${BASH_SOURCE[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" "$@" diff --git a/scripts/package_orin.sh b/scripts/package_orin.sh new file mode 100755 index 0000000..858ac9e --- /dev/null +++ b/scripts/package_orin.sh @@ -0,0 +1,12 @@ +#!/usr/bin/env bash +set -euo pipefail +root="$(cd "$(dirname "$0")/.." && pwd)" +[[ -x "$root/build/camera_risk" ]] || { echo 'Build on Orin first.' >&2; exit 1; } +cp -p "$root/build/camera_risk" "$root/orin/build/" +cp -p "$root/config/door_mask.json" "$root/orin/config/" +sed 's|../orin/engines|../engines|' "$root/config/camera.json" > "$root/orin/config/camera.json" +( + cd "$root/orin" + find . -type f ! -path './outputs/*' ! -path './logs/*' ! -name SHA256SUMS -print0 | + sort -z | xargs -0 sha256sum > SHA256SUMS +) diff --git a/src/camera.cpp b/src/camera.cpp index 78c1d2c..f95ae36 100644 --- a/src/camera.cpp +++ b/src/camera.cpp @@ -123,7 +123,7 @@ struct Camera::Impl { 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; + m.capture_monotonic_ns=m.received_monotonic_ns; // Host dequeue clock; retain raw driver timestamp separately. if(haveSequence) { auto delta=static_cast(buffer.sequence-lastSequence); if(delta==0||delta>0x7fffffffU)throw std::runtime_error("Camera sequence reset or repeated frame"); diff --git a/src/camera_pipeline.cpp b/src/camera_pipeline.cpp index cb13ae1..200aa6a 100644 --- a/src/camera_pipeline.cpp +++ b/src/camera_pipeline.cpp @@ -5,6 +5,7 @@ #include "door_risk.hpp" #include "records.hpp" #include "result_conversion.hpp" +#include "can_input.hpp" #include #include #include @@ -45,9 +46,10 @@ void attachPoses(FrameResult& result,fangjia::PoseFrameResult const& fused) { } } struct CameraPipeline::Impl { - struct Detected {CapturedFrame frame;std::vector boxes;double elapsed_ms{};}; + struct Pending {CapturedFrame frame;std::uint64_t generation;}; + struct Detected {CapturedFrame frame;std::vector boxes;double elapsed_ms{};std::uint64_t generation;}; PipelineConfig config; - BoundedQueue captures; + BoundedQueue captures; BoundedQueue detections; LatestResult results; fangjia::TrtLogger logger; @@ -56,6 +58,9 @@ struct CameraPipeline::Impl { std::unique_ptr risk; std::shared_ptr doorPixels; std::unique_ptr camera; + std::unique_ptr can; + std::atomic modelsReady{false}; + std::atomic detectorRuns{0},poseRuns{0},skippedIdle{0}; std::thread captureThread,detectorThread,inferenceThread; mutable std::mutex stateMutex; std::exception_ptr failure; @@ -65,6 +70,7 @@ struct CameraPipeline::Impl { double totalLatency{}; explicit Impl(PipelineConfig c):config(std::move(c)),captures(config.queue_capacity),detections(config.queue_capacity) { + LazyGate validateGate(config.lazy_gate); 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"); @@ -77,7 +83,7 @@ struct CameraPipeline::Impl { if(inferenceThread.joinable())inferenceThread.join(); } void stop() noexcept { - stopped=true;active=false; + stopped=true;active=false;results.clear(); // Camera stays alive until all threads have joined. start()/destruction // are owner-thread operations; requestStop() only interrupts existing work. {std::lock_guard lock(stateMutex);if(camera)camera->requestStop();} @@ -106,48 +112,71 @@ struct CameraPipeline::Impl { pose->warmup(warmup.value); detector->infer(cv::Mat(config.camera.height,config.camera.width,CV_8UC3,cv::Scalar()),warmup.value); {std::lock_guard lock(stateMutex);camera=std::make_unique(config.camera);if(stopped)camera->requestStop();} + {std::lock_guard lock(stateMutex);can=std::make_unique(config.lazy_gate);} + modelsReady=true; active=!stopped.load(); captureThread=std::thread([this]{try{captureLoop();}catch(...){fail();}}); detectorThread=std::thread([this]{try{detectorLoop();}catch(...){fail();}}); inferenceThread=std::thread([this]{try{inferenceLoop();}catch(...){fail();}}); }catch(...){stop();joinWorkers();throw;} } + InferenceStatus status() const { + std::lock_guard lock(stateMutex); + auto status=can?can->snapshot():InferenceStatus{}; + status.models_ready=modelsReady; + if(!active||stopped){status.enabled=false;status.reason=stopped?"STOPPED":"STARTING";} + return status; + } + bool eligible(std::uint64_t generation) const {auto s=status();return s.enabled&&s.generation==generation;} void captureLoop() { + std::uint64_t generation=0; while(!stopped) { 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; ++captured; + auto gate=status(); + if(gate.generation!=generation){captures.clear();detections.clear();results.clear();generation=gate.generation;} + if(!gate.enabled){++skippedIdle;continue;} + if(!captures.pushLatest({std::move(*frame),gate.generation}))break; } captures.close(); } void detectorLoop() { Stream stream; 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; + auto pending=captures.pop();if(!pending)break; + if(!eligible(pending->generation))continue; + auto startedAt=monotonicNs();++detectorRuns; + auto boxes=detector->infer(imageView(*pending->frame.image),stream.value); + if(!eligible(pending->generation))continue; + if(!detections.pushLatest({std::move(pending->frame),std::move(boxes),elapsedMs(startedAt),pending->generation}))break; } detections.close(); } void inferenceLoop() { Stream stream;fangjia::BoxTracker boxesTracker;fangjia::PoseFusionTracker fusion; + std::uint64_t generation=0; while(!stopped) { auto detected=detections.pop();if(!detected)break; + if(!eligible(detected->generation))continue; + if(generation!=detected->generation){boxesTracker=fangjia::BoxTracker{};fusion=fangjia::PoseFusionTracker{};generation=detected->generation;} if(detected->frame.metadata.frame_index>static_cast(std::numeric_limits::max()))throw std::runtime_error("Frame index limit reached; restart pipeline"); auto index=static_cast(detected->frame.metadata.frame_index); auto& boxes=detected->boxes; auto frame=imageView(*detected->frame.image); boxesTracker.update(boxes,index,frame.cols,frame.rows); auto poseStart=monotonicNs(); + ++poseRuns; auto raw=pose->infer(frame,boxes,stream.value);auto isolated=raw; for(std::size_t p=0;pinfer(masked,{box},stream.value).at(0);} } + if(!eligible(generation))continue; auto poseElapsed=elapsedMs(poseStart);auto fusionStart=monotonicNs(); auto fused=fusion.update(raw,isolated,index); auto record=makeRecord(index,raw,isolated,fused,false,false); @@ -157,6 +186,9 @@ struct CameraPipeline::Impl { result->image=std::move(detected->frame.image);result->door_mask_pixels=doorPixels; result->door_mask_calibration_verified=config.door_mask_calibration_verified; result->timing={detected->elapsed_ms,poseElapsed,elapsedMs(fusionStart),elapsedMs(result->capture.capture_monotonic_ns)}; + auto gate=status(); + if(!gate.enabled||gate.generation!=generation)continue; + result->inference=gate; FrameResultPtr immutable=result; results.publish(std::move(immutable)); {std::lock_guard lock(stateMutex);totalLatency+=result->timing.capture_to_result_ms;++published;} @@ -167,12 +199,16 @@ struct CameraPipeline::Impl { CameraPipeline::CameraPipeline(PipelineConfig config):impl_(std::make_unique(std::move(config))){} CameraPipeline::~CameraPipeline()=default; void CameraPipeline::start(){impl_->start();} -FrameResultPtr CameraPipeline::latest()const{return impl_->results.load();} +FrameResultPtr CameraPipeline::latest()const{ + auto result=impl_->results.load();auto gate=impl_->status(); + return result&&gate.enabled&&result->inference.generation==gate.generation?result:FrameResultPtr{}; +} +InferenceStatus CameraPipeline::inferenceStatus()const{return impl_->status();} bool CameraPipeline::running()const noexcept{return impl_->active.load();} void CameraPipeline::requestStop()noexcept{impl_->stop();} -void CameraPipeline::join(){impl_->joinWorkers();{std::lock_guard lock(impl_->stateMutex);impl_->camera.reset();}impl_->rethrowFailure();} +void CameraPipeline::join(){impl_->joinWorkers();{std::lock_guard lock(impl_->stateMutex);impl_->camera.reset();impl_->can.reset();}impl_->rethrowFailure();} PipelineStats CameraPipeline::stats()const { std::lock_guard lock(impl_->stateMutex);auto count=impl_->published.load(); - return {impl_->captured.load(),count,impl_->gaps.load(),impl_->errorFrames.load(),count?impl_->totalLatency/count:0}; + return {impl_->captured.load(),count,impl_->gaps.load(),impl_->errorFrames.load(),count?impl_->totalLatency/count:0,impl_->detectorRuns.load(),impl_->poseRuns.load(),impl_->skippedIdle.load()}; } } diff --git a/src/can_comm.cpp b/src/can_comm.cpp new file mode 100644 index 0000000..e12e221 --- /dev/null +++ b/src/can_comm.cpp @@ -0,0 +1,342 @@ +// can_comm.cpp - USBCANFD CAN/CANFD 收发封装实现 +// +// 代码迁移自 vision-anti-pinch_bak/main.cpp 的 CAN 模块部分。 + +#include "can_comm.h" + +#include +#include +#include +#include +#include + +#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 stop{0}; // 线程结束标志 +} THREAD_CTX; + +/* CAN 运行标志, 由信号处理函数(Ctrl+C)或外部置位 */ +static volatile bool g_can_running = true; + +static std::atomic 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)continue; + 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 diff --git a/src/can_input.cpp b/src/can_input.cpp new file mode 100644 index 0000000..d0ea59c --- /dev/null +++ b/src/can_input.cpp @@ -0,0 +1,54 @@ +#include "can_input.hpp" +#include "can_comm.h" +#include "camera.hpp" +#include +#include +#include +#include +#include +namespace cms { +struct CanInput::Impl { + static std::mutex owner_mutex; + static Impl* owner; + std::mutex mutex; + std::condition_variable wake; + LazyGate gate; + std::atomic stopped{false}; + std::thread worker; + static void receive(int64_t,uint32_t id,uint8_t const* data,uint32_t size) { + std::lock_guard lock(owner_mutex); + if(owner){std::lock_guard stateLock(owner->mutex);owner->gate.receive(id,data,size,monotonicNs());} + } + explicit Impl(LazyGateConfig config):gate(config) { + {std::lock_guard lock(owner_mutex);if(owner)throw std::logic_error("Only one CAN receiver per process");owner=this;} + CanComm::set_msg_callback(receive); + try {worker=std::thread([this] { + bool opened=false; + auto attempted=monotonicNs(); + while(!stopped) { + auto now=monotonicNs(); + auto status=snapshot(); + // Retry unplugged/stalled adapters while CAN fallback inference continues. + if(!opened||(now-attempted>5000000000ULL&&!status.can_valid)) { + if(opened)CanComm::can_stop(); + opened=CanComm::can_start();attempted=monotonicNs(); + } + std::unique_lock lock(mutex); + wake.wait_for(lock,std::chrono::seconds(1),[this]{return stopped.load();}); + } + if(opened)CanComm::can_stop(); + });}catch(...){CanComm::set_msg_callback(nullptr);std::lock_guard lock(owner_mutex);owner=nullptr;throw;} + } + ~Impl() { + stopped=true;wake.notify_all();if(worker.joinable())worker.join(); + CanComm::set_msg_callback(nullptr); + std::lock_guard lock(owner_mutex);owner=nullptr; + } + InferenceStatus snapshot() {std::lock_guard lock(mutex);return gate.snapshot(monotonicNs());} +}; +std::mutex CanInput::Impl::owner_mutex; +CanInput::Impl* CanInput::Impl::owner=nullptr; +CanInput::CanInput(LazyGateConfig config):impl_(std::make_unique(config)){} +CanInput::~CanInput()=default; +InferenceStatus CanInput::snapshot(){return impl_->snapshot();} +} diff --git a/src/config.cpp b/src/config.cpp index d67f24e..7b0955f 100644 --- a/src/config.cpp +++ b/src/config.cpp @@ -19,6 +19,12 @@ 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"); + if(j.contains("lazy_inference")) { + auto const& gate=j.at("lazy_inference"); + config.lazy_gate.threshold_percent=gate.value("threshold_percent",15); + config.lazy_gate.can_timeout_ms=gate.value("can_timeout_ms",1000); + config.lazy_gate.motion_timeout_ms=gate.value("motion_timeout_ms",300); + } return config; } } diff --git a/src/lazy_gate.cpp b/src/lazy_gate.cpp new file mode 100644 index 0000000..73905c0 --- /dev/null +++ b/src/lazy_gate.cpp @@ -0,0 +1,41 @@ +#include "lazy_gate.hpp" +#include +namespace cms { +LazyGate::LazyGate(LazyGateConfig config):config_(config) { + if(config.threshold_percent<1||config.threshold_percent>100||config.can_timeout_ms<=0||config.motion_timeout_ms<=0||config.motion_timeout_ms>config.can_timeout_ms) + throw std::invalid_argument("Invalid lazy inference gate configuration"); +} +void LazyGate::receive(std::uint32_t id,std::uint8_t const* data,std::size_t size,std::uint64_t now) { + if(id!=0x391||size<4||!data)return; + snapshot(now); // Account for a timeout before receiving a new sample. + bool continuity=have_sample_&&now>=last_sample_&&now-last_sample_<=std::uint64_t(config_.can_timeout_ms)*1000000; + for(int side=0;side<2;++side) { + int index=side*2; + if(!continuity){state_.direction[side]=0;last_change_[side]=0;} + else if(data[index]!=state_.positions[index]) { + state_.direction[side]=data[index]>state_.positions[index]?1:-1; + last_change_[side]=now; + } + } + for(int i=0;i<4;++i)state_.positions[i]=data[i]; + have_sample_=data[0]<=100&&data[2]<=100; + if(!have_sample_){state_.direction={};last_change_={};} + last_sample_=now; + snapshot(now); +} +InferenceStatus LazyGate::snapshot(std::uint64_t now) { + state_.sample_monotonic_ns=now; + state_.can_age_ms=have_sample_&&now>=last_sample_?(now-last_sample_)*1e-6:-1; + state_.can_valid=state_.can_age_ms>=0&&state_.can_age_ms<=config_.can_timeout_ms; + bool triggers[2]{}; + for(int i=0;i<2;++i) { + if(!state_.can_valid||now=std::uint64_t(config_.motion_timeout_ms)*1000000)state_.direction[i]=0; + triggers[i]=state_.can_valid&&state_.positions[i*2] +#include +#include +#include +#include +#include #include -#include +#include 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> 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;isecond(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: "<=argc)throw std::invalid_argument("Missing option value");return std::string(argv[i]);}; + for(int i=1;i monitorStop{false}; + std::thread monitor([&] { + auto reported=started-std::chrono::seconds(5);std::uint64_t generation=0; + while(!monitorStop&&pipeline.running()) { + auto now=std::chrono::steady_clock::now(); + if(duration>0&&std::chrono::duration(now-started).count()>=duration){pipeline.requestStop();break;} + auto state=pipeline.inferenceStatus(); + if(state.generation!=generation||now-reported>=std::chrono::seconds(5)) { + auto stats=pipeline.stats(); + std::cout<<"inference="<<(state.enabled?"RUNNING":"IDLE")<<" reason="<(std::chrono::steady_clock::now()-started).count(); + std::cout<<"captured="<