cyy/yq_2026
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

init

Browse Source
This commit is contained in:
ChenYouYuan
2026-03-23 06:58:09 +08:00
commit 52257cf15c
456 changed files with 91599 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
src/rm_hardware_driver/ros2_hik_camera/.clang-format Normal file
View File

@@ -0,0 +1,18 @@
---
Language: Cpp
BasedOnStyle: Google
AccessModifierOffset: -2
AlignAfterOpenBracket: AlwaysBreak
BraceWrapping:
AfterClass: true
AfterFunction: true
AfterNamespace: true
AfterStruct: true
BreakBeforeBraces: Custom
ColumnLimit: 100
ConstructorInitializerIndentWidth: 0
ContinuationIndentWidth: 2
DerivePointerAlignment: false
PointerAlignment: Middle
ReflowComments: false

55
src/rm_hardware_driver/ros2_hik_camera/.clang-tidy Normal file
View File

@@ -0,0 +1,55 @@
---
Checks: '-*,
performance-*,
-performance-unnecessary-value-param,
llvm-namespace-comment,
modernize-redundant-void-arg,
modernize-use-nullptr,
modernize-use-default,
modernize-use-override,
modernize-loop-convert,
modernize-make-shared,
modernize-make-unique,
misc-unused-parameters,
readability-named-parameter,
readability-redundant-smartptr-get,
readability-redundant-string-cstr,
readability-simplify-boolean-expr,
readability-container-size-empty,
readability-identifier-naming,
'
HeaderFilterRegex: ''
AnalyzeTemporaryDtors: false
CheckOptions:
- key: llvm-namespace-comment.ShortNamespaceLines
value: '10'
- key: llvm-namespace-comment.SpacesBeforeComments
value: '2'
- key: misc-unused-parameters.StrictMode
value: '1'
- key: readability-braces-around-statements.ShortStatementLines
value: '2'
# type names
- key: readability-identifier-naming.ClassCase
value: CamelCase
- key: readability-identifier-naming.EnumCase
value: CamelCase
- key: readability-identifier-naming.UnionCase
value: CamelCase
# method names
- key: readability-identifier-naming.MethodCase
value: camelBack
# variable names
- key: readability-identifier-naming.VariableCase
value: lower_case
- key: readability-identifier-naming.ClassMemberSuffix
value: '_'
# const static or global variables are UPPER_CASE
- key: readability-identifier-naming.EnumConstantCase
value: UPPER_CASE
- key: readability-identifier-naming.StaticConstantCase
value: UPPER_CASE
- key: readability-identifier-naming.ClassConstantCase
value: UPPER_CASE
- key: readability-identifier-naming.GlobalVariableCase
value: UPPER_CASE

78
src/rm_hardware_driver/ros2_hik_camera/CMakeLists.txt Normal file
View File

@@ -0,0 +1,78 @@
cmake_minimum_required(VERSION 3.8)
project(hik_camera)
## Use C++14
set(CMAKE_CXX_STANDARD 14)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
## By adding -Wall and -Werror, the compiler does not ignore warnings anymore,
## enforcing cleaner code.
add_definitions(-Wall -Werror)
## Export compile commands for clangd
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
find_package(ament_cmake_auto REQUIRED)
ament_auto_find_build_dependencies()
find_package(OpenCV REQUIRED)
ament_auto_add_library(${PROJECT_NAME} SHARED
src/hik_camera_node.cpp
src/recorder.cpp
src/video_player_node.cpp
)
target_include_directories(${PROJECT_NAME} PUBLIC hikSDK/include)
target_include_directories(${PROJECT_NAME} PUBLIC include ${OpenCV_INCLUDE_DIRS})
if(CMAKE_SYSTEM_PROCESSOR MATCHES "x86_64")
target_link_directories(${PROJECT_NAME} PUBLIC hikSDK/lib/amd64)
install(
DIRECTORY hikSDK/lib/amd64/
DESTINATION lib
)
elseif(CMAKE_SYSTEM_PROCESSOR MATCHES "aarch64")
target_link_directories(${PROJECT_NAME} PUBLIC hikSDK/lib/arm64)
install(
DIRECTORY hikSDK/lib/arm64/
DESTINATION lib
)
else()
message(FATAL_ERROR "Unsupport host system architecture: ${CMAKE_HOST_SYSTEM_PROCESSOR}!")
endif()
target_link_libraries(${PROJECT_NAME}
FormatConversion
MediaProcess
MvCameraControl
MVRender
MvUsb3vTL
${OpenCV_LIBS}
)
rclcpp_components_register_node(${PROJECT_NAME}
PLUGIN fyt::camera_driver::HikCameraNode
EXECUTABLE ${PROJECT_NAME}_node
)
rclcpp_components_register_node(${PROJECT_NAME}
PLUGIN fyt::camera_driver::VideoPlayerNode
EXECUTABLE video_player_node
)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
list(APPEND AMENT_LINT_AUTO_EXCLUDE
ament_cmake_copyright
ament_cmake_cpplint
ament_cmake_uncrustify
)
ament_lint_auto_find_test_dependencies()
endif()
ament_auto_package(
INSTALL_TO_SHARE
config
launch
)

100
src/rm_hardware_driver/ros2_hik_camera/README.md Normal file
View File

@@ -0,0 +1,100 @@
# ros2_hik_camera
A ROS2 packge for Hikvision USB3.0 industrial camera
## Usage
```
ros2 launch hik_camera hik_camera.launch.py
```
离线视频回放（VideoPlayerNode，用于没有相机时跑全链路/复现）：
```bash
ros2 run hik_camera video_player_node --ros-args --params-file <your_video_player_params.yaml>
```
也可以在整套工程里通过 rm_bringup 启动（推荐），此时相机参数通常来自：
- rm_bringup/config/node_params/camera_driver_params.yaml
注意：rm_bringup 里加载的是自己的参数文件，hik_camera 包内的 config 主要用于“单独启动 hik_camera.launch.py”时测试。
## Params
以下参数均为 `camera_driver` 节点的 ROS 参数（YAML 中写在 `/**/ros__parameters` 下面即可）：
- `camera_info_url`：标定文件 URL（如 `package://rm_bringup/config/camera_info.yaml`）
- `camera_name`：相机名字标签（主要给 CameraInfo/工具识别用，不影响取图）
- `camera_frame_id`：发布图像的 frame_id，默认 `camera_optical_frame`
- `use_sensor_data_qos`：是否使用 sensor_data QoS
成像控制：
- `exposure_time`：曝光时间（单位：us），支持运行时 `ros2 param set`
- `gain`：增益，支持运行时 `ros2 param set`
- `auto_white_balance`：自动白平衡（0=Off，非0=Continuous）
- `frame_rate`：期望帧率（会尝试下发到相机的 `AcquisitionFrameRate*` 节点；不同型号可能不支持）
ROI / 裁剪（已接入海康 SDK 的 Width/Height/OffsetX/OffsetY，且会按相机的 min/max/inc 自动对齐）：
- `resolution_width`
- `resolution_height`
- `offsetX`
- `offsetY`
像素格式（发布端输出格式，驱动会用 SDK 转码后发布）：
- `pixel_format`：`rgb8`（默认）/ `bgr8` / `mono8`
录像：
- `recording`：是否录制，`true` 时保存到 `~/fyt2024-log/video/<timestamp>.avi`
VideoPlayerNode 参数（与 rm_bringup/config/node_params/video_player_params.yaml 对齐）：
- `path`：视频文件路径
- `frame_rate`：发布帧率
- `start_frame`：从第几帧开始发布（前面只读不发）
- `keep_looping`：播到结尾是否循环
- `frame_id`：发布 frame_id
示例：
/**:
ros__parameters:
camera_info_url: package://rm_bringup/config/camera_info.yaml
camera_name: narrow_stereo
camera_frame_id: camera_optical_frame
pixel_format: rgb8
exposure_time: 2500
gain: 15.0
auto_white_balance: 1
frame_rate: 210
resolution_width: 1280
resolution_height: 1024
offsetX: 0
offsetY: 0
recording: false
```
## Topics
- `image_raw`：图像（Image）
- `image_raw/camera_info`：相机标定信息（CameraInfo）
- `camera_driver/heartbeat`：心跳（Int64，自增，1Hz）
## Health / Reconnect
- 驱动包含看门狗：超过 5 秒没有收到新帧会触发 `close()` 并自动重连 `open()`。
## Daheng 风格参数说明（为什么有些需要重启）
在本工程里，类似 `resolution_width/height`、`offsetX/Y`、`auto_white_balance`、`frame_rate` 这类参数属于“相机配置”，通常是在相机 `open()` 时一次性下发到硬件。
- 对 Daheng 原实现：运行时只支持动态改 `exposure_time/gain`；其它参数改 YAML 后需要重启节点生效。
- 对当前 Hik 实现：这些参数已支持运行时动态改（`ros2 param set` 会立即尝试下发到相机），但具体是否成功取决于相机型号是否暴露对应 GenICam 节点（失败会 WARN）。

26
src/rm_hardware_driver/ros2_hik_camera/config/camera_info.yaml Normal file
View File

@@ -0,0 +1,26 @@
image_width: 1280
image_height: 1024
camera_name: narrow_stereo
camera_matrix:
rows: 3
cols: 3
data: [1385.639315, 0. , 597.199071,
0. , 1387.157400 , 452.204594,
0. , 0. , 1. ]
distortion_model: plumb_bob
distortion_coefficients:
rows: 1
cols: 5
data: [-0.057934, 0.141815, -0.003889, -0.006984, 0.000000]
rectification_matrix:
rows: 3
cols: 3
data: [1., 0., 0.,
0., 1., 0.,
0., 0., 1.]
projection_matrix:
rows: 3
cols: 4
data: [1380.831055, 0. , 590.255495, 0. ,
0. , 1387.520996, 449.174676, 0. ,
0. , 0. , 1. , 0. ]

7
src/rm_hardware_driver/ros2_hik_camera/config/camera_params.yaml Normal file
View File

@@ -0,0 +1,7 @@
/camera_driver:
ros__parameters:
camera_name: narrow_stereo
camera_info_url: package://hik_camera/config/camera_info.yaml
exposure_time: 3000
gain: 16.0
use_sensor_data_qos: true

1081
src/rm_hardware_driver/ros2_hik_camera/hikSDK/include/CameraParams.h Normal file
View File

File diff suppressed because it is too large Load Diff

3398
src/rm_hardware_driver/ros2_hik_camera/hikSDK/include/MvCameraControl.h Normal file
View File

File diff suppressed because it is too large Load Diff

104
src/rm_hardware_driver/ros2_hik_camera/hikSDK/include/MvErrorDefine.h Normal file
View File

@@ -0,0 +1,104 @@
#ifndef _MV_ERROR_DEFINE_H_
#define _MV_ERROR_DEFINE_H_
/********************************************************************/
/// \~chinese
/// \name 正确码定义
/// @{
/// \~english
/// \name Definition of correct code
/// @{
#define MV_OK 0x00000000 ///< \~chinese 成功，无错误 \~english Successed, no error
/// @}
/********************************************************************/
/// \~chinese
/// \name 通用错误码定义:范围0x80000000-0x800000FF
/// @{
/// \~english
/// \name Definition of General error code
/// @{
#define MV_E_HANDLE 0x80000000 ///< \~chinese 错误或无效的句柄 \~english Error or invalid handle
#define MV_E_SUPPORT 0x80000001 ///< \~chinese 不支持的功能 \~english Not supported function
#define MV_E_BUFOVER 0x80000002 ///< \~chinese 缓存已满 \~english Buffer overflow
#define MV_E_CALLORDER 0x80000003 ///< \~chinese 函数调用顺序错误 \~english Function calling order error
#define MV_E_PARAMETER 0x80000004 ///< \~chinese 错误的参数 \~english Incorrect parameter
#define MV_E_RESOURCE 0x80000006 ///< \~chinese 资源申请失败 \~english Applying resource failed
#define MV_E_NODATA 0x80000007 ///< \~chinese 无数据 \~english No data
#define MV_E_PRECONDITION 0x80000008 ///< \~chinese 前置条件有误，或运行环境已发生变化 \~english Precondition error, or running environment changed
#define MV_E_VERSION 0x80000009 ///< \~chinese 版本不匹配 \~english Version mismatches
#define MV_E_NOENOUGH_BUF 0x8000000A ///< \~chinese 传入的内存空间不足 \~english Insufficient memory
#define MV_E_ABNORMAL_IMAGE 0x8000000B ///< \~chinese 异常图像，可能是丢包导致图像不完整 \~english Abnormal image, maybe incomplete image because of lost packet
#define MV_E_LOAD_LIBRARY 0x8000000C ///< \~chinese 动态导入DLL失败 \~english Load library failed
#define MV_E_NOOUTBUF 0x8000000D ///< \~chinese 没有可输出的缓存 \~english No Avaliable Buffer
#define MV_E_UNKNOW 0x800000FF ///< \~chinese 未知的错误 \~english Unknown error
/// @}
/********************************************************************/
/// \~chinese
/// \name GenICam系列错误:范围0x80000100-0x800001FF
/// @{
/// \~english
/// \name GenICam Series Error Codes: Range from 0x80000100 to 0x800001FF
/// @{
#define MV_E_GC_GENERIC 0x80000100 ///< \~chinese 通用错误 \~english General error
#define MV_E_GC_ARGUMENT 0x80000101 ///< \~chinese 参数非法 \~english Illegal parameters
#define MV_E_GC_RANGE 0x80000102 ///< \~chinese 值超出范围 \~english The value is out of range
#define MV_E_GC_PROPERTY 0x80000103 ///< \~chinese 属性 \~english Property
#define MV_E_GC_RUNTIME 0x80000104 ///< \~chinese 运行环境有问题 \~english Running environment error
#define MV_E_GC_LOGICAL 0x80000105 ///< \~chinese 逻辑错误 \~english Logical error
#define MV_E_GC_ACCESS 0x80000106 ///< \~chinese 节点访问条件有误 \~english Node accessing condition error
#define MV_E_GC_TIMEOUT 0x80000107 ///< \~chinese 超时 \~english Timeout
#define MV_E_GC_DYNAMICCAST 0x80000108 ///< \~chinese 转换异常 \~english Transformation exception
#define MV_E_GC_UNKNOW 0x800001FF ///< \~chinese GenICam未知错误 \~english GenICam unknown error
/// @}
/********************************************************************/
/// \~chinese
/// \name GigE_STATUS对应的错误码:范围0x80000200-0x800002FF
/// @{
/// \~english
/// \name GigE_STATUS Error Codes: Range from 0x80000200 to 0x800002FF
/// @{
#define MV_E_NOT_IMPLEMENTED 0x80000200 ///< \~chinese 命令不被设备支持 \~english The command is not supported by device
#define MV_E_INVALID_ADDRESS 0x80000201 ///< \~chinese 访问的目标地址不存在 \~english The target address being accessed does not exist
#define MV_E_WRITE_PROTECT 0x80000202 ///< \~chinese 目标地址不可写 \~english The target address is not writable
#define MV_E_ACCESS_DENIED 0x80000203 ///< \~chinese 设备无访问权限 \~english No permission
#define MV_E_BUSY 0x80000204 ///< \~chinese 设备忙，或网络断开 \~english Device is busy, or network disconnected
#define MV_E_PACKET 0x80000205 ///< \~chinese 网络包数据错误 \~english Network data packet error
#define MV_E_NETER 0x80000206 ///< \~chinese 网络相关错误 \~english Network error
#define MV_E_IP_CONFLICT 0x80000221 ///< \~chinese 设备IP冲突 \~english Device IP conflict
/// @}
/********************************************************************/
/// \~chinese
/// \name USB_STATUS对应的错误码:范围0x80000300-0x800003FF
/// @{
/// \~english
/// \name USB_STATUS Error Codes: Range from 0x80000300 to 0x800003FF
/// @{
#define MV_E_USB_READ 0x80000300 ///< \~chinese 读usb出错 \~english Reading USB error
#define MV_E_USB_WRITE 0x80000301 ///< \~chinese 写usb出错 \~english Writing USB error
#define MV_E_USB_DEVICE 0x80000302 ///< \~chinese 设备异常 \~english Device exception
#define MV_E_USB_GENICAM 0x80000303 ///< \~chinese GenICam相关错误 \~english GenICam error
#define MV_E_USB_BANDWIDTH 0x80000304 ///< \~chinese 带宽不足 该错误码新增 \~english Insufficient bandwidth, this error code is newly added
#define MV_E_USB_DRIVER 0x80000305 ///< \~chinese 驱动不匹配或者未装驱动 \~english Driver mismatch or unmounted drive
#define MV_E_USB_UNKNOW 0x800003FF ///< \~chinese USB未知的错误 \~english USB unknown error
/// @}
/********************************************************************/
/// \~chinese
/// \name 升级时对应的错误码:范围0x80000400-0x800004FF
/// @{
/// \~english
/// \name Upgrade Error Codes: Range from 0x80000400 to 0x800004FF
/// @{
#define MV_E_UPG_FILE_MISMATCH 0x80000400 ///< \~chinese 升级固件不匹配 \~english Firmware mismatches
#define MV_E_UPG_LANGUSGE_MISMATCH 0x80000401 ///< \~chinese 升级固件语言不匹配 \~english Firmware language mismatches
#define MV_E_UPG_CONFLICT 0x80000402 ///< \~chinese 升级冲突（设备已经在升级了再次请求升级即返回此错误） \~english Upgrading conflicted (repeated upgrading requests during device upgrade)
#define MV_E_UPG_INNER_ERR 0x80000403 ///< \~chinese 升级时相机内部出现错误 \~english Camera internal error during upgrade
#define MV_E_UPG_UNKNOW 0x800004FF ///< \~chinese 升级时未知错误 \~english Unknown error during upgrade
/// @}
#endif //_MV_ERROR_DEFINE_H_

93
src/rm_hardware_driver/ros2_hik_camera/hikSDK/include/MvISPErrorDefine.h Normal file
View File

@@ -0,0 +1,93 @@
#ifndef _MV_ISP_ERROR_DEFINE_H_
#define _MV_ISP_ERROR_DEFINE_H_
/************************************************************************
* 来自ISP算法库的错误码
************************************************************************/
// 通用类型
#define MV_ALG_OK 0x00000000 //处理正确
#define MV_ALG_ERR 0x10000000 //不确定类型错误
// 能力检查
#define MV_ALG_E_ABILITY_ARG 0x10000001 //能力集中存在无效参数
// 内存检查
#define MV_ALG_E_MEM_NULL 0x10000002 //内存地址为空
#define MV_ALG_E_MEM_ALIGN 0x10000003 //内存对齐不满足要求
#define MV_ALG_E_MEM_LACK 0x10000004 //内存空间大小不够
#define MV_ALG_E_MEM_SIZE_ALIGN 0x10000005 //内存空间大小不满足对齐要求
#define MV_ALG_E_MEM_ADDR_ALIGN 0x10000006 //内存地址不满足对齐要求
// 图像检查
#define MV_ALG_E_IMG_FORMAT 0x10000007 //图像格式不正确或者不支持
#define MV_ALG_E_IMG_SIZE 0x10000008 //图像宽高不正确或者超出范围
#define MV_ALG_E_IMG_STEP 0x10000009 //图像宽高与step参数不匹配
#define MV_ALG_E_IMG_DATA_NULL 0x1000000A //图像数据存储地址为空
// 输入输出参数检查
#define MV_ALG_E_CFG_TYPE 0x1000000B //设置或者获取参数类型不正确
#define MV_ALG_E_CFG_SIZE 0x1000000C //设置或者获取参数的输入、输出结构体大小不正确
#define MV_ALG_E_PRC_TYPE 0x1000000D //处理类型不正确
#define MV_ALG_E_PRC_SIZE 0x1000000E //处理时输入、输出参数大小不正确
#define MV_ALG_E_FUNC_TYPE 0x1000000F //子处理类型不正确
#define MV_ALG_E_FUNC_SIZE 0x10000010 //子处理时输入、输出参数大小不正确
// 运行参数检查
#define MV_ALG_E_PARAM_INDEX 0x10000011 //index参数不正确
#define MV_ALG_E_PARAM_VALUE 0x10000012 //value参数不正确或者超出范围
#define MV_ALG_E_PARAM_NUM 0x10000013 //param_num参数不正确
// 接口调用检查
#define MV_ALG_E_NULL_PTR 0x10000014 //函数参数指针为空
#define MV_ALG_E_OVER_MAX_MEM 0x10000015 //超过限定的最大内存
#define MV_ALG_E_CALL_BACK 0x10000016 //回调函数出错
// 算法库加密相关检查
#define MV_ALG_E_ENCRYPT 0x10000017 //加密错误
#define MV_ALG_E_EXPIRE 0x10000018 //算法库使用期限错误
// 内部模块返回的基本错误类型
#define MV_ALG_E_BAD_ARG 0x10000019 //参数范围不正确
#define MV_ALG_E_DATA_SIZE 0x1000001A //数据大小不正确
#define MV_ALG_E_STEP 0x1000001B //数据step不正确
// cpu指令集支持错误码
#define MV_ALG_E_CPUID 0x1000001C //cpu不支持优化代码中的指令集
#define MV_ALG_WARNING 0x1000001D //警告
#define MV_ALG_E_TIME_OUT 0x1000001E //算法库超时
#define MV_ALG_E_LIB_VERSION 0x1000001F //算法版本号出错
#define MV_ALG_E_MODEL_VERSION 0x10000020 //模型版本号出错
#define MV_ALG_E_GPU_MEM_ALLOC 0x10000021 //GPU内存分配错误
#define MV_ALG_E_FILE_NON_EXIST 0x10000022 //文件不存在
#define MV_ALG_E_NONE_STRING 0x10000023 //字符串为空
#define MV_ALG_E_IMAGE_CODEC 0x10000024 //图像解码器错误
#define MV_ALG_E_FILE_OPEN 0x10000025 //打开文件错误
#define MV_ALG_E_FILE_READ 0x10000026 //文件读取错误
#define MV_ALG_E_FILE_WRITE 0x10000027 //文件写错误
#define MV_ALG_E_FILE_READ_SIZE 0x10000028 //文件读取大小错误
#define MV_ALG_E_FILE_TYPE 0x10000029 //文件类型错误
#define MV_ALG_E_MODEL_TYPE 0x1000002A //模型类型错误
#define MV_ALG_E_MALLOC_MEM 0x1000002B //分配内存错误
#define MV_ALG_E_BIND_CORE_FAILED 0x1000002C //线程绑核失败
// 降噪特有错误码
#define MV_ALG_E_DENOISE_NE_IMG_FORMAT 0x10402001 //噪声特性图像格式错误
#define MV_ALG_E_DENOISE_NE_FEATURE_TYPE 0x10402002 //噪声特性类型错误
#define MV_ALG_E_DENOISE_NE_PROFILE_NUM 0x10402003 //噪声特性个数错误
#define MV_ALG_E_DENOISE_NE_GAIN_NUM 0x10402004 //噪声特性增益个数错误
#define MV_ALG_E_DENOISE_NE_GAIN_VAL 0x10402005 //噪声曲线增益值输入错误
#define MV_ALG_E_DENOISE_NE_BIN_NUM 0x10402006 //噪声曲线柱数错误
#define MV_ALG_E_DENOISE_NE_INIT_GAIN 0x10402007 //噪声估计初始化增益设置错误
#define MV_ALG_E_DENOISE_NE_NOT_INIT 0x10402008 //噪声估计未初始化
#define MV_ALG_E_DENOISE_COLOR_MODE 0x10402009 //颜色空间模式错误
#define MV_ALG_E_DENOISE_ROI_NUM 0x1040200a //图像ROI个数错误
#define MV_ALG_E_DENOISE_ROI_ORI_PT 0x1040200b //图像ROI原点错误
#define MV_ALG_E_DENOISE_ROI_SIZE 0x1040200c //图像ROI大小错误
#define MV_ALG_E_DENOISE_GAIN_NOT_EXIST 0x1040200d //输入的相机增益不存在(增益个数已达上限)
#define MV_ALG_E_DENOISE_GAIN_BEYOND_RANGE 0x1040200e //输入的相机增益不在范围内
#define MV_ALG_E_DENOISE_NP_BUF_SIZE 0x1040200f //输入的噪声特性内存大小错误
#endif //_MV_ISP_ERROR_DEFINE_H_

202
src/rm_hardware_driver/ros2_hik_camera/hikSDK/include/PixelType.h Normal file
View File

@@ -0,0 +1,202 @@
#ifndef _MV_PIXEL_TYPE_H_
#define _MV_PIXEL_TYPE_H_
//#include "Base/GCTypes.h"
/************************************************************************/
/* GigE Vision (2.0.03) PIXEL FORMATS */
/************************************************************************/
// Indicate if pixel is monochrome or RGB
#define MV_GVSP_PIX_MONO 0x01000000
#define MV_GVSP_PIX_RGB 0x02000000 // deprecated in version 1.1
#define MV_GVSP_PIX_COLOR 0x02000000
#define MV_GVSP_PIX_CUSTOM 0x80000000
#define MV_GVSP_PIX_COLOR_MASK 0xFF000000
// Indicate effective number of bits occupied by the pixel (including padding).
// This can be used to compute amount of memory required to store an image.
#define MV_PIXEL_BIT_COUNT(n) ((n) << 16)
#define MV_GVSP_PIX_EFFECTIVE_PIXEL_SIZE_MASK 0x00FF0000
#define MV_GVSP_PIX_EFFECTIVE_PIXEL_SIZE_SHIFT 16
// Pixel ID: lower 16-bit of the pixel formats
#define MV_GVSP_PIX_ID_MASK 0x0000FFFF
#define MV_GVSP_PIX_COUNT 0x46 // next Pixel ID available
enum MvGvspPixelType
{
// Undefined pixel type
#ifdef WIN32
PixelType_Gvsp_Undefined = 0xFFFFFFFF,
#else
PixelType_Gvsp_Undefined = -1,
#endif
// Mono buffer format defines
PixelType_Gvsp_Mono1p = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(1) | 0x0037),
PixelType_Gvsp_Mono2p = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(2) | 0x0038),
PixelType_Gvsp_Mono4p = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(4) | 0x0039),
PixelType_Gvsp_Mono8 = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(8) | 0x0001),
PixelType_Gvsp_Mono8_Signed = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(8) | 0x0002),
PixelType_Gvsp_Mono10 = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0003),
PixelType_Gvsp_Mono10_Packed = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x0004),
PixelType_Gvsp_Mono12 = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0005),
PixelType_Gvsp_Mono12_Packed = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x0006),
PixelType_Gvsp_Mono14 = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0025),
PixelType_Gvsp_Mono16 = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0007),
// Bayer buffer format defines
PixelType_Gvsp_BayerGR8 = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(8) | 0x0008),
PixelType_Gvsp_BayerRG8 = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(8) | 0x0009),
PixelType_Gvsp_BayerGB8 = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(8) | 0x000A),
PixelType_Gvsp_BayerBG8 = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(8) | 0x000B),
PixelType_Gvsp_BayerGR10 = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x000C),
PixelType_Gvsp_BayerRG10 = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x000D),
PixelType_Gvsp_BayerGB10 = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x000E),
PixelType_Gvsp_BayerBG10 = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x000F),
PixelType_Gvsp_BayerGR12 = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0010),
PixelType_Gvsp_BayerRG12 = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0011),
PixelType_Gvsp_BayerGB12 = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0012),
PixelType_Gvsp_BayerBG12 = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0013),
PixelType_Gvsp_BayerGR10_Packed = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x0026),
PixelType_Gvsp_BayerRG10_Packed = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x0027),
PixelType_Gvsp_BayerGB10_Packed = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x0028),
PixelType_Gvsp_BayerBG10_Packed = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x0029),
PixelType_Gvsp_BayerGR12_Packed = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x002A),
PixelType_Gvsp_BayerRG12_Packed = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x002B),
PixelType_Gvsp_BayerGB12_Packed = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x002C),
PixelType_Gvsp_BayerBG12_Packed = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x002D),
PixelType_Gvsp_BayerGR16 = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x002E),
PixelType_Gvsp_BayerRG16 = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x002F),
PixelType_Gvsp_BayerGB16 = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0030),
PixelType_Gvsp_BayerBG16 = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0031),
// RGB Packed buffer format defines
PixelType_Gvsp_RGB8_Packed = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(24) | 0x0014),
PixelType_Gvsp_BGR8_Packed = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(24) | 0x0015),
PixelType_Gvsp_RGBA8_Packed = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(32) | 0x0016),
PixelType_Gvsp_BGRA8_Packed = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(32) | 0x0017),
PixelType_Gvsp_RGB10_Packed = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(48) | 0x0018),
PixelType_Gvsp_BGR10_Packed = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(48) | 0x0019),
PixelType_Gvsp_RGB12_Packed = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(48) | 0x001A),
PixelType_Gvsp_BGR12_Packed = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(48) | 0x001B),
PixelType_Gvsp_RGB16_Packed = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(48) | 0x0033),
PixelType_Gvsp_BGR16_Packed = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(48) | 0x004B),
PixelType_Gvsp_RGBA16_Packed = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(64) | 0x0064),
PixelType_Gvsp_BGRA16_Packed = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(64) | 0x0051),
PixelType_Gvsp_RGB10V1_Packed = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(32) | 0x001C),
PixelType_Gvsp_RGB10V2_Packed = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(32) | 0x001D),
PixelType_Gvsp_RGB12V1_Packed = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(36) | 0X0034),
PixelType_Gvsp_RGB565_Packed = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(16) | 0x0035),
PixelType_Gvsp_BGR565_Packed = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(16) | 0X0036),
// YUV Packed buffer format defines
PixelType_Gvsp_YUV411_Packed = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(12) | 0x001E),
PixelType_Gvsp_YUV422_Packed = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(16) | 0x001F),
PixelType_Gvsp_YUV422_YUYV_Packed = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(16) | 0x0032),
PixelType_Gvsp_YUV444_Packed = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(24) | 0x0020),
PixelType_Gvsp_YCBCR8_CBYCR = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(24) | 0x003A),
PixelType_Gvsp_YCBCR422_8 = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(16) | 0x003B),
PixelType_Gvsp_YCBCR422_8_CBYCRY = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(16) | 0x0043),
PixelType_Gvsp_YCBCR411_8_CBYYCRYY = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(12) | 0x003C),
PixelType_Gvsp_YCBCR601_8_CBYCR = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(24) | 0x003D),
PixelType_Gvsp_YCBCR601_422_8 = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(16) | 0x003E),
PixelType_Gvsp_YCBCR601_422_8_CBYCRY = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(16) | 0x0044),
PixelType_Gvsp_YCBCR601_411_8_CBYYCRYY = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(12) | 0x003F),
PixelType_Gvsp_YCBCR709_8_CBYCR = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(24) | 0x0040),
PixelType_Gvsp_YCBCR709_422_8 = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(16) | 0x0041),
PixelType_Gvsp_YCBCR709_422_8_CBYCRY = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(16) | 0x0045),
PixelType_Gvsp_YCBCR709_411_8_CBYYCRYY = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(12) | 0x0042),
// RGB Planar buffer format defines
PixelType_Gvsp_RGB8_Planar = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(24) | 0x0021),
PixelType_Gvsp_RGB10_Planar = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(48) | 0x0022),
PixelType_Gvsp_RGB12_Planar = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(48) | 0x0023),
PixelType_Gvsp_RGB16_Planar = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(48) | 0x0024),
// 自定义的图片格式
PixelType_Gvsp_Jpeg = (MV_GVSP_PIX_CUSTOM | MV_PIXEL_BIT_COUNT(24) | 0x0001),
PixelType_Gvsp_Coord3D_ABC32f = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(96) | 0x00C0),//0x026000C0
PixelType_Gvsp_Coord3D_ABC32f_Planar = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(96) | 0x00C1),//0x026000C1
// 该值被废弃，请参考PixelType_Gvsp_Coord3D_AC32f_64; the value is discarded
PixelType_Gvsp_Coord3D_AC32f = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(40) | 0x00C2),
// 该值被废弃; the value is discarded (已放入Chunkdata)
PixelType_Gvsp_COORD3D_DEPTH_PLUS_MASK = (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(28) | 0x0001),
PixelType_Gvsp_Coord3D_ABC32 = (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(96) | 0x3001),//0x82603001
PixelType_Gvsp_Coord3D_AB32f = (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(64) | 0x3002),//0x82403002
PixelType_Gvsp_Coord3D_AB32 = (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(64) | 0x3003),//0x82403003
PixelType_Gvsp_Coord3D_AC32f_64 = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(64) | 0x00C2),//0x024000C2
PixelType_Gvsp_Coord3D_AC32f_Planar = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(64) | 0x00C3),//0x024000C3
PixelType_Gvsp_Coord3D_AC32 = (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(64) | 0x3004),//0x82403004
PixelType_Gvsp_Coord3D_A32f = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(32) | 0x00BD),//0x012000BD
PixelType_Gvsp_Coord3D_A32 = (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(32) | 0x3005),//0x81203005
PixelType_Gvsp_Coord3D_C32f = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(32) | 0x00BF),//0x012000BF
PixelType_Gvsp_Coord3D_C32 = (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(32) | 0x3006),//0x81203006
PixelType_Gvsp_Coord3D_ABC16 = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(48) | 0x00B9),//0x023000B9
PixelType_Gvsp_Coord3D_C16 = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x00B8),//0x011000B8
//无损压缩像素格式定义
PixelType_Gvsp_HB_Mono8 = (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(8) | 0x0001),
PixelType_Gvsp_HB_Mono10 = (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0003),
PixelType_Gvsp_HB_Mono10_Packed = (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x0004),
PixelType_Gvsp_HB_Mono12 = (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0005),
PixelType_Gvsp_HB_Mono12_Packed = (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x0006),
PixelType_Gvsp_HB_Mono16 = (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0007),
PixelType_Gvsp_HB_BayerGR8 = (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(8) | 0x0008),
PixelType_Gvsp_HB_BayerRG8 = (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(8) | 0x0009),
PixelType_Gvsp_HB_BayerGB8 = (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(8) | 0x000A),
PixelType_Gvsp_HB_BayerBG8 = (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(8) | 0x000B),
PixelType_Gvsp_HB_BayerRBGG8 = (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(8) | 0x0046),
PixelType_Gvsp_HB_BayerGR10 = (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x000C),
PixelType_Gvsp_HB_BayerRG10 = (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x000D),
PixelType_Gvsp_HB_BayerGB10 = (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x000E),
PixelType_Gvsp_HB_BayerBG10 = (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x000F),
PixelType_Gvsp_HB_BayerGR12 = (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0010),
PixelType_Gvsp_HB_BayerRG12 = (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0011),
PixelType_Gvsp_HB_BayerGB12 = (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0012),
PixelType_Gvsp_HB_BayerBG12 = (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0013),
PixelType_Gvsp_HB_BayerGR10_Packed = (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x0026),
PixelType_Gvsp_HB_BayerRG10_Packed = (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x0027),
PixelType_Gvsp_HB_BayerGB10_Packed = (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x0028),
PixelType_Gvsp_HB_BayerBG10_Packed = (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x0029),
PixelType_Gvsp_HB_BayerGR12_Packed = (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x002A),
PixelType_Gvsp_HB_BayerRG12_Packed = (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x002B),
PixelType_Gvsp_HB_BayerGB12_Packed = (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x002C),
PixelType_Gvsp_HB_BayerBG12_Packed = (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x002D),
PixelType_Gvsp_HB_YUV422_Packed = (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(16) | 0x001F),
PixelType_Gvsp_HB_YUV422_YUYV_Packed = (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(16) | 0x0032),
PixelType_Gvsp_HB_RGB8_Packed = (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(24) | 0x0014),
PixelType_Gvsp_HB_BGR8_Packed = (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(24) | 0x0015),
PixelType_Gvsp_HB_RGBA8_Packed = (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(32) | 0x0016),
PixelType_Gvsp_HB_BGRA8_Packed = (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(32) | 0x0017),
PixelType_Gvsp_HB_RGB16_Packed = (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(48) | 0x0033),
PixelType_Gvsp_HB_BGR16_Packed = (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(48) | 0x004B),
PixelType_Gvsp_HB_RGBA16_Packed = (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(64) | 0x0064),
PixelType_Gvsp_HB_BGRA16_Packed = (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(64) | 0x0051),
};
//enum MvUsbPixelType
//{
//
//};
//跨平台定义
//Cross Platform Definition
#ifdef WIN32
typedef __int64 int64_t;
typedef unsigned __int64 uint64_t;
#else
#include <stdint.h>
#endif
#endif /* _MV_PIXEL_TYPE_H_ */

BIN
src/rm_hardware_driver/ros2_hik_camera/hikSDK/lib/amd64/libFormatConversion.so Normal file
View File

Binary file not shown.

BIN
src/rm_hardware_driver/ros2_hik_camera/hikSDK/lib/amd64/libMVRender.so Normal file
View File

Binary file not shown.

BIN
src/rm_hardware_driver/ros2_hik_camera/hikSDK/lib/amd64/libMediaProcess.so Normal file
View File

Binary file not shown.

BIN
src/rm_hardware_driver/ros2_hik_camera/hikSDK/lib/amd64/libMvCameraControl.so Normal file
View File

Binary file not shown.

BIN
src/rm_hardware_driver/ros2_hik_camera/hikSDK/lib/amd64/libMvUsb3vTL.so Normal file
View File

Binary file not shown.

BIN
src/rm_hardware_driver/ros2_hik_camera/hikSDK/lib/arm64/libFormatConversion.so Normal file
View File

Binary file not shown.

BIN
src/rm_hardware_driver/ros2_hik_camera/hikSDK/lib/arm64/libMVRender.so Normal file
View File

Binary file not shown.

BIN
src/rm_hardware_driver/ros2_hik_camera/hikSDK/lib/arm64/libMediaProcess.so Normal file
View File

Binary file not shown.

BIN
src/rm_hardware_driver/ros2_hik_camera/hikSDK/lib/arm64/libMvCameraControl.so Normal file
View File

Binary file not shown.

BIN
src/rm_hardware_driver/ros2_hik_camera/hikSDK/lib/arm64/libMvUsb3vTL.so Normal file
View File

Binary file not shown.

46
src/rm_hardware_driver/ros2_hik_camera/include/hik_camera/recorder.hpp Normal file
View File

@@ -0,0 +1,46 @@
#ifndef HIK_CAMERA_RECORDER_HPP_
#define HIK_CAMERA_RECORDER_HPP_
// std
#include <atomic>
#include <condition_variable>
#include <deque>
#include <filesystem>
#include <mutex>
#include <thread>
#include <vector>
// OpenCV
#include <opencv2/videoio.hpp>
namespace fyt::camera_driver {
class Recorder {
public:
using Frame = std::vector<unsigned char>;
Recorder(const std::filesystem::path &file, int fps, cv::Size size);
~Recorder();
void addFrame(const Frame &frame);
void start();
void stop();
std::filesystem::path path;
private:
void recorderThread();
cv::Size size_;
int fps_;
cv::VideoWriter writer_;
std::deque<Frame> frame_queue_;
std::mutex mutex_;
std::atomic<bool> recording_{false};
std::condition_variable cv_;
std::thread recorder_thread_;
};
} // namespace fyt::camera_driver
#endif // HIK_CAMERA_RECORDER_HPP_

34
src/rm_hardware_driver/ros2_hik_camera/launch/hik_camera.launch.py Normal file
View File

@@ -0,0 +1,34 @@
import os
from ament_index_python.packages import get_package_share_directory, get_package_prefix
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
def generate_launch_description():
params_file = os.path.join(
get_package_share_directory('hik_camera'), 'config', 'camera_params.yaml')
camera_info_url = 'package://hik_camera/config/camera_info.yaml'
return LaunchDescription([
DeclareLaunchArgument(name='params_file',
default_value=params_file),
DeclareLaunchArgument(name='camera_info_url',
default_value=camera_info_url),
DeclareLaunchArgument(name='use_sensor_data_qos',
default_value='false'),
Node(
package='hik_camera',
executable='hik_camera_node',
output='screen',
emulate_tty=True,
parameters=[LaunchConfiguration('params_file'), {
'camera_info_url': LaunchConfiguration('camera_info_url'),
'use_sensor_data_qos': LaunchConfiguration('use_sensor_data_qos'),
}],
)
])

30
src/rm_hardware_driver/ros2_hik_camera/package.xml Normal file
View File

@@ -0,0 +1,30 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>hik_camera</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="1258124282@qq.com">nolem</maintainer>
<license>TODO: License declaration</license>
<!-- buildtool_depend: dependencies of the build process -->
<buildtool_depend>ament_cmake</buildtool_depend>
<!-- depend: build, export, and execution dependency -->
<depend>rclcpp</depend>
<depend>rclcpp_components</depend>
<depend>sensor_msgs</depend>
<depend>image_transport</depend>
<depend>image_transport_plugins</depend>
<depend>camera_info_manager</depend>
<depend>rm_utils</depend>
<depend>opencv</depend>
<exec_depend>camera_calibration</exec_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

514
src/rm_hardware_driver/ros2_hik_camera/src/hik_camera_node.cpp Normal file
View File

@@ -0,0 +1,514 @@
#include <camera_info_manager/camera_info_manager.hpp>
#include <chrono>
#include <cstdlib>
#include <ctime>
#include <filesystem>
#include <image_transport/image_transport.hpp>
#include <memory>
#include <mutex>
#include <rcl_interfaces/msg/set_parameters_result.hpp>
#include <rclcpp/logging.hpp>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <string>
#include <thread>
#include "MvCameraControl.h"
#include "hik_camera/recorder.hpp"
#include "rm_utils/heartbeat.hpp"
#include "rm_utils/logger/log.hpp"
namespace fyt::camera_driver
{
class HikCameraNode : public rclcpp::Node
{
public:
explicit HikCameraNode(const rclcpp::NodeOptions & options) : Node("camera_driver", options)
{
FYT_REGISTER_LOGGER("camera_driver", "~/fyt2024-log", INFO);
FYT_INFO("camera_driver", "Starting HikCameraNode!");
camera_name_ = this->declare_parameter("camera_name", "narrow_stereo");
camera_info_url_ =
this->declare_parameter("camera_info_url", "package://hik_camera/config/camera_info.yaml");
frame_id_ = this->declare_parameter("camera_frame_id", "camera_optical_frame");
pixel_format_ = this->declare_parameter("pixel_format", "rgb8");
frame_rate_ = this->declare_parameter("frame_rate", 210);
exposure_time_ = this->declare_parameter("exposure_time", 5000);
gain_ = this->declare_parameter("gain", 12.0);
resolution_width_ = this->declare_parameter("resolution_width", 1280);
resolution_height_ = this->declare_parameter("resolution_height", 1024);
offset_x_ = this->declare_parameter("offsetX", 0);
offset_y_ = this->declare_parameter("offsetY", 0);
image_msg_.header.frame_id = frame_id_;
image_msg_.encoding = pixel_format_;
bool use_sensor_data_qos = this->declare_parameter("use_sensor_data_qos", true);
auto qos = use_sensor_data_qos ? rmw_qos_profile_sensor_data : rmw_qos_profile_default;
camera_pub_ = image_transport::create_camera_publisher(this, "image_raw", qos);
camera_info_manager_ =
std::make_unique<camera_info_manager::CameraInfoManager>(this, camera_name_);
if (camera_info_manager_->validateURL(camera_info_url_)) {
camera_info_manager_->loadCameraInfo(camera_info_url_);
camera_info_msg_ = camera_info_manager_->getCameraInfo();
} else {
camera_info_manager_->setCameraName(camera_name_);
sensor_msgs::msg::CameraInfo camera_info;
camera_info_manager_->setCameraInfo(camera_info);
FYT_WARN("camera_driver", "Invalid camera info URL: {}", camera_info_url_);
}
// Heartbeat
heartbeat_ = fyt::HeartBeatPublisher::create(this);
// Param set callback
params_callback_handle_ = this->add_on_set_parameters_callback(
std::bind(&HikCameraNode::parametersCallback, this, std::placeholders::_1));
// Recorder
bool enable_recorder = this->declare_parameter("recording", false);
if (enable_recorder) {
std::string home = std::getenv("HOME") ? std::getenv("HOME") : std::string(".");
namespace fs = std::filesystem;
std::filesystem::path video_path = fs::path(home) / "fyt2024-log/video/" /
std::string(std::to_string(std::time(nullptr)) + ".avi");
recorder_ = std::make_unique<Recorder>(
video_path, frame_rate_, cv::Size(resolution_width_, resolution_height_));
recorder_->start();
FYT_INFO("camera_driver", "Recorder started! Video file: {}", video_path.string());
}
// Watch dog timer (also handles initial open)
timer_ = this->create_wall_timer(
std::chrono::milliseconds(1000), std::bind(&HikCameraNode::timerCallback, this));
// Start capture thread
capture_thread_ = std::thread([this]() { this->captureLoop(); });
FYT_INFO("camera_driver", "HikCameraNode has been initialized!");
}
~HikCameraNode() override
{
close();
if (capture_thread_.joinable()) {
capture_thread_.join();
}
if (recorder_ != nullptr) {
recorder_->stop();
FYT_INFO(
"camera_driver", "Recorder stopped! Video file {} has been saved",
recorder_->path.string());
}
FYT_INFO("camera_driver", "HikCameraNode has been destroyed!");
}
private:
static int64_t clampToInc(int64_t value, int64_t min, int64_t max, int64_t inc)
{
if (inc <= 0) {
inc = 1;
}
if (value < min) {
value = min;
}
if (value > max) {
value = max;
}
const int64_t delta = value - min;
return min + (delta / inc) * inc;
}
bool getIntNodeLocked(const char * name, MVCC_INTVALUE & out)
{
if (camera_handle_ == nullptr) {
return false;
}
return MV_CC_GetIntValue(camera_handle_, name, &out) == MV_OK;
}
bool setIntNodeLocked(const char * name, int64_t value)
{
if (camera_handle_ == nullptr) {
return false;
}
return MV_CC_SetIntValue(camera_handle_, name, value) == MV_OK;
}
void applyRoiLocked()
{
// Best-effort: not all cameras support all nodes
MVCC_INTVALUE width_info;
MVCC_INTVALUE height_info;
MVCC_INTVALUE offx_info;
MVCC_INTVALUE offy_info;
const bool has_width = getIntNodeLocked("Width", width_info);
const bool has_height = getIntNodeLocked("Height", height_info);
const bool has_offx = getIntNodeLocked("OffsetX", offx_info);
const bool has_offy = getIntNodeLocked("OffsetY", offy_info);
if (!has_width || !has_height) {
FYT_WARN("camera_driver", "Camera does not expose Width/Height nodes; ROI params ignored");
return;
}
int64_t target_w =
clampToInc(resolution_width_, width_info.nMin, width_info.nMax, width_info.nInc);
int64_t target_h =
clampToInc(resolution_height_, height_info.nMin, height_info.nMax, height_info.nInc);
int64_t target_x = 0;
int64_t target_y = 0;
if (has_offx) {
target_x = clampToInc(offset_x_, offx_info.nMin, offx_info.nMax, offx_info.nInc);
}
if (has_offy) {
target_y = clampToInc(offset_y_, offy_info.nMin, offy_info.nMax, offy_info.nInc);
}
// Some cameras constrain width/height based on offset; apply in safe order
if (has_offx) {
(void)setIntNodeLocked("OffsetX", target_x);
}
if (has_offy) {
(void)setIntNodeLocked("OffsetY", target_y);
}
(void)setIntNodeLocked("Width", target_w);
(void)setIntNodeLocked("Height", target_h);
resolution_width_ = static_cast<int>(target_w);
resolution_height_ = static_cast<int>(target_h);
offset_x_ = static_cast<int>(target_x);
offset_y_ = static_cast<int>(target_y);
FYT_INFO(
"camera_driver", "ROI applied: {}x{} offset({},{})", resolution_width_, resolution_height_,
offset_x_, offset_y_);
}
void timerCallback()
{
// Open camera
while (!is_open_ && rclcpp::ok()) {
if (!open()) {
FYT_ERROR("camera_driver", "open() failed");
close();
return;
}
}
// Watchdog: no frames for too long => reconnect
double dt = (this->now() - latest_frame_stamp_).seconds();
if (dt > 5.0) {
FYT_WARN("camera_driver", "Camera is not alive! lost frame for {:.2f} seconds", dt);
close();
}
}
bool open()
{
if (is_open_) {
close();
}
FYT_INFO("camera_driver", "Opening Hik Camera Device!");
MV_CC_DEVICE_INFO_LIST device_list;
int ret = MV_CC_EnumDevices(MV_GIGE_DEVICE | MV_USB_DEVICE, &device_list);
if (ret != MV_OK || device_list.nDeviceNum == 0) {
FYT_WARN("camera_driver", "No camera found! Enum state: [0x{:x}]", ret);
return false;
}
FYT_INFO("camera_driver", "Found {} devices", device_list.nDeviceNum);
ret = MV_CC_CreateHandle(&camera_handle_, device_list.pDeviceInfo[0]);
if (ret != MV_OK) {
FYT_ERROR("camera_driver", "CreateHandle failed: [0x{:x}]", ret);
camera_handle_ = nullptr;
return false;
}
ret = MV_CC_OpenDevice(camera_handle_);
if (ret != MV_OK) {
FYT_ERROR("camera_driver", "OpenDevice failed: [0x{:x}]", ret);
MV_CC_DestroyHandle(&camera_handle_);
camera_handle_ = nullptr;
return false;
}
{
std::lock_guard<std::mutex> lock(camera_mutex_);
// Trigger off (continuous)
(void)MV_CC_SetTriggerMode(camera_handle_, MV_TRIGGER_MODE_OFF);
// Apply ROI/offset before querying image info
applyRoiLocked();
// Set exposure / gain
(void)MV_CC_SetFloatValue(camera_handle_, "ExposureTime", static_cast<float>(exposure_time_));
(void)MV_CC_SetFloatValue(camera_handle_, "Gain", static_cast<float>(gain_));
}
// Get image info
{
std::lock_guard<std::mutex> lock(camera_mutex_);
ret = MV_CC_GetImageInfo(camera_handle_, &img_info_);
}
if (ret != MV_OK) {
FYT_ERROR("camera_driver", "GetImageInfo failed: [0x{:x}]", ret);
close();
return false;
}
// Init convert param
convert_param_ = {};
convert_param_.nWidth = img_info_.nWidthValue;
convert_param_.nHeight = img_info_.nHeightValue;
convert_param_.enDstPixelType = PixelType_Gvsp_RGB8_Packed;
// Use actual output size from camera
resolution_width_ = static_cast<int>(img_info_.nWidthValue);
resolution_height_ = static_cast<int>(img_info_.nHeightValue);
image_msg_.height = resolution_height_;
image_msg_.width = resolution_width_;
image_msg_.step = resolution_width_ * 3;
image_msg_.data.resize(static_cast<size_t>(image_msg_.height) * image_msg_.step);
camera_info_msg_.header.frame_id = frame_id_;
camera_info_msg_.width = image_msg_.width;
camera_info_msg_.height = image_msg_.height;
latest_frame_stamp_ = this->now();
{
std::lock_guard<std::mutex> lock(camera_mutex_);
ret = MV_CC_StartGrabbing(camera_handle_);
}
if (ret != MV_OK) {
FYT_ERROR("camera_driver", "StartGrabbing failed: [0x{:x}]", ret);
close();
return false;
}
is_open_ = true;
FYT_INFO("camera_driver", "Hik Camera Device Open Success!");
return true;
}
void close()
{
if (!is_open_) {
return;
}
FYT_INFO("camera_driver", "Closing Hik Camera Device!");
is_open_ = false;
std::lock_guard<std::mutex> lock(camera_mutex_);
if (camera_handle_) {
(void)MV_CC_StopGrabbing(camera_handle_);
(void)MV_CC_CloseDevice(camera_handle_);
(void)MV_CC_DestroyHandle(&camera_handle_);
camera_handle_ = nullptr;
}
}
void captureLoop()
{
MV_FRAME_OUT out_frame;
while (rclcpp::ok()) {
if (!is_open_ || camera_handle_ == nullptr) {
std::this_thread::sleep_for(std::chrono::milliseconds(10));
continue;
}
int ret;
{
std::lock_guard<std::mutex> lock(camera_mutex_);
if (!is_open_ || camera_handle_ == nullptr) {
continue;
}
ret = MV_CC_GetImageBuffer(camera_handle_, &out_frame, 1000);
}
if (ret != MV_OK) {
FYT_WARN("camera_driver", "GetImageBuffer failed: [0x{:x}]", ret);
fail_count_++;
if (fail_count_ > 5) {
close();
}
continue;
}
// Ensure output buffer
const size_t need_size =
static_cast<size_t>(out_frame.stFrameInfo.nWidth) * out_frame.stFrameInfo.nHeight * 3;
if (image_msg_.data.size() != need_size) {
image_msg_.data.resize(need_size);
}
convert_param_.pDstBuffer = image_msg_.data.data();
convert_param_.nDstBufferSize = image_msg_.data.size();
convert_param_.pSrcData = out_frame.pBufAddr;
convert_param_.nSrcDataLen = out_frame.stFrameInfo.nFrameLen;
convert_param_.enSrcPixelType = out_frame.stFrameInfo.enPixelType;
{
std::lock_guard<std::mutex> lock(camera_mutex_);
if (!is_open_ || camera_handle_ == nullptr) {
continue;
}
ret = MV_CC_ConvertPixelType(camera_handle_, &convert_param_);
}
if (ret != MV_OK) {
FYT_WARN("camera_driver", "ConvertPixelType failed: [0x{:x}]", ret);
{
std::lock_guard<std::mutex> lock(camera_mutex_);
if (camera_handle_ != nullptr) {
(void)MV_CC_FreeImageBuffer(camera_handle_, &out_frame);
}
}
fail_count_++;
continue;
}
auto stamp = this->now();
image_msg_.header.stamp = stamp;
image_msg_.header.frame_id = frame_id_;
image_msg_.height = out_frame.stFrameInfo.nHeight;
image_msg_.width = out_frame.stFrameInfo.nWidth;
image_msg_.step = out_frame.stFrameInfo.nWidth * 3;
camera_info_msg_.header = image_msg_.header;
camera_pub_.publish(image_msg_, camera_info_msg_);
latest_frame_stamp_ = stamp;
fail_count_ = 0;
if (recorder_ != nullptr) {
recorder_->addFrame(image_msg_.data);
}
{
std::lock_guard<std::mutex> lock(camera_mutex_);
if (camera_handle_ != nullptr) {
(void)MV_CC_FreeImageBuffer(camera_handle_, &out_frame);
}
}
}
}
rcl_interfaces::msg::SetParametersResult parametersCallback(
const std::vector<rclcpp::Parameter> & parameters)
{
rcl_interfaces::msg::SetParametersResult result;
result.successful = true;
for (const auto & param : parameters) {
if (param.get_name() == "exposure_time") {
exposure_time_ = param.as_int();
std::lock_guard<std::mutex> lock(camera_mutex_);
if (camera_handle_ != nullptr) {
int status =
MV_CC_SetFloatValue(camera_handle_, "ExposureTime", static_cast<float>(exposure_time_));
if (status != MV_OK) {
result.successful = false;
result.reason = "Failed to set exposure time, status = " + std::to_string(status);
} else {
FYT_INFO("camera_driver", "Set exposure_time: {}", exposure_time_);
}
}
} else if (param.get_name() == "gain") {
gain_ = param.as_double();
std::lock_guard<std::mutex> lock(camera_mutex_);
if (camera_handle_ != nullptr) {
int status = MV_CC_SetFloatValue(camera_handle_, "Gain", static_cast<float>(gain_));
if (status != MV_OK) {
result.successful = false;
result.reason = "Failed to set gain, status = " + std::to_string(status);
} else {
FYT_INFO("camera_driver", "Set gain: {}", gain_);
}
}
} else if (param.get_name() == "resolution_width") {
resolution_width_ = param.as_int();
std::lock_guard<std::mutex> lock(camera_mutex_);
if (camera_handle_ != nullptr) {
applyRoiLocked();
}
} else if (param.get_name() == "resolution_height") {
resolution_height_ = param.as_int();
std::lock_guard<std::mutex> lock(camera_mutex_);
if (camera_handle_ != nullptr) {
applyRoiLocked();
}
} else if (param.get_name() == "offsetX") {
offset_x_ = param.as_int();
std::lock_guard<std::mutex> lock(camera_mutex_);
if (camera_handle_ != nullptr) {
applyRoiLocked();
}
} else if (param.get_name() == "offsetY") {
offset_y_ = param.as_int();
std::lock_guard<std::mutex> lock(camera_mutex_);
if (camera_handle_ != nullptr) {
applyRoiLocked();
}
}
}
return result;
}
// ROS pub
sensor_msgs::msg::Image image_msg_;
image_transport::CameraPublisher camera_pub_;
// camera info
std::string camera_name_;
std::string camera_info_url_;
std::string frame_id_;
std::string pixel_format_;
std::unique_ptr<camera_info_manager::CameraInfoManager> camera_info_manager_;
sensor_msgs::msg::CameraInfo camera_info_msg_;
// Watch dog
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Time latest_frame_stamp_{0, 0, RCL_ROS_TIME};
// Heartbeat
fyt::HeartBeatPublisher::SharedPtr heartbeat_;
// Params
int frame_rate_{210};
int exposure_time_{5000};
double gain_{12.0};
int resolution_width_{1280};
int resolution_height_{1024};
int offset_x_{0};
int offset_y_{0};
// Recorder
std::unique_ptr<Recorder> recorder_;
// Camera SDK
void * camera_handle_{nullptr};
MV_IMAGE_BASIC_INFO img_info_{};
MV_CC_PIXEL_CONVERT_PARAM convert_param_{};
std::mutex camera_mutex_;
// State
std::atomic<bool> is_open_{false};
int fail_count_{0};
std::thread capture_thread_;
rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr params_callback_handle_;
};
} // namespace fyt::camera_driver
#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(fyt::camera_driver::HikCameraNode)

82
src/rm_hardware_driver/ros2_hik_camera/src/recorder.cpp Normal file
View File

@@ -0,0 +1,82 @@
#include "hik_camera/recorder.hpp"
#include <chrono>
namespace fyt::camera_driver {
Recorder::Recorder(const std::filesystem::path &file, int fps, cv::Size size)
: path(file), size_(size), fps_(fps) {}
Recorder::~Recorder() { stop(); }
void Recorder::start() {
if (recording_.exchange(true)) {
return;
}
writer_.open(
path.string(), cv::VideoWriter::fourcc('M', 'J', 'P', 'G'), fps_, size_, true);
recorder_thread_ = std::thread(&Recorder::recorderThread, this);
}
void Recorder::stop() {
if (!recording_.exchange(false)) {
return;
}
cv_.notify_all();
if (recorder_thread_.joinable()) {
recorder_thread_.join();
}
if (writer_.isOpened()) {
writer_.release();
}
}
void Recorder::addFrame(const Frame &frame) {
if (!recording_.load()) {
return;
}
{
std::lock_guard<std::mutex> lock(mutex_);
frame_queue_.push_back(frame);
}
cv_.notify_one();
}
void Recorder::recorderThread() {
while (recording_.load()) {
Frame frame;
{
std::unique_lock<std::mutex> lock(mutex_);
cv_.wait_for(lock, std::chrono::milliseconds(100), [this]() {
return !recording_.load() || !frame_queue_.empty();
});
if (!recording_.load()) {
break;
}
if (frame_queue_.empty()) {
continue;
}
frame = std::move(frame_queue_.front());
frame_queue_.pop_front();
}
if (!writer_.isOpened()) {
continue;
}
if (frame.size() != static_cast<size_t>(size_.width * size_.height * 3)) {
continue;
}
cv::Mat img(size_.height, size_.width, CV_8UC3, frame.data());
writer_.write(img);
}
}
} // namespace fyt::camera_driver

171
src/rm_hardware_driver/ros2_hik_camera/src/video_player_node.cpp Normal file
View File

@@ -0,0 +1,171 @@
// Video player node for offline debugging
#include <camera_info_manager/camera_info_manager.hpp>
#include <image_transport/image_transport.hpp>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/image_encodings.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <chrono>
#include <filesystem>
#include <memory>
#include <string>
#include <opencv2/opencv.hpp>
#include "rm_utils/heartbeat.hpp"
#include "rm_utils/logger/log.hpp"
namespace fyt::camera_driver {
class VideoPlayerNode : public rclcpp::Node {
public:
explicit VideoPlayerNode(const rclcpp::NodeOptions &options) : Node("camera_driver", options) {
FYT_REGISTER_LOGGER("camera_driver", "~/fyt2024-log", INFO);
FYT_INFO("camera_driver", "Starting VideoPlayerNode!");
video_path_ = this->declare_parameter("path", "/tmp/video.avi");
camera_info_url_ =
this->declare_parameter("camera_info_url", "package://rm_bringup/config/camera_info.yaml");
frame_id_ = this->declare_parameter("frame_id", "camera_optical_frame");
frame_rate_ = this->declare_parameter("frame_rate", 30);
start_frame_ = this->declare_parameter("start_frame", 0);
keep_looping_ = this->declare_parameter("keep_looping", true);
heartbeat_ = fyt::HeartBeatPublisher::create(this);
const std::filesystem::path video_file(video_path_);
if (!std::filesystem::exists(video_file)) {
FYT_ERROR("camera_driver", "Video file {} does not exist!", video_file.string());
rclcpp::shutdown();
return;
}
cap_.open(video_path_);
if (!cap_.isOpened()) {
FYT_ERROR("camera_driver", "Video file open failed: {}", video_path_);
rclcpp::shutdown();
return;
}
image_msg_.header.frame_id = frame_id_;
image_msg_.encoding = sensor_msgs::image_encodings::BGR8;
image_msg_.width = static_cast<uint32_t>(cap_.get(cv::CAP_PROP_FRAME_WIDTH));
image_msg_.height = static_cast<uint32_t>(cap_.get(cv::CAP_PROP_FRAME_HEIGHT));
image_msg_.step = image_msg_.width * 3;
image_msg_.data.resize(static_cast<size_t>(image_msg_.step) * image_msg_.height);
camera_info_manager_ = std::make_shared<camera_info_manager::CameraInfoManager>(
this, "camera_driver", "file://" + video_path_);
if (camera_info_manager_->validateURL(camera_info_url_)) {
camera_info_manager_->loadCameraInfo(camera_info_url_);
camera_info_ = camera_info_manager_->getCameraInfo();
} else {
camera_info_manager_->setCameraName(video_path_);
sensor_msgs::msg::CameraInfo camera_info;
camera_info.header.frame_id = frame_id_;
camera_info.header.stamp = this->now();
camera_info.width = image_msg_.width;
camera_info.height = image_msg_.height;
camera_info_manager_->setCameraInfo(camera_info);
FYT_WARN("camera_driver", "Invalid camera info URL: {}", camera_info_url_);
}
camera_info_.header.frame_id = frame_id_;
camera_info_.width = image_msg_.width;
camera_info_.height = image_msg_.height;
bool use_sensor_data_qos = this->declare_parameter("use_sensor_data_qos", true);
auto qos = use_sensor_data_qos ? rmw_qos_profile_sensor_data : rmw_qos_profile_default;
camera_pub_ = image_transport::create_camera_publisher(this, "image_raw", qos);
const int publish_fps = (frame_rate_ > 0) ? frame_rate_ : 30;
const auto period = std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<double>(1.0 / static_cast<double>(publish_fps)));
timer_ = this->create_wall_timer(period, std::bind(&VideoPlayerNode::onTimer, this));
FYT_INFO(
"camera_driver", "Video player ready: {} ({}x{}, {} fps)", video_path_, image_msg_.width,
image_msg_.height, publish_fps);
}
private:
void onTimer() {
if (!cap_.isOpened()) {
return;
}
cv::Mat frame;
cap_ >> frame;
if (frame.empty()) {
FYT_INFO("camera_driver", "Video file ends!");
if (!keep_looping_) {
rclcpp::shutdown();
return;
}
cap_.release();
cap_.open(video_path_);
frame_cnt_ = 0;
return;
}
frame_cnt_++;
if (frame_cnt_ < start_frame_) {
return;
}
if (frame.cols != static_cast<int>(image_msg_.width) ||
frame.rows != static_cast<int>(image_msg_.height) || frame.type() != CV_8UC3) {
cv::Mat bgr;
if (frame.channels() == 3) {
bgr = frame;
} else if (frame.channels() == 1) {
cv::cvtColor(frame, bgr, cv::COLOR_GRAY2BGR);
} else {
frame.convertTo(bgr, CV_8UC3);
}
if (bgr.cols != static_cast<int>(image_msg_.width) || bgr.rows != static_cast<int>(image_msg_.height)) {
cv::resize(bgr, bgr, cv::Size(static_cast<int>(image_msg_.width), static_cast<int>(image_msg_.height)));
}
std::memcpy(image_msg_.data.data(), bgr.data, image_msg_.data.size());
} else {
std::memcpy(image_msg_.data.data(), frame.data, image_msg_.data.size());
}
auto stamp = this->now();
image_msg_.header.stamp = stamp;
camera_info_.header.stamp = stamp;
camera_info_.header.frame_id = frame_id_;
image_msg_.header.frame_id = frame_id_;
camera_pub_.publish(image_msg_, camera_info_);
}
fyt::HeartBeatPublisher::SharedPtr heartbeat_;
std::string video_path_;
std::string camera_info_url_;
std::string frame_id_;
int frame_rate_{30};
int start_frame_{0};
bool keep_looping_{true};
int frame_cnt_{0};
cv::VideoCapture cap_;
image_transport::CameraPublisher camera_pub_;
sensor_msgs::msg::Image image_msg_;
sensor_msgs::msg::CameraInfo camera_info_;
std::shared_ptr<camera_info_manager::CameraInfoManager> camera_info_manager_;
rclcpp::TimerBase::SharedPtr timer_;
};
} // namespace fyt::camera_driver
#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(fyt::camera_driver::VideoPlayerNode)